MR

Transcript

1 MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL Kevin M. Lynch and Frank C. Park May 3, 2017 This document is the preprint version of Modern Robotics Mechanics, Planning, and Control c © Kevin M. Lynch and Frank C. Park This preprint is being made available for personal use only and not for further distribution. The book will be published by Cambridge University Press in May 2017, ISBN 9781107156302. Citations of the book should cite Cambridge University Press as the publisher, with a publication date of 2017. Original figures from this book may be reused provided proper citation is given. More information on the book, including software, videos, and a feedback form can be found at http://modernrobotics.org . Comments are welcome!

2

3 Contents Foreword by Roger Brockett ix xi Foreword by Matthew Mason Preface xiii 1 1 Preview 2 Configuration Space 11 2.1 Degrees of Freedom of a Rigid Body 12 . . . . . . . . . . . . . . . . 2.2 Degrees of Freedom of a Robot . . . . . . . . . . . . . . . . . . . 15 Robot Joints . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 16 2.2.2 . . . . . . . . . . . . . . . . . . . . . . 17 Gr ̈ubler’s Formula . . . . . . . . 2.3 Configuration Space: Topology and Representation 23 Configuration Space Topology . . . . . . . . . . . . . . . . 2.3.1 23 2.3.2 Configuration Space Representation . . . . . . . . . . . . 25 2.4 Configuration and Velocity Constraints . . . . . . . . . . . . . . . 29 2.5 Task Space and Workspace . . . . . . . . . . . . . . . . . . . . . 32 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.6 Summary 36 2.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 38 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.8 Exercises 38 3 Rigid-Body Motions 59 3.1 Rigid-Body Motions in the Plane . . . . . . . . . . . . . . . . . . 62 3.2 Rotations and Angular Velocities . . . . . . . . . . . . . . . . . . 68 3.2.1 . . . . . . . . . . . . . . . . . . . . . . 68 Rotation Matrices 3.2.2 Angular Velocities . . . . . . . . . . . . . . . . . . . . . . 76 3.2.3 Exponential Coordinate Representation of Rotation . . . 79 3.3 Rigid-Body Motions and Twists . . . . . . . . . . . . . . . . . . . 89 i

4 ii Contents Homogeneous Transformation Matrices . . . . . . . . . . 3.3.1 89 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3.2 97 Twists 3.3.3 Exponential Coordinate Representation of Rigid-Body Mo- . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 tions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 3.4 Wrenches 111 3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 3.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 3.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 3.8 Exercises 116 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 Forward Kinematics 137 4.1 Product of Exponentials Formula 140 . . . . . . . . . . . . . . . . . . 4.1.1 . . . . 141 First Formulation: Screw Axes in the Base Frame Examples 143 4.1.2 . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.3 Second Formulation: Screw Axes in the End-Effector Frame148 4.2 The Universal Robot Description Format . . . . . . . . . . . . . 152 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 4.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Software 159 . . . . . . . . . . . . . . . . . . . . . . . . . 160 4.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160 4.6 Exercises 5 Velocity Kinematics and Statics 171 5.1 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 178 5.1.1 Space Jacobian 178 . . . . . . . . . . . . . . . . . . . . . . . . Body Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 5.1.2 183 . . . . . . . . . 185 Visualizing the Space and Body Jacobian 5.1.3 Relationship between the Space and Body Jacobian . . . 187 5.1.4 Alternative Notions of the Jacobian 5.1.5 187 . . . . . . . . . . . . 5.1.6 . . . . . . 189 Looking Ahead to Inverse Velocity Kinematics 5.2 Statics of Open Chains 190 . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Singularity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . 191 5.4 Manipulability . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196 5.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.6 Software 201 . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202 5.8 Exercises 6 Inverse Kinematics 219 6.1 Analytic Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . 221 6.1.1 . . . . . . . . . . . . . . . . . . . . 221 6R PUMA-Type Arm 6.1.2 Stanford-Type Arms . . . . . . . . . . . . . . . . . . . . . 225 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

5 Contents iii . . . . . . . . . . . . . . . . . . . . 226 6.2 Numerical Inverse Kinematics . . . . . . . . . . . . . . . . . . Newton–Raphson Method 227 6.2.1 227 6.2.2 . . . . . . . . . Numerical Inverse Kinematics Algorithm 6.3 Inverse Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . 232 6.4 A Note on Closed Loops . . . . . . . . . . . . . . . . . . . . . . . 234 6.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6 Software 235 236 . . . . . . . . . . . . . . . . . . . . . . . . . 6.7 Notes and References 6.8 Exercises 236 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 Kinematics of Closed Chains 245 . . . . . . . . . . . . . . . . . . 7.1 Inverse and Forward Kinematics 247 7.1.1 × RPR Planar Parallel Mechanism . . . . . . . . . . . . . 247 3 Stewart–Gough Platform 249 7.1.2 . . . . . . . . . . . . . . . . . . 7.1.3 General Parallel Mechanisms . . . . . . . . . . . . . . . . 251 . . . . . . . . . . . . . . . . . . . . . . . . 252 7.2 Differential Kinematics Stewart–Gough Platform . . . . . . . . . . . . . . . . . . 252 7.2.1 General Parallel Mechanisms . . . . . . . . . . . . . . . . 254 7.2.2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256 7.3 Singularities 7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 261 7.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 262 7.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263 271 8 Dynamics of Open Chains 272 8.1 Lagrangian Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Basic Concepts and Motivating Examples 8.1.1 272 8.1.2 General Formulation . . . . . . . . . . . . . . . . . . . . . 277 Understanding the Mass Matrix 8.1.3 279 . . . . . . . . . . . . . . 8.1.4 . . . . 281 Lagrangian Dynamics vs. Newton–Euler Dynamics . . . . . . . . . . . . . . . . . . 8.2 Dynamics of a Single Rigid Body 283 8.2.1 Classical Formulation . . . . . . . . . . . . . . . . . . . . 283 8.2.2 Twist–Wrench Formulation . . . . . . . . . . . . . . . . . 288 8.2.3 Dynamics in Other Frames 290 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3 Newton–Euler Inverse Dynamics 291 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3.1 291 8.3.2 Newton-Euler Inverse Dynamics Algorithm . . . . . . . . 294 8.4 Dynamic Equations in Closed Form . . . . . . . . . . . . . . . . . 294 8.5 Forward Dynamics of Open Chains 298 . . . . . . . . . . . . . . . . . 8.6 Dynamics in the Task Space . . . . . . . . . . . . . . . . . . . . . 300 8.7 Constrained Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 301 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

6 iv Contents . . . . . . . . . . . . . . . . . . . 303 8.8 Robot Dynamics in the URDF 303 . . . . . . . . . . . . . . . . . . 8.9 Actuation, Gearing, and Friction 305 DC Motors and Gearing . . . . . . . . . . . . . . . . . . . 8.9.1 8.9.2 Apparent Inertia 310 . . . . . . . . . . . . . . . . . . . . . . . 8.9.3 Newton–Euler Inverse Dynamics Algorithm Accounting for Motor Inertias and Gearing . . . . . . . . . . . . . . . 312 Friction 314 8.9.4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Joint and Link Flexibility 8.9.5 314 8.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315 8.11 Software 319 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.12 Notes and References 320 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 321 8.13 Exercises 9 Trajectory Generation 325 9.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325 9.2 Point-to-Point Trajectories . . . . . . . . . . . . . . . . . . . . . 326 Straight-Line Paths 9.2.1 326 . . . . . . . . . . . . . . . . . . . . . 9.2.2 . . . . . . . . . . . . . 328 Time Scaling a Straight-Line Path 9.3 Polynomial Via Point Trajectories 334 . . . . . . . . . . . . . . . . . 9.4 Time-Optimal Time Scaling . . . . . . . . . . . . . . . . . . . . . 336 9.4.1 The ( s, ̇ s ) Phase Plane . . . . . . . . . . . . . . . . . . . . 339 9.4.2 The Time-Scaling Algorithm 341 . . . . . . . . . . . . . . . . A Variation on the Time-Scaling Algorithm . . . . . . . . 9.4.3 342 . . . . . . . . . . . . . . . . . . Assumptions and Caveats 9.4.4 344 9.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.6 Software 346 9.7 Notes and References 347 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348 9.8 Exercises 353 10 Motion Planning 10.1 Overview of Motion Planning . . . . . . . . . . . . . . . . . . . . 353 10.1.1 Types of Motion Planning Problems . . . . . . . . . . . . 354 10.1.2 Properties of Motion Planners . . . . . . . . . . . . . . . 355 . . . . . . . . . . . . . . . . . . 356 10.1.3 Motion Planning Methods 10.2 Foundations 358 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.2.1 Configuration Space Obstacles . . . . . . . . . . . . . . . 358 10.2.2 Distance to Obstacles and Collision Detection . . . . . . . 362 10.2.3 Graphs and Trees 364 . . . . . . . . . . . . . . . . . . . . . . . 10.2.4 Graph Search . . . . . . . . . . . . . . . . . . . . . . . . . 365 10.3 Complete Path Planners . . . . . . . . . . . . . . . . . . . . . . . 368 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

7 Contents v . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369 10.4 Grid Methods 372 . . . . . . . . . . . 10.4.1 Multi-Resolution Grid Representation . . . . . . . . . . 373 10.4.2 Grid Methods with Motion Constraints 10.5 Sampling Methods . . . . . . . . . . . . . . . . . . . . . . . . . . 378 . . . . . . . . . . . . . . . . . . . . . 379 10.5.1 The RRT Algorithm 10.5.2 The PRM Algorithm . . . . . . . . . . . . . . . . . . . . . 384 . . . . . . . . . . . . . . . . . . . . . . . 386 10.6 Virtual Potential Fields 10.6.1 A Point in C-space . . . . . . . . . . . . . . . . . . . . . . 386 10.6.2 Navigation Functions . . . . . . . . . . . . . . . . . . . . . 389 . . . . . . . . . . . . . . . . . . . . . 10.6.3 Workspace Potential 390 10.6.4 Wheeled Mobile Robots 391 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.6.5 Use of Potential Fields in Planners 392 10.7 Nonlinear Optimization . . . . . . . . . . . . . . . . . . . . . . . 392 10.8 Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394 10.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . 10.10Notes and References 397 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398 10.11Exercises 403 11 Robot Control 11.1 Control System Overview . . . . . . . . . . . . . . . . . . . . . . 404 11.2 Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405 11.2.1 Error Response . . . . . . . . . . . . . . . . . . . . . . . . 406 . . . . . . . . . . . . . . . . . . . 406 11.2.2 Linear Error Dynamics 413 11.3 Motion Control with Velocity Inputs . . . . . . . . . . . . . . . . 11.3.1 Motion Control of a Single Joint . . . . . . . . . . . . . . 414 11.3.2 Motion Control of a Multi-joint Robot . . . . . . . . . . . 418 . . . . . . . . . . . . . . . . . 11.3.3 Task-Space Motion Control 419 11.4 Motion Control with Torque or Force Inputs 420 . . . . . . . . . . . . . . . . . . . . . . . . . . 421 11.4.1 Motion Control of a Single Joint . . . . . . . . . . . 429 11.4.2 Motion Control of a Multi-joint Robot 11.4.3 Task-Space Motion Control . . . . . . . . . . . . . . . . . 433 11.5 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434 11.6 Hybrid Motion–Force Control . . . . . . . . . . . . . . . . . . . . 437 . . . . . . . . . . . . . 437 11.6.1 Natural and Artificial Constraints 11.6.2 A Hybrid Motion–Force Controller 439 . . . . . . . . . . . . . 11.7 Impedance Control . . . . . . . . . . . . . . . . . . . . . . . . . . 441 11.7.1 Impedance-Control Algorithm . . . . . . . . . . . . . . . . 443 11.7.2 Admittance-Control Algorithm 444 . . . . . . . . . . . . . . . 11.8 Low-Level Joint Force/Torque Control . . . . . . . . . . . . . . . 445 11.9 Other Topics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 448 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

8 vi Contents 449 11.10Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451 11.11Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 452 . . . . . . . . . . . . . . . . . . . . . . . . . 11.12Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 453 11.13Exercises 12 Grasping and Manipulation 461 463 12.1 Contact Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 463 12.1.1 First-Order Analysis of a Single Contact . . . . . . . . . . 12.1.2 Contact Types: Rolling, Sliding, and Breaking Free . . . . 465 12.1.3 Multiple Contacts . . . . . . . . . . . . . . . . . . . . . . 468 . . . . . . . . . . . . . . . . . . . . . 472 12.1.4 Collections of Bodies . . . . . . . . . . . . . . . . . . . 12.1.5 Other Types of Contacts 472 . . . . . . . . . . . . . . . . . . 12.1.6 Planar Graphical Methods 473 12.1.7 Form Closure . . . . . . . . . . . . . . . . . . . . . . . . . 478 12.2 Contact Forces and Friction . . . . . . . . . . . . . . . . . . . . . 484 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 484 12.2.1 Friction 12.2.2 Planar Graphical Methods . . . . . . . . . . . . . . . . . . 487 . . . . . . . . . . . . . . . . . . . . . . . . . 489 12.2.3 Force Closure 12.2.4 Duality of Force and Motion Freedoms 494 . . . . . . . . . . . 12.3 Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494 12.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 501 12.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 503 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 504 12.6 Exercises 13 Wheeled Mobile Robots 513 13.1 Types of Wheeled Mobile Robots . . . . . . . . . . . . . . . . . . 514 13.2 Omnidirectional Wheeled Mobile Robots . . . . . . . . . . . . . . 515 13.2.1 Modeling 515 . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2.2 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . 520 13.2.3 Feedback Control . . . . . . . . . . . . . . . . . . . . . . . 520 13.3 Nonholonomic Wheeled Mobile Robots . . . . . . . . . . . . . . . 520 13.3.1 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . 521 13.3.2 Controllability . . . . . . . . . . . . . . . . . . . . . . . . 528 . . . . . . . . . . . . . . . . . . . . . . . 537 13.3.3 Motion Planning 13.3.4 Feedback Control 542 . . . . . . . . . . . . . . . . . . . . . . . 13.4 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 546 13.5 Mobile Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . 548 13.6 Summary 552 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 554 13.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 555 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

9 Contents vii 565 A Summary of Useful Formulas 575 B Other Representations of Rotations B.1 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 575 B.1.1 Algorithm for Computing the ZYX Euler Angles . . . . . 577 . . . . . . . . . . . . . 577 B.1.2 Other Euler Angle Representations . . . . . . . . . . . . . . . . . . . . . . . B.2 Roll–Pitch–Yaw Angles 580 . . . . . . . . . . . . . . . . . . . . . . . . . . . 581 B.3 Unit Quaternions . . . . . . . . . . . . . . . . . . . . 582 B.4 Cayley–Rodrigues Parameters C Denavit–Hartenberg Parameters 585 C.1 Assigning Link Frames . . . . . . . . . . . . . . . . . . . . . . . . 585 C.2 Why Four Parameters are Sufficient . . . . . . . . . . . . . . . . 589 . . . . . . . . . . . . . . . . . . 590 C.3 Manipulator Forward Kinematics C.4 Examples 591 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . C.5 Relation Between the PoE and D–H Representations . . . . . . . 593 C.6 A Final Comparison . . . . . . . . . . . . . . . . . . . . . . . . . 595 D Optimization and Lagrange Multipliers 597 Bibliography 599 Index 617 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

10 viii Contents May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

11 Foreword by Roger Brockett In the 1870s, Felix Klein was developing his far-reaching Erlangen Program, which cemented the relationship between geometry and group theoretic ideas. With Sophus Lie’s nearly simultaneous development of a theory of continuous (Lie) groups, important new tools involving infinitesimal analysis based on Lie algebraic ideas became available for the study of a very wide range of geomet- ric problems. Even today, the thinking behind these ideas continues to guide developments in important areas of mathematics. Kinematic mechanisms are, of course, more than just geometry; they need to accelerate, avoid collisions, etc., but first of all they are geometrical objects and the ideas of Klein and Lie apply. The groups of rigid motions in two or three dimensions, as they appear in robotics, are important examples in the work of Klein and Lie. In the mathematics literature the representation of elements of a Lie group in terms of exponentials usually takes one of two different forms. These are known as exponential coordinates of the first kind and exponential coordinates of the ) ··· x ( A A x + 1 2 2 1 second kind. For the first kind one has = X . For the second kind e A x A x 1 1 2 2 e this is replaced by X = e ··· . Up until now, the first choice has found little utility in the study of kinematics whereas the second choice, a special case having already shown up in Euler parametrizations of the orthogonal group, turns out to be remarkably well-suited for the description of open kinematic chains consisting of the concatenation of single degree of freedom links. This is all nicely explained in Chapter 4 of this book. Together with the fact that − 1 A PAP − 1 , the second form allows one to express a wide variety of Pe e = P kinematic problems very succinctly. From a historical perspective, the use of the product of exponentials to represent robotic movement, as the authors have done here, can be seen as illustrating the practical utility of the 150-year-old ideas of the geometers Klein and Lie. In 1983 I was invited to speak at the triennial Mathematical Theory of Net- ix

12 x Foreword works and Systems Conference in Beer Sheva, Israel, and after a little thought I decided to try to explain something about what my recent experiences had taught me. By then I had some experience in teaching a robotics course that discussed kinematics, including the use of the product of exponentials represen- At tation of kinematic chains. From the 1960s onward e had played a central role in system theory and signal processing, so at this conference a familiarity, even an affection, for the matrix exponential could be counted on. Given this, it Ax e -related for the talk. Although I had was natural for me to pick something no reason to think that there would be many in the audience with an interest in kinematics, I still hoped I could say something interesting and maybe even inspire further developments. The result was the paper referred to in the preface that follows. In this book, Frank and Kevin have provided a wonderfully clear and patient explanation of their subject. They translate the foundation laid out by Klein and Lie 150 years ago to the modern practice of robotics, at a level appropriate for undergraduate engineers. After an elegant discussion of fundamental prop- erties of configuration spaces, they introduce the Lie group representations of rigid-body configurations, and the corresponding representations of velocities and forces, used throughout the book. This consistent perspective is carried through foundational robotics topics including forward, inverse, and differential kinematics of open chains, robot dynamics, trajectory generation, and robot control, and more specialized topics such as kinematics of closed chains, motion planning, robot manipulation, planning and control for wheeled mobile robots, and control of mobile manipulators. I am confident that this book will be a valuable resource for a generation of students and practitioners of robotics. Roger Brockett Cambridge, Massachusetts, USA November, 2016 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

13 Foreword by Matthew Mason Robotics is about turning ideas into action. Somehow, robots turn abstract goals into physical action: sending power to motors, monitoring motions, and guiding things towards the goal. Every human can perform this trick, but it is nonetheless so intriguing that it has captivated philosophers and scientists, including Descartes and many others. What is the secret? Did some roboticist have a eureka moment? Did some pair of teenage entrepreneurs hit on the key idea in their garage? To the con- trary, it is not a single idea. It is a substantial body of scientific and engineer- ing results, accumulated over centuries. It draws primarily from mathematics, physics, mechanical engineering, electrical engineering, and computer science, but also from philosophy, psychology, biology and other fields. Robotics is the gathering place of these ideas. Robotics provides motivation. Robotics tests ideas and steers continuing research. Finally, robotics is the proof. Observing a robot’s behavior is the nearly compelling proof that machines can be aware of their surroundings, can develop meaningful goals, and can act effectively to accomplish those goals. The same principles apply to a thermostat or a fly-ball governor, but few are persuaded by watching a thermostat. Nearly all are persuaded by watching a robot soccer team. The heart of robotics is motion – controlled programmable motion – which brings us to the present text. Modern Robotics imparts the most important insights of robotics: the nature of motion, the motions available to rigid bodies, the use of kinematic constraint to organize motions, the mechanisms that enable general programmable motion, the static and dynamic character of mechanisms, and the challenges and approaches to control, programming, and planning mo- tions. Modern Robotics presents this material with a clarity that makes it acces- sible to undergraduate students. It is distinguished from other undergraduate texts in two important ways. xi

14 xii Foreword Modern Robotics First, in addressing rigid-body motion, presents not only the classical geometrical underpinnings and representations, but also their ex- pression using modern matrix exponentials, and the connection to Lie algebras. The rewards to the students are two-fold: a deeper understanding of motion, and better practical tools. Second, goes beyond a focus on robot mechanisms to ad- Modern Robotics dress the interaction with objects in the surrounding world. When robots make contact with the real world, the result is an ad hoc kinematic mechanism, with associated statics and dynamics. The mechanism includes kinematic loops, un- actuated joints, and nonholonomic constraints, all of which will be familiar concepts to students of . Modern Robotics Even if this is the only robotics course students take, it will enable them to analyze, control, and program a wide range of physical systems. With its introduction to the mechanics of physical interaction, Modern Robotics is also an excellent beginning for the student who intends to continue with advanced courses or with original research in robotics. Matthew T. Mason Pittsburgh, PA, USA November, 2016 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

15 Preface It was at the IEEE International Conference on Robotics and Automation in Pasadena in 2008 when, over a beer, we decided to write an undergraduate textbook on robotics. Since 1996, Frank had been teaching robot kinematics to Seoul National University undergraduates using his own lecture notes; by 2008 these notes had evolved to the kernel around which this book was written. Kevin had been teaching his introductory robotics class at Northwestern University from his own set of notes, with content drawn from an eclectic collection of papers and books. We believe that there is a distinct and unifying perspective to mechanics, planning, and control for robots that is lost if these subjects are studied inde- pendently, or as part of other more traditional subjects. At the 2008 meeting, we noted the lack of a textbook that (a) treated these topics in a unified way, with plenty of exercises and figures, and (b), most importantly, was written at a level appropriate for a first robotics course for undergraduates with only freshman-level physics, ordinary differential equations, linear algebra, and a lit- tle bit of computing background. We decided that the only sensible recourse was to write such a book ourselves. (We didn’t know then that it would take us more than eight years to finish the project!) A second motivation for this book, and one that we believe sets it apart from other introductory treatments on robotics, is its emphasis on modern geometric techniques. Often the most salient physical features of a robot are best captured by a geometric description. The advantages of the geometric approach have been recognized for quite some time by practitioners of classical screw theory. What has made these tools largely inaccessible to undergraduates—the primary tar- get audience for this book—is that they require an entirely new language of notations and constructs (screws, twists, wrenches, reciprocity, transversality, conjugacy, etc.), and their often obscure rules for manipulation and transfor- mation. On the other hand, the mostly algebraic alternatives to screw theory often mean that students end up buried in the details of calculation, losing the xiii

16 xiv Preface simple and elegant geometric interpretation that lies at the heart of what they are calculating. The breakthrough that makes the techniques of classical screw theory ac- cessible to a more general audience arrived in the early 1980’s, when Roger Brockett showed how to mathematically describe kinematic chains in terms of the Lie group structure of the rigid-body motions [20]. This discovery allowed one, among other things, to re-invent screw theory simply by appealing to basic linear algebra and linear differential equations. With this “modern screw the- ory” the powerful tools of modern differential geometry can be brought to bear on a wide-ranging collection of robotics problems, some of which we explore here, others of which are covered in the excellent but more advanced graduate textbook by Murray, Li and Sastry [122]. As the title indicates, this book covers what we feel to be the fundamentals of robot mechanics, together with the basics of planning and control. A thor- ough treatment of all the chapters would likely take two semesters, particularly when coupled with programming assignments or experiments with robots. The contents of Chapters 2-6 constitute the minimum essentials, and these topics should probably be covered in sequence. The instructor can then selectively choose content from the remaining chap- ters. At Seoul National University, the undergraduate course M2794.0027 Intro- duction to Robotics covers, in one semester, Chapters 2-7 and parts of Chapters 10, 11, and 12. At Northwestern, ME 449 Robotic Manipulation covers, in an 11- week quarter, Chapters 2-6 and 8, then touches on different topics in Chapters 9-13 depending on the interests of the students and instructor. A course focus- ing on the kinematics of robot arms and wheeled vehicles could cover chapters 2-7 and 13, while a course on kinematics and motion planning could addition- ally include Chapters 9 and 10. A course on the mechanics of manipulation would cover Chapters 2-6, 8, and 12, while a course on robot control would cover Chapters 2-6, 8, 9, and 11. If the instructor prefers to avoid dynamics (Chapter 8), the basics of robot control (Chapters 11 and 13) can be covered by assuming control of velocity at each actuator, not forces and torques. A course focusing only on motion planning could cover Chapters 2 and 3, Chapter 10 in depth (possibly supplemented by research papers or other references cited in that chapter), and Chapter 13. To help the instructor choose which topics to teach and to help the student keep track of what she has learned, we have included a summary at the end of each chapter and a summary of important notation and formulas used through- out the book (Appendix A). For those whose primary interest in this text is as an introductory reference, we have attempted to provide a reasonably com- prehensive, though by no means exhaustive, set of references and bibliographic May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

17 Preface xv notes at the end of each chapter. Some of the exercises provided at the end of each chapter extend the basic results covered in the book, and for those who wish to probe further, these should be of some interest in their own right. Some of the more advanced material in the book can be used to support independent study projects. Another important component of the book is the software, which is written to reinforce the concepts in the book and to make the formulas operational. The software was developed primarily by Kevin’s ME 449 students at Northwestern and is freely downloadable from http://modernrobotics.org . Video lectures that accompany the textbook will also be available at the website. The intent of the video content is to “flip” the classroom. Students watch the brief lectures on their own time, rewinding and rewatching as needed, and class time is fo- cused more on collaborative problem-solving. This way, the professor is present when the students are applying the material and discovering the gaps in their understanding, creating the opportunity for interactive mini-lectures addressing the concepts that need most reinforcing. We believe that the added value of the professor is greatest in this interactive role, not in delivering a lecture the same way it was delivered the previous year. This approach has worked well for Kevin’s introduction to mechatronics course, http://nu32.org . Video content is generated using the Lightboard, http://lightboard.info , created by Michael Peshkin at Northwestern University. We thank him for sharing this convenient and effective tool for creating instructional videos. V-REP robot simulation software We have also found the to be a valuable supplement to the book and its software. This simulation software allows stu- dents to interactively explore the kinematics of robot arms and mobile manipu- lators and to animate trajectories that are the result of exercises on kinematics, dynamics, and control. While this book presents our own perspective on how to introduce the fun- damental topics in first courses on robot mechanics, planning, and control, we acknowledge the excellent textbooks that already exist and that have served our field well. Among these, we would like to mention as particularly influential the books by Murray, Li, and Sastry [122]; Craig [32]; Spong, Hutchinson, and Vidyasagar [177]; Siciliano, Sciavicco, Villani, and Oriolo [171]; Mason [109]; Corke [30]; and the motion planning books by Latombe [80], LaValle [83], and Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki, and Thrun [27]. In ad- dition, the Handbook of Robotics [170], edited by Siciliano and Khatib with a multimedia extension edited by Kr ̈oger ( http://handbookofrobotics.org ), is a landmark in our field, collecting the perspectives of hundreds of leading researchers on a huge variety of topics relevant to modern robotics. It is our pleasure to acknowledge the many people who have been the sources May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

18 xvi Preface of help and inspiration in writing this book. In particular, we would like to thank our Ph.D. advisors, Roger Brockett and Matt Mason. Brockett laid down much of the foundation for the geometric approach to robotics employed in this book. Mason’s pioneering contributions to analysis and planning for manipula- tion form a cornerstone of modern robotics. We also thank the many students who provided feedback on various versions of this material, in M2794.0027 at Seoul National University and in ME 449 at Northwestern University. In par- ticular, Frank would like to thank Seunghyeon Kim, Keunjun Choi, Jisoo Hong, Jinkyu Kim, Youngsuk Hong, Wooyoung Kim, Cheongjae Jang, Taeyoon Lee, Soocheol Noh, Kyumin Park, Seongjae Jeong, Sukho Yoon, Jaewoon Kwen, Jinhyuk Park, and Jihoon Song, as well as Jim Bobrow and Scott Ploen from his time at UC Irvine. Kevin would like to thank Matt Elwin, Sherif Mostafa, Nelson Rosa, Jarvis Schultz, Jian Shi, Mikhail Todes, Huan Weng, and Zack Woodruff. Finally, and most importantly, we thank our wives and families, for putting up with our late nights and our general unavailability, and for supporting us as we made the final push to finish the book. Without the love and support of Hyunmee, Shiyeon, and Soonkyu (Frank) and Yuko, Erin, and Patrick (Kevin), this book would not exist. We dedicate this book to them. Kevin M. Lynch Evanston, Illinois, USA Frank C. Park Seoul, Korea November, 2016 Publication note. The authors consider themselves to be equal contributors to this book. Author order is alphabetical. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

19 Chapter 1 Preview As an academic discipline, robotics is a relatively young field with highly am- bitious goals, the ultimate one being the creation of machines that can behave and think like humans. This attempt to create intelligent machines naturally leads us first to examine ourselves – to ask, for example, why our bodies are designed the way they are, how our limbs are coordinated, and how we learn and perform complex tasks. The sense that the fundamental questions in robotics are ultimately questions about ourselves is part of what makes robotics such a fascinating and engaging endeavor. Our focus in this book is on mechanics, planning, and control for robot mechanisms . Robot arms are one familiar example. So are wheeled vehicles, as are robot arms mounted on wheeled vehicles. Basically, a mechanism is con- structed by connecting rigid bodies, called links , together by means of joints , so that relative motion between adjacent links becomes possible. of Actuation the joints, typically by electric motors, then causes the robot to move and exert forces in desired ways. The links of a robot mechanism can be arranged in serial fashion, like the familiar open-chain arm shown in Figure 1.1(a). Robot mechanisms can also have links that form closed loops, such as the Stewart–Gough platform shown in Figure 1.1(b). In the case of an open chain, all the joints are actuated, while in the case of mechanisms with closed loops, only a subset of the joints may be actuated. Let us examine more closely the current technology behind robot mecha- nisms. The links are moved by actuators, which typically are electrically driven (e.g., by DC or AC motors, stepper motors, or shape memory alloys) but can also be driven by pneumatic or hydraulic cylinders. In the case of rotating 1

20 2 (a) An open-chain industrial manipulator, (b) Stewart–Gough platform. Closed loops are formed from the base plat- visualized in V-REP [154]. form, through the legs, through the top platform, and through the legs back to the base platform. Open-chain and closed-chain robot mechanisms. Figure 1.1: electric motors, these would ideally be lightweight, operate at relatively low ro- tational speeds (e.g., in the range of hundreds of RPM), and be able to generate large forces and torques. Since most currently available motors operate at low torques and at up to thousands of RPM, speed reduction and torque ampli- fication are required. Examples of such transmissions or transformers include gears, cable drives, belts and pulleys, and chains and sprockets. These speed- reduction devices should have zero or low slippage and backlash (defined as the amount of rotation available at the output of the speed-reduction device without motion at the input). Brakes may also be attached to stop the robot quickly or to maintain a stationary posture. Robots are also equipped with sensors to measure the motion at the joints. For both revolute and prismatic joints, encoders, potentiometers, or resolvers measure the displacement and sometimes tachometers are used to measure ve- locity. Forces and torques at the joints or at the end-effector of the robot can be measured using various types of force–torque sensors. Additional sensors may be used to help localize objects or the robot itself, such as vision-only cameras, RGB-D cameras which measure the color (RGB) and depth (D) to each pixel, laser range finders, and various types of acoustic sensor. The study of robotics often includes artificial intelligence and computer per- ception, but an essential feature of any robot is that it moves in the physical May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

21 Chapter 1. Preview 3 world. Therefore, this book, which is intended to support a first course in robotics for undergraduates and graduate students, focuses on mechanics, mo- tion planning, and control of robot mechanisms. In the rest of this chapter we provide a preview of the rest of the book. Chapter 2: Configuration Space As mentioned above, at its most basic level a robot consists of rigid bodies connected by joints, with the joints driven by actuators. In practice the links may not be completely rigid, and the joints may be affected by factors such as elasticity, backlash, friction, and hysteresis. In this book we ignore these effects for the most part and assume that all links are rigid. With this assumption, Chapter 2 focuses on representing the configuration of a robot system, which is a specification of the position of every point of the robot. Since the robot consists of a collection of rigid bodies connected by joints, our study begins with understanding the configuration of a rigid body. We see that the configuration of a rigid body in the plane can be described using three variables (two for the position and one for the orientation) and the configuration of a rigid body in space can be described using six variables (three for the position and three for the orientation). The number of variables is the degrees of freedom (dof) of the rigid body. It is also the dimension number of configuration space , the space of all configurations of the body. of the The dof of a robot, and hence the dimension of its configuration space, is the sum of the dof of its rigid bodies minus the number of constraints on the motion of those rigid bodies provided by the joints. For example, the two most popular joints, revolute (rotational) and prismatic (translational) joints, allow only one motion freedom between the two bodies they connect. Therefore a revolute or prismatic joint can be thought of as providing five constraints on the motion of one spatial rigid body relative to another. Knowing the dof of a rigid body and the number of constraints provided by joints, we can derive Gr ̈ubler’s formula for calculating the dof of general robot mechanisms. For robots such as the industrial manipulator of Figure open-chain 1.1(a), each joint is independently actuated and the dof is simply the sum of the freedoms provided by each joint. For closed chains like the Stewart–Gough platform in Figure 1.1(b), Gr ̈ubler’s formula is a convenient way to calculate a lower bound on the dof. Unlike open-chain robots, some joints of closed chains are not actuated. Apart from calculating the dof, other configuration space concepts of interest include the topology (or “shape”) of the configuration space and its repre- sentation . Two configuration spaces of the same dimension may have different May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

22 4 shapes, just like a two-dimensional plane has a different shape from the two- dimensional surface of a sphere. These differences become important when de- termining how to represent the space. The surface of a unit sphere, for example, could be represented using a minimal number of coordinates, such as latitude ) subject to x,y,z and longitude, or it could be represented by three numbers ( 2 2 2 y + z the constraint = 1. The former is an explicit parametrization + x of the space. Each implicit parametrization of the space and the latter is an type of representation has its advantages, but in this book we will use implicit representations of configurations of rigid bodies. A robot arm is typically equipped with a hand or gripper, more generally called an , which interacts with objects in the surrounding world. end-effector To accomplish a task such as picking up an object, we are concerned with the configuration of a reference frame rigidly attached to the end-effector, and not necessarily the configuration of the entire arm. We call the space of positions and orientations of the end-effector frame the task space and note that there is not a one-to-one mapping between the robot’s configuration space and the workspace task space. The is defined to be the subset of the task space that the end-effector frame can reach. Chapter 3: Rigid-Body Motions This chapter addresses the problem of how to describe mathematically the mo- tion of a rigid body moving in three-dimensional physical space. One convenient way is to attach a reference frame to the rigid body and to develop a way to quantitatively describe the frame’s position and orientation as it moves. As a first step, we introduce a 3 × 3 matrix representation for describing a frame’s orientation; such a matrix is referred to as a rotation matrix . A rotation matrix is parametrized by three independent coordinates. The most natural and intuitive way to visualize a rotation matrix is in terms of its representation. That is, given a rotation matrix R , exponential coordinate 3 ∈ ω there exists some unit vector ˆ and angle θ R [0 ,π ] such that the rota- ∈ tion matrix can be obtained by rotating the identity frame (that is, the frame corresponding to the identity matrix) about ˆ ω by θ . The exponential coordi- 3 nates are defined as = ˆ ωθ ∈ R , which is a three-parameter representation. ω There are several other well-known coordinate representations, e.g., Euler an- gles, Cayley–Rodrigues parameters, and unit quaternions, which are discussed in Appendix B. Another reason for focusing on the exponential description of rotations is that they lead directly to the exponential description of rigid-body motions. The latter can be viewed as a modern geometric interpretation of classical screw May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

23 Chapter 1. Preview 5 theory. Keeping the classical terminology as much as possible, we cover in detail the linear algebraic constructs of screw theory, including the unified description (also known as spa- of linear and angular velocities as six-dimensional twists ), and an analogous description of three-dimensional forces and tial velocities moments as six-dimensional (also known as spatial forces ). wrenches Chapter 4: Forward Kinematics For an open chain, the position and orientation of the end-effector are uniquely forward kinematics determined from the joint positions. The problem is to find the position and orientation of the reference frame attached to the end- effector given the set of joint positions. In this chapter we present the product of exponentials (PoE) formula describing the forward kinematics of open chains. As the name implies, the PoE formula is directly derived from the expo- nential coordinate representation for rigid-body motions. Aside from providing an intuitive and easily visualizable interpretation of the exponential coordinates as the twists of the joint axes, the PoE formula offers other advantages, like eliminating the need for link frames (only the base frame and end-effector frame are required, and these can be chosen arbitrarily). In Appendix C we also present the Denavit–Hartenberg (D–H) representa- tion for forward kinematics. The D–H representation uses fewer parameters but requires that reference frames be attached to each link following special rules of assignment, which can be cumbersome. Details of the transformation from the D–H to the PoE representation are also provided in Appendix C. Chapter 5: Velocity Kinematics and Statics Velocity kinematics refers to the relationship between the joint linear and an- gular velocities and those of the end-effector frame. Central to velocity kine- matics is the Jacobian of the forward kinematics. By multiplying the vector of joint-velocity rates by this configuration-dependent matrix, the twist of the end-effector frame can be obtained for any given robot configuration. Kine- matic singularities , which are configurations in which the end-effector frame loses the ability to move or rotate in one or more directions, correspond to those configurations at which the Jacobian matrix fails to have maximal rank. The manipulability ellipsoid , whose shape indicates the ease with which the robot can move in various directions, is also derived from the Jacobian. Finally, the Jacobian is also central to static force analysis. In static equilib- rium settings, the Jacobian is used to determine what forces and torques need to be exerted at the joints in order for the end-effector to apply a desired wrench. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

24 6 The definition of the Jacobian depends on the representation of the end- effector velocity, and our preferred representation of the end-effector velocity is as a six-dimensional twist. We touch briefly on other representations of the end-effector velocity and their corresponding Jacobians. Chapter 6: Inverse Kinematics The inverse kinematics problem is to determine the set of joint positions that achieves a desired end-effector configuration. For open-chain robots, the inverse kinematics is in general more involved than the forward kinematics: for a given set of joint positions there usually exists a unique end-effector position and orientation but, for a particular end-effector position and orientation, there may exist multiple solutions to the jont positions, or no solution at all. In this chapter we first examine a popular class of six-dof open-chain struc- tures whose inverse kinematics admits a closed-form analytic solution. Itera- tive numerical algorithms are then derived for solving the inverse kinematics of general open chains by taking advantage of the inverse of the Jacobian. If the open-chain robot is kinematically redundant , meaning that it has more joints than the dimension of the task space, then we use the pseudoinverse of the Jacobian. Chapter 7: Kinematics of Closed Chains While open chains have unique forward kinematics solutions, closed chains of- ten have multiple forward kinematics solutions, and sometimes even multiple solutions for the inverse kinematics as well. Also, because closed chains possess both actuated and passive joints, the kinematic singularity analysis of closed chains presents subtleties not encountered in open chains. In this chapter we study the basic concepts and tools for the kinematic analysis of closed chains. We begin with a detailed case study of mechanisms such as the planar five-bar linkage and the Stewart–Gough platform. These results are then generalized into a systematic methodology for the kinematic analysis of more general closed chains. Chapter 8: Dynamics of Open Chains Dynamics is the study of motion taking into account the forces and torques that cause it. In this chapter we study the dynamics of open-chain robots. In forward analogy to the notions of a robot’s forward and inverse kinematics, the dynamics problem is to determine the resulting joint accelerations for a given set of joint forces and torques. The inverse dynamics problem is to determine May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

25 Chapter 1. Preview 7 the input joint torques and forces needed for desired joint accelerations. The dynamic equations relating the forces and torques to the motion of the robot’s links are given by a set of second-order ordinary differential equations. The dynamics for an open-chain robot can be derived using one of two ap- proaches. In the Lagrangian approach, first a set of coordinates – referred to as generalized coordinates in the classical dynamics literature – is chosen to parametrize the configuration space. The sum of the potential and kinetic energies of the robot’s links are then expressed in terms of the generalized coordinates and their time derivatives. These are then substituted into the Euler–Lagrange equations , which then lead to a set of second-order differ- ential equations for the dynamics, expressed in the chosen coordinates for the configuration space. , i.e., Newton–Euler f = m a approach builds on the generalization of The the equations governing the acceleration of a rigid body given the wrench acting on it. Given the joint variables and their time derivatives, the Newton–Euler approach to inverse dynamics is: to propagate the link velocities and accelera- tions outward from the proximal link to the distal link, in order to determine the velocity and acceleration of each link; to use the equations of motion for a rigid body to calculate the wrench (and therefore the joint force or torque) that must be acting on the outermost link; and to proceed along the links back toward the base of the robot, calculating the joint forces or torques needed to create the motion of each link and to support the wrench transmitted to the dis- tal links. Because of the open-chain structure, the dynamics can be formulated recursively. In this chapter we examine both approaches to deriving a robot’s dynamic equations. Recursive algorithms for both the forward and inverse dynamics, as well as analytical formulations of the dynamic equations, are presented. Chapter 9: Trajectory Generation What sets a robot apart from an automated machine is that it should be easily reprogrammable for different tasks. Different tasks require different motions, and it would be unreasonable to expect the user to specify the entire time- history of each joint for every task; clearly it would be desirable for the robot’s control computer to “fill in the details” from a small set of task input data. This chapter is concerned with the automatic generation of joint trajectories from this set of task input data. Formally, a trajectory consists of a path , which is a purely geometric description of the sequence of configurations achieved by a robot, and a time scaling , which specifies the times at which those configu- rations are reached. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

26 8 Often the input task data is given in the form of an ordered set of joint values, called control points, together with a corresponding set of control times. On the basis of this data the trajectory generation algorithm produces a trajectory for each joint which satisfies various user-supplied conditions. In this chapter we focus on three cases: (i) point-to-point straight-line trajectories in both joint space and task space; (ii) smooth trajectories passing through a sequence of timed “via points”; and (iii) time-optimal trajectories along specified paths, subject to the robot’s dynamics and actuator limits. Finding paths that avoid collisions is the subject of the next chapter on motion planning. Chapter 10: Motion Planning This chapter addresses the problem of finding a collision-free motion for a robot through a cluttered workspace, while avoiding joint limits, actuator limits, and path planning other physical constraints imposed on the robot. The problem is a subproblem of the general motion planning problem that is concerned with finding a collision-free path between a start and goal configuration, usually without regard to the dynamics, the duration of the motion, or other constraints on the motion or control inputs. There is no single planner applicable to all motion planning problems. In this chapter we consider three basic approaches: grid-based methods, sampling methods, and methods based on virtual potential fields. Chapter 11: Robot Control A robot arm can exhibit a number of different behaviors depending on the task and its environment. It can act as a source of programmed motions for tasks such as moving an object from one place to another, or tracing a trajectory for manufacturing applications. It can act as a source of forces, for example when grinding or polishing a workpiece. In tasks such as writing on a chalkboard, it must control forces in some directions (the force pressing the chalk against the board) and motions in other directions (the motion in the plane of the board). In certain applications, e.g., haptic displays, we may want the robot to act like a programmable spring, damper, or mass, by controlling its position, velocity, or acceleration in response to forces applied to it. In each of these cases, it is the job of the robot controller to convert the task specification to forces and torques at the actuators. Control strategies to achieve the behaviors described above are known as motion (or position ) con- impedance con- trol force control , hybrid motion–force control , and , trol . Which of these behaviors is appropriate depends on both the task and May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

27 Chapter 1. Preview 9 the environment. For example, a force-control goal makes sense when the end- effector is in contact with something, but not when it is moving in free space. We also have a fundamental constraint imposed by the mechanics, irrespective of the environment: the robot cannot independently control both motions and forces in the same direction. If the robot imposes a motion then the environment determines the force, and vice versa. Most robots are driven by actuators that apply a force or torque to each joint. Hence, precisely controlling a robot requires an understanding of the relationship between the joint forces and torques and the motion of the robot; this is the domain of dynamics. Even for simple robots, however, the dynamic equations are complex and dependent on a precise knowledge of the mass and inertia of each link, which may not be readily available. Even if it were, the dynamic equations would still not reflect physical phenomena such as friction, elasticity, backlash, and hysteresis. Most practical control schemes compensate for these uncertainties by using feedback control . After examining the performance limits of feedback control without a dynamic model of the robot, we study motion control algorithms, computed torque control , that combine approximate dynamic mod- such as eling with feedback control. The basic lessons learned for robot motion control are then applied to force control, hybrid motion–force control, and impedance control. Chapter 12: Grasping and Manipulation The focus of earlier chapters is on characterizing, planning, and controlling the motion of the robot itself. To do useful work, the robot must be capable of manipulating objects in its environment. In this chapter we model the con- tact between the robot and an object, specifically the constraints on the object motion imposed by a contact and the forces that can be transmitted through a frictional contact. With these models we study the problem of choosing contacts form closure and force closure grasping. We also to immobilize an object by apply contact modeling to manipulation problems other than grasping, such as pushing an object, carrying an object dynamically, and testing the stability of a structure. Chapter 13: Wheeled Mobile Robots The final chapter addresses the kinematics, motion planning, and control of wheeled mobile robots and of wheeled mobile robots equipped with robot arms. A mobile robot can use specially designed omniwheels or mecanum wheels May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

28 10 to achieve omnidirectional motion, including spinning in place or translating in any direction. Many mobile bases, however, such as cars and differential- drive robots, use more typical wheels, which do not slip sideways. These no-slip constraints are fundamentally different from the loop-closure constraints found in closed chains; the latter are holonomic , meaning that they are configuration constraints, while the former are nonholonomic , meaning that the velocity constraints cannot be integrated to become equivalent configuration constraints. Because of the different properties of omnidirectional mobile robots ver- sus nonholonomic mobile robots, we consider their kinematic modeling, motion planning, and control separately. In particular, the motion planning and con- trol of nonholonomic mobile robots is more challenging than for omnidirectional mobile robots. Once we have derived their kinematic models, we show that the odometry problem – the estimation of the chassis configuration based on wheel encoder data – can be solved in the same way for both types of mobile robots. Similarly, for mobile manipulators consisting of a wheeled base and a robot arm, we show that feedback control for mobile manipulation (controlling the motion of the end-effector using the arm joints and wheels) is the same for both types of mobile robots. The fundamental object in mobile manipulation is the Jacobian mapping joint rates and wheel velocities to end-effector twists. Each chapter concludes with a summary of important concepts from the chapter, and Appendix A compiles some of the most used equations into a handy reference. Videos supporting the book can be found at the book’s website, http: //modernrobotics.org . Some chapters have associated software, downloadable from the website. The software is meant to be neither maximally robust nor efficient but to be readable and to reinforce the concepts in the book. You are encouraged to read the software, not just use it, to cement your understanding of the material. Each function contains a sample usage in the comments. The software package may grow over time, but the core functions are documented in the chapters themselves. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

29 Chapter 2 Configuration Space links , A robot is mechanically constructed by connecting a set of bodies, called . Actuators , such as electric motors, to each other using various types of joints end- deliver forces or torques that cause the robot’s links to move. Usually an effector , such as a gripper or hand for grasping and manipulating objects, is attached to a specific link. All the robots considered in this book have links that can be modeled as rigid bodies. Perhaps the most fundamental question one can ask about a robot is, where is it? The answer is given by the robot’s : a specification of the configuration positions of all points of the robot. Since the robot’s links are rigid and of a 1 known shape, only a few numbers are needed to represent its configuration. For example, the configuration of a door can be represented by a single number, the angle θ about its hinge. The configuration of a point on a plane can be described by two coordinates, ( x,y ). The configuration of a coin lying heads up on a flat table can be described by three coordinates: two coordinates ( x,y ) that specify the location of a particular point on the coin, and one coordinate ( ) that specifies the coin’s orientation. (See Figure 2.1). θ The above coordinates all take values over a continuous range of real num- degrees of freedom ( dof ) of a robot is the smallest bers. The number of number of real-valued coordinates needed to represent its configuration. In the example above, the door has one degree of freedom. The coin lying heads up on a table has three degrees of freedom. Even if the coin could lie either heads up or tails up, its configuration space still would have only three degrees of freedom; a fourth variable, representing which side of the coin faces up, takes values in the discrete set { heads , tails } , and not over a continuous range of real 1 Compare with trying to represent the configuration of a soft object like a pillow. 11

30 12 2.1. Degrees of Freedom of a Rigid Body θ ˆy ˆy ) y x, ( θ y ) x, ( ˆx ˆx ) (c) (a (b) (a) The configuration of a door is described by the angle Figure 2.1: . (b) The θ configuration of a point in a plane is described by coordinates ( ). (c) The config- x,y x,y,θ θ defines the direction in uration of a coin on a table is described by ( ), where which Abraham Lincoln is looking. values like the other three coordinates. Definition 2.1. configuration of a robot is a complete specification of The the position of every point of the robot. The minimum number n of real-valued coordinates needed to represent the configuration is the number of degrees of freedom ( ) of the robot. The n -dimensional space containing all possible dof configuration space C-space ). The configurations of the robot is called the ( configuration of a robot is represented by a point in its C-space. In this chapter we study the C-space and degrees of freedom of general robots. Since our robots are constructed from rigid links, we examine first the degrees of freedom of a single rigid body, and then the degrees of freedom of general multi-link robots. Next we study the shape (or topology) and geometry of C-spaces and their mathematical representation. The chapter concludes with a discussion of the C-space of a robot’s end-effector, its task space . In the following chapter we study in more detail the mathematical representation of the C-space of a single rigid body. Degrees of Freedom of a Rigid Body 2.1 Continuing with the example of the coin lying on the table, choose three points A , B , and C on the coin (Figure 2.2(a)). Once a coordinate frame ˆx–ˆy is 2 attached to the plane, the positions of these points in the plane are written 2 The unit axes of coordinate frames are written with a hat, indicating they are unit vectors, and in a non-italic font, e.g., ˆx, ˆy, and ˆz. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

31 Chapter 2. Configuration Space 13 ˆz ˆy C C d AC d BC A A d AB B B ˆx ˆy ˆx (c) (a (b) ) (a) Choosing three points fixed to the coin. (b) Once the location of A is Figure 2.2: chosen, B centered at A . Once the location of B is must lie on a circle of radius d AB must lie at the intersection of circles centered at chosen, and B . Only one of these C A two intersections corresponds to the “heads up” configuration. (c) The configuration A of a coin in three-dimensional space is given by the three coordinates of , two angles on the sphere of radius d to the point centered at A , and one angle to the point B AB on the circle defined by the intersection of the a sphere centered at A and a sphere C B centered at . x x ( ), ( x ). If the points could be placed independently ,y ,y ), and ( ,y C B C B A A anywhere in the plane, the coin would have six degrees of freedom – two for each of the three points. But, according to the definition of a rigid body, the distance A and point B , denoted d ( A,B ), is always constant regardless of between point d where the coin is. Similarly, the distances B,C ) and d ( A,C ) must be constant. ( x ), ( ,y ), and The following equality constraints on the coordinates ( x ,y B A A B ( x ,y ) must therefore always be satisfied: C C √ 2 2 + ( − x ) − ( y , y d ) x = ( A,B d ) = B AB A B A √ 2 2 d ( x ( − x , ) B,C + ( y d − y = ) ) = C B BC C B √ 2 2 ) = + ( ( x . − x d ) d ( y = − y A,C ) C A A C AC To determine the number of degrees of freedom of the coin on the table, first choose the position of point in the plane (Figure 2.2(b)). We may choose it A to be anything we want, so we have two degrees of freedom to specify, namely x restricts the ). Once ( x ,y ) is specified, the constraint d ( A,B ) = d ( ,y A A A AB A choice of ( x ,y . ) to those points on the circle of radius d A centered at B B AB A point on this circle can be specified by a single parameter, e.g., the angle specifying the location of B on the circle centered at A . Let’s call this angle −−→ φ and define it to be the angle that the vector AB makes with the ˆx-axis. AB May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

32 14 2.1. Degrees of Freedom of a Rigid Body B , there are only two possible Once we have chosen the location of point : at the intersections of the circle of radius C centered at A d locations for AC (Figure 2.2(b)). These two solutions B and the circle of radius d centered at BC B and A correspond to heads or tails. In other words, once we have placed and ( A,C chosen heads or tails, the two constraints d d and d ( B,C ) = d ) = BC AC ,y eliminate the two apparent freedoms provided by ( ), and the location of x C C C is fixed. The coin has exactly three degrees of freedom in the plane, which x can be specified by ( ,y ). ,φ AB A A D Suppose that we choose to specify the position of an additional point d ( A,D ) = d , on the coin. This introduces three additional constraints: AD ( B,D ) = d , , and d ( C,D d d redundant . One of these constraints is ) = CD BD i.e., it provides no new information; only two of the three constraints are inde- pendent. The two freedoms apparently introduced by the coordinates ( ,y ) x D D are then immediately eliminated by these two independent constraints. The same would hold for any other newly chosen point on the coin, so that there is no need to consider additional points. We have been applying the following general rule for determining the number of degrees of freedom of a system: degrees of freedom = (sum of freedoms of the points) − (number of independent constraints) . (2.1) This rule can also be expressed in terms of the number of variables and inde- pendent equations that describe the system: degrees of freedom = (number of variables) − (2.2) . (number of independent equations) This general rule can also be used to determine the number of freedoms of a rigid body in three dimensions. For example, assume our coin is no longer confined to the table (Figure , B , 2.2(c)). The coordinates for the three points A C are now given by ( x ,y and ), respectively. ,z ,z ), ( x ,y ,y x ,z ), and ( B B C B C A C A A Point A can be placed freely (three degrees of freedom). The location of point B is subject to the constraint d A,B ) = d ( , meaning it must lie on the sphere of AB 1 = 2 freedoms to specify, which can d centered at radius . Thus we have 3 − A AB be expressed as the latitude and longitude for the point on the sphere. Finally, the location of point C must lie at the intersection of spheres centered at A and B of radius d , respectively. In the general case the intersection of and d BC AC two spheres is a circle, and the location of point C can be described by an angle that parametrizes this circle. Point C therefore adds 3 − 2 = 1 freedom. Once the position of point C is chosen, the coin is fixed in space. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

33 Chapter 2. Configuration Space 15 In summary, a rigid body in three-dimensional space has six freedoms, which can be described by the three coordinates parametrizing point A , the two angles , and one angle parametrizing point B , provided A , B , C parametrizing point are noncollinear. Other representations for the configuration of a rigid C and body are discussed in Chapter 3. We have just established that a rigid body moving in three-dimensional space, which we call a , has six degrees of freedom. Similarly, spatial rigid body a rigid body moving in a two-dimensional plane, which we henceforth call a , has three degrees of freedom. This latter result can also planar rigid body be obtained by considering the planar rigid body to be a spatial rigid body with z = = z six degrees of freedom but with the three independent constraints A B z = 0. C Since our robots consist of rigid bodies, Equation (2.1) can be expressed as follows: degrees of freedom = (sum of freedoms of the bodies) − (number of independent constraints) . (2.3) Equation (2.3) forms the basis for determining the degrees of freedom of general robots, which is the topic of the next section. Degrees of Freedom of a Robot 2.2 Consider once again the door example of Figure 2.1(a), consisting of a single rigid body connected to a wall by a hinge joint. From the previous section we know that the door has only one degree of freedom, conveniently represented θ . Without the hinge joint, the door would be free to by the hinge joint angle move in three-dimensional space and would have six degrees of freedom. By connecting the door to the wall via the hinge joint, five independent constraints are imposed on the motion of the door, leaving only one independent coordinate ( θ ). Alternatively, the door can be viewed from above and regarded as a planar body, which has three degrees of freedom. The hinge joint then imposes two θ ). independent constraints, again leaving only one independent coordinate ( The door’s C-space is represented by some range in the interval [0 , 2 π ) over which θ is allowed to vary. In both cases the joints constrain the motion of the rigid body, thus re- ducing the overall degrees of freedom. This observation suggests a formula for determining the number of degrees of freedom of a robot, simply by counting the number of rigid bodies and joints. In this section we derive precisely such May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

34 16 2.2. Degrees of Freedom of a Robot Typical robot joints. Figure 2.3: a formula, called Gr ̈ubler’s formula, for determining the number of degrees of freedom of planar and spatial robots. 2.2.1 Robot Joints Figure 2.3 illustrates the basic joints found in typical robots. Every joint con- nects exactly two links; joints that simultaneously connect three or more links revolute joint are not allowed. The (R), also called a hinge joint, allows ro- tational motion about the joint axis. The prismatic joint (P), also called a sliding or linear joint, allows translational (or rectilinear) motion along the di- rection of the joint axis. The helical joint (H), also called a screw joint, allows simultaneous rotation and translation about a screw axis. Revolute, prismatic, and helical joints all have one degree of freedom. cylindrical joint (C) Joints can also have multiple degrees of freedom. The has two degrees of freedom and allows independent translations and rotations about a single fixed joint axis. The universal joint (U) is another two-degree- of-freedom joint that consists of a pair of revolute joints arranged so that their joint axes are orthogonal. The spherical joint (S), also called a ball-and-socket joint, has three degrees of freedom and functions much like our shoulder joint. A joint can be viewed as providing freedoms to allow one rigid body to move relative to another. It can also be viewed as providing constraints on the possible motions of the two rigid bodies it connects. For example, a revolute joint can be viewed as allowing one freedom of motion between two rigid bodies May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

35 Chapter 2. Configuration Space 17 c Constraints Constraints c between two between two Joint type f planar spatial dof rigid bodies rigid bodies 2 5 Revolute (R) 1 Prismatic (P) 1 2 5 1 N/A 5 Helical (H) 2 Cylindrical (C) 4 N/A 2 4 Universal (U) N/A Spherical (S) 3 N/A 3 The number of degrees of freedom Table 2.1: and constraints c provided by common f joints. in space, or it can be viewed as providing five constraints on the motion of one rigid body relative to the other. Generalizing, the number of degrees of freedom of a rigid body (three for planar bodies and six for spatial bodies) minus the number of constraints provided by a joint must equal the number of freedoms provided by that joint. The freedoms and constraints provided by the various joint types are sum- marized in Table 2.1. Gr ̈ubler’s Formula 2.2.2 The number of degrees of freedom of a mechanism with links and joints can be calculated using Gr ̈ubler’s formula , which is an expression of Equation (2.3). Proposition 2.2. Consider a mechanism consisting of N links, where ground J is also regarded as a link. Let m be the number of be the number of joints, degrees of freedom of a rigid body ( m = 3 for planar mechanisms and m = 6 for spatial mechanisms), f be be the number of freedoms provided by joint i , and c i i i , where f + the number of constraints provided by joint . Then = m for all i c i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

36 18 2.2. Degrees of Freedom of a Robot (b) ) (a Figure 2.4: (a) Four-bar linkage. (b) Slider–crank mechanism. Gr ̈ubler’s formula for the number of degrees of freedom of the robot is J ∑ c − − N ( m dof = 1) i ︷︷ ︸ ︸ =1 i rigid body freedoms ︸ ︸ ︷︷ joint constraints J ∑ ( ) m − f ( N − m 1) − = i =1 i J ∑ (2.4) . f 1 − J ) + = m ( N − i =1 i This formula holds only if all joint constraints are independent. If they are not independent then the formula provides a lower bound on the number of degrees of freedom. Below we apply Gr ̈ubler’s formula to several planar and spatial mechanisms. We distinguish between two types of mechanism: open-chain mechanisms serial mechanisms (also known as closed-chain mechanisms . A ) and closed-chain mechanism is any mechanism that has a closed loop. A person standing with both feet on the ground is an example of a closed-chain mech- anism, since a closed loop can be traced from the ground, through the right leg, through the waist, through the left leg, and back to ground (recall that the ground itself is a link). An open-chain mechanism is any mechanism without a closed loop; an example is your arm when your hand is allowed to move freely in space. Example 2.3 (Four-bar linkage and slider–crank mechanism) . The planar four- bar linkage shown in Figure 2.4(a) consists of four links (one of them ground) arranged in a single closed loop and connected by four revolute joints. Since all the links are confined to move in the same plane, we have m = 3. Subsituting May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

37 Chapter 2. Configuration Space 19 ) (a (b) (c ) (d) Figure 2.5: k -link planar serial chain. (b) Five-bar planar linkage. (c) Stephenson (a) six-bar linkage. (d) Watt six-bar linkage. = 4, J = 4, and f 4, into Gr ̈ubler’s formula, we see that the = 1, N = 1 ,..., i i four-bar linkage has one degree of freedom. The slider–crank closed-chain mechanism of Figure 2.4(b) can be analyzed in two ways: (i) the mechanism consists of three revolute joints and one prismatic J = 4 and each joint ( = 4, including the ground = 1) and four links ( N f i link), or (ii) the mechanism consists of two revolute joints ( f = 1) and one RP i joint (the RP joint is a concatenation of a revolute and prismatic joint, so that N = 3; remember that each joint connects precisely f = 2) and three links ( i two bodies). In both cases the mechanism has one degree of freedom. Example 2.4 . Let us now apply Gr ̈ubler’s (Some classical planar mechanisms) formula to several classical planar mechanisms. The k -link planar serial chain of revolute joints in Figure 2.5(a) (called a k R robot for its k revolute joints) J has = k + 1 links ( k links plus ground), and N = k joints, and, since all the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

38 20 2.2. Degrees of Freedom of a Robot A planar mechanism with two overlapping joints. Figure 2.6: joints are revolute, = 1 for all i . Therefore, f i = − 1 dof = 3(( k ) + k + 1) k k − 2.5(b), N = 5 (four links as expected. For the planar five-bar linkage of Figure plus ground), = 5, and since all joints are revolute, each f = 1. Therefore, J i − − 5) + 5 = 2 . dof = 3(5 1 2.5(c), we have N For the Stephenson six-bar linkage of Figure J = 7, and = 6, f = 1 for all i , so that i dof = 3(6 − 1 − 7) + 7 = 1 . Finally, for the Watt six-bar linkage of Figure 2.5(d), we have = 6, J = 7, N f i = 1 for all and , so that, like the Stephenson six-bar linkage, i dof = 3(6 1 − 7) + 7 = 1 . − Example 2.5 (A planar mechanism with overlapping joints) . The planar mech- anism illustrated in Figure has three links that meet at a single point on 2.6 the right of the large link. Recalling that a joint by definition connects exactly two links, the joint at this point of intersection should not be regarded as a single revolute joint. Rather, it is correctly interpreted as two revolute joints overlapping each other. Again, there is more than one way to derive the number of degrees of freedom of this mechanism using Gr ̈ubler’s formula: (i) The mech- anism consists of eight links ( N = 8), eight revolute joints, and one prismatic joint. Substituting into Gr ̈ubler’s formula yields dof = 3(8 − 1 − 9) + 9(1) = 3 . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

39 Chapter 2. Configuration Space 21 ) (a (b) Figure 2.7: (a) A parallelogram linkage. (b) The five-bar linkage in a regular and singular configuration. (ii) Alternatively, the lower-right revolute–prismatic joint pair can be regarded as a single two-dof joint. In this case the number of links is = 7, with seven N revolute joints, and a single two-dof revolute–prismatic pair. Substituting into Gr ̈ubler’s formula yields − 1 − dof = 3(7 . 8) + 7(1) + 1(2) = 3 Example 2.6 . For the parallelogram (Redundant constraints and singularities) 2.7(a), N linkage of Figure J = 6, and f = 1 for each joint. From = 5, i Gr ̈ubler’s formula, the number of degrees of freedom is 3(5 − 1 − 6) + 6 = 0. A mechanism with zero degrees of freedom is by definition a rigid structure. It is clear from examining the figure, though, that the mechanism can in fact move with one degree of freedom. Indeed, any one of the three parallel links, with its two joints, has no effect on the motion of the mechanism, so we should have − 1 − 4) + 4 = 1. In other words, the constraints provided calculated dof = 3(4 by the joints are not independent, as required by Gr ̈ubler’s formula. A similar situation arises for the two-dof planar five-bar linkage of Fig- ure 2.7(b). If the two joints connected to ground are locked at some fixed angle, the five-bar linkage should then become a rigid structure. However, if the two middle links are the same length and overlap each other, as illustrated in Figure 2.7(b), these overlapping links can rotate freely about the two overlap- ping joints. Of course, the link lengths of the five-bar linkage must meet certain specifications in order for such a configuration to even be possible. Also note that if a different pair of joints is locked in place, the mechanism does become a rigid structure as expected. Gr ̈ubler’s formula provides a lower bound on the degrees of freedom for cases like those just described. Configuration space singularities arising in closed chains are discussed in Chapter 7. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

40 22 2.2. Degrees of Freedom of a Robot R R S S S R S Figure 2.8: The Delta robot. (Delta robot) . The Delta robot of Figure Example 2.7 consists of two 2.8 platforms – the lower one mobile, the upper one stationary – connected by three legs. Each leg contains a parallelogram closed chain and consists of three revolute joints, four spherical joints, and five links. Adding the two platforms, there are N = 17 links and J = 21 joints (nine revolute and 12 spherical). By Gr ̈ubler’s formula, dof = 6(17 − 1 − 21) + 9(1) + 12(3) = 15 . Of these 15 degrees of freedom, however, only three are visible at the end- effector on the moving platform. In fact, the parallelogram leg design ensures that the moving platform always remains parallel to the fixed platform, so that z – y the Delta robot acts as an x Cartesian positioning device. The other 12 – internal degrees of freedom are accounted for by torsion of the 12 links in the parallelograms (each of the three legs has four links in its parallelogram) about their long axes. Example 2.8 (Stewart–Gough platform) . The Stewart–Gough platform of Fig- ure 1.1(b) consists of two platforms – the lower one stationary and regarded as ground, the upper one mobile – connected by six universal–prismatic–spherical (UPS) legs. The total number of links is 14 ( N = 14). There are six universal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

41 Chapter 2. Configuration Space 23 f joints (each with two degrees of freedom, = 2), six prismatic joints (each with i = 1), and six spherical joints (each with three de- f a single degree of freedom, i f = 3). The total number of joints is 18. Substituting these grees of freedom, i m = 6 yields values into Gr ̈ubler’s formula with − dof = 6(14 − 18) + 6(1) + 6(2) + 6(3) = 6 . 1 In some versions of the Stewart–Gough platform the six universal joints are replaced by spherical joints. By Gr ̈ubler’s formula this mechanism has 12 degrees of freedom; replacing each universal joint by a spherical joint introduces an extra degree of freedom in each leg, allowing torsional rotations about the leg axis. Note, however, that this torsional rotation has no effect on the motion of the mobile platform. The Stewart–Gough platform is a popular choice for car and airplane cockpit simulators, as the platform moves with the full six degrees of freedom of motion of a rigid body. On the one hand, the parallel structure means that each leg needs to support only a fraction of the weight of the payload. On the other hand, this structure also limits the range of translational and rotational motion of the platform relative to the range of motion of the end-effector of a six-dof open chain. Configuration Space: Topology and Represen- 2.3 tation Configuration Space Topology 2.3.1 Until now we have been focusing on one important aspect of a robot’s C-space – its dimension, or the number of degrees of freedom. However, the shape of the space is also important. Consider a point moving on the surface of a sphere. The point’s C-space is two dimensional, as the configuration can be described by two coordinates, latitude and longitude. As another example, a point moving on a plane also x,y ). While both a plane and has a two-dimensional C-space, with coordinates ( the surface of a sphere are two dimensional, clearly they do not have the same shape – the plane extends infinitely while the sphere wraps around. Unlike the plane, a larger sphere has the same shape as the original sphere, in that it wraps around in the same way. Only its size is different. For that matter, an oval-shaped American football also wraps around similarly to a sphere. The only difference between a football and a sphere is that the football has been stretched in one direction. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

42 24 2.3. Configuration Space: Topology and Representation ) ( ) ) ( ( a b a,b An open interval of the real line, denoted ( Figure 2.9: ), can be deformed to an open semicircle. This open semicircle can then be deformed to the real line by the mapping illustrated: beginning from a point at the center of the semicircle, draw a ray that intersects the semicircle and then a line above the semicircle. These rays show that every point of the semicircle can be stretched to exactly one point on the line, and vice versa. Thus an open interval can be continuously deformed to a line, so an open interval and a line are topologically equivalent. The idea that the two-dimensional surfaces of a small sphere, a large sphere, and a football all have the same kind of shape, which is different from the shape of a plane, is expressed by the topology of the surfaces. We do not attempt a 3 rigorous treatment in this book, but we say that two spaces are topologically if one can be continuously deformed into the other without cutting equivalent or gluing. A sphere can be deformed into a football simply by stretching, without cutting or gluing, so those two spaces are topologically equivalent. You cannot turn a sphere into a plane without cutting it, however, so a sphere and a plane are not topologically equivalent. Topologically distinct one-dimensional spaces include the circle, the line, or and a closed interval of the line. The circle is written mathematically as S 1 1 E or E S , indicating , a one-dimensional “sphere.” The line can be written as 1 a one-dimensional Euclidean (or “flat”) space. Since a point in E is usually represented by a real number (after choosing an origin and a length scale), it is 1 often written as R R instead. A closed interval of the line, which contains its or 1 a,b ⊂ R . (An open interval ( endpoints, can be written [ a,b ) does not include ] a and b and is topologically equivalent to a line, since the open the endpoints interval can be stretched to a line, as shown in Figure 2.9. A closed interval is not topologically equivalent to a line, since a line does not contain endpoints.) n n R is the -dimensional Euclidean space and S In higher dimensions, is the n -dimensional surface of a sphere in ( n + 1)-dimensional space. For example, n 2 is the two-dimensional surface of a sphere in three-dimensional space. S Note that the topology of a space is a fundamental property of the space is independent of how we choose coordinates to represent points in the itself and space . For example, to represent a point on a circle, we could refer to the point 3 For those familiar with concepts in topology, all the spaces we consider can be viewed as embedded in a higher-dimensional Euclidean space, inheriting the Euclidean topology of that space. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

43 Chapter 2. Configuration Space 25 θ by the angle from the center of the circle to the point, relative to a chosen zero angle. Or, we could choose a reference frame with its origin at the center ) subject to the x,y of the circle and represent the point by the two coordinates ( 2 2 y constraint + x = 1. No matter what our choice of coordinates is, the space itself does not change. Some C-spaces can be expressed as the Cartesian product of two or more spaces of lower dimension; that is, points in such a C-space can be represented as the union of the representations of points in the lower-dimensional spaces. For example: 1 2 × S The C-space of a rigid body in the plane can be written as , R • since the configuration can be represented as the concatenation of the 2 1 R and an angle coordinates ( x,y representing S ) representing . θ 1 1 • × S R . (We will occa- The C-space of a PR robot arm can be written sionally ignore joint limits, i.e., bounds on the travel of the joints, when expressing the topology of the C-space; with joint limits, the C-space is the Cartesian product of two closed intervals of the line.) 2 1 1 n • S = T The C-space of a 2R robot arm can be written , where T S is × the n -dimensional surface of a torus in an ( n + 1)-dimensional space. (See n 1 1 1 1 S ×···× × ( n copies of S S ) is equal to T Table 2.2.) Note that , S 2 n S S not is not topologically equivalent to a torus ; for example, a sphere 2 . T • The C-space of a planar rigid body (e.g., the chassis of a mobile robot) 3 2 2 2 1 × S = R R × T with a 2R robot arm can be written as . T × As we saw in Section 2.1 when we counted the degrees of freedom of a • rigid body in three dimensions, the configuration of a rigid body can be 3 2 described by a point in R , plus a point on a two-dimensional sphere S , 1 S plus a point on a one-dimensional circle , giving a total C-space of 3 1 2 R × × S . S 2.3.2 Configuration Space Representation To perform computations, we must have a numerical representation of the space, consisting of a set of real numbers. We are familiar with this idea from linear algebra – a vector is a natural way to represent a point in a Euclidean space. It is important to keep in mind that the representation of a space involves a choice, and therefore it is not as fundamental as the topology of the space, which is independent of the representation. For example, the same point in a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

44 26 2.3. Configuration Space: Topology and Representation sample representation topology system ( x, y ) ˆy ˆx 2 2 R point on a plane E latitude ◦ 90 longitude ◦ − 90 ◦ ◦ 180 − 180 2 ◦ ◦ ◦ ◦ S , 180 [ ) × [ − 90 spherical pendulum , 90 − ] 180 θ 2 π 2 0 θ 2 π 0 1 2 1 1 2R robot arm S T × S = [0 , 2 π ) × [0 , 2 π ) θ 2 π ... ... 0 ˆx 1 1 1 × E R × rotating sliding knob [0 , 2 π ) S Table 2.2: Four topologically different two-dimensional C-spaces and example co- ordinate representations. In the latitude-longitude representation of the sphere, the ◦ ◦ − 90 and 90 each correspond to a single point (the South Pole and the North latitudes ◦ ◦ and Pole, respectively), and the longitude parameter wraps around at 180 180 − ; the edges with the arrows are glued together. Similarly, the coordinate representations of the torus and cylinder wrap around at the edges marked with corresponding arrows. three-dimensional space can have different coordinate representations depending on the choice of reference frame (the origin and the direction of the coordinate May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

45 Chapter 2. Configuration Space 27 axes) and the choice of length scale, but the topology of the underlying space is the same regardless of theses choices. While it is natural to choose a reference frame and length scale and to use a vector to represent points in a Euclidean space, representing a point on a curved space, such as a sphere, is less obvious. One solution for a sphere is to use latitude and longitude coordinates. A choice of n coordinates, or parameters, -dimensional space is called an explicit parametrization of to represent an n the space. Such an explicit parametrization is valid for a particular range of ◦ ◦ ◦ ◦ 90 the parameters (e.g., [ − 90 180 ] for latitude and [ , 180 , ) for longitude for − a sphere, where, on Earth, negative values correspond to “south” and “west,” respectively). The latitude–longitude representation of a sphere is unsatisfactory if you ◦ ) or South Pole are walking near the North Pole (where the latitude equals 90 ◦ (where the latitude equals − ), where taking a very small step can result in a 90 large change in the coordinates. The North and South Poles are singularities of the representation, and the existence of singularities is a result of the fact that a sphere does not have the same topology as a plane, i.e., the space of the two real numbers that we have chosen to represent the sphere (latitude and longitude). The location of these singularities has nothing to do with the sphere itself, which looks the same everywhere, and everything to do with the chosen representation of it. Singularities of the parametrization are particularly problematic when representing velocities as the time rate of change of coordinates, since these representations may tend to infinity near singularities even if the point on the √ 2 2 2 + ̇ ̇ x + ̇ z (which is what the speed y sphere is moving at a constant speed would be had you represented the point as ( x,y,z ) instead). If you can assume that the configuration never approaches a singularity of the representation, you can ignore this issue. If you cannot make this assumption, there are two ways to overcome the problem. • Use more than one coordinate chart on the space, where each coordinate chart is an explicit parametrization covering only a portion of the space such that, within each chart, there is no singularity. As the configuration representation approaches a singularity in one chart, e.g., the North or South Pole, you simply switch to another chart where the North and South Poles are far from singularities. If we define a set of singularity-free coordinate charts that overlap each other and cover the entire space, like the two charts above, the charts are said to form an atlas of the space, much as an atlas of the Earth consists of several maps that together cover the Earth. An advantage of using an atlas of coordinate charts is that the representation always uses the minimum May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

46 28 2.3. Configuration Space: Topology and Representation number of numbers. A disadvantage is the extra bookkeeping required to switch representations between coordinate charts to avoid singularities. (Note that Euclidean spaces can be covered by a single coordinate chart without singularities.) Use an implicit representation instead of an explicit parametrization. • n -dimensional space as embedded in An implicit representation views the dimensions, just as a two-dimensional a Euclidean space of more than n unit sphere can be viewed as a surface embedded in a three-dimensional Euclidean space. An implicit representation uses the coordinates of the higher-dimensional space (e.g., ( x,y,z ) in the three-dimensional space), but subjects these coordinates to constraints that reduce the number of 2 2 2 y degrees of freedom (e.g., + z + = 1 for the unit sphere). x A disadvantage of this approach is that the representation has more num- bers than the number of degrees of freedom. An advantage is that there are no singularities in the representation – a point moving smoothly around the sphere is represented by a smoothly changing ( x,y,z ), even at the North and South poles. A single representation is used for the whole sphere; multiple coordinate charts are not needed. Another advantage is that while it may be very difficult to construct an explicit parametrization, or atlas, for a closed-chain mechanism, it is easy to find an implicit representation: the set of all joint coordinates subject to the loop-closure equations that define the closed loops (Section 2.4). We will use implicit representations throughout the book, beginning in the next chapter. In particular, we use nine numbers, subject to six con- straints, to represent the three orientation freedoms of a rigid body in space. This is called a . In addition to being singularity- rotation matrix free (unlike three-parameter representations such as roll–pitch–yaw an- 4 gles ), the rotation matrix representation allows us to use linear algebra to perform computations such as rotating a rigid body or changing the 5 reference frame in which the orientation of a rigid body is expressed. In summary, the non-Euclidean shape of many C-spaces motivates our use of implicit representations of C-space throughout this book. We return to this 4 Roll–pitch–yaw angles and Euler angles use three parameters for the space of rotations 1 2 2 1 S S × and one for S S ), and therefore are subject to singularities as discussed (two for above. 5 Another singularity-free implicit representation of orientations, the unit quaternion, uses only four numbers subject to the constraint that the 4-vector be of unit length. In fact, this representation is a double covering of the set of orientations: for every orientation, there are two unit quaternions. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

47 Chapter 2. Configuration Space 29 θ 2 θ 3 L 2 ˆy L 3 L θ 1 1 L 4 ˆx θ 4 The four-bar linkage. Figure 2.10: topic in the next chapter. 2.4 Configuration and Velocity Constraints For robots containing one or more closed loops, usually an implicit represen- tation is more easily obtained than an explicit parametrization. For example, consider the planar four-bar linkage of Figure 2.10, which has one degree of free- dom. The fact that the four links always form a closed loop can be expressed by the following three equations: L cos θ , + L ) = 0 cos( θ θ + θ + ) + ··· + L ··· cos( θ + 1 4 2 1 4 2 1 1 L , sin θ ) = 0 + L θ sin( θ + + θ ··· ) + ··· + L + sin( θ 1 2 1 2 4 1 1 4 θ . + θ = 0 + θ π + θ 2 − 4 1 2 3 These equations are obtained by viewing the four-bar linkage as a serial chain with four revolute joints in which (i) the tip of link L always coincides with 4 L the origin and (ii) the orientation of link is always horizontal. 4 These equations are sometimes referred to as loop-closure equations . For the four-bar linkage they are given by a set of three equations in four unknowns. The set of all solutions forms a one-dimensional curve in the four-dimensional joint space and constitutes the C-space. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

48 30 2.4. Configuration and Velocity Constraints In this book, when vectors are used in a linear algebra computation, T = [1 2 3] they are treated as column vectors, e.g., p . When a computation is not imminent, however, we often think of a vector simply as an ordered , , 3). = (1 2 list of variables, e.g., p Thus, for general robots containing one or more closed loops, the configura- T θ θ = [ ] θ ∈ tion space can be implicitly represented by the column vector ··· n 1 n and loop-closure equations of the form R   g ( θ ,...,θ ) n 1 1   . . (2.5) , = 0 ( θ g ) =   . ) θ ( ,...,θ g k n 1 k independent equations, with k ≤ n . Such constraints are known a set of 6 holonomic constraints , ones that reduce the dimension of the C-space. as − (assuming that all n The C-space can be viewed as a surface of dimension k n constraints are independent) embedded in R . ( θ ) = 0, Suppose that a closed-chain robot with loop-closure equations g n k ). Differentiating → R g , is in motion, following the time trajectory θ ( t : R g θ ( t )) = 0 with respect to t , we obtain both sides of ( d g ( θ ( t )) = 0; dt (2.6) thus   ∂g ∂g 1 1 ̇ ̇ ( ) ( θ ) θ θ θ + ··· + 1 n   ∂θ ∂θ 1 n   .   . . = 0 .     ∂g ∂g k k ̇ ̇ θ ( θ ( + θ ··· + θ ) ) n 1 ∂θ ∂θ 1 n T ̇ ̇ ··· This can be expressed as a matrix multiplying a column vector [ θ : ] θ n 1   ∂g ∂g 1 1   ( ( θ ) ··· θ ) ̇ θ   1 ∂θ ∂θ 1 n     . . . .   . . . . , = 0   . . . .     ̇ ∂g ∂g k k θ n ··· ) θ θ ( ) ( ∂θ ∂θ 1 n 6 Viewing a rigid body as a collection of points, the distance constraints between the points, as we saw earlier, can be viewed as holonomic constraints. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

49 Chapter 2. Configuration Space 31 θ ˆz ˆy φ y ) ( x, ˆx Figure 2.11: A coin rolling on a plane without slipping. which we can write as ∂g ̇ ) = 0 θ ( . (2.7) θ ∂θ ̇ Here, the joint-velocity vector denotes the derivative of θ θ with respect to i i k × n n ̇ ) time ∈ R t , ∂g , and θ, ( θ ∈ R θ . The constraints (2.7) can be written /∂θ ̇ θ ) A θ = 0 , (2.8) ( k × n ) ∈ R where A θ . Velocity constraints of this form are called Pfaffian con- ( straints . For the case of A ( θ ) = ∂g ( θ ) /∂θ , one could regard g ( θ ) as being the “integral” of A θ ); for this reason, holonomic constraints of the form g ( θ ) = 0 ( integrable constraints are also called – the velocity constraints that they imply can be integrated to give equivalent configuration constraints. We now consider another class of Pfaffian constraints that is fundamentally different from the holonomic type. To illustrate this with a concrete example, consider an upright coin of radius r rolling on a plane as shown in Figure 2.11. x,y ) on the plane, The configuration of the coin is given by the contact point ( φ , and the angle of rotation θ . The C-space of the coin is the steering angle 2 2 2 R therefore T , where T is the two-dimensional torus parametrized by the × angles φ and θ . This C-space is four dimensional. We now express, in mathematical form, the fact that the coin rolls without slipping. The coin must always roll in the direction indicated by (cos sin φ ), φ, ̇ r θ : with forward speed [ ] [ ] φ x cos ̇ ̇ = r θ . (2.9) φ y ̇ sin May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

50 32 2.5. Task Space and Workspace T q = [ q q q = q Collecting the four C-space coordinates into a single vector ] 4 2 1 3 T 2 2 R [ × T x y φ θ ] , the above no-slip rolling constraint can then be expressed ∈ in the form [ ] 1 0 0 q r cos − 3 q ̇ = 0 . (2.10) sin r 0 1 0 − q 3 × 2 4 ) ̇ R = 0, A ( q ) ∈ q ( A These are Pfaffian constraints of the form . q A ( ) given in (2.10), These constraints are not integrable; that is, for the q 2 4 : → R g such that R ( q ) /∂q = there does not exist a differentiable function ∂g ( q ). If this were not the case then there would have to exist a differentiable A q g ) that satisfied the following four equalities: ( 1 + ) = ) = 1 −→ g ,q ( q ∂g q ,q /∂q h q ( 1 2 1 3 1 4 1 1 ∂g ) /∂q ,q = 0 −→ g ,q ( q ) = h q ( 2 1 3 2 4 1 1 h ( ) = 0 −→ g ,q ∂g q ) = /∂q ,q ( q 1 3 2 1 4 3 1 ∂g , /∂q ) = − r cos q ,q −→ g ,q ( q ) = − rq q cos q ( + h 4 3 1 1 2 3 3 4 1 4 h 4, differentiable in each of its variables. By inspection , i = 1 ,..., for some i it should be clear that no such g ( q ) exists. Similarly, it can be shown that 1 g ( q ) does not exist, so that the constraint (2.10) is nonintegrable. A Pfaffian 2 constraint that is nonintegrable is called a nonholonomic constraint. Such constraints reduce the dimension of the feasible velocities of the system but do not reduce the dimension of the reachable C-space. The rolling coin can reach any point in its four-dimensional C-space despite the two constraints on 7 2.30. its velocity. See Exercise In a number of robotics contexts nonholonomic constraints arise that involve the conservation of momentum and rolling without slipping, e.g., wheeled vehicle kinematics and grasp contact kinematics. We examine nonholonomic constraints 13. in greater detail in our study of wheeled mobile robots in Chapter 2.5 Task Space and Workspace We now introduce two more concepts relating to the configuration of a robot: the task space and the workspace. Both relate to the configuration of the end- effector of a robot, not to the configuration of the entire robot. The is a space in which the robot’s task can be naturally ex- task space pressed. For example, if the robot’s task is to plot with a pen on a piece of paper, 2 R . If the task is to manipulate a rigid body, a natural the task space would be 7 Some texts define the number of degrees of freedom of a system to be the dimension of the feasible velocities, e.g., two for the rolling coin. We always refer to the dimension of the C-space as the number of degrees of freedom. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

51 Chapter 2. Configuration Space 33 ) (a ) (b θ 1 θ 2 θ 3 (c ) (d ) Figure 2.12: Examples of workspaces for various robots: (a) a planar 2R open chain; (b) a planar 3R open chain; (c) a spherical 2R open chain; (d) a 3R orienting mechanism. representation of the task space is the C-space of a rigid body, representing the position and orientation of a frame attached to the robot’s end-effector. This is the default representation of task space. The decision of how to define the task space is driven by the task, independently of the robot. The workspace is a specification of the configurations that the end-effector of the robot can reach. The definition of the workspace is primarily driven by the robot’s structure, independently of the task. Both the task space and the workspace involve a choice by the user; in particular, the user may decide that some freedoms of the end-effector (e.g., its orientation) do not need to be represented. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

52 34 2.5. Task Space and Workspace The task space and the workspace are distinct from the robot’s C-space. A point in the task space or the workspace may be achievable by more than one robot configuration, meaning that the point is not a full specification of the robot’s configuration. For example, for an open-chain robot with seven joints, the six-dof position and orientation of its end-effector does not fully specify the robot’s configuration. Some points in the task space may not be reachable at all by the robot, such as some points on a chalkboard. By definition, however, all points in the workspace are reachable by at least one configuration of the robot. Two mechanisms with different C-spaces may have the same workspace. For example, considering the end-effector to be the Cartesian tip of the robot (e.g., the location of a plotting pen) and ignoring orientations, the planar 2R open chain with links of equal length three (Figure 2.12(a)) and the planar 3R open 2.12(b)) have the same workspace chain with links of equal length two (Figure despite having different C-spaces. Two mechanisms with the same C-space may also have different workspaces. For example, taking the end-effector to be the Cartesian tip of the robot and ignoring orientations, the 2R open chain of Figure 2.12(a) has a planar disk as 2.12(c) has the surface of a its workspace, while the 2R open chain of Figure sphere as its workspace. Attaching a coordinate frame to the tip of the tool of the 3R open-chain 2.12(d), we see that the frame can achieve any “wrist” mechanism of Figure orientation by rotating the joints but the Cartesian position of the tip is always fixed. This can be seen by noting that the three joint axes always intersect at the tip. For this mechanism, we would probably define the workspace to be the 2 1 × three-dof space of orientations of the frame, S , which is different from the S 3 T . The task space depends on the task; if the job is to point a laser C-space pointer, then rotations about the axis of the laser beam are immaterial and the 2 S task space would be , the set of directions in which the laser can point. Example 2.9. The SCARA robot of Figure is an RRRP open chain that is 2.13 widely used for tabletop pick-and-place tasks. The end-effector configuration is completely described by the four parameters ( ), where ( x,y,z ) denotes x,y,z,φ φ the Cartesian position of the end-effector center point and denotes the ori- entation of the end-effector in the x – y -plane. Its task space would typically be 1 3 R S defined as , and its workspace would typically be defined as the reachable × 1 x,y,z ) Cartesian space, since all orientations φ ∈ S points in ( can be achieved at all reachable points. Example 2.10. A standard 6R industrial manipulator can be adapted to spray- painting applications as shown in Figure 2.14. The paint spray nozzle attached May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

53 Chapter 2. Configuration Space 35 θ 2 θ 1 θ 3 θ 4 φ ˆz ˆy z y, x, ( ) ˆx Figure 2.13: SCARA robot. Figure 2.14: A spray-painting robot. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

54 36 2.6. Summary to the tip can be regarded as the end-effector. What is important to the task is the Cartesian position of the spray nozzle, together with the direction in which the spray nozzle is pointing; rotations about the nozzle axis (which points in the direction in which paint is being sprayed) do not matter. The nozzle configuration can therefore be described by five coordinates: ( x,y,z ) for the ) to describe Cartesian position of the nozzle and spherical coordinates ( θ,φ the direction in which the nozzle is pointing. The task space can be written 3 2 3 2 × . The workspace could be the reachable points in R as × S R , or, to S simplify visualization, the user could define the workspace to be the subset of 3 R corresponding to the reachable Cartesian positions of the nozzle. 2.6 Summary • A robot is mechanically constructed from links that are connected by various types of joint. The links are usually modeled as rigid bodies. An end-effector such as a gripper may be attached to some link of the robot. Actuators deliver forces and torques to the joints, thereby causing motion of the robot. • The most widely used one-dof joints are the revolute joint, which allows rotation about the joint axis, and the prismatic joint, which allows trans- lation in the direction of the joint axis. Some common two-dof joints include the cylindrical joint, which is constructed by serially connecting a revolute and prismatic joint, and the universal joint, which is constructed by orthogonally connecting two revolute joints. The spherical joint, also known as the ball-and-socket joint, is a three-dof joint whose function is similar to the human shoulder joint. • The configuration of a rigid body is a specification of the location of all its points. For a rigid body moving in the plane, three independent parame- ters are needed to specify the configuration. For a rigid body moving in three-dimensional space, six independent parameters are needed to specify the configuration. • The configuration of a robot is a specification of the configuration of all its links. The robot’s configuration space is the set of all possible robot configurations. The dimension of the C-space is the number of degrees of freedom of a robot. • The number of degrees of freedom of a robot can be calculated using May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

55 Chapter 2. Configuration Space 37 Gr ̈ubler’s formula, J ∑ dof = 1 − J ) + m N − ( f , i =1 i = 3 for planar mechanisms and m = 6 for spatial mechanisms, N where m is the number of joints, J is the number of links (including the ground link), f is the number of degrees of freedom of joint i . If the constraints and i enforced by the joints are not independent then Gr ̈ubler’s formula provides a lower bound on the number of degrees of freedom. A robot’s C-space can be parametrized explicitly or represented implicitly. • For a robot with n degrees of freedom, an explicit parametrization uses n coordinates, the minimum necessary. An implicit representation involves coordinates with m ≥ n , with the m coordinates subject to m − n m constraint equations. With an implicit parametrization, a robot’s C-space n can be viewed as a surface of dimension embedded in a space of higher m . dimension • The C-space of an n -dof robot whose structure contains one or more closed loop-closure equations of the loops can be implicitly represented using k m m k ) = 0, where θ ∈ R g and g : R ( → R form . Such constraint equations θ are called holonomic constraints. Assuming that θ varies with time t , the holonomic constraints g θ ( t )) = 0 can be differentiated with respect to t ( to yield ∂g ̇ ( θ ) θ = 0 , ∂θ ∂g ( θ ) /∂θ where k × m matrix. is a • A robot’s motion can also be subject to velocity constraints of the form ̇ ( θ A θ = 0 , ) where A ( θ ) is a k × m matrix that cannot be expressed as the differential θ of some function ). In other words, there does not exist any g ( θ ) ,g : g ( m k R → , such that R ∂g . ( θ ) ( θ ) = A ∂θ Such constraints are said to be nonholonomic constraints, or nonintegrable constraints. These constraints reduce the dimension of feasible velocities of the system but do not reduce the dimension of the reachable C-space. Nonholonomic constraints arise in robot systems subject to conservation of momentum or rolling without slipping. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

56 38 2.7. Notes and References A robot’s task space is a space in which the robot’s task can be naturally • expressed. A robot’s workspace is a specification of the configurations that the end-effector of the robot can reach. Notes and References 2.7 In the kinematics literature, structures that consist of links connected by joints are also called mechanisms or linkages. The number of degrees of freedom of a mechanism, also referred to as its mobility, is treated in most texts on mecha- nism analysis and design, e.g., [43, 114]. The notion of a robot’s configuration space was first formulated in the context of motion planning by Lozano-Perez 83, [95]; more recent and advanced treatments can be found in [80, 27]. As apparent from some of the examples in this chapter, a robot’s configuration space can be nonlinear and curved, as can its task space. Such spaces often have the mathematical structure of a differentiable manifold, which are the cen- tral objects of study in differential geometry. Some accessible introductions to differential geometry are [119, 38, 17]. 2.8 Exercises In the exercises below, if you are asked to “describe” a C-space, you should indicate its dimension and whatever you know about its topology (e.g., using R , S , and T , as with the examples in Sections 2.3.1 and 2.3.2). Exercise 2.1 derive a formula, in terms of 2.1 Using the methods of Section , for the number of degrees of freedom of a rigid body in n -dimensional space. n Indicate how many of these dof are translational and how many are rotational. 2 1 = 2, the topology is R S × Describe the topology of the C-space (e.g., for n ). Exercise 2.2 Find the number of degrees of freedom of your arm, from your torso to your palm (just past the wrist, not including finger degrees of freedom). Keep the center of the ball-and-socket joint of your shoulder stationary (do not “hunch” your shoulders). Find the number of degrees of freedom in two ways: (a) add up the degrees of freedom at the shoulder, elbow, and wrist joints; (b) fix your palm flat on a table with your elbow bent and, without moving the center of your shoulder joint, investigate with how many degrees of freedom you can still move your arm. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

57 Chapter 2. Configuration Space 39 Human A Robot Figure 2.15: Robot used for human arm rehabilitation. Do your answers agree? How many constraints were placed on your arm when you placed your palm at a fixed configuration on the table? In the previous exercise, we assumed that your arm is a serial Exercise 2.3 chain. In fact, between your upper arm bone (the humerus) and the bone complex just past your wrist (the carpal bones), your forearm has two bones, the radius and the ulna, which are part of a closed chain. Model your arm, from your shoulder to your palm, as a mechanism with joints and calculate the number of degrees of freedom using Gr ̈ubler’s formula. Be clear on the number of freedoms of each joint you use in your model. Your joints may or may not be of the standard types studied in this chapter (R, P, H, C, U, and S). Exercise 2.4 Assume each of your arms has n degrees of freedom. You are driving a car, your torso is stationary relative to the car (owing to a tight seatbelt!), and both hands are firmly grasping the wheel, so that your hands do not move relative to the wheel. How many degrees of freedom does your arms-plus-steering wheel system have? Explain your answer. Exercise 2.5 Figure 2.15 shows a robot used for human arm rehabilitation. Determine the number of degrees of freedom of the chain formed by the human arm and the robot Exercise 2.6 The mobile manipulator of Figure 2.16 consists of a 6R arm and multi-fingered hand mounted on a mobile base with a single wheel. You can May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

58 40 2.8. Exercises Mobile manipulator. Figure 2.16: 2.11 – the think of the wheeled base as the same as the rolling coin in Figure wheel (and base) can spin together about an axis perpendicular to the ground, and the wheel rolls without slipping. The base always remains horizontal. (Left unstated are the means to keep the base horizontal and to spin the wheel and base about an axis perpendicular to the ground.) (a) Ignoring the multi-fingered hand, describe the configuration space of the mobile manipulator. (b) Now suppose that the robot hand rigidly grasps a refrigerator door handle and, with its wheel and base completely stationary, opens the door using only its arm. With the door open, how many degrees of freedom does the mechanism formed by the arm and open door have? (c) A second identical mobile manipulator comes along, and after parking its mobile base, also rigidly grasps the refrigerator door handle. How many degrees of freedom does the mechanism formed by the two arms and the open refrigerator door have? Exercise 2.7 Three identical SRS open-chain arms are grasping a common object, as shown in Figure 2.17. (a) Find the number of degrees of freedom of this system. (b) Suppose there are now a total of n such arms grasping the object. How many degrees of freedom does this system have? (c) Suppose the spherical wrist joint in each of the n arms is now replaced by May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

59 Chapter 2. Configuration Space 41 Spherical Joint spherical joint Revolute Joint revolute joint Figure 2.17: Three cooperating SRS arms grasping a common object. a universal joint. How many degrees of freedom does this system have? Exercise 2.8 Consider a spatial parallel mechanism consisting of a moving n plate connected to a fixed plate by identical legs. For the moving plate to have six degrees of freedom, how many degrees of freedom should each leg have, as a function of n ? For example, if n = 3 then the moving plate and fixed plate are connected by three legs; how many degrees of freedom should each leg have for the moving plate to move with six degrees of freedom? Solve for arbitrary n . Exercise 2.9 Use the planar version of Gr ̈ubler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.18. Comment on whether your results agree with your intuition about the possible motions of these mechanisms. Exercise 2.10 Use the planar version of Gr ̈ubler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.19. Comment on whether your results agree with your intuition about the possible motions of these mechanisms. Exercise 2.11 Use the spatial version of Gr ̈ubler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.20. Comment on whether your results agree with your intuition about the possible motions of these mechanisms. Exercise 2.12 Use the spatial version of Gr ̈ubler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.21. Comment on whether your results agree with your intuition about the possible motions of May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

60 42 2.8. Exercises (a) (b) ForkJoint Slider Slider (c) (d) (e) (f) Figure 2.18: A first collection of planar mechanisms. these mechanisms. Exercise 2.13 In the parallel mechanism shown in Figure 2.22, six legs of May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

61 Chapter 2. Configuration Space 43 (b) (a) (c) (d) A second collection of planar mechanisms. Figure 2.19: identical length are connected to a fixed and moving platform via spherical joints. Determine the number of degrees of freedom of this mechanism using Gr ̈ubler’s formula. Illustrate all possible motions of the upper platform. Exercise 2.14 The 3 × UPU platform of Figure 2.23 consists of two platforms– the lower one stationary, the upper one mobile–connected by three UPU legs. (a) Using the spatial version of Gr ̈ubler’s formula, verify that it has three degrees of freedom. (b) Construct a physical model of the 3 × UPU platform to see if it indeed has three degrees of freedom. In particular, lock the three P joints in place; does the robot become a rigid structure as predicted by Gr ̈ubler’s formula, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

62 44 2.8. Exercises SJoi nt P Joint R Joint CircularP Joint (a) (b) Circular nt SJoi UniversalJoin t P Joint (c) (d) SJoint (e) (f) Figure 2.20: A first collection of spatial parallel mechanisms. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

63 Chapter 2. Configuration Space 45 R R U U U (b) (a) R R P R R RR U R S (d) (c) Figure 2.21: A second collection of spatial parallel mechanisms. or does it move? The mechanisms of Figures 2.24(a) and 2.24(b). Exercise 2.15 (a) The mechanism of Figure 2.24(a) consists of six identical squares arranged in a single closed loop, connected by revolute joints. The bottom square is fixed to ground. Determine the number of degrees of freedom using Gr ̈ubler’s formula. (b) The mechanism of Figure 2.24(b) also consists of six identical squares connected by revolute joints, but arranged differently (as shown). Deter- mine the number of degrees of freedom using Gr ̈ubler’s formula. Does May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

64 46 2.8. Exercises Figure 2.22: A 6 × SS platform. Figure 2.23: The 3 × UPU platform. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

65 Chapter 2. Configuration Space 47 R R R R R R R R R R stationary stationary R R (a) (b) Figure 2.24: Two mechanisms. your result agree with your intuition about the possible motions of this mechanism? Figure 2.25 shows a spherical four-bar linkage, in which four Exercise 2.16 links (one of the links is the ground link) are connected by four revolute joints to form a single-loop closed chain. The four revolute joints are arranged so that they lie on a sphere such that their joint axes intersect at a common point. (a) Use Gr ̈ubler’s formula to find the number of degrees of freedom. Justify your choice of formula. (b) Describe the configuration space. (c) Assuming that a reference frame is attached to the center link, describe its workspace. Exercise 2.17 Figure 2.26 shows a parallel robot used for surgical applications. As shown in Figure 2.26(a), leg A is an RRRP chain, while legs B and C are RRRUR chains. A surgical tool is rigidly attached to the end-effector. (a) Use Gr ̈ubler’s formula to find the number of degrees of freedom of the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

66 48 2.8. Exercises The spherical four-bar linkage. Figure 2.25: LegC End-effector Surgicaltool LegD LegB End-effector LegD Surgicaltool LegA LegD Base Base Point A Point A (a) (b) Figure 2.26: Surgical manipulator. mechanism in Figure 2.26(a). (b) Now suppose that the surgical tool must always pass through point A in Figure 2.26(a). How many degrees of freedom does the manipulator have? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

67 Chapter 2. Configuration Space 49 platform P U P base Figure 2.27: The 3 × PUP platform. (c) Legs A, B, and C are now replaced by three identical RRRR legs as shown in Figure 2.26(b). Furthermore, the axes of all R joints pass through point . Use Gr ̈ubler’s formula to derive the number of degrees of freedom of A this mechanism. Exercise 2.18 Figure 2.27 shows a 3 × PUP platform, in which three identical PUP legs connect a fixed base to a moving platform. The P joints on both the fixed base and moving platform are arranged symmetrically. Use Gr ̈ubler’s formula to find the number of degrees of freedom. Does your answer agree with your intuition about this mechanism? If not, try to explain any discrepancies without resorting to a detailed kinematic analysis. Exercise 2.19 The dual-arm robot of Figure 2.28 is rigidly grasping a box. The box can only slide on the table; the bottom face of the box must always be in contact with the table. How many degrees of freedom does this system have? Exercise 2.20 The dragonfly robot of Figure 2.29 has a body, four legs, and four wings as shown. Each leg is connected to each adjacent leg by a USP linkage. Use Gr ̈ubler’s formula to answer the following questions. (a) Suppose the body is fixed and only the legs and wings can move. How many degrees of freedom does the robot have? (b) Now suppose the robot is flying in the air. How many degrees of freedom does the robot have? (c) Now suppose the robot is standing with all four feet in contact with the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

68 50 2.8. Exercises S S R R R S S Dual-arm robot. Figure 2.28: R S P P U S Figure 2.29: Dragonfly robot. ground. Assume that the ground is uneven and that each foot–ground contact can be modeled as a point contact with no slip. How many degrees of freedom does the robot have? Explain your answer. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

69 Chapter 2. Configuration Space 51 R RPR R Contact (a) (b) (c) Figure 2.30: A caterpillar robot. Exercise 2.21 A caterpillar robot. (a) A caterpillar robot is hanging by its tail end as shown in Figure 2.30(a). The robot consists of eight serially connected rigid links (one head, one tail, and six body links). The six body links are connected by revolute– prismatic–revolute joints, while the head and tail are connected to the body by revolute joints. Find the number of degrees of freedom of this robot. (b) The caterpillar robot is now crawling on a leaf as shown in Figure 2.30(b). Suppose that all six body links must make contact with the leaf at all times but the links can slide and rotate on the leaf. Find the number of degrees of freedom of this robot during crawling. (c) Now suppose the caterpillar robot crawls on the leaf as shown in Fig- ure 2.30(c), with only the first and last body links in contact with the leaf. Find the number of degrees of freedom of this robot during crawling. Exercise 2.22 The four-fingered hand of Figure 2.31(a) consists of a palm and four URR fingers (the U joints connect the fingers to the palm). (a) Assume that the fingertips are points and that one fingertip is in contact with the table surface (sliding of the fingertip point-contact is allowed). How many degrees of freedom does the hand have? What if two fingertips are in sliding point contact with the table? Three? All four? (b) Repeat part (a) but with each URR finger replaced by an SRR finger (each universal joint is replaced by a spherical joint). (c) The hand (with URR fingers) now grasps an ellipsoidal object, as shown May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

70 52 2.8. Exercises U R R (a) (b) (c) Figure 2.31: (a) A four-fingered hand with palm. (b) The hand grasping an ellip- soidal object. (c) A rounded fingertip that can roll on the object without sliding. in Figure 2.31(b). Assume that the palm is fixed in space and that no slip occurs between the fingertips and object. How many degrees of freedom does the system have? 2.31(c). (d) Now assume that the fingertips are hemispheres as shown in Figure Each fingertip can roll on the object but cannot slip or break contact with the object. How many degrees of freedom does the system have? For a single fingertip in rolling contact with the object, comment on the dimen- sion of the space of feasible fingertip velocities relative to the object versus the number of parameters needed to represent the fingertip configuration relative to the object (the number of degrees of freedom). (Hint: You may want to experiment by rolling a ball around on a tabletop to get some intuition.) Exercise 2.23 Consider the slider–crank mechanism of Figure 2.4(b). A ro- tational motion at the revolute joint fixed to ground (the “crank”) causes a translational motion at the prismatic joint (the “slider”). Suppose that the two links connected to the crank and slider are of equal length. Determine the con- figuration space of this mechanism, and draw its projected version on the space defined by the crank and slider joint variables. Exercise 2.24 The planar four-bar linkage. (a) Use Gr ̈ubler’s formula to determine the number of degrees of freedom of a planar four-bar linkage floating in space. (b) Derive an implicit parametrization of the four-bar’s configuration space as follows. First, label the four links 1, 2, 3, and 4, and choose three points A,B,C on link 1, D,E,F on link 2, G,H,I on link 3, and J,K,L on link May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

71 Chapter 2. Configuration Space 53 A B h a b 2 2 + α β ˆy β ψ θ φ ˆx g α (a) (b) Figure 2.32: Planar four-bar linkage. 4. The four-bar linkage is constructed in such a way that the following with , C D four pairs of points are each connected by a revolute joint: G , I F J , and L with A . Write down explicit constraints with with A,...,H on the coordinates for the eight points (assume that a fixed reference frame has been chosen, and denote the coordinates for point ), and similarly for the other points). Based on by = ( x A ,y ,z p A A A A counting the number of variables and constraints, how many degrees of freedom does the configuration space have? If it differs from the result you obtained in (a), try to explain why. In this exercise we examine in more detail the representation Exercise 2.25 of the C-space for the planar four-bar linkage of Figure 2.32. Attach a fixed reference frame and label the joints and link lengths as shown in the figure. The x,y ( A and B are given by ) coordinates for joints θ, a ( A a cos θ sin θ ) , ) = ( B ( ψ ) = ( g + b cos ψ, b sin ψ ) . ( Using the fact that the link connecting is of fixed length h , i.e., ‖ A B θ ) − and A 2 2 ) ‖ B = h ( , we have the constraint ψ 2 2 2 2 b + 2 gb cos ψ + a g − 2( a cos θ ( + + b cos ψ ) + ab sin θ sin ψ ) = h g . Grouping the coefficients of cos and sin ψ , the above equation can be expressed ψ in the form α ( θ ) cos ψ + β ( θ ) sin ψ = γ ( θ ) , (2.11) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

72 54 2.8. Exercises where α gb − 2 ab cos θ, ( θ ) = 2 ) = − 2 ab sin θ, θ β ( 2 2 2 2 − g γ ( b θ − a ) = + 2 ag cos θ. h − as a function of , by first dividing both sides of Equa- θ We now express ψ √ 2 2 tion (2.11) by β to obtain + α α γ β √ √ √ = cos (2.12) . + sin ψ ψ 2 2 2 2 2 2 β α β + β α α + + − 1 2.32(b), the angle φ = tan φ is given by ( β/α ), so that Referring to Figure Equation (2.12) becomes γ √ cos( ) = ψ − φ . 2 2 + β α Therefore ( ) ) ( γ β − 1 − 1 √ ± cos . ψ = tan 2 2 α + β α 2 2 2 α (a) Note that a solution exists only if γ β + . What are the physical ≤ implications if this constraint is not satisfied? (b) Note that, for each value of the input angle θ , there exist two possible . What do these two solutions look like? ψ values of the output angle (c) Draw the configuration space of the mechanism in – ψ space for the fol- θ a = b = g = h = 1. lowing link length values: √ a = 1, b = 2, h = (d) Repeat (c) for the following link length values: 5, g = 2. = 1, = 1, = 1, h a b (e) Repeat (c) for the following link length values: √ = g 3. The tip coordinates for the two-link planar 2R robot of Fig- Exercise 2.26 ure 2.33 are given by x = 2 cos θ + cos( θ + θ ) 1 2 1 y θ + sin( θ + θ = 2 sin ) . 1 2 1 (a) What is the robot’s configuration space? (b) What is the robot’s workspace (i.e., the set of all points reachable by the tip)? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

73 Chapter 2. Configuration Space 55 y ( x, ) θ 2 ˆy θ 1 ˆx Figure 2.33: Two-link planar 2R open chain. x = 1 and x = (c) Suppose infinitely long vertical barriers are placed at 1. − What is the free C-space of the robot (i.e., the portion of the C-space that does not result in any collisions with the vertical barriers)? Exercise 2.27 The workspace of a planar 3R open chain. (a) Consider a planar 3R open chain with link lengths (starting from the fixed base joint) 5, 2, and 1, respectively. Considering only the Cartesian point of the tip, draw its workspace. (b) Now consider a planar 3R open chain with link lengths (starting from the fixed base joint) 1, 2, and 5, respectively. Considering only the Cartesian point of the tip, draw its workspace. Which of these two chains has a larger workspace? (c) A not-so-clever designer claims that he can make the workspace of any planar open chain larger simply by increasing the length of the last link. Explain the fallacy behind this claim. Exercise 2.28 Task space. (a) Describe the task space for a robot arm writing on a blackboard. (b) Describe the task space for a robot arm twirling a baton. Exercise 2.29 Give a mathematical description of the topologies of the C- spaces of the following systems. Use cross products, as appropriate, of spaces k m n R T , S such as a closed interval [ , and a,b ] of a line and , where k , m , and n are chosen appropriately. (a) The chassis of a car-like mobile robot rolling on an infinite plane. (b) The car-like mobile robot (chassis only) driving around on a spherical asteroid. (c) The car-like mobile robot (chassis only) on an infinite plane with an RRPR robot arm mounted on it. The prismatic joint has joint limits, but the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

74 56 2.8. Exercises d θ r ) ( x,y φ 1 Top view Side view A side view and a top view of a diff-drive robot. Figure 2.34: revolute joints do not. (d) A free-flying spacecraft with a 6R arm mounted on it and no joint limits. Exercise 2.30 Describe an algorithm that drives the rolling coin of Figure 2.11 from any arbitrary initial configuration in its four-dimensional C-space to any arbitrary goal configuration, despite the two nonholonomic constraints. The ̇ ̇ control inputs are the rolling speed φ θ and the turning speed . You should explain clearly in words or pseudocode how the algorithm would work. It is not necessary to give actual code or formulas. Exercise 2.31 A differential-drive mobile robot has two wheels that do not steer but whose speeds can be controlled independently. The robot goes forward and backward by spinning the wheels in the same direction at the same speed, and it turns by spinning the wheels at different speeds. The configuration of the x,y ) location of the point halfway between robot is given by five variables: the ( the wheels, the heading direction θ of the robot’s chassis relative to the x -axis of φ of the two wheels about the and φ the world frame, and the rotation angles 2 1 2.34). Assume that the radius of axis through the centers of the wheels (Figure r and the distance between the wheels is 2 d . each wheel is q = ( x,y,θ,φ ,φ (a) Let ) be the configuration of the robot. If the two control 1 2 ̇ ̇ ω inputs are the angular velocities of the wheels φ , write and φ = = ω 2 1 2 1 down the vector differential equation ̇ q = g . The vector ( q ) ω ω + g ) ( q 2 2 1 1 fields g 13.3) and ( q ) and g (see Section ( q ) are called control vector fields 2 1 express how the system moves when the respective unit control signal is applied. (b) Write the corresponding Pfaffian constraints A ( q ) ̇ q = 0 for this system. How many Pfaffian constraints are there? (c) Are the constraints holonomic or nonholonomic? Or how many are holo- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

75 Chapter 2. Configuration Space 57 nomic and how many nonholonomic? Exercise 2.32 Determine whether the following differential constraints are holonomic or nonholonomic: (a) q . ) ̇ q = 0 + (1 + cos q q ) ̇ (1 + cos + 4) ̇ + (cos q q + cos q 1 2 2 2 3 1 1 (b) − ̇ q ) = 0 cos q q + ̇ q + sin( q q + q cos( ) − ̇ q 4 1 3 2 2 1 1 2 q . q ̇ − ̇ q cos q = 0 sin 3 1 4 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

76 58 2.8. Exercises May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

77 Chapter 3 Rigid-Body Motions In the previous chapter, we saw that a minimum of six numbers is needed to specify the position and orientation of a rigid body in three-dimensional physical space. In this chapter we develop a systematic way to describe a rigid body’s position and orientation which relies on attaching a reference frame to the body. The configuration of this frame with respect to a fixed reference frame is then represented as a 4 × 4 matrix. This matrix is an example of an implicit representation of the C-space, as discussed in the previous chapter: the actual six-dimensional space of rigid-body configurations is obtained by applying ten constraints to the 16-dimensional space of 4 × 4 real matrices. Such a matrix not only represents the configuration of a frame, but can also be used to (1) translate and rotate a vector or a frame, and (2) change the rep- resentation of a vector or a frame from coordinates in one frame to coordinates in another frame. These operations can be performed by simple linear algebra, which is a major reason why we choose to represent a configuration as a 4 × 4 matrix. The non-Euclidean (i.e., non-“flat”) nature of the C-space of positions and orientations leads us to use a matrix representation. A rigid body’s velocity, 6 however, can be represented simply as a point in R , defined by three angular velocities and three linear velocities, which together we call a spatial velocity or twist . More generally, even though a robot’s C-space may not be a vector space, the set of feasible velocities at any point in the C-space always forms a vector space. For example, consider a robot whose C-space is the sphere 2 S : although the C-space is not flat, at any point on the sphere the space of velocities can be thought of as the plane (a vector space) tangent to that point on the sphere. 59

78 60 Any rigid-body configuration can be achieved by starting from the fixed (home) reference frame and integrating a constant twist for a specified time. Such a motion resembles the motion of a screw, rotating about and translat- ing along the same fixed axis. The observation that all configurations can be achieved by a screw motion motivates a six-parameter representation of the exponential coordinates configuration called the . The six parameters can be divided into the parameters describing the direction of the screw axis and a scalar to indicate how far the screw motion must be followed to achieve the desired configuration. This chapter concludes with a discussion of forces. Just as angular and linear 6 R , moments (torques) velocities are packaged together into a single vector in spatial force and forces are packaged together into a six-vector called a or wrench . To illustrate the concepts and to provide a synopsis of the chapter, we begin with a motivating planar example. Before doing so, we make some remarks about vector notation. A Word about Vectors and Reference Frames free vector is a geometric quantity with a length and a direction. Think A n R . It is called “free” because it is not necessarily rooted of it as an arrow in anywhere; only its length and direction matter. A linear velocity can be viewed as a free vector: the length of the arrow is the speed and the direction of the arrow is the direction of the velocity. A free vector is denoted by an upright text symbol, e.g., v. If a reference frame and length scale have been chosen for the underlying space in which v lies then this free vector can be moved to a position such that the base of the arrow is at the origin without changing the orientation. The free vector v can then be represented by its coordinates in the reference frame. n ∈ We write the vector in italics, v , where v is at the “head” of the arrow R in the frame’s coordinates. If a different reference frame and length scale are chosen then the representation v will change but the underlying free vector v is unchanged. coordinate free ; it refers to a physical In other words, we say that v is quantity in the underlying space, and it does not care how we represent it. However, v is a representation of v that depends on the choice of coordinate frame. A point p in physical space can also be represented as a vector. Given a choice of reference frame and length scale for physical space, the point p can be represented as a vector from the reference frame origin to p; its vector May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

79 Chapter 3. Rigid-Body Motions 61 p p b } b { ˆy p a a ˆx b ˆy b ˆx a } { a Figure 3.1: The point p exists in physical space, and it does not care how we { a } , with unit coordinate axes ˆx and ˆy represent it. If we fix a reference frame , we a a at a different location, a , 2). If we fix a reference frame { b } p = (1 can represent p as a p = (4 , − 2). different orientation, and a different length scale, we can represent p as b n p ∈ R . Here, as before, a different representation is denoted in italics by choice of reference frame and length scale for physical space leads to a different n p ∈ R representation for the same point p in physical space. See Figure 3.1. In the rest of this book, a choice of length scale will always be assumed, but we will be dealing with reference frames at different positions and orientations. A reference frame can be placed anywhere in space, and any reference frame leads to an equally valid representation of the underlying space and the objects fixed frame , or space in it. We always assume that exactly one stationary , denoted { s } , has been defined. This might be attached to a corner of a frame room, for example. Similarly, we often assume that at least one frame has been attached to some moving rigid body, such as the body of a quadrotor flying in the room. This body frame , denoted { b } , is the stationary frame that is coincident with the body-attached frame at any instant. While it is common to attach the origin of the { b } frame to some important point on the body, such as its center of mass, this is not necessary. The origin of { b } frame does not even need to be on the physical body itself, as long as its the configuration relative to the body, viewed from an observer stationary relative to the body, is constant. Important! All frames in this book are stationary, inertial, frames. When we refer to a body frame { b } , we mean a motionless frame that is instantaneously coincident with a frame that is fixed to a (possibly moving) body. This is important to keep in mind, since you may have had a dynamics course that used non-inertial moving frames attached to rotating bodies. Do not confuse these with the stationary, inertial, body frames of this book. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

80 62 3.1. Rigid-Body Motions in the Plane positive ˆz rotation ˆx ˆy (Left) The ˆx, ˆy, and ˆz axes of a right-handed reference frame are aligned Figure 3.2: with the index finger, middle finger, and thumb of the right hand, respectively. (Right) A positive rotation about an axis is in the direction in which the fingers of the right hand curl when the thumb is pointed along the axis. For simplicity, we will usually refer to a body frame as a frame attached to a moving rigid body. Despite this, at any instant, by “body frame” we actually mean the stationary frame that is instantaneously coincident with the frame moving along with the body. all frames are stationary It is worth repeating one more time: . right-handed , as illustrated in Figure 3.2. A All reference frames are positive rotation about an axis is defined as the direction in which the fingers of the right hand curl when the thumb is pointed along the axis (Figure 3.2). 3.1 Rigid-Body Motions in the Plane Consider the planar body (the gray shape) in Figure 3.3; its motion is confined to the plane. Suppose that a length scale and a fixed reference frame { } have s been chosen as shown, with unit axes ˆx and ˆy . (Throughout this book, the s s hat notation indicates a unit vector.) Similarly, we attach a reference frame with unit axes ˆx to the planar body. Because this frame moves with and ˆy b b the body, it is called the body frame and is denoted { b } . To describe the configuration of the planar body, only the position and orientation of the body frame with respect to the fixed frame need to be specified. The body-frame origin p can be expressed in terms of the coordinate axes of May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

81 Chapter 3. Rigid-Body Motions 63 ˆx b ˆy θ b ˆy } { b s p ˆx } s { s { b } is expressed in the fixed-frame coordinates { s } by the Figure 3.3: The body frame , and the directions of the unit axes ˆx and ˆy vector . In this example, p = (2 p 1) and b b √ √ ◦ 5). = (cos θ, θ = 60 ) = (0 . 5 , 1 / sin 2) and ˆy . = ( − sin θ, cos θ ) = ( − 1 / , so ˆx 2 , 0 θ b b s as { } = p (3.1) ˆx p + p . ˆy y x s s You are probably more accustomed to writing this vector as simply p = ( p ); ,p y x this is fine when there is no possibility of ambiguity about reference frames, but writing p as in Equation (3.1) clearly indicates the reference frame with respect ) is defined. p to which ( ,p y x b } relative The simplest way to describe the orientation of the body frame { { s } to the fixed frame θ , as shown in Figure 3.3. is by specifying the angle Another (admittedly less simple) way is to specify the directions of the unit } b , in the form of { axes ˆx and ˆy relative to { s } b b ˆx (3.2) = cos θ ˆx , + sin θ ˆy s b s ˆy θ sin θ ˆx − + cos = ˆy (3.3) . s s b At first sight this seems to be a rather inefficient way of representing the body- frame orientation. However, imagine if the body were to move arbitrarily in three-dimensional space; a single angle θ would not suffice to describe the ori- entation of the displaced reference frame. We would actually need three angles, but it is not yet clear how to define an appropriate set of three angles. However, expressing the directions of the coordinate axes of { b } in terms of coefficients of the coordinate axes of { s } , as we have done above for the planar case, is straightforward. Assuming we agree to express everything in terms of { s } then, just as the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

82 64 3.1. Rigid-Body Motions in the Plane ˆx b θ } { b ˆy q b ψ φ ˆx c p ˆy { } c s r ˆy c ˆx s s { } The frame { b } in { s } is given by ( P,p ), and the frame { c } in { b } is given Figure 3.4: Q,q by ( c } in { s } , described by ( R,r ). The { ). From these we can derive the frame , q , and r and the coordinate-axis directions of the p numerical values of the vectors three frames are evident from the grid of unit squares. 2 ∈ R point p can be represented as a column vector of the form p [ ] p x = p , (3.4) p y the two vectors ˆx and ˆy can also be written as column vectors and packaged b b into the following 2 × P : 2 matrix ] [ cos θ − sin θ . ] = = [ˆx ˆy P (3.5) b b sin θ θ cos P P rotation matrix . Although The matrix consists of is an example of a P four numbers, they are subject to three constraints (each column of must be a unit vector, and the two columns must be orthogonal to each other), and the one remaining degree of freedom is parametrized by θ . Together, the pair ( P,p ) provides a description of the orientation and position of { b } relative to { s } . Now refer to the three frames in Figure 3.4. Repeating the approach above, and expressing c } in { { } as the pair ( R,r ), we can write s ] [ [ ] φ sin r − φ cos x = , R (3.6) . = r φ cos φ sin r y May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

83 Chapter 3. Rigid-Body Motions 65 { We could also describe the frame { b } . Letting q denote the c } relative to { } { c } expressed in b b } coordinates, { vector from the origin of to the origin of denote the orientation of { c } relative to { b } , we can write { c } and letting Q b as the pair ( Q,q ), where relative to { } [ [ ] ] q ψ sin ψ cos − x = q = , Q . (3.7) q ψ ψ sin cos y Q,q ) (the configuration of c } relative to { b } ) and ( P,p ) (the If we know ( { ), we can compute the configuration of b relative to { s } } { c } configuration of { { s } relative to as follows: R PQ (convert Q to the { s } frame) (3.8) = s = r p (convert q to the { Pq } frame and vector-sum with p ) . (3.9) + Thus ( P,p ) not only represents a configuration of { b } in { s } ; it can also be used to convert the representation of a point or frame from { coordinates to { s } b } coordinates. { } and { c } . The Now consider a rigid body with two frames attached to it, d d } frame { s } , and { c } is initially described by ( R,r ) { is initially coincident with { s } (Figure 3.5(a)). Then the body is moved in such a way that { d } moves to in ′ s } , becoming coincident with a frame { b } described by ( P,p ) in { d } . Where { { c } end up after this motion? Denoting the configuration of the new frame does ′ ′ ′ } { R c ,r as ( ), you can verify that ′ R PR, (3.10) = ′ Pr + p, (3.11) r = P,p which is similar to Equations (3.8) and (3.9). The difference is that ( ) is ex- pressed in the same frame as ( R,r ), so the equations are not viewed as a change of coordinates, but instead as a rigid-body displacement (also known as a 1 ): in Figure 3.5(a) transformation according © rotates { c } rigid-body motion 2 { to © translates it by p in and transformation s } . P Thus we see that a rotation matrix–vector pair such as ( P,p ) can be used for three purposes: (a) to represent a configuration of a rigid body in s } (Figure 3.3); { (b) to change the reference frame in which a vector or frame is represented (Figure 3.4); (c) to displace a vector or a frame (Figure 3.5(a)). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

84 66 3.1. Rigid-Body Motions in the Plane c { } } { c p 2 + p Pr s } d { d { b, } β 1 c } { Pr c { } p r } s,d { s,d { } (b ) ) (a (a) The frame { d } , fixed to an elliptical rigid body and initially coincident Figure 3.5: ′ (which is coincident with the stationary frame , is displaced to { d { } } { b } ), s with P then translating according to p , where ( P,p ) is the by first rotating according to . The same transformation takes the frame { } in { s } b { c } , also representation of ′ 1 c . The transformation marked } attached to the rigid body, to { © rigidly rotates 2 { about the origin of { s } , and then transformation c © translates the frame by p } { } . (b) Instead of viewing this displacement as a rotation followed by expressed in s a translation, both rotation and translation can be performed simultaneously. The ◦ = 90 displacement can be viewed as a rotation of about a fixed point s. β Referring to Figure 3.5(b), note that the rigid-body motion illustrated in 3.5(a), expressed as a rotation followed by a translation, can be obtained Figure by simply rotating the body about a fixed point s by an angle β . This is a planar 1 screw motion . The displacement can therefore be parametrized example of a by the three screw coordinates ( β,s 2) denotes the ,s , ), where ( s ) = (0 ,s y x x y coordinates for the point s (i.e., the screw axis out of the page) in the fixed frame s } . { Another way to represent the screw motion is to consider it as the dis- placement obtained by following simultaneous angular and linear velocities for a given distance. Inspecting Figure 3.5(b), we see that rotating about s with a unit angular velocity ( ω = 1 rad/s) means that a point at the origin of the { s } frame moves at two units per second initially in the +ˆx-direction of the { s } ) = (2 frame, i.e., = ( v 0). We can package these together in the three- ,v , v y x 1 If the displacement is a pure translation without rotation, then s lies at infinity. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

85 Chapter 3. Rigid-Body Motions 67 = ( vector ,v ω,v ) = (1 , 2 , 0), a representation of the screw axis . Following S x y θ π/ 2 yields the final displacement. Thus we = this screw axis for an angle = ( π/ 2 can represent the displacement using the three coordinates 0). S θ ,π, exponential These coordinates have some advantages, and we call these the coordinates for the planar rigid-body displacement. To represent the combination of an angular and a linear velocity, called a S = ( twist , we take a screw axis ,v = 1, and scale it by multi- ) , where ω ω,v y x ̇ ̇ θ V = S θ . The net displacement plying by some rotation speed, . The twist is by an angle θ obtained by rotating about the screw axis S is equivalent to the ̇ S at a speed θ = θ for unit time, so displacement obtained by rotating about ̇ S V θ can also be considered a set of exponential coordinates. = In the rest of this chapter we Preview of the remainder of this chapter. generalize the concepts above to three-dimensional rigid-body motions. For this purpose consider a rigid body occupying three-dimensional physical space, as shown in Figure 3.6. Assume that a length scale for physical space has been } and body frame { { } have been chosen s chosen, and that both the fixed frame b as shown. Throughout this book all reference frames are right-handed – the unit ˆx , ˆy , ˆz } always satisfy ˆx × ˆy = ˆz. Denote the unit axes of the fixed frame axes { { ˆx , ˆy { , ˆz . Let p denote } and the unit axes of the body frame by } ˆx ˆz , ˆy , by b b s s s b the vector from the fixed-frame origin to the body-frame origin. In terms of the fixed-frame coordinates, p can be expressed as ˆy = ˆx + p p p (3.12) + p ˆz . 1 3 2 s s s The axes of the body frame can also be expressed as + = + r ˆy r ˆx r ˆz , (3.13) ˆx 31 s s 11 b 21 s (3.14) = r , ˆx ˆz + r ˆy ˆy r + 22 s s 12 32 b s ˆy + r (3.15) ˆx . ˆz r ˆz = r + 33 23 s s 13 b s 3 × 3 3 R ∈ R ∈ p Defining as and R     p r r r 1 12 11 13     p r r r = p = [ˆx ] = ˆz ˆy , R (3.16) , 2 23 22 21 b b b p r r r 3 31 32 33 the 12 parameters given by ( R,p ) then provide a description of the position and orientation of the rigid body relative to the fixed frame. Since the orientation of a rigid body has three degrees of freedom, only three of the nine entries in R can be chosen independently. One three-parameter May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

86 68 3.2. Rotations and Angular Velocities ˆz b ˆx b ˆy b ˆz s p ˆy s ˆx s Mathematical description of position and orientation. Figure 3.6: representation of rotations is provided by the exponential coordinates, which define an axis of rotation and the angle rotated about that axis. We leave Euler an- other popular representations of orientations (the three-parameter and the roll–pitch–yaw angles , the gles , Cayley–Rodrigues parameters and the unit quaternions , which use four variables subject to one constraint) to Appendix B. We then examine the six-parameter exponential coordinates for the config- uration of a rigid body that arise from integrating a six-dimensional twist con- sisting of the body’s angular and linear velocities. This representation follows from the Chasles–Mozzi theorem which states that every rigid-body displace- ment can be obtained by a finite rotation and translation about a fixed screw axis. We conclude with a discussion of forces and moments. Rather than treat these as separate three-dimensional quantities, we merge the moment and force wrench . The twist and wrench, and rules for vectors into a six-dimensional manipulating them, form the basis for the kinematic and dynamic analyses in subsequent chapters. 3.2 Rotations and Angular Velocities 3.2.1 Rotation Matrices We argued earlier that, of the nine entries in the rotation matrix R , only three can be chosen independently. We begin by expressing a set of six explicit con- straints on the entries of R . Recall that the three columns of R correspond to May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

87 Chapter 3. Rigid-Body Motions 69 { the body-frame unit axes , ˆy ˆx , ˆz . The following conditions must therefore } b b b be satisfied. , ˆy (a) The unit norm condition: ˆx are all unit vectors, i.e., , and ˆz b b b 2 2 2 r + r + = 1 , r 11 31 21 2 2 2 r r + r + (3.17) = 1 , 32 22 12 2 2 2 r + r r = 1 . + 23 13 33 (b) The orthogonality condition: ˆx · = ˆx · ˆz = ˆy · · ˆz = 0 (here ˆy denotes b b b b b b the inner product), or , + r r r + r r = 0 r 31 11 32 21 12 22 r r (3.18) + r , r = 0 r r + 32 12 33 23 22 13 r . r = 0 + r r r r + 23 21 13 11 31 33 These six constraints can be expressed more compactly as a single set of con- R straints on the matrix , T = R (3.19) I, R T denotes the transpose of R and I denotes the identity matrix. where R There is still the matter of accounting for the fact that the frame is right- , where × ˆy denotes the cross product) rather than = ˆz × handed (i.e., ˆx b b b ˆz ˆy − = left-handed (i.e., ˆx × ); our six equality constraints above do not dis- b b b tinguish between right- and left-handed frames. We recall the following formula × 3 matrix M : denoting the three columns for evaluating the determinant of a 3 c M a , b , and by , respectively, its determinant is given by of T T T a det ( b × c ) = c M ( a × b ) = b = ( c × a ) . (3.20) Substituting the columns for R into this formula then leads to the constraint R . (3.21) det = 1 Note that, had the frame been left-handed, we would have det = − 1. In R summary, the six equality constraints represented by Equation (3.19) imply that det R = ± 1; imposing the additional constraint det R = 1 means that only right-handed frames are allowed. The constraint det = 1 does not change the R number of independent continuous variables needed to parametrize R . The set of 3 × 3 rotation matrices forms the special orthogonal group SO (3), which we now formally define. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

88 70 3.2. Rotations and Angular Velocities The special orthogonal group (3), also known as the Definition 3.1. SO 3 real matrices R group of rotation matrices, is the set of all 3 × that satisfy (i) T = and (ii) det R R R I = 1. SO (3) and is denoted 2 rotation matrices is a subgroup of The set of 2 × SO (2). special orthogonal group The (2) is the set of all 2 × 2 Definition 3.2. SO T that satisfy (i) R R = R and (ii) det R = 1. real matrices I R ∈ SO From the definition it follows that every (2) can be written [ ] [ ] r r θ sin − cos θ 12 11 = R = , r r sin θ cos θ 21 22 θ ∈ [0 , 2 where ). The elements of SO (2) represent planar orientations and the π elements of (3) represent spatial orientations. SO Properties of Rotation Matrices 3.2.1.1 SO (2) and SO (3) are called groups because they The sets of rotation matrices 2 satisfy the properties required of a mathematical group. Specifically, a group consists of a set of elements and an operation on two elements (matrix multipli- in the group, the following properties n )) such that, for all A , B ( cation for SO are satisfied: • closure: AB is also in the group. associativity: ( AB ) C = • ( BC ). A • There exists an element I in the group identity element existence: = SO (the identity matrix for )) such that AI ( IA = A . n 1 − There exists an element A • in the group inverse element existence: − 1 1 − A such that AA A = I . = Proofs of these properties are given below, using the fact that the identity matrix I is a trivial example of a rotation matrix. Proposition 3.3. The inverse of a rotation matrix R ∈ SO (3) is also a rotation − 1 T = R , i.e., R matrix, and it is equal to the transpose of . R 2 More specifically, the SO ( n ) groups are also called matrix Lie groups (where “Lie” is pronounced “Lee”) because the elements of the group form a differentiable manifold. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

89 Chapter 3. Rigid-Body Motions 71 T − 1 T T implies that R R Proof. = R R and RR = = I . Since I The condition T T R = 1, R is also a rotation matrix. R det = det Proposition 3.4. The product of two rotation matrices is a rotation matrix. T R ,R Proof. ∈ SO (3), their product Given ) = R R satisfies ( R R R ( ) R 2 1 1 2 2 1 2 1 T T T R R R = R R R = I . Further, det R R = det R · det R = 1. Thus R R 2 1 2 1 1 2 2 2 1 1 2 2 satisfies the conditions for a rotation matrix. Multiplication of rotation matrices is associative, R Proposition 3.5. R ( ) R 3 1 2 . For the special ( R R = ) , but generally not commutative, R R 6 = R R R 3 1 1 2 2 1 2 case of rotation matrices in SO (2) , rotations commute. Associativity and nocommutativity follows from the properties of matrix Proof. multiplication in linear algebra. Commutativity for planar rotations follows from a direct calculation. Another important property is that the action of a rotation matrix on a vector (e.g., rotating the vector) does not change the length of the vector. 3 x ∈ R ∈ and R Proposition 3.6. SO (3) , the vector y = Rx For any vector x . has the same length as 2 T T T T T 2 y Proof. y = ( Rx ) This follows from Rx = x ‖ R y Rx ‖ x = x = ‖ x ‖ = . 3.2.1.2 Uses of Rotation Matrices 3.10 3.1, Analogously to the discussion after Equations and (3.11) in Section there are three major uses for a rotation matrix R : (a) to represent an orientation; (b) to change the reference frame in which a vector or a frame is represented; (c) to rotate a vector or a frame. In the first use, R is thought of as representing a frame; in the second and third uses, R is thought of as an operator that acts on a vector or frame (changing its reference frame or rotating it). To illustrate these uses, refer to Figure 3.7, which shows three different coor- { dinate frames – a } , { b } , and { c } – representing the same space. These frames are chosen to have the same origin, since we are only representing orientations, but, to make the axes clear, the figure shows the same space drawn three times. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

90 72 3.2. Rotations and Angular Velocities ˆx ˆz ˆz c b a ˆy b ˆz c ˆy c { } b } c { ˆx ˆy a a } { a ˆx b p p p Figure 3.7: The same space and the same point p represented in three different frames with different orientations. } , { A point p in the space is also shown. Not shown is a fixed space frame s a } . The orientations of the three frames relative to { which is aligned with } { s can be written       − 0 1 0 − 1 0 1 0 0 0       1 0 − 0 1 0 0 0 1 0 = , R = , R = , R c a b 1 0 0 1 0 0 1 0 0 and the location of the point p in these frames can be written       0 1 1       − 1 − 1 1 . = , p , p = p = c b a − 1 0 0 ◦ Note that } is obtained by rotating { a } about ˆz { by 90 { , and b c } is obtained a ◦ by rotating } about ˆy { by − 90 b . b Representing an orientation R When we write , we are implicitly referring c to the orientation of frame { } relative to the fixed frame { s } . We can be more c explicit about this by writing it as R of the : we are representing the frame { c } sc { } of the first subscript. This notation second subscript relative to the frame s s } ; for example, allows us to express one frame relative to another that is not { relative to is the orientation of { c } R { b } . bc If there is no possibility of confusion regarding the frames involved, we may R . simply write 3.7, we see that Inspecting Figure     0 1 − 1 0 0 0     0 − 1 − 1 0 0 0 = . = , R R ac ca 0 − 1 0 0 0 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

91 Chapter 3. Rigid-Body Motions 73 − 1 R ; that is, R = = R I A simple calculation shows that or, equivalently, R ca ac ac ca T R R and = . In fact, for any two frames { d } from Proposition 3.3, { e } , ac ca − 1 T R = R R . = de ed ed 3.7. You can verify this fact using any two frames in Figure The rotation matrix R represents the Changing the reference frame ab { b } in orientation of a } , and R . represents the orientation of { c } in { b } { bc { c in { a } can be A straightforward calculation shows that the orientation of } computed as R = R R . (3.22) ab bc ac R In the previous equation, can be viewed as a representation of the orientation bc of c } , while R { can be viewed as a mathematical operator that changes the ab reference frame from b } to { a } , i.e., { . ) R ( } a { to reference frame from { b } = change R = R R bc ac ab bc A subscript cancellation rule helps us to remember this property. When multiplying two rotation matrices, if the second subscript of the first matrix matches the first subscript of the second matrix, the two subscripts “cancel” and a change of reference frame is achieved: R = R R R . R = bc ac ab b a bc   A rotation matrix is just a collection of three unit vectors, so the reference frame of a vector can also be changed by a rotation matrix using a modified version of the subscript cancellation rule: . p p = R = p R ab b a a b b   3.7. You can verify these properties using the frames and points in Figure Rotating a vector or a frame The final use of a rotation matrix is to rotate 3.8 shows a frame a vector or a frame. Figure c } initially aligned with { s } with { axes { ˆx , ˆy , ˆz } . If we rotate the frame { c } about a unit axis ˆ ω by an amount θ , ′ ′ ′ ′ ˆy { ˆx the new frame, , c } , ˆz (light gray), has coordinate axes } . The rotation { ′ ′ = R , but instead } represents the orientation of { c matrix R relative to { s } sc ′ s } to { c we can think of it as representing the rotation operation that takes } . { Emphasizing our view of R as a rotation operator, instead of as an orientation, we can write R = Rot(ˆ ω,θ ) , May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

92 74 3.2. Rotations and Angular Velocities , ˆy , ˆz } is rotated by θ about a unit axis Figure 3.8: ˆx A coordinate frame with axes { ˆy in this figure). The orientation of the final frame, with − (which is aligned with ˆ ω ′ ′ ′ axes , ˆz { ˆx , is written as R relative to the original frame. } , ˆy meaning the operation that rotates the orientation represented by the identity . Examples of rotation operations R matrix to the orientation represented by about coordinate frame axes are     θ 0 sin θ cos 0 0 1     θ − sin θ 0 cos 0 1 0 Rot(ˆx ) = Rot(ˆy , ,θ ,θ , ) = sin 0 cos θ 0 sin − θ cos θ θ   cos θ θ − sin 0   sin θ cos θ 0 . ) = ,θ Rot(ˆz 0 1 0 ω ω ω More generally, as we will see in Section , ˆ ω 3.2.3.3, for ˆ , ˆ = (ˆ ), 3 1 2 Rot(ˆ ω,θ ) =   2 + ˆ ω c s (1 − c ω ) ˆ ω ) + ˆ ˆ ω c (1 − c − ) − ˆ ω (1 s ω ˆ ω ˆ θ 1 3 3 θ 2 θ 1 θ 2 θ θ 1 2   − c ) + ˆ ω ˆ c ˆ + ˆ ω ω ω (1 s ˆ c − ) ˆ ω (1 ˆ ω s (1 − c ω ) − , θ θ 1 3 2 θ θ 1 θ θ 2 3 2 2 (1 (1 − c ˆ ) − ˆ ω ω s ) ˆ ω c ˆ ω − (1 − c ω ) + ˆ ω ˆ s + ˆ c ω θ 1 1 θ θ 3 2 θ θ 2 θ 3 3 where s = sin θ and c (3) can be obtained by rotating = cos θ . Any R ∈ SO θ θ θ about some ˆ ω . Note also that Rot(ˆ ω,θ ) = from the identity matrix by some − ˆ ω, − θ ). Rot( Now, say that R and that we want to represents some { b } relative to { s } sb rotate b } by θ about a unit axis ˆ ω , i.e., by a rotation R = Rot(ˆ ω,θ ). To be { ω is clear about what we mean, we have to specify whether the axis of rotation ˆ coordinates. Depending on our choice, the { } coordinates or { expressed in } s b same numerical ˆ ω (and therefore the same numerical R ) corresponds to different rotation axes in the underlying space, unless the { s } and { b } frames are aligned. ′ ω Letting } be the new frame after a rotation by θ about ˆ b { = ˆ ω (the rotation s ′′ ω is considered to be in the fixed frame, { s } ), and letting { b axis ˆ } be the new May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

93 Chapter 3. Rigid-Body Motions 75 ˆz ˆz ◦ ˆy 90 ˆx ˆy ˆx ◦ , 90 (ˆ ) R =R z ot ˆz s ˆx b ◦ 90 ˆz ˆx s b RR R = sb sb ˆy ˆy b b fixed frame { b } ˆz rotation b { b } } s { ˆx ˆy ◦ ˆz s s b ˆx 90 b ˆz b R R = R } b { sb sb body frame rotation ˆz b ˆy b ◦ Figure 3.9: z, 90 (Top) The rotation operator ) gives the orientation of R = Rot(ˆ the right-hand frame in the left-hand frame. (Bottom) On the left are shown a fixed R { } and a body frame { b } , which can be expressed as s frame . The quantity RR sb sb ◦ ′ b } by 90 about the fixed-frame axis ˆz rotates rotates to { b { } . The quantity R R s sb ◦ ′′ } about the body-frame axis ˆz { to { b . } b by 90 b ω ω θ = ˆ about ˆ (the rotation axis ˆ ω is considered to be frame after a rotation by b { b } ), representations of these new frames can be calculated in the body frame as ′ s R in { } by frame ( R (3.23) ) = RR = rotate R sb sb sb ′′ = rotate R R by in { b } frame ( R (3.24) ) = R R. sb sb sb In other words, premultiplying by R = Rot(ˆ ω,θ ) yields a rotation about an axis ˆ ω considered to be in the fixed frame, and postmultiplying by R yields a rotation about ˆ considered as being in the body frame. ω Rotation by R in the { s } frame and in the { b } frame is illustrated in Fig- ure 3.9. To rotate a vector v , note that there is only one frame involved, the frame May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

94 76 3.2. Rotations and Angular Velocities ω ˆ ˆz ˆz( t ) ˆz( t ) t +∆ θ ∆ ̇ =ˆ θ ω ω t ) y( ˆ t +∆ ˆy ˆ ) y( t ˆx ̇ ˆ × ω x= ˆx t x( ˆ ) t t ) x( ˆ +∆ Figure 3.10: (Left) The instantaneous angular velocity vector. (Right) Calculating ̇ ˆx. v is represented, and therefore ˆ in which must be interpreted as being in this ω ′ frame. The rotated vector , in that same frame, is v ′ v = Rv. 3.2.2 Angular Velocities Referring to Figure 3.10(a), suppose that a frame with unit axes { ˆx , ˆy , ˆz } is attached to a rotating body. Let us determine the time derivatives of these unit ̇ ˆx, first note that ˆx is of unit length; only the direction axes. Beginning with of ˆx can vary with time (the same goes for ˆy and ˆz). If we examine the body , the change in frame orientation can be described as t + ∆ t and frame at times t θ about some unit axis ˆw passing through the origin. The a rotation of angle ∆ axis ˆw is coordinate-free; it is not yet represented in any particular reference frame. In the limit as ∆ approaches zero, the ratio ∆ θ/ ∆ t t becomes the rate of ̇ rotation θ , and ˆw can similarly be regarded as the instantaneous axis of rotation. ̇ θ In fact, ˆw and can be combined to define the angular velocity w as follows: ̇ w = ˆw (3.25) θ. 3.10(b), it should be evident that Referring to Figure ̇ × ˆx , (3.26) ˆx = w ̇ ˆy = w × ˆy , (3.27) ̇ ˆz = w × ˆz . (3.28) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

95 Chapter 3. Rigid-Body Motions 77 To express these equations in coordinates, we have to choose a reference frame in which to represent w. We can choose any reference frame, but two } { b } . Let us start s and the body frame natural choices are the fixed frame { with fixed-frame coordinates. Let R ( t ) be the rotation matrix describing { s } ̇ ) R ( t the orientation of the body frame with respect to the fixed frame at time t ; ( ), denoted r ( t ), describes ˆx is its time rate of change. The first column of t R 1 ( in fixed-frame coordinates; similarly, t ) and r r ( t ) respectively describe ˆy and 3 2 3 , let ω ˆz in fixed-frame coordinates. At a specific time ∈ R t be the angular s velocity w expressed in fixed-frame coordinates. Then Equations (3.26)–(3.28) can be expressed in fixed-frame coordinates as 2 = ω ̇ × r . , i = 1 , 3 , r i i s These three equations can be rearranged into the following single 3 × 3 matrix equation: ̇ = [ R (3.29) × r R. ω × × r ω ω ] = × r ω 3 s 2 s s 1 s To eliminate the cross product on the right in Equation (3.29), we introduce some new notation, rewriting ω skew- × R as [ ω 3 ] R , where [ ω × ] is a 3 s s s 3 matrix representation of ∈ R : ω symmetric s 3 T ∈ x Definition 3.7. x Given a vector ] = [ x R x , define 3 2 1   − x x 0 3 2   0 − x x . (3.30) x [ ] = 1 3 x 0 x − 2 1 The matrix [ x ] is a 3 × 3 skew-symmetric matrix representation of x ; that is, T [ [ x ] x . ] = − 3 3 real skew-symmetric matrices is called so (3). The set of all 3 × A useful property involving rotations and skew-symmetric matrices is the following. 3 ∈ ∈ R Given any and R ω SO (3) , the following always Proposition 3.8. holds: T [ ω ] R R = [ Rω ] . (3.31) 3 so (3) is called the Lie algebra of the Lie group SO (3). The set of skew-symmetric matrices ̇ It consists of all possible R when R = I . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

96 78 3.2. Rotations and Angular Velocities T Letting be the i th row of R r Proof. , we have i   T T T ( ) r r × r ( ω × r ) ) r ω r ( ω × 2 1 3 1 1 1   T T T T × ( ω × r r ) r × ω ( ω ) r ( ) r r ] R = [ R ω   3 1 2 2 2 2 T T T r ) r ( ω ( ω × r × ) r r ) ( ω × r 3 1 2 3 3 3   T T r 0 − ω r ω 3 2   T T r ω r 0 − ω =   3 1 T T ω − 0 ω r r 1 2 Rω ] , = [ (3.32) × 3 matrices, where the second line makes use of the determinant formula for 3 T M is a 3 × 3 matrix with columns { a,b,c } , then det M = a i.e., if ( b × c ) = T T × b ) = b c ( c × a ). ( a With the skew-symmetric notation, we can rewrite Equation (3.29) as ̇ ω R = R. (3.33) ] [ s 1 − to get R We can post-multiply both sides of Equation (3.33) by 1 − ̇ [ ] = RR . (3.34) ω s ω be w expressed in body-frame coordinates. To see how to obtain Now let b ω ω and vice versa, we write from explicitly as R . Then ω and ω are R sb b b s s two different vector representations of the same angular velocity w and, by our subscript cancellation rule, = R ω . Therefore ω s sb b − 1 − 1 T (3.35) ω = R = R ω = R ω ω . s s s b sb Let us now express this relation in skew-symmetric matrix form: T ] = [ R ω ] [ ω b s T = [ ω R ] R (by Proposition 3.8) s T T ̇ ( RR = ) R R 1 − T ̇ ̇ = R R = R. (3.36) R ̇ R and In summary, two equations relate R to the angular velocity w: May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

97 Chapter 3. Rigid-Body Motions 79 R ( ) denote the orientation of the rotating frame as seen Let Proposition 3.9. t the angular velocity of the rotating frame. from the fixed frame. Denote by w Then − 1 ̇ RR = [ , (3.37) ω ] s 1 − ̇ = [ ω R ] , R (3.38) b 3 ∈ R where is the fixed-frame vector representation of w and [ ω (3) ] ∈ ω so s s 3 × 3 matrix representation, and where is its 3 ∈ R is the body-frame vector ω b × w ω representation of ] ∈ so (3) is its 3 [ 3 matrix representation. and b It is important to note that ω is not the angular velocity relative to a moving b ω frame is the angular velocity relative to the stationary frame. Rather, { b } b that is instantaneously coincident with a frame attached to the moving body. ω does not It is also important to note that the fixed-frame angular velocity s depend on the choice of body frame . Similarly, the body-frame angular velocity . While Equations (3.37) and does not depend on the choice of fixed frame ω b ̇ (3.38) may appear to depend on both frames (since R and R individually depend − 1 ̇ } and { b } ), the product on both RR { s is independent of { b } and the product − 1 ̇ is independent of { s } . R R { d } can be Finally, an angular velocity expressed in an arbitrary frame { c } if we know the rotation that takes { c } to { d } , represented in another frame using our now-familiar subscript cancellation rule: ω = R ω . cd d c Exponential Coordinate Representation of Rotation 3.2.3 expo- We now introduce a three-parameter representation for rotations, the nential coordinates for rotation . The exponential coordinates parametrize a rotation matrix in terms of a rotation axis (represented by a unit vector ˆ ω ) 3 θ about that axis; the vector ˆ ωθ ∈ R and an angle of rotation then serves as the three-parameter exponential coordinate representation of the rotation. Writing ˆ and θ individually is the axis-angle representation of a rotation. ω ωθ for a rotation matrix R can The exponential coordinate representation ˆ be interpreted equivalently as: • the axis ˆ ω and rotation angle θ such that, if a frame initially coincident { with s } were rotated by θ about ˆ ω , its final orientation relative to { s } would be expressed by R ; or May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

98 80 3.2. Rotations and Angular Velocities ωθ • s } such that, if a frame initially expressed in the angular velocity ˆ { } ωθ s for one unit of time (i.e., ˆ ωθ is integrated followed ˆ coincident with { ; or R over this time interval), its final orientation would be expressed by ω expressed in { s • such that, if a frame initially the angular velocity ˆ } { } followed ˆ ω for θ units of time (i.e., ˆ ω is integrated over s coincident with . R this time interval) its final orientation would be expressed by The latter two views suggest that we consider exponential coordinates in the setting of linear differential equations. Below we briefly review some key results from linear differential equations theory. 3.2.3.1 Essential Results from Linear Differential Equations Theory Let us begin with the simple scalar linear differential equation x ( t ) = ax ( t ) , (3.39) ̇ is constant, and the initial condition x ) ∈ R , a ∈ R t x (0) = x ( is given. where 0 Equation (3.39) has solution at t ) = e x x ( . 0 It is also useful to remember the series expansion of the exponential function: 3 2 ( at ) at ) ( at + + ··· . + = 1 + at e 3! 2! Now consider the vector linear differential equation , t ) = Ax ( t ̇ ( (3.40) x ) n n × n ∈ R where , A ∈ R x ( t is constant, and the initial condition x (0) = x ) is 0 given. From the above scalar result one can conjecture a solution of the form At ( t ) = e x x (3.41) 0 At e now needs to be defined in a meaningful where the matrix exponential way. Again mimicking the scalar case, we define the matrix exponential to be 2 3 ) At ( ( At ) At = + e At + I + + ··· (3.42) 3! 2! The first question to be addressed is under what conditions this series converges, so that the matrix exponential is well defined. It can be shown that if A is con- stant and finite then this series is always guaranteed to converge to a finite limit; May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

99 Chapter 3. Rigid-Body Motions 81 the proof can be found in most texts on ordinary linear differential equations and is not covered here. The second question is whether Equation (3.41), using Equation (3.42), is At ) = , x indeed a solution to Equation (3.40). Taking the time derivative of ( x e t 0 ( ) d At x ( t ) = x e ̇ 0 dt ) ( 2 3 2 3 A t t A d + I = + At x + ··· + 0 3! 2! dt ( ) 3 2 A t 2 A A = t x + ··· + + 0 2! At Ae = x 0 Ax ( = ) , (3.43) t At ( x ) = e x which proves that t is indeed a solution. That this is a unique 0 solution follows from the basic existence and uniqueness result for linear ordinary differential equations, which we invoke here without proof. AB 6 = BA for arbitrary square matrices While and B , it is always true A that At At = e Ae A (3.44) for any square A t . You can verify this directly using the series and scalar expansion for the matrix exponential. Therefore, in line four of Equation (3.43), A could also have been factored to the right, i.e., At . ) = e ( ̇ x t Ax 0 At While the matrix exponential e is defined as an infinite series, closed- A can be expressed as form expressions are often available. For example, if n − 1 n × n × n A for some PDP and invertible P ∈ R D ∈ R then = 2 ( At ) At I At + + = e + ··· 2! 2 t 1 − 1 − 1 − PDP PDP t )( PDP ) + ( ) + ( I = + ··· 2! 2 ( Dt ) 1 − ) + ··· P + Dt + = P ( I 2! 1 − Dt P Pe = . (3.45) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

100 82 3.2. Rotations and Angular Velocities If moreover is diagonal, i.e., { d D ,d D ,...,d = diag } , then its matrix expo- 2 1 n nential is particularly simple to evaluate:   d t 1 0 ··· e 0 d t 2   e ··· 0 0   Dt (3.46) . e =   . . . . . . . .   . . . . d t n 0 0 ··· e We summarize the results above in the following proposition. The linear differential equation ̇ x ( t ) = Ax ( t ) with initial Proposition 3.10. n × n n ) ∈ R x (0) = , where is constant and x ( t x ∈ R A , has solution condition 0 At x ( t ) = e x (3.47) 0 where 2 3 t t At 3 2 = + tA + I e A (3.48) A + + ··· . 3! 2! At e The matrix exponential further satisifies the following properties: At At At (a) ) /dt = Ae e = e d A . ( − 1 n × n n × n At = PDP If = and invertible P ∈ (b) for some D ∈ then e R A R − 1 Dt P . Pe B A A + B e If e AB = e = BA (c) . then A − A 1 − ) = e (d) ( . e The third property can be established by expanding the exponentials and comparing terms. The fourth property follows by setting = − A in the third B property. 3.2.3.2 Exponential Coordinates of Rotations The exponential coordinates of a rotation can be viewed equivalently as (1) a 3 , ω ∈ R ω unit axis of rotation ˆ ‖ ˆ ω ‖ = 1) together with a rotation angle about (ˆ the axis θ ∈ R , or (2) as the 3-vector obtained by multiplying the two together, 3 ˆ ∈ R . When we represent the motion of a robot joint in the next chapter, ωθ the first view has the advantage of separating the description of the joint axis from the motion θ about the axis. Referring to Figure 3.11, suppose that a three-dimensional vector p (0) is ( rotated by about ˆ ω to p θ θ ); here we assume that all quantities are expressed May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

101 Chapter 3. Rigid-Body Motions 83 p ) ( θ ˆ ω θ p (0 ) φ ( (0) is rotated by an angle about the axis ˆ ω , to p p θ ). Figure 3.11: The vector θ in fixed-frame coordinates. This rotation can be achieved by imagining that ω has unit magnitude) from p (0) rotates at a constant rate of 1 rad/s (since ˆ = 0 to time = θ . Let p ( t ) denote the path traced by the tip of the vector. t t ( t ), denoted ̇ p , is then given by The velocity of p p ω × p. (3.49) ̇ = ˆ φ be the constant angle between p ( t ) and ˆ ω . Observe To see why this is true, let p that p ‖ sin φ about the ˆ ω -axis. Then ̇ p is tangent to traces a circle of radius ‖ p ‖ sin φ , which is equivalent to Equation (3.49). the path with magnitude ‖ The differential equation (3.49) can be expressed as (see Equation (3.30)) ̇ = [ˆ ω ] p (3.50) p p (0). This is a linear differential equation of the form with initial condition x Ax , which we studied earlier; its solution is given by = ̇ [ˆ ω ] t p p ( t ) = (0) . e t and θ are interchangeable, the equation above can also be written Since [ˆ ω ] θ e p ( θ ) = p (0) . ] θ [ˆ ω Let us now expand the matrix exponential e in series form. A straight- 3 3 ] − ω ω ], and therefore we can replace [ˆ ω ] = forward calculation shows that [ˆ [ˆ 4 2 5 3 ω ] ] by − [ˆ ω by − , [ˆ ω ] [ˆ by − [ˆ ω ] ω = [ˆ ω ], and so on, obtaining ], [ˆ 2 3 θ θ ω θ [ˆ 2 ] 3 ] + + [ˆ ω ··· = I + [ˆ ω ] θ + [ˆ ω ] e 3! 2! ( ( ) ) 2 4 5 6 3 θ θ θ θ θ 2 + ω ] + . + −··· ] − − θ ω + [ˆ I [ˆ −··· = 6! 4! 2! 3! 5! May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

102 84 3.2. Rotations and Angular Velocities and cos θ θ Now recall the series expansions for sin : 3 5 θ θ θ θ sin = − + −··· 5! 3! 2 4 θ θ + θ cos − −··· = 1 4! 2! [ˆ ω ] θ e therefore simplifies to the following: The exponential 3 θ R ˆ , such that ∈ is any scalar and ωθ Proposition 3.11. Given a vector 3 R [ˆ is a unit vector, the matrix exponential of ˆ ω ω θ = [ˆ ωθ ] ∈ so (3) is ∈ ] ω ] θ 2 [ˆ ω,θ ) = = I + sin θ [ˆ ω ] + (1 − cos θ )[ˆ ω ] e ∈ SO (3) . (3.51) Rot(ˆ Rodrigues’ formula Equation (3.51) is also known as for rotations. We have shown how to use the matrix exponential to construct a rotation ] [ˆ ω θ . Further, the quantity θ e p and an angle ω matrix from a rotation axis ˆ 3 R θ about the fixed-frame axis ˆ ω by an angle has the effect of rotating . p ∈ consists of three column vectors, Similarly, considering that a rotation matrix R ′ θ ] [ˆ ω e the rotation matrix = = Rot(ˆ ω,θ ) R is the orientation achieved by R R R by θ about the axis ˆ ω in the fixed frame. Reversing the order of rotating [ˆ ω ] θ ′′ = Re = R Rot(ˆ ω,θ ) is the orientation achieved by R matrix multiplication, R by θ about ˆ ω in the body frame. rotating The frame b } in Figure 3.12 is obtained by rotation from { Example 3.12. { s } about a unit axis ˆ ω = an initial orientation aligned with the fixed frame 1 ◦ = 30 , 0 . 5) by an angle θ 0 866 (0 = 0 . 524 rad. The rotation matrix represen- . , 1 b } can be calculated as tation of { [ˆ ω ] θ 1 1 e = R 2 + sin θ ω [ˆ = I ] + (1 − cos θ ] )[ˆ ω 1 1 1 1     2 − 0 . 5 0 0 866 − 866 . 5 0 0 0 . .     0 0 . 5 0 0 5 . 0 0 = I + 0 . 5 134 . + 0 0 − 0 0 866 0 0 − . 866 0 .   0 . 866 − 0 . 250 0 . 433   0 0 . 967 . . 058 0 250 = . 433 0 . 058 0 . 899 − 0 . The orientation of the frame } can be represented by R or by the unit axis b { ω = (0 , 0 524 rad, i.e., the exponential coordi- 866 , 0 . 5) and the angle θ ˆ = 0 . . 1 1 . nates ˆ 262). θ . = (0 , 0 ω 453 , 0 1 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

103 Chapter 3. Rigid-Body Motions 85 ˆz s ˆz b ◦ =3 θ 0 ˆy b } s { ˆ ω } { b ˆx ˆy ˆx b s s ◦ { } is obtained by a rotation from { s } by θ Figure 3.12: = 30 b about The frame 1 866 = (0 , 0 . ω , 0 . 5). ˆ 1 { b } is then rotated by θ If about a fixed-frame axis ˆ ω , i.e., 6 = ˆ ω 1 2 2 ′ [ˆ ω ] θ 2 2 R e = R, { b } to be then the frame ends up at a different location than that reached were θ in the body frame, i.e., about an axis expressed as ˆ ω rotated by 2 2 ′′ θ ] [ˆ ω [ˆ θ ′ ] ω 2 2 2 2 = Re R = = R 6 e R. Our next task is to show that for any rotation matrix R ∈ SO (3), one can [ˆ ω ] θ always find a unit vector ˆ = e ω θ such that R . and scalar Matrix Logarithm of Rotations 3.2.3.3 3 ωθ ∈ R represents the exponential coordinates of a rotation matrix R , then If ˆ ωθ ] = [ˆ ω ] θ is the matrix logarithm of the rotation the skew-symmetric matrix [ˆ 4 . The matrix logarithm is the inverse of the matrix exponential. Just as the R matrix exponential “integrates” the matrix representation of an angular velocity ∈ ] θ ∈ so [ˆ R ω SO (3), the matrix (3) for one second to give an orientation logarithm “differentiates” an R ∈ SO (3) to find the matrix representation of a constant angular velocity [ˆ ω ] θ ∈ so (3) which, if integrated for one second, rotates a frame from I R . In other words, to → ω θ ∈ exp : [ˆ (3) ] R ∈ SO (3) , so log : R ∈ SO (3) → [ˆ ω ] θ ∈ so (3) . 4 the matrix logarithm” to refer both to a specific matrix which is a We use the term “ logarithm of R as well as to the algorithm that calculates this specific matrix. Also, while a 1 − π can have more than one matrix logarithm (just as sin matrix (0) has solutions 0 ,π, 2 R , etc.), we commonly refer to “the” matrix logarithm, i.e., the unique solution returned by the matrix logarithm algorithm. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

104 86 3.2. Rotations and Angular Velocities ω ] θ [ˆ e To derive the matrix logarithm, let us expand each entry for in Equa- tion (3.51),   2 − c (1 − c ) ˆ ω ˆ ω (1 − c ) + ˆ ˆ ω s ˆ ω ˆ ω (1 − c ) + ˆ ω s ω θ 3 3 θ θ θ 2 1 2 1 θ θ 1 2   ω (1 − c ˆ ) + ˆ ω ω s ˆ c ω + ˆ (1 (1 s ω − c ) ˆ ω ˆ ω ˆ − c ) − , θ 1 θ 3 2 θ θ 3 θ 2 θ 1 2 2 (1 − c ω ) − ˆ ω ˆ s ω ˆ ω ) ˆ ω c (1 − c − ) + ˆ ω (1 s ˆ c ω + ˆ 2 θ θ 1 3 3 θ θ 1 θ 2 θ 3 (3.52) ω = (ˆ ω where ˆ , ˆ ω θ , ˆ ω = sin ), and we use again the shorthand notation s θ 3 1 2 = cos θ . Setting the above matrix equal to the given ∈ SO (3) and and c R θ subtracting the transpose from both sides leads to the following: − r r θ, = 2ˆ ω sin 1 23 32 r − r = 2ˆ ω sin θ, 31 2 13 r r = 2ˆ ω sin θ. − 21 12 3 = 0 (or, equivalently, θ is not an integer multiple of Therefore, as long as sin θ 6 π ), we can write 1 ( r − r , ) = ω ˆ 23 32 1 2 sin θ 1 ˆ = ω ( r − r ) , 2 13 31 2 sin θ 1 ω ˆ = . ) ( r r − 3 12 21 θ 2 sin The above equations can also be expressed in skew-symmetric matrix form as   ω − ˆ ω 0 ˆ 3 2 ) ( 1 T   ω ˆ ω 0 − ˆ = − R . ω (3.53) [ˆ R ] = 3 1 2 sin θ ˆ 0 ω ˆ − ω 2 1 ω represents the axis of rotation for the given R . Because of the Recall that ˆ is an integer multiple θ sin ] is not well defined if θ term in the denominator, [ˆ ω 5 of π . We address this situation next, but for now let us assume that sin θ 6 = 0 and find an expression for R equal to (3.52) and taking the trace of θ . Setting both sides (recall that the trace of a matrix is the sum of its diagonal entries), R = r r + r (3.54) + tr θ. = 1 + 2 cos 33 11 22 2 2 2 θ = tr θ = 1. For any R satisfying 1 + 2 cos ω + ˆ + ˆ ω ω The above follows since ˆ 3 2 1 θ is not an integer multiple of π , R can be expressed as the exponential such that [ˆ ω ] θ e with [ˆ ω ] as given in Equation (3.53). 5 Singularities such as this are unavoidable for any three-parameter representation of rota- tion. Euler angles and roll–pitch–yaw angles suffer from similar singularities. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

105 Chapter 3. Rigid-Body Motions 87 θ = k is some integer. When k is Let us now return to the case kπ , where R I so the vector we have rotated back to = ω an even integer, regardless of ˆ k is an odd integer (corresponding to θ ˆ ± π, ± 3 π,... , ω is undefined. When = = − 1), the exponential formula (3.51) simplifies to which in turn implies tr R 2 ω ] π [ˆ = I + 2[ˆ ω ] e . (3.55) R = The three diagonal terms of Equation (3.55) can be manipulated to give √ r + 1 ii . 3 , 2 , , i = 1 (3.56) = ω ˆ ± i 2 The off-diagonal terms lead to the following three equations: 2ˆ ˆ ω ω = r , 12 1 2 = ˆ ω ω 2ˆ r (3.57) , 23 3 2 ω r ˆ ω , = 2ˆ 13 1 3 R must be symmetric: r From Equation (3.55) we also know that = r , 21 12 r = r r , r = . Equations (3.56) and (3.57) may both be necessary to 13 31 23 32 [ˆ ω ] θ e . Once such a solution has been found then ω R obtain a solution for ˆ , = where θ = ± π, ± 3 π,... From the above it can be seen that solutions for θ π intervals. If exist at 2 θ ] then the following algorithm can be used to ,π to the interval [0 we restrict compute the matrix logarithm of the rotation matrix R ∈ SO (3). R ∈ SO (3), find a θ ∈ [0 ,π ] and a unit rotation axis Algorithm: Given 3 [ˆ ω ] θ 3 ˆ ω ‖ = 1, such that e ∈ R ‖ , = R . The vector ˆ ωθ ∈ R ˆ comprises the ω exponential coordinates for and the skew-symmetric matrix [ˆ ω ] θ R so (3) is ∈ the matrix logarithm of R . (a) If R = I then θ = 0 and ˆ ω is undefined. (b) If tr = = π . Set ˆ ω equal to any of the following three vectors θ R 1 then − that is a feasible solution:   r 13 1   √ r ω ˆ = (3.58) 23 ) r 2(1 + 33 1 + r 33 or   r 12 1   √ r 1 + (3.59) = ˆ ω 22 ) r 2(1 + 22 r 32 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

106 88 3.2. Rotations and Angular Velocities ˆ ω θ π π − π . The exponential coordinates r = ˆ ωθ Figure 3.13: SO (3) as a solid ball of radius may lie anywhere within the solid ball. or   r 1 + 11 1   √ r (3.60) . ˆ = ω 21 2(1 + r ) 11 r 31 (Note that if ˆ is a solution, then so is − ˆ ω .) ω ) ( 1 1 − = cos (c) Otherwise (tr R − 1) θ ∈ [0 ,π ) and 2 1 T ] = R (3.61) . ( R − [ˆ ω ) θ 2 sin R ∈ (3) satisfies one of the three cases in the algorithm, for Since every SO there exists a matrix logarithm [ˆ R ] θ and therefore a set of exponential every ω ωθ coordinates ˆ . Because the matrix logarithm calculates exponential coordinates ˆ satisfy- ωθ || ˆ ing ||≤ π , we can picture the rotation group SO (3) as a solid ball of radius ωθ 3 = (see Figure ∈ R π in this solid ball, let ˆ ω r r/ ‖ r ‖ be the 3.13): given a point unit axis in the direction from the origin to the point r and let θ = ‖ r ‖ be the distance from the origin to , so that r = ˆ ωθ . The rotation matrix correspond- r ing to r can then be regarded as a rotation about the axis ˆ ω by an angle θ . For May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

107 Chapter 3. Rigid-Body Motions 89 R ∈ (3) such that tr R 6 = − 1, there exists a unique r in the interior of any SO [ r ] R R R = = 1, log . In the event that tr is given e the solid ball such that − by two antipodal points on the surface of this solid ball. That is, if there exists ] r − [ [ r ] e with r ‖ = π then R = ‖ e = R such that also holds; both r and some r r − R . correspond to the same rotation 3.3 Rigid-Body Motions and Twists In this section we derive representations for rigid-body configurations and ve- locities that extend, but otherwise are analogous to, those in Section 3.2 for rotations and angular velocities. In particular, the homogeneous transforma- T R ; a screw axis S is analogous tion matrix is analogous to the rotation matrix ̇ to a rotation axis ˆ can be expressed as S ω θ and is analogous to an ; a twist V 6 ̇ ω = ˆ θ ; and exponential coordinates S θ ∈ R angular velocity for rigid-body ω 3 ωθ R ∈ for rotations. motions are analogous to exponential coordinates ˆ Homogeneous Transformation Matrices 3.3.1 We now consider representations for the combined orientation and position of a rigid body. A natural choice would be to use a rotation matrix R ∈ SO (3) to represent the orientation of the body frame { b } in the fixed frame { s } and a 3 . Rather than identifying vector to represent the origin of { b } in { s } R R ∈ p p separately, we package them into a single matrix as follows. and Definition 3.13. The special Euclidean group SE (3), also known as the rigid-body motions or homogeneous transformation matrices group of 3 R , is the set of all 4 × 4 real matrices T of the form in   r p r r 1 11 12 13 ] [   p r r r R p 22 23 2 21   = = T , (3.62)   0 1 r r p r 3 33 32 31 1 0 0 0 3 R ∈ SO (3) and p ∈ R where is a column vector. An element ∈ SE (3) will sometimes be denoted ( R,p ). In this section we T will establish some basic properties of SE (3) and explain why we package R and p into this matrix form. Many robotic mechanisms we have encountered thus far are planar. With planar rigid-body motions in mind, we make the following definition: May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

108 90 3.3. Rigid-Body Motions and Twists The special Euclidean group (2) is the set of all 3 Definition 3.14. SE × 3 real matrices T of the form [ ] R p = T , (3.63) 1 0 2 (2), p ∈ R , and 0 denotes a row vector of two zeros. where R ∈ SO SE ∈ A matrix T (2) is always of the form     θ p r θ p sin − r cos 12 11 1 1     sin p r r θ p θ cos = = T , 2 2 21 22 0 1 0 1 0 0 where ∈ [0 , 2 π ). θ Properties of Transformation Matrices 3.3.1.1 We now list some basic properties of transformation matrices, which can be I is a trivial example of a transforma- proven by calculation. First, the identity SE tion matrix. The first three properties confirm that (3) is a group. The inverse of a transformation matrix T Proposition 3.15. SE (3) is also ∈ a transformation matrix, and it has the following form: ] [ ] [ − 1 T T R p R p − R 1 − = = (3.64) . T 1 0 1 0 Proposition 3.16. The product of two transformation matrices is also a trans- formation matrix. Proposition 3.17. The multiplication of transformation matrices is associa- ( T T tive, so that . ) T T = T T ( T = T 6 ) , but generally not commutative: T T 3 1 3 2 1 2 2 1 2 1 Before stating the next proposition, we note that, just as in Section 3.1, 3 Rx + p , where x ∈ R it is often useful to calculate the quantity and ( R,p ) represents , making it a four-dimensional vector, this x . If we append a ‘1’ to T computation can be performed as a single matrix multiplication: [ [ ] ] ][ ] [ x p + x Rx R p = = T (3.65) . 1 1 0 1 1 T T x The vector [ is the representation of x in homogeneous coordinates , 1] and accordingly T ∈ SE (3) is called a homogenous transformation. When, by an abuse of notation, we write Tx , we mean Rx + p . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

109 Chapter 3. Rigid-Body Motions 91 3 ∈ ) ∈ SE (3) and x,y R,p R T , the following = ( Given Proposition 3.18. hold: Tx − Ty ‖ = ‖ (a) − y ‖ , where ‖·‖ denotes the standard Euclidean norm in ‖ x √ 3 T x = ‖ , i.e., ‖ R . x x 3 Tx − Tz,Ty − Tz 〉 = 〈 x − z,y − z 〉 for all z ∈ R (b) , where 〈· , ·〉 denotes the 〈 T 3 x x,y 〉 = , R y . 〈 standard Euclidean inner product in 3 is regarded as a transformation on points in R In Proposition 3.18, ; T trans- T x to Tx . Property (a) then asserts that T preserves distances, forms a point 3 T preserves angles. Specifically, if x,y,z ∈ R while property (b) asserts that represent the three vertices of a triangle then the triangle formed by the trans- formed vertices Tx,Ty,Tz } has the same set of lengths and angles as those { { x,y,z (the two triangles are said to be isometric ). One can of the triangle } x,y,z } to be the points on a rigid body, in which case easily imagine taking { Tx,Ty,Tz } represents a displaced version of the rigid body. It is in this sense { SE (3) can be identified with rigid-body motions. that Uses of Transformation Matrices 3.3.1.2 As was the case for rotation matrices, there are three major uses for a transfor- mation matrix T : (a) to represent the configuration (position and orientation) of a rigid body; (b) to change the reference frame in which a vector or frame is represented; (c) to displace a vector or frame. T is thought of as representing the configuration of a frame; in In the first use, the second and third uses, T is thought of as an operator that acts to change the reference frame or to move a vector or a frame. To illustrate these uses, we refer to the three reference frames { a } , { b } , and { c , and the point v, in Figure 3.14. The frames were chosen in such a way } that the alignment of their axes is clear, allowing the visual confirmation of calculations. The fixed frame { s Representing a configuration. is coincident with { a } } and the frames { a } , { b } , and { c } , represented by T = = ( R T ,p ), sa sa sb sa May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

110 92 3.3. Rigid-Body Motions and Twists ˆx b ˆy b } { b ˆz ˆy a c p bc ˆz p b ˆx ab c v c { } p ac } a { ˆz ˆy ˆx c a a Figure 3.14: Three reference frames in space, and a point v that can be represented , } as v in = (0 b 0 , 1 . 5). { b R { ,p by ), and T } = ( R s ,p ( ), respectively, can be expressed relative to sc sb sb sc sc the rotations       1 0 0 0 1 0 1 0 0 −       0 1 0 − 1 0 0 0 0 1 R = , R = = , R . sa sb sc 0 0 1 1 0 0 0 1 0 { s } can be written The location of the origin of each frame relative to       1 − 0 0       1 − 2 0 . , p = = , p = p sc sb sa 0 0 0 { a } is collocated with { s } , the transformation matrix T Since constructed from sa R ) is the identity matrix. ,p ( sa sa { s } ; for Any frame can be expressed relative to any other frame, not just to example, T : = ( R } ,p c ) represents { b } relative to { bc bc bc     1 0 0 0     − 3 0 0 − 1 R = = . , p bc bc 1 − − 1 0 0 It can also be shown, using Proposition 3.15, that 1 − T T = de ed for any two frames { d } and { e } . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

111 Chapter 3. Rigid-Body Motions 93 By a subscript Changing the reference frame of a vector or a frame. cancellation rule analogous to that for rotations, for any three reference frames { } b } , and { c } , and any vector v expressed in { b } as v , , a { b T T = T = T T ab bc ac b bc a   T v = T , v v = b ab a b a b   is the vector v expressed in v a } . where { a Displacing (rotating and translating) a vector or a frame. A transfor- T , viewed as the pair ( R,p mation matrix ω,θ ) ,p ), can act on a frame ) = (Rot(ˆ T by rotating it by θ about an axis ˆ ω and translating it by p . By a minor sb abuse of notation, we can extend the 3 R = Rot(ˆ ω,θ ) to × 3 rotation operator 4 transformation matrix that rotates without translating, × a 4 [ ] R 0 , ) = ω,θ Rot(ˆ 1 0 and we can similarly define a translation operator that translates without ro- tating,   1 0 0 p x   p 0 1 0 y   Trans( p ) = .   p 0 0 1 z 1 0 0 0 p, ‖ p ‖ ), (To parallel the rotation operator more directly, we could write Trans( ˆ a translation along the unit direction ˆ p by a distance ‖ p ‖ , but we will use the simpler notation with = ˆ ‖ p ‖ .) p p T = ( R,p ) determines T by Whether we pre-multiply or post-multiply sb -axis and p are interpreted as in the fixed frame whether the ˆ s } or in the ω { { b } : body frame ′ T = TT (fixed frame) = Trans( p ) Rot(ˆ ω,θ ) T sb sb sb [ ][ [ ] ] R p R p p + RR Rp sb sb sb sb = = (3.66) 0 1 0 1 0 1 ′′ p = T (body frame) T = T ) Trans( T ) Rot(ˆ ω,θ sb sb sb ] [ ][ [ ] + p R R R R p R p p sb sb sb sb sb (3.67) . = = 0 1 0 0 1 1 The fixed-frame transformation (corresponding to pre-multiplication by T ) can ω be interpreted as first rotating the b } frame by θ about an axis ˆ { in the { s } May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

112 94 3.3. Rigid-Body Motions and Twists ˆx b ˆy b ˆx b { b } b { } ˆz s ˆx b 1 ˆz b ˆz b 2 ˆy b 1 ˆz ˆy s { b } b } s { ˆy ˆx s s ˆz b ˆx b s { } 2 ˆy ˆx s s ˆy ˆz b b { b } Figure 3.15: Fixed-frame and body-frame transformations corresponding to ˆ ω = ◦ ◦ , 1), θ = 90 (0 , and p = (0 , 2 , 0). (Left) The frame { b } , 0 about ˆz is rotated by 90 s ′ { b } . (Right) The , resulting in the new frame and then translated by two units in ˆy s ◦ b and then rotated by 90 is translated by two units in ˆy } { frame about its ˆz axis, b ′′ resulting in the new frame b } . { { b } to move if it is not coincident frame (this rotation will cause the origin of with the origin of { s } ), then translating it by p in the { s } frame to get a frame ′ { . The body-frame transformation (corresponding to post-multiplication by b } { { by p considered to be in the } b } ) can be interpreted as first translating b T frame, then rotating about ˆ ω in this new body frame (this does not move the ′′ { b } . origin of the frame) to get Fixed-frame and body-frame transformations are illustrated in Figure 3.15 ◦ with ˆ ω = (0 , 0 , 1), θ = 90 for a transformation , and p = (0 , 2 , 0), yielding T   1 0 0 − 0   1 0 2 0   T ,p ) ω,θ ) = = (Rot(ˆ .   1 0 0 0 0 1 0 0 { b } represented by Beginning with the frame   1 0 0 0   − 0 − 1 0 2   = T , sb   0 0 0 1 0 0 1 0 ′ TT b the new frame } achieved by a fixed-frame transformation { and the new sb May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

113 Chapter 3. Rigid-Body Motions 95 } { c d } { } { b e }{ a { } Figure 3.16: Assignment of reference frames. ′′ b frame } achieved by a body-frame transformation T { T are given by sb     0 1 0 0 0 1 0 2     0 4 − 1 0 − 0 0 1 2     ′′ ′ . T , T = T = T = TT = sb sb sb sb     0 1 0 − 0 1 0 0 0 0 1 0 0 0 0 0 1 Example 3.19. Figure 3.16 shows a robot arm mounted on a wheeled mobile { b } platform moving in a room, and a camera fixed to the ceiling. Frames { c } are respectively attached to the wheeled platform and the end-effector and of the robot arm, and frame { d } is attached to the camera. A fixed frame { a } has been established, and the robot must pick up an object with body can be calculated e . Suppose that the transformations T frame and { } T de db from measurements obtained with the camera. The transformation T can bc be calculated using the arm’s joint-angle measurements. The transformation T is assumed to be known in advance. Suppose these calculated and known ad transformations are given as follows:   0 1 250 − 0   − 150 0 − 1 0   = T , db   0 200 − 0 1 0 0 1 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

114 96 3.3. Rigid-Body Motions and Twists   0 0 − 1 300   0 1 0 100 −   , T = de   1 120 0 − 0 0 1 0 0   0 0 − 1 400   1 − 0 0 50   , T = ad   0 300 0 1 − 1 0 0 0 √ √   0 − 1 / 2 30 − 1 / 2 √ √   1 2 − / 40 2 − 0 1 /   . = T bc   1 0 0 25 0 0 0 1 In order to calculate how to move the robot arm so as to pick up the object, the T , must be determined. configuration of the object relative to the robot hand, ce We know that T T , = T T T ad de ce ab bc not given to us directly is where the only quantity besides T T . However, ab ce since T = T T , we can determine T as follows: ab ce db ad − 1 ) T T = ( T T T T . ad bc ad ce de db From the given transformations we obtain   280 1 0 0   0 1 0 50 −   , = T T de ad   0 0 0 1 1 0 0 0 √ √   0 − 1 / 2 / 2 230 − 1 √ √   / 0 1 / 2 − 1 2 160   T T T = , db ad bc   0 0 75 1 0 1 0 0   75 − 0 1 0 √ √ √   1 2 2 / 70 0 2 / 1 / − 1 −   √ √ √ , T ) T ( T = bc ad db   / / 1 2 2 0 390 − 2 / 1 − 0 0 1 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

115 Chapter 3. Rigid-Body Motions 97 is evaluated to be T from which ce   75 0 0 1 − √ √ √   − / 1 1 / 2 2 0 − 260 / 2   √ √ √ T . = ce   130 / 1 − 2 − 1 / / 2 0 2 1 0 0 0 3.3.2 Twists We now consider both the linear and angular velocities of a moving frame. { s } and { b } denote the fixed (space) and moving (body) frames, As before, respectively. Let [ ] ) t ( p ) t R ( ( T t ( T ) = ) = t (3.68) sb 1 0 { b } as seen from { s } . To keep the notation unclut- denote the configuration of T T . tered, for the time being we write instead of the usual sb 1 − ̇ by R we discovered that pre- or post-multiplying 3.2.2 results R In Section in a skew-symmetric representation of the angular velocity vector, either in fixed- or body-frame coordinates. One might reasonably ask whether a similar 1 − − 1 ̇ ̇ ̇ TT T and carry similar physical , i.e., whether T T property carries over to interpretations. 1 − ̇ T : Let us first see what happens when we pre-multiply T by ][ ] [ T T ̇ R − R p p ̇ R 1 − ̇ T T = 1 0 0 0 ] [ T T ̇ R R ̇ p R = 0 0 [ ] ] v ω [ b b = . (3.69) 0 0 T ̇ R Recall that = [ ω R ] is just the skew-symmetric matrix representation of the b angular velocity expressed in b } coordinates. Also, ̇ p is the linear velocity of { T b } expressed in the fixed frame the origin of s } , and R { ̇ p = v is this linear { b velocity expressed in the frame { b } . Putting these two observations together, 1 − ̇ we can conclude that T represents the linear and angular velocities of the T { b } currently aligned with the moving frame relative to the stationary frame moving frame. 1 − ̇ and T T suggests that it is reasonable to merge ω The above calculation of b v into a single six-dimensional velocity vector. We define the spatial velocity b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

116 98 3.3. Rigid-Body Motions and Twists } b { p ̇ { } s p − v s Figure 3.17: v . The initial (solid line) and displaced Physical interpretation of s (dashed line) configurations of a rigid body. 6 body twist to be , in the body frame , or simply the [ ] ω b 6 = V ∈ (3.70) R . b v b Just as it is convenient to have a skew-symmetric matrix representation of an angular velocity vector, it is convenient to have a matrix representation of a twist, as shown in Equation (3.69). We will stretch the [ · ] notation, writing [ ] ] ω [ v b b 1 − ̇ V T = [ ] = T (3.71) ∈ se (3) , b 0 0 3 ω 4 matrices of this form is ] ∈ so (3) and v × ∈ R . The set of all 4 where [ b b se (3) and comprises the matrix representations of the twists associated called 7 (3). SE with the rigid-body configurations 6 The term “twist” has been used in different ways in the mechanisms and screw theory literature. In robotics, however, it is common to use the term to refer to a spatial velocity. We mostly use the term “twist” instead of “spatial velocity” to minimize verbiage, e.g., “body twist” versus “spatial velocity in the body frame.” 7 ̇ se SE (3). It consists of all possible T when (3) is called the Lie algebra of the Lie group T = I . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

117 Chapter 3. Rigid-Body Motions 99 − 1 − 1 ̇ ̇ TT T , let us evaluate : Now that we have a physical interpretation for T ] ][ [ T T ̇ p R − R p ̇ R − 1 ̇ TT = 1 0 0 0 ] [ T T ̇ ̇ − RR ̇ p p RR = 0 0 [ ] ] [ ω v s s = . (3.72) 0 0 T ̇ Observe that the skew-symmetric matrix [ ω ] = is the angular velocity RR s T ̇ = ̇ expressed in fixed-frame coordinates but that p RR v p is not the linear − s velocity of the body-frame origin expressed in the fixed frame (that quantity would simply be ̇ ). If we write v as p s = ̇ p − v (3.73) × p = ̇ ω + ω , × ( − p ) p s s s the physical meaning of v can now be inferred: imagining the moving body s v to be infinitely large, is the instantaneous velocity of the point on this body s currently at the fixed-frame origin, expressed in the fixed frame (see Figure 3.17). v and v As we did for , we assemble ω into a six-dimensional twist, and ω s b b s [ [ ] ] ω ω ] v [ s s s 6 − 1 ̇ = V , [ V R ] = ∈ ∈ = TT (3.74) , (3) se s s v 0 0 s V × 4 matrix representation of V ] is the 4 V where [ the spatial . We call s s s velocity in the space frame spatial twist , or simply the . If we regard the moving body as being infinitely large, there is an appealing = ( ω ,v and natural symmetry between V V = ( ω ,v ): ) and s b s b b s , and ω { b } is the angular velocity expressed in ω is the angular velocity (a) s b { s } . expressed in } v is the linear velocity of a point at the origin of { b (b) expressed in { b } , b and { s } expressed in is the linear velocity of a point at the origin of v s } . s { V V from We can obtain as follows: s b − 1 ̇ ] = T [ T V b − 1 = T [ V (3.75) ] T. s Going the other way, − 1 ] [ V [ ] = T V T . (3.76) s b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

118 100 3.3. Rigid-Body Motions and Twists Writing out the products in Equation (3.76), we get ] [ T T ] R ω − R [ [ Rv ] R R p + ω b b b V = s 0 0 T 3 ] R = [ which, using R ] (Proposition 3.8) and [ ω ] p = − [ p ] ω for p,ω ∈ R [ , ω Rω V and V can be manipulated into the following relation between : b s [ ] [ ][ ] ω R 0 ω s b = . v p ] R R [ v s b V Because the 6 × 6 matrix pre-multiplying is useful for changing the frame b of reference for twists and wrenches, as we will see shortly, we give it its own name. T = ( R,p ) ∈ SE (3), its adjoint representation Definition 3.20. Given [Ad ] is T [ ] R 0 6 × 6 ] = [Ad ∈ R . T R R p ] [ 6 R is , the adjoint map associated with T For any V ∈ ′ = [Ad ] V , V T which is sometimes also written as ′ V = Ad . ( V ) T 6 ∈ se (3) of V ∈ R In terms of the matrix form [ , V ] ′ 1 − . [ T T V ] V ] = [ The adjoint map satisfies the following properties, verifiable by direct calcu- lation: T Proposition 3.21. Let ∈ SE (3) and V = ( ω,v ) . Then ,T 2 1 V (3.77) . ( V )) = Ad V ] ( (Ad ) or [Ad ][Ad Ad ] V = [Ad T T T T T T T T 2 1 2 2 1 2 1 1 T SE (3) the following holds: Also, for any ∈ − 1 − 1 ] (3.78) [Ad = [Ad ] , T T − 1 , = T and The second property follows from the first on choosing T T T = 2 1 so that 1 − − 1 ) = Ad (Ad ( V )) = Ad (3.79) . Ad V ( V ) = V ( I T T T T May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

119 Chapter 3. Rigid-Body Motions 101 3.3.2.1 Summary of Results on Twists The main results on twists derived thus far are summarized in the following proposition: } , a body frame { b s , and a Given a fixed (space) frame } Proposition 3.22. { ( t differentiable ∈ SE (3) , where T ) sb [ ] t ) R ( t ) ( p ) = ( T t (3.80) , sb 1 0 then [ ] ] ω [ v b b 1 − ̇ T ] = = [ V T ∈ (3.81) se (3) b sb sb 0 0 is the matrix representation of the body twist , and ] [ ] ω [ v s s − 1 ̇ T = [ T V (3.82) (3) se ] = ∈ s sb sb 0 0 spatial twist . The twists V is the matrix representation of the and V are b s related by [ ][ ] [ ] ω ω 0 R s b V = = = [Ad (3.83) , V ] s T b sb v [ p ] R R v s b [ [ ][ ] ] T ω ω R 0 b s V = = = [Ad V (3.84) ] . b T s T T bs v v ] R − [ p R b s } { } and More generally, for any two frames d c , a twist represented as V in { c { c } is related to its representation V by in { d } d V = [Ad ] V , V = [Ad . ] V T c d T d c cd dc Again analogously to the case of angular velocities, it is important to realize that, for a given twist, its fixed-frame representation V does not depend on the s choice of the body frame { b } , and its body-frame representation V does not b { s } . depend on the choice of the fixed frame Example 3.23. Figure 3.18 shows a top view of a car, with a single steerable front wheel, driving on a plane. The ˆz is into the -axis of the body frame { b } b } is out of the page. The angle of -axis of the fixed frame { s page and the ˆz s the front wheel of the car causes the car’s motion to be a pure angular velocity May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

120 102 3.3. Rigid-Body Motions and Twists ˆy b v b ˆy s } b { ˆx b { s } ˆx s w v s r Figure 3.18: The twist corresponding to the instantaneous motion of the chassis of a three-wheeled vehicle can be visualized as an angular velocity w about the point r. w = 2 rad/s about an axis out of the page at the point r in the plane. Inspecting ω = (2 , − 1 , 0) or r the figure, we can write r as = (2 , − 1 . 4 , 0), w as r 2) = (0 , 0 , s s b = (0 , 0 , ω 2), and T or as − sb b   − 4 0 1 0 ] [   . 4 0 1 0 0 R p sb sb   . T = = sb   − 1 0 0 0 0 1 1 0 0 0 From the figure and simple geometry, we get ω × ( − r ) = v × ω = = ( − 2 , − 4 , 0) , r s s s s s × = v × ( − r ) = r ω ω = (2 . 8 , 4 , 0) , b b b b b and thus obtain the twists V : and V b s     0 0     0 0     [ ] [ ]     ω 2 ω 2 − s b     V = = = , V = . s b     v − 2 v 8 2 . s b         − 4 4 0 0 To confirm these results, try calculating V . = [Ad V ] s b T sb 3.3.2.2 The Screw Interpretation of a Twist ̇ can be viewed as ˆ ω Just as an angular velocity θ , where ˆ ω is the unit rotation ω ̇ axis and θ is the rate of rotation about that axis, a twist V can be interpreted ̇ in terms of a screw axis S and a velocity θ about the screw axis. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

121 Chapter 3. Rigid-Body Motions 103 s ˆ ˆz ̇ θ h ˆ s ̇ θ ̇ s × q θ ˆ − q ˆy ˆx =p = h itch rs p rs linea peed/angula eed S Figure 3.19: q , a unit direction ˆ s , and a pitch A screw axis represented by a point . h A screw axis represents the familiar motion of a screw: rotating about the axis while also translating along the axis. One representation of a screw axis 3 q, ˆ s,h } S q ∈ R is the collection is any point on the axis, ˆ s is a unit { , where h is the screw pitch , which defines the vector in the direction of the axis, and ̇ θ ratio of the linear velocity along the screw axis to the angular velocity about the screw axis (Figure 3.19). 3.19 and geometry, we can write the twist V ω,v ) corre- Using Figure = ( ̇ about S (represented by { q, ˆ sponding to an angular velocity } ) as s,h θ [ ] [ ] ̇ s ˆ ω θ = V = . ̇ ̇ v θ × + h ˆ s θ q s ˆ − v is the sum of two terms: one due to translation Note that the linear velocity ̇ h ˆ s along the screw axis, θ , and the other due to the linear motion at the origin ̇ induced by rotation about the axis, − θ × q . The first term is in the direction ˆ s , while the second term is in the plane orthogonal to ˆ . It is not hard to s s of ˆ = ( ω,v ) where show that, for any 6 = 0, there exists an equivalent screw axis V ω T ̇ ̇ ̇ and velocity θ { , where ˆ s = ω/ ‖ ω ‖ , q, θ = ‖ ω ‖ , h = ˆ ω ˆ v/ s,h θ , and q is chosen } ̇ − ˆ s so that the term θ × q provides the portion of v orthogonal to the screw axis. If ω h of the screw is infinite. In this case ˆ s is chosen as = 0, then the pitch ̇ ‖ ‖ v/ ‖ θ is interpreted as the linear velocity v v ‖ along ˆ s . , and Instead of representing the screw axis S using the cumbersome collection { ˆ s,h } , with the possibility that h may be infinite and with the nonuniqueness q, of q (any q along the screw axis may be used), we instead define the screw axis S using a normalized version of any twist V = ( ω,v ) corresponding to motion along the screw: May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

122 104 3.3. Rigid-Body Motions and Twists ω 6 S = V / ‖ ω ‖ = ( ω/ ‖ ω ‖ ,v/ ‖ ω ‖ ). The screw axis S is simply (a) If = 0 then normalized by the length of the angular velocity vector. The angular V ̇ ̇ velocity about the screw axis is ω ‖ , such that S θ θ = V . = ‖ ‖ = 0 then ω V / ‖ v ‖ = (0 ,v/ S v ‖ ). The screw axis S is simply V (b) If = normalized by the length of the linear velocity vector. The linear velocity ̇ ̇ = θ v ‖ , such that S along the screw axis is θ = V . ‖ This leads to the following definition of a “unit” (normalized) screw axis: Definition 3.24. S is written as For a given reference frame, a screw axis [ ] ω 6 = ∈ R , S v ‖ ω ‖ = 1 or (ii) ω = 0 and ‖ where either (i) ‖ = 1. If (i) holds then v = v − × q + hω , where q is a point on the axis of the screw and ω is the pitch of h the screw ( h = 0 for a pure rotation about the screw axis). If (ii) holds then the pitch of the screw is infinite and the twist is a translation along the axis defined by v . Important: Although we use the pair ( ω,v ) for both a normalized screw axis (where one of ω ‖ or ‖ v ‖ must be unity) and a general twist S ‖ ω v ), the meaning should be clear (where there are no constraints on V and from the context. is just a normalized twist, the 4 4 matrix representation S Since a screw axis × ] of S = ( ω,v ) is [ S   [ ] 0 − ω ω 3 2 ω v ] [   ω 0 − ω , (3) se ∈ , S ] = ω [ [ (3) so ∈ (3.85) ] = 3 1 0 0 ω − 0 ω 2 1 where the bottom row of [ S ] consists of all zeros. Also, a screw axis represented S S in a frame { a } is related to the representation as by in a frame { b } b a S = [Ad . ] S , S = [Ad ] S b b T a T a ba ab 3.3.3 Exponential Coordinate Representation of Rigid-Body Motions 3.3.3.1 Exponential Coordinates of Rigid-Body Motions In the planar example in Section 3.1, we saw that any planar rigid-body dis- placement can be achieved by rotating the rigid body about some fixed point May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

123 Chapter 3. Rigid-Body Motions 105 in the plane (for a pure translation, this point lies at infinity). A similar result Chasles–Mozzi theorem also exists for spatial rigid-body displacements: the states that every rigid-body displacement can be expressed as a displacement in space. along a fixed screw axis S ωθ for rotations, we define the six- By analogy to the exponential coordinates ˆ dimensional exponential coordinates of a homogeneous transformation 6 ∈ R T as S is the screw axis and θ is the distance that must be S θ , where to T . If the pitch I traveled along the screw axis to take a frame from the origin = ( ω,v ) is finite then of the screw axis ω ‖ = 1 and θ corresponds to the S ‖ angle of rotation about the screw axis. If the pitch of the screw is infinite then ‖ v ‖ = 1 and θ corresponds to the linear distance traveled along the ω = 0 and screw axis. Also by analogy to the rotation case, we define a matrix exponential (exp) and matrix logarithm (log): ] θ ∈ se (3) exp : [ T ∈ SE (3) , S → T ∈ SE (3) → [ S ] θ ∈ se (3) . log : We begin by deriving a closed-form expression for the matrix exponential ] θ [ S e . Expanding the matrix exponential in series form leads to 3 2 θ θ 3 θ S [ 2 ] S + [ ··· + ] I ] + [ θ ] S + [ = S e 3! 2! [ ] 2 3 θ [ ω ] θ θ ) v θ ( G e 2 (3.86) ··· ] ω + [ + . ] ) = θ ( ω , G Iθ = + [ 1 0 3! 2! 3 = Using the identity [ − [ ω ], G ( θ ) can be simplified to ω ] 2 3 θ θ 2 ω ] G ( θ ) = Iθ + [ ] + ··· + [ ω 3! 2! ( ( ) ) 2 3 6 7 4 5 θ θ θ θ θ θ 2 [ ω ] + [ = ] + − Iθ + ω + −··· − −··· 7! 5! 6! 4! 2! 3! 2 . − cos θ )[ ω ] + ( θ − sin θ )[ = ] Iθ + (1 (3.87) ω Putting everything together leads to the following proposition: ‖ Let = ( ω,v ) Proposition 3.25. S ω ‖ = 1 then, for any be a screw axis. If distance θ ∈ R traveled along the axis, ] [ ) ( 2 [ ω ] θ v ω Iθ + (1 − cos θ ] ω ] + ( θ − sin θ )[ )[ e [ S θ ] (3.88) . e = 1 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

124 106 3.3. Rigid-Body Motions and Twists and ‖ v ‖ = 1 , then If ω = 0 [ ] I vθ ] S [ θ e = (3.89) . 1 0 3.3.3.2 Matrix Logarithm of Rigid-Body Motions The above derivation essentially provides a constructive proof of the Chasles– ) ∈ SE (3), one can always find Mozzi theorem. That is, given an arbitrary ( R,p = ( S θ such that a screw axis ω,v ) and a scalar [ ] R p [ S θ ] , (3.90) e = 1 0 i.e., the matrix [ ] [ ω ] θ vθ ] θ = [ (3) S ∈ se 0 0 = ( ). T R,p is the matrix logarithm of R,p ) written as T ∈ Algorithm: (3), find a θ ∈ [0 ,π ] and a screw Given ( SE 6 = ( ω,v ) ∈ R axis (where at least one of ‖ ω ‖ and ‖ v ‖ is unity) such that S S ] θ [ 6 = T . The vector S θ ∈ R e comprises the exponential coordinates for T and the matrix [ S θ ∈ se (3) is the matrix logarithm of T . ] R = I then set ω = 0, v = p/ ‖ p ‖ , and θ (a) If ‖ p ‖ . = SO (b) Otherwise, use the matrix logarithm on ω (written as (3) to determine R ω SO (3) algorithm) and ˆ for in the . Then v is calculated as θ − 1 G (3.91) v ( θ ) p = where ) ( θ 1 1 1 1 2 1 − − cot [ − I ] + ω ] . [ (3.92) ω ) = θ ( G 2 2 θ 2 θ The verification of Equation (3.92) is left as an exercise. In this example, the rigid-body motion is confined to the ˆx – Example 3.26. s can be repre- -plane. The initial frame { b } and final frame { c } in Figure 3.20 ˆy s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

125 Chapter 3. Rigid-Body Motions 107 ω s rad/ =1 3 3 =( q , . 37 . 3 37) ˆy b θ ˆx b { b } ˆx c ˆy c ˆy s c { } ˆx s { } s =( v 37) 3 − , 37 . 3 . Two frames in a plane. Figure 3.20: SE (3) matrices sented by the   ◦ ◦ 0 1 cos 30 − sin 30 ◦ ◦   0 2 cos 30 sin 30   , T = sb   0 1 0 0 0 0 1 0   ◦ ◦ sin 60 − cos 60 0 2 ◦ ◦   sin 60 cos 60 0 1   . = T sc   0 0 1 0 0 1 0 0 -plane, the corresponding screw has an –ˆy Because the motion occurs in the ˆx s s axis in the direction of the ˆz -axis and has zero pitch. The screw axis S = ( ω,v ), s expressed in { s } , therefore has the form ω = (0 , 0 ,ω , ) 3 v v = ( ,v . , 0) 2 1 T = to T We seek the screw motion that displaces the frame at ; i.e., T sc sb sc θ [ S ] e or T sb − 1 θ [ S ] T e = T , sc sb May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

126 108 3.4. Wrenches where   0 0 − ω v 3 1   ω 0 0 v 3 2   . [ ] = S   0 0 0 0 0 0 0 0 1 − T T We can apply the matrix logarithm algorithm directly to to obtain [ S ] sc sb S θ as follows: (and therefore ) and     0 ω 1       ω 0 . 1 0 0 − 3 37 2           π ω 1 0 − 0 3 37 . 1 3 ◦       = , θ = S ) rad (or 30 S [ ] = = . ,       37 v 3 . 0 0 0 0 6 1         3 . 37 − v 0 0 0 0 2 0 v 3 The value of means that the constant screw axis, expressed in the fixed frame S s , is represented by an angular velocity of 1 rad/s about ˆz and a linear velocity { } s { s } ) of (3 . 37 , − 3 . 37 , 0) expressed in the { s } (of a point currently at the origin of frame. Alternatively, we can observe that the displacement is not a pure translation ◦ T – and we and T – have rotation components that differ by an angle of 30 sc sb ◦ θ and ω = 1. We can also graphically determine = 30 quickly determine that 3 q = ( q -plane through which the screw axis passes; ,q the point ) in the ˆx –ˆy s x y s q . . 37 , 3 for our example this point is given by 37). = (3 For planar rigid-body motions such as this one, we could derive a planar matrix logarithm algorithm that maps elements of SE (2) to elements of se (2), which have the form   0 − ω v 1   0 v ω . 2 0 0 0 3.4 Wrenches Consider a linear force f acting on a rigid body at a point r. Defining a reference 3 ∈ } , the point r can be represented as r frame a R { and the force f can be a 3 3 in ∈ R represented as . This force creates a torque or moment m f ∈ R a a the { a } frame: m . = r f × a a a Note that the point of application of the force along its line of action is imma- terial. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

127 Chapter 3. Rigid-Body Motions 109 { } b r b f r a { } a r F . and F Figure 3.21: Relation between wrench representations a b Just as with twists, we can merge the moment and force into a single six- , or wrench , expressed in the { a dimensional frame, F } : spatial force a [ ] m a 6 = F ∈ . (3.93) R a f a If more than one wrench acts on a rigid body, the total wrench on the body is simply the vector sum of the individual wrenches, provided that the wrenches are expressed in the same frame. A wrench with a zero linear component is called a pure moment . { a } frame can be represented in another frame A wrench in the b } (Fig- { ure T F is known. One way to derive the relationship between F 3.21) if and ba b a is to derive the appropriate transformations between the individual force and moment vectors on the basis of techniques we have already used. A simpler and more insightful way to derive the relationship between F a F , however, is to (1) use the results we have already derived relating and b V and V of the same twist, and (2) use the fact that the power representations b a generated (or dissipated) by an ( V ) pair must be the same regardless of the F , frame in which it is represented. (Imagine if we could create power simply by changing our choice of reference frame!) Recall that the dot product of a force and a velocity is a power, and power is a coordinate-independent quantity. Because of this, we know that T T (3.94) F = V V . F a b a b 3.22 we know that V From Proposition = [Ad , and therefore Equa- ] V T a b ab tion (3.94) can be rewritten as T T F V F ] V ) = ([Ad T b b a b ab T T F [Ad = V ] . a T b ab May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

128 110 3.4. Wrenches L L 1 2 ˆy ˆy h f g ˆx ˆx h a ˆx f ˆz a Figure 3.22: A robot hand holding an apple subject to gravity. Since this must hold for all , this simplifies to V b T ] F = [Ad . (3.95) F a b T ab Similarly, T = [Ad F F ] . (3.96) T a b ba b Given a wrench { a } as F Proposition 3.27. and in { , represented in } as F a F , the two representations are related by b T T = Ad F (3.97) , ( ) = [Ad F F ] b T a a T ab ab T T F = Ad ( F ) = [Ad (3.98) . F ] T b a b T ba ba Since we usually have a fixed space frame s } and a body frame { b } , we can { spatial wrench F body wrench and a define a F . b s The robot hand in Figure is holding an apple with a mass 3.22 Example 3.28. 2 1 kg in a gravitational field g = 10 m/s of 0 (rounded to keep the numbers . . 5 kg. What is simple) acting downward on the page. The mass of the hand is 0 the force and torque measured by the six-axis force–torque sensor between the hand and the robot arm? We define frames { f } at the force–torque sensor, { h } at the center of mass of the hand, and { } at the center of mass of the apple. According to the a 3.22, the gravitational wrench on the hand in coordinate axes in Figure h } is { given by the column vector F 0) = (0 , 0 , 0 , 0 , − 5 N , h { a } is and the gravitational wrench on the apple in F . = (0 , 0 , 0 , 0 , 0 , 1 N) a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

129 Chapter 3. Rigid-Body Motions 111 = 10 cm and L L = 15 cm, the transformation matrices T Given and T af 1 2 hf are     1 0 0 0 . 1 m 0 − 0 0 . 25 m 1 −     0 0 0 0 1 0 1 0     . = , T = T af hf     0 0 1 0 1 0 − 0 0 1 0 0 0 0 1 0 0 The wrench measured by the six-axis force–torque sensor is T T F F ] + [Ad F = [Ad ] T f h a T af hf T T 0 . 5 Nm 0 − 5 N 0] + [0 0 = [0 0 − 0 . 25 Nm 0 − 1 N 0] − T . 0 . 75 Nm 0 − 6 N 0] = [0 0 − 3.5 Summary The following table succinctly summarizes some of the key concepts from the chapter, as well as the parallelism between rotations and rigid-body motions. For more details, consult the appropriate section of the chapter. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

130 112 3.5. Summary Rigid-Body Motions Rotations (3) : 3 × 3 matrices T ∈ SE (3) : 4 SO 4 matrices ∈ R × [ ] R p T R = 1 T = R , R = I, det 1 0 3 SO (3) ,p ∈ R where ∈ R ] [ T T R p R − 1 − 1 − T = = T R R 0 1 change of coordinate frame: change of coordinate frame: = = , R p = p T R R T , T p = p R T bc ab b ac a ab ac b a bc ab ab b { : displacing a frame { b } : } rotating a frame [ ] Rot(ˆ ω,θ ) p = Rot(ˆ ω,θ ) T = R 1 0 ′ ′ = ˆ = RR R : T ω ω = TT about ˆ : rotate θ sb sb sb sb s origin), translate θ about ˆ = ˆ ω (moves { b } ω p in { s } rotate s ′′ ′′ = R } R R T , : = T b T : translate p in { sb sb sb sb rotate θ ω in new body frame rotate ω about ˆ θ about ˆ ω = ˆ b ] [ ω 3 6 ∈ S = ω R , unit rotation axis is ˆ ∈ R “unit” screw axis is , v ‖ where ω ‖ = 1 where either (i) ‖ ω ‖ = 1 or ˆ (ii) ω = 0 and ‖ v ‖ = 1 for a screw axis { q, ˆ s,h } with finite h , [ ] [ ] ω ˆ s S = = − ˆ s × q + h ˆ v s ̇ ̇ = ω angular velocity is θ twist is V ω S = ˆ θ continued... May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

131 Chapter 3. Rigid-Body Motions 113 Rigid-Body Motions (cont.) Rotations (cont.) [ ] ω 3 6 V = ∈ R for any 3-vector, e.g., , ∈ R for , ω v   ] [ 0 ω − ω 3 2 ω v ] [   ω − 0 ω ] = [ ω ∈ so (3) [ se ] = ∈ V (3) 3 1 0 0 0 ω − ω 1 2 3 R identities, ,R ∈ SO ω,x (the pair ( ω,v ) can be a twist V ∈ (3): T ] = − [ ω ] , , [ ω ] x = − [ x ] ω, or a “unit” screw axis S [ ω T T ω ] = ([ x ][ ω ]) [ ,R [ ω ] R x = [ Rω ] depending on the context) ][ 1 1 − − − 1 1 − ̇ ̇ ̇ ̇ ] = [ T V V = [ ] , T ] ω = [ R TT RR ] , R = [ ω b s b s [ ] R 0 6 × 6 ] = [Ad ∈ R T p R R ] [ 1 − − 1 = [Ad , identities: [Ad ] ] T T [Ad ][Ad ] = [Ad ] T T T T 1 2 1 2 change of coordinate frame: change of coordinate frame: ˆ ω V = R ] ˆ ω = [Ad , ω V = R , ω S S ] = [Ad T T ab a a b b ab a b a b ab ab 3 6 ∈ ωθ ∈ R ∈ exp coords for T SO SE (3): S θ ∈ R R exp coords for (3): ˆ ω ] θ ∈ so (3) → R ∈ SO (3) exp : [ S ] θ ∈ se (3) → T ∈ SE (3) exp : [ˆ [ ] [ ω ] θ e ∗ [ˆ ] ω θ [ S θ ] = = T = e ) = e ω,θ R = Rot(ˆ 1 0 2 + sin θ [ˆ ω ] + (1 − cos θ )[ˆ ω ] I where ∗ = 2 − − cos θ )[ ω ] + ( θ + (1 sin θ )[ ω ] ( ) v Iθ log : R ∈ SO (3) → [ˆ ω ] θ ∈ so (3) log : T ∈ SE (3) → [ S ] θ ∈ se (3) algorithm in Section algorithm in Section 3.3.3.2 3.2.3.3 moment change of coord frame: wrench change of coord frame: T ,f = R ) = [Ad m m F m = ( ] F a b a ab T a a b ba Software 3.6 The following functions are included in the software distribution accompany- ing the book. The code below is in MATLAB format, but it is available in other languages. For more details on the software, consult the code and its documentation. invR = RotInv(R) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

132 114 3.6. Software R . Computes the inverse of the rotation matrix so3mat = VecToso3(omg) × omg . 3 skew-symmetric matrix corresponding to Returns the 3 omg = so3ToVec(so3mat) 3 skew-symmetric matrix so3mat . Returns the 3-vector corresponding to the 3 × [omghat,theta] = AxisAng3(expc3) ω and the rotation amount θ from the 3-vector ˆ ωθ of Extracts the rotation axis ˆ expc3 exponential coordinates for rotation, . R = MatrixExp3(so3mat) R SO (3) corresponding to the matrix exponen- Computes the rotation matrix ∈ tial of so3mat ∈ so (3). so3mat = MatrixLog3(R) so3mat ∈ so (3) of the rotation matrix R ∈ Computes the matrix logarithm SO (3). T = RpToTrans(R,p) Builds the homogeneous transformation matrix T corresponding to a rotation 3 matrix SO (3) and a position vector p ∈ R R . ∈ [R,p] = TransToRp(T) Extracts the rotation matrix and position vector from a homogeneous transfor- mation matrix T . invT = TransInv(T) Computes the inverse of a homogeneous transformation matrix T . se3mat = VecTose3(V) Returns the se (3) matrix corresponding to a 6-vector twist V . V = se3ToVec(se3mat) se (3) matrix se3mat . Returns the 6-vector twist corresponding to an AdT = Adjoint(T) Computes the 6 × 6 adjoint representation [Ad ] of the homogeneous transfor- T T . mation matrix S = ScrewToAxis(q,s,h) Returns a normalized screw axis representation S of a screw described by a unit May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

133 Chapter 3. Rigid-Body Motions 115 s in the direction of the screw axis, located at the point , with pitch h . vector q [S,theta] = AxisAng(expc6) S Extracts the normalized screw axis and the distance traveled along the screw θ from the 6-vector of exponential coordinates S θ . T = MatrixExp6(se3mat) T Computes the homogeneous transformation matrix SE (3) corresponding to ∈ se3mat (3). se the matrix exponential of ∈ se3mat = MatrixLog6(T) se3mat ∈ se (3) of the homogeneous transfor- Computes the matrix logarithm mation matrix T ∈ SE (3). 3.7 Notes and References The exponential coordinates for rotations introduced in this chapter are also re- ferred to in the kinematics literature as the Euler–Rodrigues parameters. Other representations for rotations such as Euler angles, Cayley–Rodrigues parame- ters, and unit quaternions are described in Appendix B; further details on these and related parametrizations of the rotation group SO (3) can be found in, e.g., [169, 186, 122, 135]. 113, Classical screw theory has its origins in the works of Mozzi and Chasles, who independently discovered that the motion of a rigid body can be obtained as a rotation about some axis followed by a translation about the same axis [25]. Ball’s treatise [6] is often regarded as the classical reference on screw theory, while more modern treatments can be found in Bottema and Roth [18], Angeles [2], and McCarthy [113]. The identification of elements of classical screw theory with the Lie group structure of the rigid body motions SE (3) was first made by Brockett in [20], who went considerably further and showed that the forward kinematics of open chains can be expressed as the product of matrix exponentials (this is the subject of the next chapter). Derivations of the formulas for the matrix exponentials, logarithms, their derivatives, and other related formulas can be found in [92, 129, 131, 122]. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

134 116 3.8. Exercises 3.8 Exercises , ˆy In terms of the ˆx , ˆz Exercise 3.1 coordinates of a fixed space frame { s } , s s s 0 has its ˆx the frame -axis pointing in the direction (0 , { , 1) and its ˆy } -axis a a a 1 , 0 , 0), and the frame { b pointing in the direction ( has its ˆx − -axis pointing } b 0 , 0) and its ˆy in the direction (1 -axis pointing in the direction (0 , 0 , − 1). , b (a) Draw by hand the three frames, at different locations so that they are easy to see. R and R (b) Write down the rotation matrices . sb sa − 1 (c) Given , how do you calculate R without using a matrix inverse? R sb sb − 1 Write down and verify its correctness using your drawing. R sb R and R , how do you calculate R (d) Given (again without using ma- sb ab sa trix inverses)? Compute the answer and verify its correctness using your drawing. (e) Let R = R be considered as a transformation operator consisting of sb ◦ 90 R . Calculate R a rotation about ˆx by = R R − , and think of sa sa 1 as a rotation of R R , and as a representation of an orientation, R as 1 sa the new orientation after the rotation has been performed. Does the new ◦ correspond to a rotation of orientation R about the world- by − 90 R sa 1 fixed ˆx -axis or about the body-fixed ˆx -axis? Now calculate R = RR . s 2 sa a ◦ Does the new orientation correspond to a rotation of R R by − 90 sa 2 -axis or about the body-fixed ˆx about the world-fixed ˆx -axis? a s = (1 to change the representation of the point (f) Use 3) (which is R , 2 , p b sb { b } coordinates) to { s } coordinates. in 3) in p , 2 , = (1 { s } coordinates. Calculate (g) Choose a point p represented by s ′′ ′ T R R . For each operation, should the result be and p = = p p p s s sb sb interpreted as changing coordinates (from the { s } frame to { b } ) without moving the point p or as moving the location of the point without changing the reference frame of the representation? , { as ω (h) An angular velocity w is represented in = (3 } 2 , 1). What is its s s ω in representation { a } ? a (i) By hand, calculate the matrix logarithm [ˆ ω ] θ of R . (You may verify your sa answer with software.) Extract the unit angular velocity ˆ ω and rotation amount . Redraw the fixed frame { s } and in it draw ˆ ω . θ (j) Calculate the matrix exponential corresponding to the exponential coor- dinates of rotation ˆ ωθ = (1 , 2 , 0). Draw the corresponding frame relative to { s } , as well as the rotation axis ˆ ω . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

135 Chapter 3. Rigid-Body Motions 117 ) ( 1 1 1 √ √ √ with Let p be a point whose coordinates are = , , − Exercise 3.2 p 2 3 6 respect to the fixed frame ˆx–ˆy–ˆz. Suppose that p is rotated about the fixed- frame ˆx-axis by 30 degrees, then about the fixed-frame ˆy-axis by 135 degrees, and 120 degrees. Denote the coordinates of finally about the fixed-frame ˆz-axis by − ′ p . this newly rotated point by ′ ? p (a) What are the coordinates ′ ′ for the = Rp (b) Find the rotation matrix p R you obtained in such that p (a). ′ 3 ′ 3 Suppose that and p Exercise 3.3 ∈ ∈ R R are related by p p , = Rp i i i i i = 1 , 2 3, for some unknown rotation matrix R . Find, if it exists, the rotation , ′ p 7→ p for the three input–output pairs R , where i i √ √ ′ , 2) 7→ p 0 , , , 2 , 2 2) = (0 = ( p 1 1 ( ) √ 1 1 ′ √ √ 1 = (1 = , p , − 1) 7→ p , , − 2 , 2 2 2 2 √ √ √ ′ 2 2 , 0) 7→ p . , = ( − 2) 2 , − p 2 , = (0 3 3 Exercise 3.4 R R = In this exercise you are asked to prove the property ab bc R by of Equation (3.22). Define the unit axes of frames { a } , { b } , and { c } ac the triplets of orthogonal unit vectors ˆx { , ˆy , , ˆz } } , { ˆx ˆz , ˆy , , ˆz ˆy } , and { ˆx , c b a b c a a c b { b } can be expressed in terms respectively. Suppose that the unit axes of frame { a } by of the unit axes of frame ˆy , ˆx + r ˆx r = + r ˆz b 11 31 21 a a a ˆy r ˆx + r = ˆy , + r ˆz a 12 32 22 a a b ˆz . = r ˆz ˆx r + r + ˆy 23 33 13 a b a a { } can be expressed in terms of Similarly, suppose that the unit axes of frame c the unit axes of frame { b } by ˆx = s , ˆx ˆz + s s ˆy + 21 31 b b 11 c b ˆy , = s ˆz ˆx s + s + ˆy b 32 12 b 22 b c ˆz . = s ˆz ˆx s + s + ˆy 23 b 33 13 b c b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

136 118 3.8. Exercises R = R . From the above prove that R ab bc ac 3 ωθ Exercise 3.5 for the SO (3) matrix ∈ Find the exponential coordinates ˆ R   − 0 0 1   0 − 1 0 . 0 1 0 R = Rot(ˆx ,π/ 2)Rot(ˆz ,π ), find the unit vector ˆ ω Exercise 3.6 Given and [ˆ ω ] θ θ R = e . angle such that Exercise 3.7 (a) Given the rotation matrix   0 0 1   1 0 0 − , = R 0 0 1 3 ∈ R ω , ‖ ˆ find all possible values for ˆ ω = 1, and θ ∈ [0 , 2 π ) such that ‖ ω ] θ [ˆ = R . e 3 v ∈ R are related by (b) The two vectors ,v 1 2 [ˆ ω ] θ e = Rv v = v 1 1 2 2 1 2 3 has length 1, and where ˆ ∈ [ − ω ]. Given ˆ ω = ( ∈ R θ π,π , , = ,v ) 1 3 3 3 , 0 , 1) ,v (1 = (0 , 1 , 1), find all the angles θ that satisfy the above equation. 2 Exercise 3.8 (a) Suppose that we are seeking the logarithm of a rotation matrix R whose trace is − 1. From the exponential formula ω θ 2 ] [ˆ + sin θ [ˆ ω ] + (1 − cos θ )[ˆ ω e ] = ‖ ω ‖ = 1 , I , R = − 1 implies θ = π , the above equation simplifies and recalling that tr to   2 2 ) 2ˆ ω ω ˆ ω 2ˆ ˆ ω ω + ˆ 1 ω − 2(ˆ 1 1 3 2 3 2 2 2 2   2ˆ ω ˆ ) ω + ˆ ω ω 2(ˆ ω ˆ ω 2ˆ 1 − . = = ] ω R + 2[ˆ I 3 2 1 2 3 1 2 2 ) ω + ˆ 2ˆ 2(ˆ ω ˆ ω 2ˆ ω ω ˆ ω 1 − 2 1 3 2 2 1 2 2 2 = 1, is it correct to conclude that + ˆ ω + ˆ ω Using the fact that ˆ ω 3 2 1 √ √ √ + 1 + 1 r r + 1 r 11 22 33 ˆ = ω ω , = , , ˆ ˆ ω = 1 3 2 2 2 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

137 Chapter 3. Rigid-Body Motions 119 r i,j )th entry of R , is also a solution? where denotes the ( ij 3 2 − [ˆ ω ], the identity R = I = ω ] ] can be written (b) Using the fact that [ˆ ω + 2[ˆ in the alternative forms 2 = 2[ˆ ω ] R , − I 3 ] − I ) = 2 [ˆ ω ] ] ( = − 2 [ˆ ω R , [ˆ ω ω ] ( R + I ) = 0 . [ˆ ω , The resulting equation consists of three linear equations in (ˆ ˆ ω ). , ˆ ω 3 1 2 What is the relation between the solution to this linear system and the logarithm of R ? Exploiting the known properties of rotation matrices, determine Exercise 3.9 the minimum number of arithmetic operations (multiplication and division, ad- dition and subtraction) required to multiply two rotation matrices. Exercise 3.10 Because arithmetic precision is only finite, the numerically obtained product of two rotation matrices is not necessarily a rotation matrix; T that is, the resulting rotation A A = I as desired. may not exactly satisfy A × 3 3 R A Devise an iterative numerical procedure that takes an arbitrary matrix ∈ R SO (3) that minimizes ∈ and produces a matrix 2 T A R − = tr ( A A R )( ‖ − R ) ‖ . − (Hint: See Appendix D for the relevant background on optimization.) Exercise 3.11 Properties of the matrix exponential. n × n A B A + B (a) Under what conditions on general e A,B = e does e R hold? ∈ A V V ) are ] and B = [ V ,v ], where (b) If ω = ( ω = ( ,v = [ ) and V a b a b b b a a B A e V = does V arbitrary twists, then under what conditions on e and a b B A + e hold? Try to give a physical description of this condition. Exercise 3.12 (a) Given a rotation matrix = Rot(ˆz ,α ), where Rot(ˆz ,α ) indicates a rota- A α , find all rotation matrices tion about the ˆz-axis by an angle ∈ SO (3) R that satisfy AR = RA . (b) Given rotation matrices A = Rot(ˆz ,α ) and B = Rot(ˆz ,β ), with α 6 = β , find all rotation matrices ∈ SO (3) that satisfy AR = RB . R (c) Given arbitrary rotation matrices A,B ∈ SO (3), find all solutions R ∈ SO (3) to the equation AR = RB . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

138 120 3.8. Exercises Exercise 3.13 (a) Show that the three eigenvalues of a rotation matrix SO (3) each have R ∈ μ unit magnitude, and conclude that they can always be written − + { iν,μ 2 2 , where μ } + ν = 1. 1 iν, ∈ SO (3) can always be factored in the (b) Show that a rotation matrix R form   μ ν 0 − 1   ν μ 0 − , = A A R 0 0 1 2 2 SO = 1. (Hint: Denote the eigenvector associ- μ ∈ + ν A where (3) and 3 μ by x + iy , x,y ∈ R iν , and the eigenvector ated with the eigenvalue + 3 z ∈ R . For the purposes of this prob- associated with the eigenvalue 1 by { x,y,z } can always be chosen lem you may assume that the set of vectors to be linearly independent.) 3 Exercise 3.14 ∈ R Given , ‖ ω ‖ = 1, and θ a nonzero scalar, show that ω ) ( ) ( 1 θ 1 1 1 − 1 2 2 ] + ω [ . − cot ] − ω [ I ] + ( = ω )[ θ sin − θ ] ω )[ θ cos − + (1 Iθ θ 2 2 θ 2 3 ω − = (Hint: From the identity [ [ ω ], express the inverse as a quadratic matrix ] ω polynomial in [ ].) Exercise 3.15 { 0 } and a moving frame { 1 (a) Given a fixed frame initially aligned with } { 0 } , perform the following sequence of rotations on { 1 } : 1. Rotate { 1 } about the { 0 } frame ˆx-axis by α ; call this new frame { 2 } . frame ˆy-axis by 2. Rotate } about the { 0 } 2 β ; call this new frame { 3 } . { 3. Rotate { 3 } about the { 0 } frame ˆz-axis by γ ; call this new frame { 4 } . What is the final orientation ? R 04 (b) Suppose that the third step above is replaced by the following: “Rotate { 3 } about the ˆz-axis of frame { 3 } by γ ; call this new frame { 4 } .” What is the final orientation R ? 04 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

139 Chapter 3. Rigid-Body Motions 121 (c) Find for the following transformations: T ca     1 1 1 1 √ √ √ √ 0 0 − 1 − 0 2 2 2 2 1 1     √ √ 0 1 0 1 0 0     2 2 T = . , T = ab cb 1 1     √ √ 0 0 − 1 1 0 0 2 2 1 0 1 0 0 0 0 0 In terms of the ˆx , ˆy , ˆz coordinates of a fixed space frame Exercise 3.16 s s s s } , frame { a } has its ˆx -axis pointing in the direction (0 { , 0 , 1) and its ˆy -axis a a } 1 , 0), and frame { b 0 has its ˆx , -axis pointing in the − pointing in the direction ( b 0 , 0) and its ˆy direction (1 -axis pointing in the direction (0 , 0 , − 1). The origin , b { a } is at (3 , 0 , 0) in { s } and the origin of { b } is at (0 , 2 , 0) is { s } . of . { and { b } relative to { s } } a (a) Draw by hand a diagram showing R and R and the transformation (b) Write down the rotation matrices sa sb T matrices and T . sb sa − 1 , how do you calculate T (c) Given without using a matrix inverse? T sb sb − 1 Write T and verify its correctness using your drawing. sb (d) Given T and T (again without using ma- , how do you calculate T ab sb sa trix inverses)? Compute the answer and verify its correctness using your drawing. (e) Let T be considered as a transformation operator consisting of a T = sb ◦ 90 and a translation along ˆy by 2 units. Calculate rotation about ˆx by − = T T T . Does T and correspond to a rotation and translation about ˆx s 1 1 sa ˆy , respectively (a world-fixed transformation of T ), or a rotation and sa s translation about ˆx and ˆy , respectively (a body-fixed transformation of a a )? Now calculate = TT . Does T T correspond to a body-fixed or T sa sa 2 2 T ? world-fixed transformation of sa T (f) Use to change the representation of the point p } = (1 , 2 , 3) in { b b sb coordinates to { s } coordinates. (g) Choose a point p represented by p coordinates. Calcu- = (1 , 2 , 3) in { s } s − 1 ′ ′′ . For each operation, should the result p p and p late = T = T p s sb s sb b { } be interpreted as changing coordinates (from the { s } ) with- frame to out moving the point p, or as moving the location of the point without changing the reference frame of the representation? (h) A twist V is represented in { s } as V 3). What is its = (3 , 2 , 1 , − 1 , − 2 , − s representation V in frame { a } ? a S ] θ of T . (You may verify your (i) By hand, calculate the matrix logarithm [ sa answer with software.) Extract the normalized screw axis S and rotation amount . Find the { q, ˆ s,h } representation of the screw axis. Redraw the θ fixed frame { s } and in it draw S . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

140 122 3.8. Exercises (j) Calculate the matrix exponential corresponding to the exponential coordi- 3 , 1 , 2 S = (0 , 0 , 0). Draw the corresponding nates of rigid-body motion θ , s } , as well as the screw axis S . frame relative to { ˆx ˆx b c } { c } { b ˆy ˆz c b ˆy c ˆz b 2 ˆz d } d { ˆz a ˆy d 1 ˆx d ˆy { } a a ˆx a 1 Figure 3.23: Four reference frames defined in a robot’s workspace. Four reference frames are shown in the robot workspace of Exercise 3.17 { Figure 3.23: the fixed frame } , the end-effector frame effector { b } , the camera a frame { c } , and the workpiece frame { d } . (a) Find T and T in terms of the dimensions given in the figure. cd ad (b) Find given that T ab   1 0 0 4   0 1 0 0   T . = bc   0 0 1 0 0 0 0 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

141 Chapter 3. Rigid-Body Motions 123 r } { a { } } s { { } e Figure 3.24: A robot arm mounted on a spacecraft. Exercise 3.18 Consider a robot arm mounted on a spacecraft as shown in { e } , a satellite { s } Figure 3.24, in which frames are attached to the Earth , the spacecraft a } , and the robot arm { r } , respectively. { T T , T . , and (a) Given T , find es rs ar ea (b) Suppose that the frame { s } origin as seen from { e } is (1 , 1 , 1) and that   1 0 1 0 −   0 0 1 1   T = . er   0 1 1 − 0 0 0 1 0 Write down the coordinates of the frame { s } origin as seen from frame { r } . Exercise 3.19 3.25. Two satellites are circling the Earth as shown in Figure Frames { 1 } and { 2 } are rigidly attached to the satellites in such a way that their ˆx-axes always point toward the Earth. Satellite 1 moves at a constant speed May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

142 124 3.8. Exercises ˆy 1 e1 Satellit v 1 { 1 } ˆx 1 v 2 ˆy 2 ◦ 30 R 1 R 2 { } 2 ˆx 2 Satellit e2 ˆz 0 { 0 } ˆy 0 ˆx 0 Figure 3.25: Two satellites circling the Earth. . To simplify matters, ignore , while satellite 2 moves at a constant speed v v 2 1 { 0 } is located at the rotation of the Earth about its own axis. The fixed frame the center of the Earth. Figure 3.25 shows the position of the two satellites at = 0. t T (a) Derive the frames , T as a function of t . 02 01 (b) Using your results from part (a), find t . T as a function of 21 ˆy r radius = c ˆy a ˆx { c } c ˆz ˆz c ˆy ' b ˆx ˆz a a } a { ˆx ˆx { b } b ˆy ˆz b L radius =2 r D Figure 3.26: A high-wheel bicycle. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

143 Chapter 3. Rigid-Body Motions 125 Consider the high-wheel bicycle of Figure 3.26, in which the Exercise 3.20 a diameter of the front wheel is twice that of the rear wheel. Frames { b } } and { } is attached c are attached respectively to the centers of the wheels, and frame { to the top of the front wheel. Assuming that the bike moves forward in the as a function of the front wheel’s rotation angle θ (assume T ˆy-direction, find ac θ = 0 at the instant shown in the figure). NorthStar b { } ◦ 30 ω 1 r Doc kingport radius: r ˆz a ˆz ˆy c c p c { } ˆy a a } { R ˆx a ω 2 Figure 3.27: A spacecraft and space station. Exercise 3.21 The space station of Figure 3.27 moves in circular orbit around the Earth, and at the same time rotates about an axis always pointing toward the North Star. Owing to an instrument malfunction, a spacecraft heading toward the space station is unable to locate the docking port. An Earth-based ground station sends the following information to the spacecraft:     0 − 1 0 − 100 0   0 0 300 1     800 = T , p = , ab a   1 0 500 0 0 1 0 0 0 a where -frame coordinates. is the vector p expressed in { p } a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

144 126 3.8. Exercises -frame { b } r , the vector r expressed in (a) From the given information, find b coordinates. T at the instant shown in the figure. Assume here that the ˆy- (b) Determine bc { a } and { c } frames are coplanar with the docking port. and ˆz-axes of the 1 } { Laser L 2 ˆz 2 ˆx 1 v } 2 { ˆx 2 ˆy ˆy 1 2 ) t ( =0 L 1 T 2 { 0 } ˆx ˆz 0 3 ˆy 3 ˆy 0 rget Ta } { 3 ω θ T t =0 ( ) 1 A laser tracking a moving target. Figure 3.28: Exercise 3.22 A target moves along a circular path at constant angular ve- locity ω rad/s in the ˆx–ˆy-plane, as shown in Figure 3.28. The target is tracked by a laser mounted on a moving platform, rising vertically at constant speed v . Assume that at t = 0 the laser and the platform start at L , while the target 1 . starts at frame T 1 T . ,T (a) Derive the frames ,T t as functions of 03 01 12 T . as a function of t (b) Using your results from part (a), derive 23 Exercise 3.23 Two toy cars are moving on a round table as shown in Fig- ure 3.29. Car 1 moves at a constant speed v along the circumference of the 1 table, while car 2 moves at a constant speed along a radius; the positions of v 2 the two vehicles at t = 0 are shown in the figures. . and T (a) Find as a function of t T 02 01 (b) Find T as a function of t . 12 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

145 Chapter 3. Rigid-Body Motions 127 ˆz 1 ˆz 2 pV To w ie ˆx 1 ˆy { } 1 2 ˆy 1 { 2 } ˆx 2 v 1 } { 1 ˆx 1 ˆy 2 v 2 2 } { ◦ ˆy R =2 ˆx 1 45 2 ◦ 45 ˆy =1 L 0 ˆx 0 } { 0 =2 H ˆz 0 ˆy =1 L 0 L =1 ˆx 0 { } 0 =1 L Two toy cars on a round table. Figure 3.29: Figure 3.30 shows the configuration, at t = 0, of a robot Exercise 3.24 arm whose first joint is a screw joint of pitch h = 2. The arm’s link lengths L = 3. Suppose that all joint angular are L L = L = 5, and = 10, 3 2 4 1 velocities are constant, with values ω 4 rad/s. Find = π/ 4, ω π/ = π/ 8, ω − = 3 2 1 (4) SE (3), i.e., the configuration of the end-effector frame { b } relative to ∈ T sb s } at time t = 4. { the fixed frame Exercise 3.25 A camera is rigidly attached to a robot arm, as shown in Figure X ∈ SE (3) is constant. The robot arm moves 3.31. The transformation A from posture 1 to posture 2. The transformations SE (3) and B ∈ SE (3) ∈ are measured and can be assumed to be known. (a) Suppose that X and A are given as follows:     1 0 0 1 0 0 1 0     0 1 0 0 1 0 1 0     = X , A . =     0 0 1 0 1 0 1 0 − 0 0 0 1 0 0 1 0 What is B ? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

146 128 3.8. Exercises ˆz s ω 1 L 2 L 4 A ω 2 L 3 ˆz b L 1 ˆy b ω 3 { } b ˆx b s } { ˆy ˆx s s Figure 3.30: A robot arm with a screw joint. (b) Now suppose that [ [ ] ] R p p R A A B B = A , B = 1 0 1 0 are known and we wish to find ] [ R p X X . X = 1 0 3 ] [ [ β ] α and = and = e e Set R . What are the conditions on α ∈ R R B A 3 ∈ R β for a solution R to exist? X k equations (c) Now suppose that we have a set of A ,...,k. X = XB = 1 for i i i Assume that A and B are all known. What is the minimum number k i i for which a unique solution exists? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

147 Chapter 3. Rigid-Body Motions 129 c } { X po se1 camera A } { t tip se2 po { c } X B t { } Figure 3.31: A camera rigidly attached to a robot arm. Exercise 3.26 Draw the screw axis for which q = (3 , 0 , 0), ˆ s = (0 , 0 , 1), and h = 2. Exercise 3.27 Draw the screw axis for the twist V = (0 , 2 , 2 , 4 , 0 , 0). 2 Exercise 3.28 ω 3) = (1 , Assume that the space-frame angular velocity is , s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

148 130 3.8. Exercises b for a moving body with frame } { at   − 1 0 0   0 0 − 1 = R 0 0 1 s } relative to the space frame ω { in { b } . . Calculate the body’s angular velocity b { a } and { b Two frames are attached to a moving rigid body. Exercise 3.29 } { a } in space-frame coordinates is the same as the twist Show that the twist of { b } in space-frame coordinates. of ˆz 2 ˆz 2 ˆx 2 ˆy 2 ˆy { 2 } 2 ˆx 2 } 2 { 1 1 ˆz ˆz ˆz 1 ˆz 1 0 0 ˆy ˆy 1 1 0 { } ˆy ˆy 0 } { 0 0 } 1 { { 1 } ˆx ˆx 1 1 ˆx ˆx 0 0 1 1 (b) A second screw motion. (a) A first screw motion. Figure 3.32: A cube undergoing two different screw motions. Exercise 3.30 A cube undergoes two different screw motions from frame { 1 } { 2 } as shown in Figure 3.32. In both cases, (a) and (b), the initial to frame configuration of the cube is   1 0 0 0   0 1 0 1   = T . 01   0 0 1 0 0 0 0 1 (a) For each case, (a) and (b), find the exponential coordinates θ ω,v ) S = ( θ [ S ] θ T = . T such that , where no constraints are placed on ω or v e 02 01 ]. (b) Repeat (a), this time with the constraint that ωθ ‖∈ [ − π,π ‖ Exercise 3.31 In Example 3.19 and Figure 3.16, the block that the robot must pick up weighs 1 kg, which means that the robot must provide approximately May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

149 Chapter 3. Rigid-Body Motions 131 { (which you can assume 10 N of force in the ˆz e -direction of the block’s frame } e in the e } F is at the block’s center of mass). Express this force as a wrench { e frame. Given the transformation matrices in Example 3.19, express this same c } as F . { wrench in the end-effector frame c { a } and { b } in physical space, and Exercise 3.32 Given two reference frames b o , define the distance between frames { a } and { } } as { a fixed frame √ 2 2 || + || p θ ≡ dist( ,T ) T ab oa ob θ ] [ˆ ω e R . Suppose that the fixed frame is displaced to another frame = where ab ′ ′ ′ ′ o T ST , T } and that = ST { for some constant S = ( R ,p ) ∈ SE (3). = o s o s o b oa b a ′ ′ T (a) Evaluate dist( ,T ) using the above distance formula. o b o a ′ ′ does dist( T )? ,T (b) Under what conditions on ) = dist( T ,T S a o ob b oa o (a) Find the general solution to the differential equation ̇ x Exercise 3.33 = Ax , where [ ] − 2 1 A = . 1 − 0 ( t ) as t What happens to the solution ? x →∞ (b) Do the same for ] [ 2 − 1 = A . 1 2 What happens to the solution x ) as t →∞ ? t ( 2 2 × 2 Let , A ∈ R x ∈ Exercise 3.34 , and consider the linear differential R equation ̇ x ( t ) = Ax ( t ). Suppose that [ ] − 3 t e x t ) = ( − 3 t e − 3 x (0) = (1 , − 3), and is a solution for the initial condition ] [ t e ) = t x ( t e At x (0) = (1 , 1). Find A and e is a solution for the initial condition . f Exercise 3.35 = Ax + x ( t ), where Given a differential equation of the form ̇ n ∈ R x and f ( t ) is a given differentiable function of t , show that the general May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

150 132 3.8. Exercises solution can be written ∫ t − s ( t ) A At ( ds. ) s f e x ) = ( (0) + x e t 0 − At ) = e (Hint: Define z x ( t ) and evaluate ̇ z ( t ).) ( t B, answer the following questions re- Referring to Appendix Exercise 3.36 lated to ZXZ Euler angles. (a) Derive a procedure for finding the ZXZ Euler angles of a rotation matrix. (b) Using the results of (a), find the ZXZ Euler angles for the following rota- tion matrix:   1 1 √ √ − 0 2 2   1 1 1 √ − − .   2 2 2 1 1 1 √ 2 2 2 Exercise 3.37 Consider a wrist mechanism with two revolute joints θ and 1 θ , in which the end-effector frame orientation R ∈ SO (3) is given by 2 ω ω θ ] θ [ˆ [ˆ ] 2 1 2 1 e R = , e 1 1 √ √ , 0 with ˆ 1) and ˆ ω ω = (0 , , = (0 , ). Determine whether the following − 1 2 2 2 ) for the θ orientation is reachable (that is, find, if it exists, a solution ( ,θ 1 2 following R ):   1 1 √ √ 0 − 2 2   1 0 0 R = 1 1 √ √ 0 2 2 Exercise 3.38 Show that rotation matrices of the form   r 0 r 11 12   r r r 23 22 21 r r r 32 31 33 and φ as follows: can be parametrized using just two parameters θ   cos θ − sin θ 0   cos φ cos θ cos φ − sin φ sin θ . sin cos θ sin φ cos φ θ φ sin What should the range of values be for θ and φ ? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

151 Chapter 3. Rigid-Body Motions 133 α ˆz 0 ˆz 0 } { 0 ˆy 0 ˆz ˆx 3 0 } 3 { ˆy 3 ˆx 3 β ◦ 45 γ Figure 3.33: A three-degree-of-freedom wrist mechanism. Exercise 3.39 Figure 3.33 shows a three-dof wrist mechanism in its zero position (i.e., all joint angles are set to zero). (a) Express the tool-frame orientation R = ( α,β,γ ) as a product of three R 03 rotation matrices. (b) Find all possible angles ( α,β,γ ) for the two values of R given below. If 03 no solution exists, explain why this is so in terms of the analogy between SO (3) and a solid ball of radius π . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

152 134 3.8. Exercises   0 1 0   0 1 0 = (i) R . 03 1 0 0 − 2 1 2 [ˆ ω ] π/ √ √ e = (0 (ii) = , R , , where ˆ ω ). 03 5 5 Exercise 3.40 Refer to Appendix B. (a) Verify formulas (B.10) and (B.11) for the unit quaternion representation R ∈ SO (3). of a rotation R (b) Verify formula (B.12) for the rotation matrix representation of a unit 3 q ∈ . quaternion S (c) Verify the product rule for two unit quaternions. That is, given two unit 3 R,Q q,p corresponding respectively to the rotations ∈ ∈ quaternions S (3), find a formula for the unit quaternion representation of the product SO ∈ SO (3). RQ Exercise 3.41 The Cayley transform of Equation (B.18) in Appendix B can be generalized to higher orders as follows: k − k [ r ]) R ( = ( + [ r ]) I − . (3.99) I (a) For the case k = 2, show that the rotation R corresponding to r can be computed from the formula T 8 − r r 1 2 (3.100) . ] + ] r [ [ r − R I = 4 2 2 T T r r ) r ) (1 + (1 + r (b) Conversely, given a rotation matrix R , show that a vector r that satisfies Equation (3.100) can be obtained as θ = ω tan r , (3.101) ˆ − 4 ω is the unit vector along the axis of rotation for R , and where, as before, ˆ is the corresponding rotation angle. Is this solution unique? θ (c) Show that the angular velocity in the body frame obeys the following relation: ( ) 1 T T ̇ (1 r r = r ) I + 2[ r ] + 2 rr − (3.102) ω. 4 π that exists for the standard (d) Explain what happens to the singularity at Cayley–Rodrigues parameters. Discuss the relative advantages and dis- advantages of the modified Cayley–Rodrigues parameters, particularly for order k = 4 and higher. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

153 Chapter 3. Rigid-Body Motions 135 (e) Compare the number of arithmetic operations needed for multiplying two rotation matrices, two unit quaternions, or two Cayley–Rodrigues repre- sentations. Which requires the fewest arithmetic operations? 3 in your favorite program- Exercise 3.42 Rewrite the software for Chapter ming language. × 3 matrix is Exercise 3.43 Write a function that returns “true” if a given 3 of being a rotation matrix and “false” otherwise. It is up to you how within  to define the “distance” between a random 3 × 3 real matrix and the closest SO member of (3). If the function returns “true,” it should also return the “nearest” matrix in (3). See, for example, Exercise 3.10. SO Write a function that returns “true” if a given 4 4 matrix is Exercise 3.44 ×  of an element of within (3) and “false” otherwise. SE Exercise 3.45 Write a function that returns “true” if a given 3 × 3 matrix is within  of an element of so (3) and “false” otherwise. Exercise 3.46 Write a function that returns “true” if a given 4 4 matrix is ×  se (3) and “false” otherwise. within of an element of The primary purpose of the provided software is to be easy Exercise 3.47 to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness, nor does it do full error-checking on its inputs. Familiarize yourself with the whole code in your favorite language by reading the functions and their comments. This should help cement your understanding of the material in this chapter. Then: (a) Rewrite one function to do full error-checking on its input, and have the function return a recognizable error value if the function is called with an improper input (e.g., an argument to the function is not an element of (3), as expected). (3), SE (3), so (3), or se SO (b) Rewrite one function to improve its computational efficiency, perhaps by using what you know about properties of rotation or transformation ma- trices. (c) Can you reduce the numerical sensitivity of either of the matrix logarithm functions? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

154 136 3.8. Exercises Exercise 3.48 Use the provided software to write a program that allows the user to specify an initial configuration of a rigid body by , a screw axis specified T ˆ s,h } in the fixed frame { s } , and the total distance traveled along the by { q, [ S ] θ θ . The program should calculate the final configuration T = screw axis, T e 1 a distance θ , as well as the attained when the rigid body follows the screw S θ/ θ/ 2, and 3 θ/ 4. At the initial, intermediate, intermediate configurations at 4, { b } axes of the rigid and final configurations, the program should plot the S body. The program should also calculate the screw axis and the distance 1 following S that takes the rigid body from T to the origin and it should θ 1 1 1 S , . Test the program with q = (0 , 2 plot the screw axis 0), ˆ s = (0 , 0 , 1), h = 2, 1 θ π , and =   1 0 0 2   0 1 0 0   . = T   0 0 1 0 0 0 0 1 Exercise 3.49 In this chapter, we developed expressions for the matrix expo- (3) to (3) and elements nential for spatial motions mapping elements of SO so (3) to SE of se (3). Similarly, we developed algorithms for the matrix logarithm going the other direction. We could also develop matrix exponentials for planar motions, from so (2) SO (2) and from se (2) to to (2), as well as the matrix logarithms going SE from (2) to so (2) and SE (2) to se (2). For the SO (2) to SO (2) case there so is a single exponential coordinate. For the se (2) to SE (2) case there are three exponential coordinates, corresponding to a twist with three elements set to zero, V , 0 ,ω 0). ,v = (0 ,v , y z x For planar rotations and planar twists we could apply the matrix exponen- tials and logarithms that we derived for the spatial case by simply expressing the so (2), SO (2), se (2), and SE (2) elements as elements of so (3), SO (3), se (3), and (3). Instead, in this problem, write down explicitly the matrix expo- SE nential and logarithm for the so (2) to SO (2) case using a single exponential coordinate, and the matrix exponential and logarithm for the se (2) to SE (2) case using three exponential coordinates. Then provide software implementa- tions of each of the four functions in your favorite programming language, and provide execution logs that show that they function as expected. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

155 Chapter 4 Forward Kinematics of a robot refers to the calculation of the position The forward kinematics . Figure 4.1 and orientation of its end-effector frame from its joint coordinates θ illustrates the forward kinematics problem for a 3R planar open chain. The link with origin located at the , L lengths are , and L L . Choose a fixed frame { 0 } 3 2 1 4 } has been attached to { base joint as shown, and assume an end-effector frame ) and orientation of the x,y the tip of the third link. The Cartesian position ( φ θ ,θ ) are then given by end-effector frame as functions of the joint angles ( ,θ 1 2 3 = L + cos θ (4.1) x L , cos( θ ) + θ θ ) + L + cos( θ θ + 1 1 3 2 3 1 2 1 2 ) + = sin θ + L y sin( θ + θ L L sin( θ + θ + θ ) , (4.2) 2 1 2 1 3 3 1 2 1 φ = θ + θ (4.3) + θ . 3 1 2 If one is only interested in the ( x,y ) position of the end-effector, the robot’s x – y task space is then taken to be the -plane, and the forward kinematics would consist of Equations (4.1) and ( 4.2) only. If the end-effector’s position and orien- tation both matter, the forward kinematics would consist of the three equations (4.1)–(4.3). While the above analysis can be done using only basic trigonometry, it is not difficult to imagine that for more general spatial chains the analysis can become considerably more complicated. A more systematic method of deriving the forward kinematics might involve attaching reference frames to each link; in Figure 4.1 the three link reference frames are respectively labeled { 1 } , { 2 } , and } . The forward kinematics can then be written as a product of four 3 { homogeneous transformation matrices: = (4.4) T T T T , T 23 04 34 12 01 137

156 138 φ } 4 { L 3 y ) x, ( } { 3 θ 3 L 2 { } 2 L 1 θ { } 1 2 { } 0 θ 1 Figure 4.1: Forward kinematics of a 3R planar open chain. For each frame, the ˆx- and ˆy-axis is shown; the ˆz-axes are parallel and out of the page. where     sin cos θ 0 L θ − θ θ sin cos − 0 0 2 1 2 1 1     sin θ 0 θ 0 cos sin θ θ 0 0 cos 2 2 1 1     , , T = T = 12 01     0 1 0 0 0 1 0 0 1 0 0 0 0 1 0 0     L 1 0 0 θ sin θ 0 − L cos 3 2 3 3     0 0 1 0 cos θ 0 0 sin θ 3 3     . (4.5) , T = = T 34 23     0 0 1 0 0 1 0 0 1 0 0 0 0 1 0 0 T depends only on is constant and that each remaining T Observe that i 1 ,i 34 − the joint variable θ . i As an alternative to this approach, let us define M to be the position and orientation of frame 4 } when all joint angles are set to zero (the “home” or { “zero” position of the robot). Then   + L L + 1 0 0 L 1 2 3   0 0 1 0   = M , (4.6)   0 0 0 1 0 0 0 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

157 Chapter 4. Forward Kinematics 139 Now consider each revolute joint axis to be a zero-pitch screw axis. If and θ 1 are held at their zero position then the screw axis corresponding to rotating θ 2 frame as { } 0 about joint 3 can be expressed in the   0   0   [ ]   ω 1 3   . S = = 3   0 v 3     ( L + L − ) 2 1 0 4.1. You should be able to confirm this by simple visual inspection of Figure When the arm is stretched out straight to the right at its zero configuration, = 1 rad/s about imagine a turntable rotating with an angular velocity of ω 3 v of the point on the turntable at the the axis of joint 3. The linear velocity 3 { 0 } is in the − ˆy -direction at a rate of origin of L units/s. Algebraically, + L 2 1 0 = } ω − × q , , where q v is any point on the axis of joint 3 expressed in { 0 3 3 3 3 e.g., = ( L 0). q L , , 0 + 2 1 3 S The screw axis can be expressed in se (3) matrix form as 3   0 1 0 − 0 [ ]   v + L ) [ ω ] 1 0 0 − ( L 1 2   . [ S = ] = 3   0 0 0 0 0 0 0 0 0 0 Therefore, for any , the matrix exponential representation for screw motions θ 3 from the previous chapter allows us to write θ ] [ S 3 3 = 0). e M (for θ = = θ (4.7) T 04 2 1 Now, for θ = 0 and any fixed (but arbitrary) θ , rotation about joint 2 can be 1 3 viewed as applying a screw motion to the rigid (link 2)/(link 3) pair, i.e., θ ] [ S ] θ S [ 2 3 2 3 (4.8) e e T M (for θ = = 0), 04 1 where [ S ] and M are as defined previously, and 3   1 0 − 0 0   L − 0 1 0 1   S [ ] = . (4.9) 2   0 0 0 0 0 0 0 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

158 140 4.1. Product of Exponentials Formula θ Finally, keeping θ and fixed, rotation about joint 1 can be viewed as applying 3 2 a screw motion to the entire rigid three-link assembly. We can therefore write, θ ,θ for arbitrary values of ( ,θ ), 1 3 2 ] S θ [ [ S θ ] S ] θ [ 3 3 2 2 1 1 M, (4.10) e e T e = 04 where   0 1 0 0 −   1 0 0 0   (4.11) . ] = S [ 1   0 0 0 0 0 0 0 0 Thus the forward kinematics can be expressed as a product of matrix exponen- tials, each corresponding to a screw motion. Note that this latter derivation of the forward kinematics does not use any link reference frames; only { } and M 0 must be defined. In this chapter we consider the forward kinematics of general open chains. One widely used representation for the forward kinematics of open chains relies (D–H parameters), and this rep- on the Denavit–Hartenberg parameters resentation uses Equation (4.4). Another representation relies on the product of exponentials (PoE) formula, which corresponds to Equation (4.10). The advantage of the D–H representation is that it requires the minimum number of n -joint robot, it uses 3 n parameters to describe the robot’s kinematics: for an n numbers to describe the robot’s structure and numbers to describe the joint n numbers to de- values. The PoE representation is not minimal (it requires 6 scribe the screw axes, in addition to the n joint values), but it has advantages n over the D–H representation (e.g., no link frames are necessary) and it is our preferred choice of forward kinematics representation. The D–H representation, and its relationship to the PoE representation, is described in Appendix C. 4.1 Product of Exponentials Formula { } (e.g., To use the PoE formula, it is only necessary to assign a stationary frame s at the fixed base of the robot or anywhere else that is convenient for defining { a reference frame) and a frame } at the end-effector, described by M when b the robot is at its zero position. It is common to define a frame at each link, though, typically at the joint axis; these are needed for the D–H representation and they are useful for displaying a graphic rendering of a geometric model of the robot and for defining the mass properties of the link, which we will need 8. Thus when we are defining the kinematics of an n -joint starting in Chapter robot, we may either (1) minimally use the frames { s } and { b } if we are only May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

159 Chapter 4. Forward Kinematics 141 θ 1 θ n − 2 θ n − 1 θ n M [ S ] θ n n e M [ S θ ] S ] θ [ − n 1 n 1 n − n e M e [ S ] S [ ] θ θ ] [ S θ − 1 − 2 − n n − 1 n 2 n n n e e M e Figure 4.2: Illustration of the PoE formula for an n -link spatial open chain. interested in the kinematics, or (2) refer to { s } as frame { 0 } , use frames { i } for i = 1 (the frames for links i at joints i ), and use one more frame { n + 1 } ,...,n ) is { } ) at the end-effector. The frame { n (corresponding to } (i.e., { b } b + 1 fixed relative to n } , but it is at a more convenient location to represent the { { n + 1 } configuration of the end-effector. In some cases we dispense with frame and simply refer to { n } as the end-effector frame { b } . 4.1.1 First Formulation: Screw Axes in the Base Frame The key concept behind the PoE formula is to regard each joint as applying a screw motion to all the outward links. To illustrate this consider a general 4.2, consisting of n one-dof joints spatial open chain like the one shown in Figure that are connected serially. To apply the PoE formula, you must choose a fixed { base frame } and an end-effector frame { b } attached to the last link. Place the s robot in its zero position by setting all joint values to zero, with the direction of positive displacement (rotation for revolute joints, translation for prismatic M ∈ SE (3) denote the configuration of the joints) for each joint specified. Let end-effector frame relative to the fixed base frame when the robot is in its zero position. Now suppose that joint n is displaced to some joint value θ . The end- n May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

160 142 4.1. Product of Exponentials Formula then undergoes a displacement of the form M effector frame [ ] θ S n n (4.12) M, = T e SE (3) is the new configuration of the end-effector frame and S where = ∈ T n n ,v ( ) is the screw axis of joint ω as expressed in the fixed base frame. If joint n n 3 ω ∈ R is a n is revolute (corresponding to a screw motion of zero pitch) then n ; v − = unit vector in the positive direction of joint axis ω n × q any , with q n n n n n as written in coordinates in the fixed base frame; arbitrary point on joint axis 3 is a unit n is prismatic then ω is the joint angle. If joint = 0, v θ ∈ R and n n n represents the prismatic θ vector in the direction of positive translation, and n extension/retraction. If we assume that joint n − 1 is also allowed to vary then this has the effect n − 1 (and by extension to link n , since link of applying a screw motion to link is connected to link − 1 via joint n ). The end-effector frame thus undergoes n n a displacement of the form ( ) ] [ θ S [ S θ ] − n n 1 − 1 n n T = e e M (4.13) . θ Continuing with this reasoning and now allowing all the joints ( ,...,θ ) to n 1 vary, it follows that θ ] S [ θ [ S ] ] θ S [ n n − 1 1 n − 1 1 n e ) = θ e ( T e M. (4.14) ··· This is the product of exponentials formula describing the forward kinematics of an n -dof open chain. Specifically, we call Equation (4.14) the space form of the product of exponentials formula, referring to the fact that the screw axes are expressed in the fixed space frame. To summarize, to calculate the forward kinematics of an open chain using the space form of the PoE formula (4.14), we need the following elements: M ∈ SE (3) when the robot is at its home (a) the end-effector configuration position; (b) the screw axes S expressed in the fixed base frame, corresponding ,..., S n 1 to the joint motions when the robot is at its home position; (c) the joint variables θ ,...,θ . 1 n Unlike the D–H representation, no link reference frames need to be defined. Further advantages will come to light when we examine the velocity kinematics in the next chapter. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

161 Chapter 4. Forward Kinematics 143 θ 1 ˆz 1 ˆy 1 ˆz 0 L 1 ˆy ˆy 0 2 ˆz ˆx 2 1 ˆx 0 ˆx 2 θ 2 L 2 ˆy 3 ˆz 3 θ 3 ˆx 3 A 3R spatial open chain. Figure 4.3: Examples 4.1.2 We now derive the forward kinematics for some common spatial open chains using the PoE formula. Example 4.1 (3R spatial open chain) . Consider the 3R open chain of Fig- ure 4.3, shown in its home position (all joint variables set equal to zero). Choose { 0 } and end-effector frame { 3 } as indicated in the figure, and ex- the fixed frame press all vectors and homogeneous transformations in terms of the fixed frame. The forward kinematics has the form ] S [ S ] θ [ [ S θ ] θ 1 2 3 1 3 2 e e θ ( T M, e ) = where ∈ (3) is the end-effector frame configuration when the robot is in SE M M can be obtained as its zero position. By inspection   L 0 0 1 1   0 1 0 0   = M .   1 0 0 − L − 2 1 0 0 0 0 The screw axis 1) = ( ω , ,v S ) for joint axis 1 is then given by ω , = (0 1 1 1 1 and v 0) (the fixed frame origin (0,0,0) is a convenient choice for the = (0 , 0 , 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

162 144 4.1. Product of Exponentials Formula point lying on joint axis 1). To determine the screw axis S q for joint axis 2 1 , -direction, so that ω 2, observe that joint axis 2 points in the = (0 ˆy − 1 , 0). − 2 0 v = ( L ). Finally, to , 0 , 0), in which case Choose L = q ω − × q , = (0 , 0 − 2 2 1 1 2 2 , ω S determine the screw axis = (1 for joint axis 3, note that 0 , 0). Choosing 3 3 − = (0 , 0 , q L 0). ), it follows that v , = − ω L × q − = (0 , 3 3 2 2 3 3 In summary, we have the following 4 4 matrix representations for the three × , and S joint screw axes S S : , 3 2 1   0 − 1 0 0   1 0 0 0   [ , ] = S 1   0 0 0 0 0 0 0 0   0 1 − 0 0   0 0 0 0   , ] = [ S 2   L − 0 1 0 1 0 0 0 0   0 0 0 0   0 0 − 1 − L 2   . ] = S [ 3   0 1 0 0 0 0 0 0 It will be more convenient to list the screw axes in the following tabular form: i v ω i i (0 , 0 , 1) (0 , 0 , 0) 1 , (0 − 1 , 0) (0 , 0 , − L ) 2 1 3 (1 , 0 , 0) (0 ,L 0) , 2 Example 4.2 . For the robot in Figure 4.1, we expressed (3R planar open chain) M S (Equation (4.6)) and the screw axes the end-effector home configuration i as follows: ω v i i i (0 , 0 , 1 (0 , 0 , 0) 1) 2 (0 , 0 , 1) (0 , − L 0) , 1 , (0 , 0 , 1) (0 , − ( L 0) + L 3 ) 2 1 Since the motion is in the ˆx–ˆy-plane, we could equivalently write each screw axis S as a 3-vector ( ω ,v ,v ): x z y i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

163 Chapter 4. Forward Kinematics 145 L L L ˆz s ˆz b ˆy s ˆx ˆy s s } { ˆx b b } b { PoE forward kinematics for the 6R open chain. Figure 4.4: v i ω i i 1 (0 1 0) , 2 (0 , − L 1 ) 1 3 (0 , − ( L )) + L 1 1 2 and M as an element of SE (2):   1 0 + L L L + 1 2 3   0 0 1 = M . 0 0 1 In this case, the forward kinematics would use the simplified matrix exponential for planar motions (Exercise 3.49). Example 4.3 (6R spatial open chain) We now derive the forward kinematics . 4.4. Six-dof arms play an important role in of the 6R open chain of Figure robotics because they have the minimum number of joints that allows the end- effector to move a rigid body in all its degrees of freedom, subject only to limits on the robot’s workspace. For this reason, six-dof robot arms are sometimes called general purpose manipulators. The zero position and the direction of positive rotation for each joint axis are as shown in the figure. A fixed frame { s } and end-effector frame { b } are also assigned as shown. The end-effector frame in the zero position is then M   1 0 0 0   L 0 1 0 3   M = (4.15)   0 0 0 1 1 0 0 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

164 146 4.1. Product of Exponentials Formula ˆz s L 1 ˆx s ˆy s L 2 ˆz b ˆy b ˆx b The RRPRRR spatial open chain. Figure 4.5: = (0 , The screw axis for joint 1 is in the direction , 1). The most convenient ω 0 1 = (0 lying on joint axis 1 is the origin, so that v 0). The choice for point , 0 q , 1 1 , 1 , 0). ω screw axis for joint 2 is in the ˆy-direction of the fixed frame, so = (0 2 , = (0 Choosing q , 0), we have v 0). The screw axis for joint 3 is = (0 , 0 , 0 2 2 ω 0). = ( − 1 , in the direction , 0). Choosing q , = (0 , 0 , 0) leads to v 0 = (0 , 0 3 3 3 , ω 1 , 0 − 0). Choosing q = = ( The screw axis for joint 4 is in the direction 4 4 0) leads to v = (0 , 0 ,L ). The screw axis for joint 5 is in the direction ,L, (0 4 ). The screw ω − 1 , 0 , 0); choosing q L = (0 , 2 L, 0) leads to v 2 = (0 , 0 , = ( 5 5 5 axis for joint 6 is in the direction ω 0) leads = (0 , 1 , 0); choosing q , = (0 , 0 6 6 ), v , 0 , 0). In summary, the screw axes S to = ( ω 6, are as ,v ,..., = (0 i = 1 i i i 6 follows: i ω v i i (0 , 0 , 1 (0 , 0 , 0) 1) 2 , 1 , 0) (0 , 0 , 0) (0 0) 3 1 , 0 , − (0 , 0 , 0) ( 4 ( − 1 , 0 , 0) (0 , 0 ,L ) ) 5 1 , 0 , 0) (0 , 0 , 2 L − ( 6 , 1 , 0) (0 , 0 , 0) (0 Example 4.4 (An RRPRRR spatial open chain) . In this example we consider the six-degree-of-freedom RRPRRR spatial open chain of Figure 4.5. The end- effector frame in the zero position is given by   1 0 0 0   L + 0 1 0 L 1 2   = M .   0 0 0 1 0 0 0 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

165 Chapter 4. Forward Kinematics 147 S ω ,v ) are listed in the following table: The screw axes = ( i i i v i ω i i , 0 (0 , 0 1) 0) , , (0 1 2 , 0) (0 , 0 , 0) (1 , 0 , 0 , 0) (0 , 1 , 0) 3 (0 , (0 , 0) (0 , 0 , 0) 4 1 , , , 0) (0 , 0 (1 − L 5 ) 0 1 , 1 , 0) (0 , 0 , 0) (0 6 ω = 0 and Note that the third joint is prismatic, so that v is a unit vector in 3 3 the direction of positive translation. Example 4.5 . (Universal Robots’ UR5 6R robot arm) Universal Robots’ UR5 6R robot arm is shown in Figure 4.6. Each joint is directly driven by a brushless motor combined with 100 : 1 zero-backlash harmonic drive gearing, which greatly increases the torque available at the joint while reducing its maximum speed. Figure 4.6 shows the screw axes ,..., S when the robot is at its zero position. S 1 6 } in the zero position is given by b { The end-effector frame   − 1 0 0 L + L 1 2   0 0 1 W W + 1 2   = M .   H 0 − H 1 0 2 1 1 0 0 0 S = ( ω ,v ) are listed in the following table: The screw axes i i i ω i v i i (0 (0 0 , 1 , , 0 , 0) 1) 2 (0 , 1 , 0) ( − H 0) , 0 , 1 3 1 , 0) ( − H (0 , 0 ,L , ) 1 1 1 , 0) ( − H (0 , , ,L 4 + L ) 0 2 1 1 , 0 , − 1) ( − 5 (0 ,L 0) + L , W 2 1 1 6 , , 0) ( H (0 H 1 , 0 ,L + L ) − 2 2 1 1 = θ − π/ 2 and θ As an example of the forward kinematics, set = π/ 2, with 5 2 all other joint angles equal to zero. Then the configuration of the end-effector is [ S θ ] θ ] [ S S ] θ [ [ S θ ] θ ] [ S S ] θ [ 4 6 5 3 3 5 4 2 2 6 1 1 e e M T e e e ( θ ) = e 2 2 S π/ ] π/ 2 [ [ S ] − 5 2 I = e IM Ie 2 − S π/ ] π/ [ [ S ] 2 5 2 e = e M May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

166 148 4.1. Product of Exponentials Formula S 5 S 4 S 1 S 6 S 3 S 2 Figure 4.6: (Left) Universal Robots’ UR5 6R robot arm. (Right) Shown at its zero position. Positive rotations about the axes indicated are given by the usual right-hand rule. is the distance along the ˆy W -direction between the anti-parallel axes of joints 1 s W L = 109 mm, W = 89 mm, = 82 mm, L H = 425 mm, 1 and 5. = 392 mm, 1 2 1 2 1 = 95 mm. H 2 0 since e = I . Evaluating, we get     708 1 0 0 . 0 − 1 0 . 089 0 0     − 926 . 1 0 0 0 0 1 0 0 ] S 2 π/ [ − 2 ] S [ π/ 5 2     , = , e e =     0 1 0 0 . 0 089 0 1 0 0 0 0 1 0 0 1 0 where the linear units are meters, and   095 . 1 0 0 − 0   109 . 0 0 1 0 π/ ] 2 [ 2 π/ ] S S [ − 2 5   T ( θ ) = e M e =   0 1 0 988 0 . 0 1 0 0 4.7. as shown in Figure Second Formulation: Screw Axes in the End-Effector 4.1.3 Frame − 1 M PM − 1 P The matrix identity e M e = M (Proposition 3.10) can also be ex- − 1 M PM P e = Me pressed as M . Beginning with the rightmost term of the pre- viously derived product of exponentials formula, if we repeatedly apply this May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

167 Chapter 4. Forward Kinematics 149 ˆz b ˆx b θ 2 = π/ 5 ˆz b ˆx b ˆz s ˆx s = π/ 2 − θ 2 Figure 4.7: (Left) The UR5 at its home position, with the axes of joints 2 and 5 , = ( indicated. (Right) The UR5 at joint angles ,...,θ θ ) = (0 , − π/ 2 θ 0 , 0 ,π/ 2 , 0). 1 6 identity then after iterations we obtain n S ] θ [ [ S ] θ n 1 n 1 ) = e T θ ··· e M ( 1 − S Mθ ] θ ] S [ M [ n n 1 1 ··· = Me e 1 − − 1 S ] θ [ M Mθ M ] [ [ S S Mθ ] 1 n 1 − n n − 1 1 n ··· e Me e = − 1 − 1 − 1 ] ] Mθ S [ M M Mθ Mθ [ S [ M S ] 1 1 − n − 1 n 1 n n ··· e e Me = B θ ] θ ] [ [ B B [ θ ] 1 n − 1 − n n 1 1 n e = Me e ··· , (4.16) − 1 − 1 S where each [ . [ S ,...,n ] M , i.e., B = 1 = [Ad i ] is given by M ] B , i i i i M Equation (4.16) is an alternative form of the product of exponentials formula, representing the joint axes as screw axes B in the end-effector (body) frame i when the robot is at its zero position. We call Equation (4.16) the body form of the product of exponentials formula. It is worth thinking about the order of the transformations expressed in the space-form PoE formula (Equation (4.14)) and in the body-form formula May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

168 150 4.1. Product of Exponentials Formula M is first transformed by the most distal (Equation (4.16)). In the space form, joint, progressively moving inward to more proximal joints. Note that the fixed space-frame representation of the screw axis for a more proximal joint is not affected by the joint displacement at a distal joint (e.g., joint 3’s displacement does not affect joint 2’s screw axis representation in the space frame). In the is first transformed by the first joint, progressively moving out- body form, M ward to more distal joints. The body-frame representation of the screw axis for a more distal joint is not affected by the joint displacement at a proximal joint (e.g., joint 2’s displacement does not affect joint 3’s screw axis representation in the body frame.) Therefore, it makes sense that we need to determine the is unaffected by more distal S screw axes only at the robot’s zero position: any i transformations, and any B is unaffected by more proximal transformations. i . (6R spatial open chain) Example 4.6 We now express the forward kinematics of the 6R open chain of Figure 4.4 in the second form, ] B [ θ [ B ] ] θ B [ θ 1 1 2 6 6 2 e ) = θ Me ( T e . ··· Assume the same fixed and end-effector frames and zero position as found pre- viously; is still the same as in Equation (4.15), obtained as the end-effector M frame as seen from the fixed frame with the chain in its zero position. The screw axis for each joint axis, expressed with respect to the end-effector frame in its zero position, is given in the following table: ω v i i i (0 , 0 , 1 ( − 3 L, 0 , 0) 1) 2 (0 , 1 , 0) (0 , 0 , 0) 3 ( 1 , 0 , 0) (0 , 0 , − 3 L ) − , 4 1 , 0 , 0) (0 − 0 , − 2 L ) ( 5 ( − 1 , 0 , 0) (0 , 0 , − L ) 0 6 1 , 0) (0 , , , 0) (0 Example 4.7 (Barrett Technology’s WAM 7R robot arm) . Barrett Technol- ogy’s WAM 7R robot arm is shown in Figure 4.8. The extra (seventh) joint means that the robot is for the task of positioning its end-effector redundant frame in SE (3); in general, for a given end-effector configuration in the robot’s workspace, there is a one-dimensional set of joint variables in the robot’s seven- dimensional joint space that achieves that configuration. This extra degree of freedom can be used for obstacle avoidance or to optimize some objective func- tion such as minimizing the motor power needed to hold the end-effector at that configuration. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

169 Chapter 4. Forward Kinematics 151 ˆz b ˆx b = 60 mm L 3 Wrist J5,J6,J7 L = 300 mm 2 = 45 mm W 1 Elbow J4 L = 550 mm 1 ˆz s Shoulder J1,J2,J3 ˆx s Barrett Technology’s WAM 7R robot arm at its zero configuration (right). Figure 4.8: At the zero configuration, axes 1, 3, 5, and 7 are along ˆz and axes 2, 4, and 6 are s aligned with ˆy out of the page. Positive rotations are given by the right-hand rule. s Axes 1, 2, and 3 intersect at the origin of s } and axes 5, 6, and 7 intersect at a point { { } 60mm from . The zero configuration is singular, as discussed in Section 5.3. b Also, some joints of the WAM are driven by motors placed at the base of the robot, reducing the robot’s moving mass. Torques are transferred from the motors to the joints by cables winding around drums at the joints and motors. Because the moving mass is reduced, the motor torque requirements are decreased, allowing low (cable) gear ratios and high speeds. This design is in contrast with that of the UR5, where the motor and harmonic drive gearing for each joint are directly at the joint. 4.8 illustrates the WAM’s end-effector frame screw axes B ,..., B Figure 1 7 when the robot is at its zero position. The end-effector frame { b } in the zero May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

170 152 4.2. The Universal Robot Description Format position is given by   1 0 0 0   0 0 1 0   M . =   + 0 0 1 L L L + 1 3 2 1 0 0 0 The screw axes = ( ω B ,v ) are listed in the following table: i i i ω v i i i , 0 , 1 (0 , 0 , 0) (0 1) (0 , 1 2 0) ( L 0) + L , + L 0 , , 3 1 2 , (0 , 1) (0 0 0 , 0) 3 , (0 , 1 , 0) ( L ) 4 L ,W , 0 + 3 2 1 (0 (0 0 5 1) , , 0 , 0) , 6 (0 , 1 , 0) ( L 0) , 0 , 3 0 0 , 1) (0 , , , 0) 7 (0 ◦ ◦ ◦ Figure 4.9 shows the WAM arm with , θ and = θ 45 − , θ = 45 = − 90 6 2 4 all other joint angles equal to zero, giving   3157 1 0 − 0 0 .   0 0 1 0 [ B [ ] π/ 4 π/ − 4 B − ] π/ 2 [ ] B 2 6 4   = e e . ( θ Me T ) =   1 0 6571 . 0 0 1 0 0 0 The Universal Robot Description Format 4.2 The (URDF) is an XML (eXtensible Universal Robot Description Format Markup Language) file format used by the Robot Operating System (ROS) to describe the kinematics, inertial properties, and link geometry of robots. A URDF file describes the joints and links of a robot: • Joints. Joints connect two links: a parent link and a child link. A few of the possible joint types include prismatic, revolute (including joint limits), continuous (revolute without joint limits), and fixed (a virtual joint that does not permit any motion). Each joint has an origin frame that defines the position and orientation of the child link frame relative to the parent link frame when the joint variable is zero. The origin is on the joint’s axis. Each joint has an axis 3-vector, a unit vector expressed in the child link’s frame, in the direction of positive rotation for a revolute joint or positive translation for a prismatic joint. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

171 Chapter 4. Forward Kinematics 153 ˆz b ˆx b ˆx b θ = − π/ 2 6 ˆz b = 4 − π/ θ 4 ˆz s ˆx s π/ 4 = θ 2 Figure 4.9: (Left) The WAM at its home configuration, with the axes of joints 2, 4, and 6 indicated. (Right) The WAM at θ = ( θ 0). ,...,θ , ) = (0 ,π/ 4 , 0 , − π/ 4 , 0 , − π/ 2 7 1 Links. • While the joints fully describe the kinematics of a robot, the links define its mass properties. These start to be needed in Chapter 8, when we begin to study the dynamics of robots. The elements of a link include its ; an origin frame that defines the position and orientation of a mass frame at the link’s center of mass relative to the link’s joint frame described above; and an inertia matrix, relative to the link’s center of mass frame, specified by the six elements on or above the diagonal. (As we will see × 3 symmetric in Chapter 8, the inertia matrix for a rigid body is a 3 positive-definite matrix. Since the inertia matrix is symmetric, it is only necessary to define the terms on and above the diagonal.) Note that most links have two frames rigidly attached: a first frame at the joint (defined by the joint element that connects the link to its parent) and a second frame at the link’s center of mass (defined by the link element). A URDF file can represent any robot with a tree structure. This includes serial-chain robot arms and robot hands, but not a Stewart platform or other May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

172 154 4.2. The Universal Robot Description Format J5 L5 L2 J1 L1 J3 J2 L4 L3 base link, J4 L0 parent: J1’s parent link, L0 J1’s child link, L1 child: L0 – x the origin: z – y and roll–pitch–yaw coords J1 of the L1 frame relative to the L1 L0 frame when J1 is zero – unit vector along the y z – x axis: the J2 rotation axis in the L1 frame L2 J3 J5 L5 L3 L5’s mass mass: J4 the x – y – z and roll–pitch– origin: yaw coords of a frame at the center of L4 mass of L5, relative to the L5 frame six unique entries of inertia inertia: matrix in the origin frame Figure 4.10: A five-link robot represented as a tree, where the nodes of the tree are the links and the edges of the tree are the joints. mechanisms with closed loops. An example of a robot with a tree structure is shown in Figure 4.10. The orientation of a frame { } relative to a frame { a } is represented using b -axis; then a pitch roll–pitch–yaw coordinates: first, a roll about the fixed ˆx a -axis. -axis; then a yaw about the fixed ˆz about the fixed ˆy a a 4.11) are The kinematics and mass properties of the UR5 robot arm (Figure defined in the URDF file below, which demonstrates the syntax of the joint’s parent , child elements ( origin , and axis ) and the link’s elements ( mass , , origin , and inertia ). A URDF requires a frame defined at every joint, so we { define frames } to { 6 } in addition to the fixed base frame 1 0 } (i.e., { s } ) and { the end-effector frame { 7 } (i.e., { b } ). Figure 4.11 gives the extra information needed to fully write the URDF. Although the joint types in the URDF are defined as “continuous,” the UR5 joints do in fact have joint limits; they are omitted here for simplicity. The mass May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

173 Chapter 4. Forward Kinematics 155 { 4 } ˆy { 5 } ˆx ˆy 3 } { 6 { } ˆz { b } 93 mm 4 } { 392.25 mm + 1 } { y + } 5 { ˆx 94.65 mm { 2 } x z − { s } { } 3 } { 6 ˆy } b { ˆy 119.7 mm 82.3 mm ˆx y − y + 425 mm + y { 1 } x 135.85 mm + 89.159 mm z + { } 2 { } s Figure 4.11: The orientations of the frames { s } (also called { 0 } ), { b } (also called { 7 ), and { 1 } through { 6 } are illustrated on the translucent UR5. The frames { s } } } { } are aligned with each other; frames { 2 1 and { 3 } are aligned with each other; and and frames { 4 } , { 5 } , and { 6 } are aligned with each other. Therefore, only the axes are labeled. Just below the image of the robot is a of frames s } , { 2 } , { 4 } , and { b } { skeleton indicating how the frames are offset from each other, including distances and { s } frame). directions (expressed in the and inertial properties listed here are not exact. The UR5 URDF file (kinematics and inertial properties only). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

174 156 4.2. The Universal Robot Description Format May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

175 Chapter 4. Forward Kinematics 157 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

176 158 4.3. Summary Beyond the properties described above, a URDF can describe other proper- ties of a robot, such as its visual appearance (including geometric models of the links) as well as simplified representations of link geometries that can be used for collision detection in motion planning algorithms. Summary 4.3 • } and a reference Given an open chain with a fixed reference frame { s b } attached to some point on its last link – this frame is denoted frame { ( the end-effector frame – the forward kinematics is the mapping ) from T θ { to the position and orientation of b θ in { s } . the joint values } • In the Denavit–Hartenberg representation the forward kinematics of an open chain is described in terms of the relative displacements between reference frames attached to each link. If the link frames are sequentially { 0 } ,..., { n + 1 } , where { 0 } is the fixed frame { s } labeled { i } is a frame , attached to link at joint i (with i = 1 i ), and { n + 1 } is the end- ,...,n effector frame { b } then the forward kinematics is expressed as ( T T ( θ ) = T ) ( θ θ ) ··· T n 0 1 ,n ,n 1 n 01 +1 n,n +1 − θ denotes the joint i variable and T indicates the (fixed) con- where i n,n +1 figuration of the end-effector frame in { n } . If the end-effector frame { b } is chosen to be coincident with } then we can dispense with the frame n { n + 1 } . { The Denavit–Hartenberg convention requires that reference frames as- • signed to each link obey a strict convention (see Appendix C). Follow- ing this convention, the link frame transformation T between link − 1 ,i i } frames i − 1 } and { i { can be parametrized using only four parameters, the Denavit–Hartenberg parameters. Three of these parameters describe the kinematic structure, while the fourth is the joint value. Four numbers is the minimum needed to represent the displacement between two link frames. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

177 Chapter 4. Forward Kinematics 159 The forward kinematics can also be expressed as the following product of • exponentials (the space form), ] θ S [ [ S ] θ n n 1 1 M, e ··· θ e T ( ) = = ( ω ,v ) denotes the screw axis associated with positive motion where S i i i i along joint { s } coordinates, θ i is the joint- expressed in fixed-frame i M SE (3) denotes the position and orientation of the end- variable, and ∈ effector frame { b } when the robot is in its zero position. It is not necessary M to define individual link frames; it is only necessary to define and the screw axes . ,..., S S 1 n • The product of exponentials formula can also be written in the equivalent body form, θ ] B [ θ [ B ] 1 n n 1 ) = θ Me ( T ··· , e 1 − = [Ad ) is the screw axis cor- B where ] S ,v , i ω ,...,n ; B = ( = 1 i i i i i M responding to joint axis i , expressed in { b } , with the robot in its zero position. • The Universal Robot Description Format (URDF) is a file format used by the Robot Operating System and other software for representing the kinematics, inertial properties, visual properties, and other information for general tree-like robot mechanisms, including serial chains. A URDF file includes descriptions of joints, which connect a parent link and a child link and fully specify the kinematics of the robot, as well as descriptions of links, which specify its inertial properties. Software 4.4 Software functions associated with this chapter are listed in MATLAB format below. T = FKinBody(M,Blist,thetalist) Computes the end-effector frame given the zero position of the end-effector M , the list of joint screws Blist expressed in the end-effector frame, and the list of joint values . thetalist T = FKinSpace(M,Slist,thetalist) Computes the end-effector frame given the zero position of the end-effector M , the list of joint screws Slist expressed in the fixed-space frame, and the list of joint values thetalist . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

178 160 4.5. Notes and References Notes and References 4.5 The literature on robot kinematics is quite extensive, and with very few excep- tions most approaches are based on the Denavit–Hartenberg (D–H) parameters originally presented in [34] and summarized in Appendix C. Our approach is based on the Product of Exponentials (PoE) formula first presented by Brockett in [20]. Computational aspects of the PoE formula are discussed in [132]. Appendix C also elucidates in some detail the many advantages of the PoE formula over the D–H parameters, e.g., the elimination of link reference frames, the uniform treatment of revolute and prismatic joints, and the intuitive geo- metric interpretation of the joint axes as screws. These advantages more than offset the lone advantage of the D–H parameters, namely that they constitute a minimal set. Moreover, it should be noted that when using D–H parameters, there are differing conventions for assigning link frames, e.g., some methods align the joint axis with the ˆx-axis rather than the ˆz-axis of the link frame as we have done. Both the link frames and the accompanying D–H parameters need to be specified together in order to have a complete description of the robot’s forward kinematics. In summary, unless using a minimal set of parameters to represent a joint’s spatial motion is critical, there is no compelling reason to prefer the D–H pa- rameters over the PoE formula. In the next chapter, an even stronger case can be made for preferring the PoE formula to model the forward kinematics. Exercises 4.6 Familiarize yourself with the functions FKinBody and FKinSpace Exercise 4.1 in your favorite programming language. Can you make these functions more computationally efficient? If so, indicate how. If not, explain why not. The RRRP SCARA robot of Figure 4.12 is shown in its zero Exercise 4.2 position. Determine the end-effector zero position configuration M , the screw axes S = 1 and the joint in { 0 } , and the screw axes B ` in { b } . For ` = = ` 1 i 2 i 0 variable values θ = (0 ,π/ 2 , − π/ 2 , 1), use both the FKinSpace and the FKinBody functions to find the end-effector configuration ∈ SE (3). Confirm that they T agree with each other. Exercise 4.3 Determine the end-effector frame screw axes B for the 3R robot i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

179 Chapter 4. Forward Kinematics 161 θ θ 2 3 θ 1 θ 4 ˆz b ˆy b b } { ˆx b { 0 } ˆz 0  0 ˆy 0 ˆx 0   2 1 Figure 4.12: An RRRP SCARA robot for performing pick-and-place operations. in Figure 4.3. Determine the end-effector frame screw axes Exercise 4.4 for the RRPRRR B i 4.5. robot in Figure Exercise 4.5 Determine the end-effector frame screw axes for the UR5 B i robot in Figure 4.6. Exercise 4.6 Determine the space frame screw axes S for the WAM robot in i Figure 4.8. The PRRRRR spatial open chain of Figure is shown in its Exercise 4.7 4.13 zero position. Determine the end-effector zero position configuration M , the S . in { screw axes } , and the screw axes B } in { b 0 i i Exercise 4.8 The spatial RRRRPR open chain of Figure 4.14 is shown in its zero position, with fixed and end-effector frames chosen as indicated. Determine the end-effector zero position configuration , the screw axes S , and M { 0 } in i the screw axes B . in { b } i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

180 162 4.6. Exercises L L L L 2 3 4 1 θ θ θ 3 4 5 ˆz b ˆy b ˆx θ b 6 θ 2 b } { h ˆz 0 0 { } ˆy 0 ˆx 0 θ 1 A PRRRRR spatial open chain at its zero configuration. Figure 4.13: L 3 L 4 θ 3 L 2 L 2 θ 4 θ 5 θ 1 L 5 θ 6 θ 2 L 1 L 6 ˆx 0 ˆx b ˆz 0 ˆz b ˆy ˆy b 0 { b } } { 0 A spatial RRRRPR open chain. Figure 4.14: The spatial RRPPRR open chain of Figure Exercise 4.9 is shown in its 4.15 zero position. Determine the end-effector zero position configuration M , the screw axes S . in { 0 } , and the screw axes B } in { b i i Exercise 4.10 The URRPR spatial open chain of Figure 4.16 is shown in its zero position. Determine the end-effector zero position configuration M , the screw axes S . in { 0 } , and the screw axes B } in { b i i Exercise 4.11 The spatial RPRRR open chain of Figure 4.17 is shown in its May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

181 Chapter 4. Forward Kinematics 163 ˆz 0 0 } { ˆy 0 ˆx 0 θ 1 L L L θ 4 θ 5 L θ 6 L L ˆx b ˆy L b ˆz b θ 3 θ { } b 2 Figure 4.15: A spatial RRPPRR open chain with prescribed fixed and end-effector frames. M , the zero position. Determine the end-effector zero position configuration S . in { 0 } , and the screw axes B screw axes in { b } i i The RRPRRR spatial open chain of Figure 4.18 is shown in Exercise 4.12 its zero position (all joints lie on the same plane). Determine the end-effector { , the screw axes zero position configuration B in M 0 } , and the screw axes S i i in { b } . Setting θ . = π and all other joint variables to zero, find T T and 06 60 5 The spatial RRRPRR open chain of Figure is shown in Exercise 4.13 4.19 its zero position. Determine the end-effector zero position configuration M , the S . in { 0 } , and the screw axes B } screw axes { b in i i The RPH robot of Figure 4.20 is shown in its zero position. Exercise 4.14 M , the screw axes S in Determine the end-effector zero position configuration i { s } , and the screw axes B FKinBody in { b } . Use both the FKinSpace and the i functions to find the end-effector configuration ∈ SE (3) when θ = ( π/ 2 T 3 ,π ). , Confirm that the results agree. Exercise 4.15 The HRR robot in Figure 4.21 is shown in its zero position. Determine the end-effector zero position configuration M , the screw axes S in i { 0 } , and the screw axes B . in { b } i Exercise 4.16 The forward kinematics of a four-dof open chain in its zero May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

182 164 4.6. Exercises √ 3 L L 2 L √ L 3 θ 3 θ 4 ˆx b L θ 5 L θ 6 ˆy ˆz b b b } { θ θ 1 2 ˆx 0 } 0 { ˆy 0 ˆz 0 Figure 4.16: A URRPR spatial open-chain robot. position is written in the following exponential form: ] A [ θ [ A ] ] θ A [ [ A θ ] θ 2 4 4 1 3 1 2 3 e ) = θ e ( T Me . e Suppose that the manipulator’s zero position is redefined as follows: ( θ . ,θ ) ,θ ,α ,θ ,α ) = ( α ,α 1 2 3 3 4 2 1 4 ′ θ the forward kinematics can then be written = Defining , − α θ ,i = 1 ,..., 4 i i i ′ ′ ′ ′ ′ ′ ′ ′ A ] θ ] [ A ′ ] θ A [ ′ ] θ ′ ′ [ ′ θ [ A 2 4 2 4 1 1 3 3 e θ ( ,θ e M e ,θ e ) = T ,θ . 3 4 2 04 1 ′ ′ . A Find and each of the M i Exercise 4.17 Figure 4.22 shows a snake robot with end-effectors at each end. Reference frames { b are attached to the two end-effectors, as shown. } and { b } 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

183 Chapter 4. Forward Kinematics 165 θ 3 } 0 { } { b ˆy b ˆz ˆy θ b 0 2 ˆz 0 ˆx ˆx b 0 θ θ 1 5 1 θ 4 1 1 1 Figure 4.17: An RPRRR spatial open chain. LL LL L L θ 3 θ θ 4 2 L θ 5 θ 1 ◦ 45 ˆz 0 L } { 0 ˆy 0 ˆz b ˆx 0 θ L 6 b { } ˆy b ˆx b Figure 4.18: An RRPRRR spatial open chain. (a) Suppose that end-effector 1 is grasping a tree (which can be thought of as “ground”) and end-effector 2 is free to move. Assume that the robot ∈ (3) can be expressed in the is in its zero configuration. Then SE T b b 2 1 following product of exponentials form: θ ] [ S [ ] θ θ S [ S ] 2 1 2 1 5 5 T M. ··· e = e e b b 2 1 Find S . , S M , and 5 3 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

184 166 4.6. Exercises θ 5 θ 4 θ 2 1 θ 3 θ θ 1 6 ˆx b ◦ { b } ˆy 45 b 1 ˆz 0 ˆz b ˆy 0 { } 0 ˆx 0 111 1 Figure 4.19: A spatial RRRPRR open chain with prescribed fixed and end-effector frames. L 2 θ θ 1 3 pitch . h 1 m/rad θ =0 2 ˆz s ˆx b L L 3 1 } s { ˆy b ˆy b { } s ˆx s ˆz b L 0 Figure 4.20: An RPH open chain shown at its zero position. All arrows along/about the joint axes are drawn in the positive direction (i.e., in the direction of increasing joint value). The pitch of the screw joint is 0 . 1 m/rad, i.e., it advances linearly by 0 1 m for every radian rotated. The link lengths are . = 4, L = 2, and = 3, L L 1 0 2 L = 1 (figure not drawn to scale). 3 (b) Now suppose that end-effector 2 is rigidly grasping a tree and end-effector T (3) can be expressed in the following 1 is free to move. Then SE ∈ b b 1 2 product of exponentials form: ] A [ [ A ] θ A [ A θ ] θ ] [ A ] θ [ θ 1 4 5 3 1 2 3 2 5 4 T Ne e e = e . e b b 1 2 A Find , A , and N . 4 2 Exercise 4.18 The two identical PUPR open chains of Figure 4.23 are shown May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

185 Chapter 4. Forward Kinematics 167 θ 1 L 2 L 4 ˆz 0 { 0 } θ ˆy ˆx 0 2 0 L 3 L 1 ˆz b θ 3 ˆx ˆy b b b } { HRR robot. The pitch of the screw joint is denoted by h . Figure 4.21: in their zero position. { A } and end-effector frame { a (a) In terms of the given fixed frame , the } forward kinematics for the robot on the left (robot A) can be expressed in the following product of exponentials form: S θ ] [ S [ ] θ θ ] [ S 2 1 2 1 5 5 T . ··· e = e M e Aa a . S and S Find 2 4 (b) Suppose that the end-effector of robot A is inserted into the end-effector of robot B in such a way that the origins of the end-effectors coincide; the two robots then form a single-loop closed chain. Then the configuration space of the single-loop closed chain can be expressed in the form θ ] S − [ B [ ] φ θ ] − [ B S ] φ [ θ − [ B ] ] φ S [ − [ B θ ] φ ] S − [ B [ ] φ θ ] [ S 3 1 1 5 2 2 2 2 5 3 1 3 5 3 4 4 4 1 5 4 e e e e e e e = M e e e M ∈ SE (3) and , = ( ω for some constant ,v ), for i = 1 ,..., 5. Find B B i i 3 i n × n A − 1 − A ) , and M , ( e . (Hint: Given any ). A ∈ = e R B 5 Exercise 4.19 The RRPRR spatial open chain of Figure 4.24 is shown in its zero position. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

186 168 4.6. Exercises θ 4 Tree ˆy LL L } b { 2 ˆx ˆz θ θ 3 5 L End-effector2 Tree θ θ 2 1 ˆz ˆy L b } { 1 ˆx L End-effector1 Figure 4.22: Snake robot. (a) The forward kinematics can be expressed in the form ] A [ θ [ θ A ] ] θ A [ 5 1 2 1 2 5 . M ··· M e e M = e T sb 2 1 5 , M ,M C may be helpful.) , Find . (Hint: Appendix A and A 2 3 2 3 (b) Expressing the forward kinematics in the form θ ] S [ [ S θ ] θ ] S [ 2 1 2 5 1 5 e = e ··· T e M, sb , and S A ,..., S find in terms of the quantities M ,..., ,...,M A M 5 5 1 1 5 1 appearing in (a). The spatial PRRPRR open chain of Figure 4.25 is shown in its Exercise 4.20 zero position, with space and end-effector frames chosen as indicated. Derive its forward kinematics in the form θ ] S [ θ [ S ] ] θ S [ [ S θ ] θ ] S [ S [ ] θ 1 3 4 2 2 4 3 5 1 5 6 6 e e e Me = , e e T 0 n May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

187 Chapter 4. Forward Kinematics 169 L 2 φ 3 φ ˆz 5 b φ 4 ˆx b } b { ˆy b L 2 φ 2 θ ˆz 5 L ˆy a 1 a θ 4 φ 1 ˆx a ˆz θ B 3 a } { ˆx B θ 2 L 1 L 4 } { ˆy B B θ 1 L 3 ˆz A ˆy A ˆx A A } { Figure 4.23: Two PUPR open chains. M ∈ SE where (3). Exercise 4.21 C.) For each T ∈ (Refer to Appendix (3) below, find, if SE they exist, values for the four parameters ( α,a,d,φ ) that satisfy T = Rot(ˆx ,α ) Trans(ˆx ,a ) Trans(ˆz ,d ) Rot(ˆz ,φ ) .   0 1 1 3   1 0 0 0   (a) T . =   0 1 0 1 0 0 0 1   β 0 β sin cos 1   β − cos sin 0 0 β   . = (b) T   2 − 1 − 0 0 0 0 1 0   1 − 0 0 − 1   0 0 − 1 0   T (c) = .   1 0 2 0 1 0 0 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

188 4.6. Exercises 170 L L ˆx 3 ˆz 3 ˆy 3 θ 3 ˆz 2 ˆy 2 θ 1 θ L 4 θ ˆx ˆz 2 2 1 ˆy 1 ˆx 1 L L ˆz 4 ˆy ˆx 4 5 ˆy 0 ˆx 4 ˆx 0 ˆz 5 ˆz ˆy 0 5 θ { b s } { } 5 A spatial RRPRR open chain. Figure 4.24: ˆx 0 L 0 } { ˆz 0 θ 1 ˆy 0 θ 2 L θ 3 L L { } b ˆy θ b 4 θ θ 6 5 L ˆz b L L L ˆx b L Figure 4.25: A spatial PRRPRR open chain. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

189 Chapter 5 Velocity Kinematics and Statics In the previous chapter we saw how to calculate the robot end-effector frame’s position and orientation for a given set of joint positions. In this chapter we examine the related problem of calculating the twist of the end-effector of an open chain from a given set of joint positions and velocities. 6 V ∈ R Before we reach the representation of the end-effector twist as , start- ing in Section 5.1, let us consider the case where the end-effector configuration m is represented by a minimal set of coordinates x and the velocity is given ∈ R m x = dx/dt ∈ R . In this case, the forward kinematics can be written as by ̇ ( t ) = f ( θ ( t )) , x n θ R is a set of joint variables. By the chain rule, the time derivative where ∈ at time t is ∂f ( θ ) dθ ( t ) ) θ ∂f ( ̇ x = ̇ = θ ∂θ ∂θ dt ̇ ( θ ) = θ, J m × n θ ) ∈ R where J ( is called the Jacobian . The Jacobian matrix represents ̇ , and it to the joint velocity the linear sensitivity of the end-effector velocity ̇ θ x is a function of the joint variables θ . To provide a concrete example, consider a 2R planar open chain (left-hand 171

190 172 ) θ ( J 1 L 2 θ 2 ˆx J ) θ ( 2 2 θ ) ( − J 2 L 1 θ 1 ˆx ) − ( θ J 1 1 (Left) A 2R robot arm. (Right) Columns 1 and 2 of the Jacobian Figure 5.1: ̇ ̇ ̇ θ correspond to the endpoint velocity when = 0) and when = 1 (and θ = 1 (and θ 2 1 2 ̇ = 0), respectively. θ 1 side of Figure 5.1) with forward kinematics given by cos( L cos θ x + L ) = θ θ + 1 2 2 1 1 1 x L = sin θ . + L ) sin( θ θ + 1 2 2 1 1 2 Differentiating both sides with respect to time yields ̇ ̇ ̇ − L x = ̇ θ sin ) − L θ ( θ θ + + θ θ ) sin( 1 1 2 1 2 2 1 1 1 ̇ ̇ ̇ ̇ = L x ) cos θ θ + L + ( , θ θ + ) cos( θ θ 1 2 2 1 2 1 1 2 1 ̇ x J ( θ ) θ : which can be rearranged into an equation of the form ̇ = ][ [ [ ] ] ̇ x − L sin θ − L ̇ sin( θ + θ ) − L sin( θ + θ ) θ 1 2 1 1 1 1 2 2 1 2 = (5.1) . ̇ θ + θ ) L cos( θ + θ ) L θ x L cos( cos ̇ + θ 2 1 1 1 2 2 2 2 1 2 J ( θ ) as J J ( θ ) and Writing the two columns of as ( θ ), and the tip velocity ̇ x 2 1 , Equation (5.1) becomes v tip ̇ ̇ J (5.2) ( θ ) v θ . + J θ ( θ ) = 2 1 1 2 tip J ( ) are not collinear, it is possible to generate a tip ) and J As long as ( θ θ 2 1 velocity v in any arbitrary direction in the x -plane by choosing appropri- – x 2 1 tip ̇ ̇ ( J θ θ ) depend on the joint values . Since ate joint velocities θ and θ ) and J ( 2 1 2 1 θ ) and θ θ , one may ask whether there are any configurations at which J ( 1 2 1 ◦ is 0 θ ) become collinear. For our example the answer is yes: if and J ( θ or 2 2 ◦ then, regardless of the value of θ , J ) will be collinear and ( θ ) and J ( θ 180 1 1 2 the Jacobian ( θ ) becomes a singular matrix. Such configurations are therefore J called singularities ; they are characterized by a situation where the robot tip is unable to generate velocities in certain directions. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

191 Chapter 5. Velocity Kinematics and Statics 173 A ̇ x ̇ θ 2 2 B A D ) θ ( J ̇ ̇ x θ 1 1 B D C C Mapping the set of possible joint velocities, represented as a square in the Figure 5.2: ̇ ̇ – θ space, through the Jacobian to find the parallelogram of possible end-effector θ 1 2 velocities. The extreme points A, B, C, and D in the joint velocity space map to the extreme points A, B, C, and D in the end-effector velocity space. L = L = 1 and consider the robot at two different Now let’s substitute 1 2 θ = (0 ,π/ nonsingular postures: θ = (0 , 3 π/ 4). The Jacobians J ( θ ) at 4) and these two configurations are ] ([ [ ]) ([ ]) ] [ − 0 71 − 0 . 71 71 . 0 − 71 . 0 − 0 . 0 = J and = J . . 3 71 . 0 71 π/ 1 0 . 29 − 0 . 71 4 4 π/ The right-hand side of Figure 5.1 illustrates the robot at the θ 4 configu- = π/ 2 ration. Column of the Jacobian matrix, J i ( θ ), corresponds to the tip velocity i ̇ when = 1 and the other joint velocity is zero. These tip velocities (and θ i therefore columns of the Jacobian) are indicated in Figure 5.1. The Jacobian can be used to map bounds on the rotational speed of the joints v to bounds on , as illustrated in Figure 5.2. Rather than mapping a polygon tip of joint velocities through the Jacobian as in Figure 5.2, we could instead map a unit circle of joint velocities in the θ -plane. This circle represents an – θ 2 1 “iso-effort” contour in the joint velocity space, where total actuator effort is considered to be the sum of squares of the joint velocities. This circle maps through the Jacobian to an ellipse in the space of tip velocities, and this ellipse 1 manipulability ellipsoid . 5.3 Figure is referred to as the shows examples of this mapping for the two different postures of the 2R arm. As the manipulator configuration approaches a singularity, the ellipse collapses to a line segment, since the ability of the tip to move in one direction is lost. 1 A two-dimensional ellipsoid, as in our example, is commonly referred to as an ellipse. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

192 174 ̇ x 2 ̇ x 1 ̇ θ 2 ̇ x 2 ) θ ( J ̇ θ 1 x ̇ 1 Figure 5.3: Manipulability ellipsoids for two different postures of the 2R planar open chain. Using the manipulability ellipsoid one can quantify how close a given posture is to a singularity. For example, we can compare the lengths of the major and minor principal semi-axes of the manipulability ellipsoid, respectively denoted ` ` . The closer the ellipsoid is to a circle, i.e., the closer the ratio and max min ` /` is to 1, the more easily can the tip move in arbitrary directions and min max thus the more removed it is from a singularity. The Jacobian also plays a central role in static analysis. Suppose that an external force is applied to the robot tip. What are the joint torques required to resist this external force? This question can be answered via a conservation of power argument. As- suming that negligible power is used to move the robot, the power measured at the robot’s tip must equal the power generated at the joints. Denoting the tip force vector generated by the robot as f and the joint torque vector by τ , the tip May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

193 Chapter 5. Velocity Kinematics and Statics 175 T Figure 5.4: Mapping joint torque bounds to tip force bounds. conservation of power then requires that T T ̇ v τ f = θ, tip tip ̇ ̇ . Since ) v = J ( θ θ for all arbitrary joint velocities θ , the equality tip T T ̇ ̇ f ) θ θ = τ J ( θ tip 2 ̇ θ . This can only be true if must hold for all possible T = J (5.3) ( θ ) f τ . tip The joint torque needed to create the tip force f τ is calculated from the tip equation above. J ( θ ) is a square matrix dependent For our two-link planar chain example, θ on θ is not a singularity then both J ( θ ) and its transpose . If the configuration are invertible, and Equation (5.3) can be written T − 1 − T θ )) ) J = (( τ = J ( f ( θ ) τ. (5.4) tip Using the equation above one can now determine, under the same static equi- librium assumption, what input torques are needed to generate a desired tip force, e.g., the joint torques needed for the robot tip to push against a wall with 2 ̇ Since the robot is at equilibrium, the joint velocity θ is technically zero. This can be ̇ considered the limiting case as θ approaches zero. To be more formal, we could invoke the “principle of virtual work,” which deals with infinitesimal joint displacements instead of joint velocities. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

194 176 T Force ellipsoids for two different postures of the 2R planar open chain. Figure 5.5: θ of the robot at equilibrium and a specified normal force. For a given posture a set of joint torque limits such as 1 N m ≤ τ 1 N m ≤ − , 1 1 N m 1 N m τ , − ≤ ≤ 2 then Equation (5.4) can be used to generate the set of all possible tip forces as indicated in Figure 5.4. As for the manipulability ellipsoid, a force ellipsoid can be drawn by map- ping a unit circle “iso-effort” contour in the – τ -plane to an ellipsoid in the τ 1 2 T − 5.5). – f tip-force plane via the Jacobian transpose inverse J ) (see Figure f ( θ 1 2 The force ellipsoid illustrates how easily the robot can generate forces in dif- ferent directions. As is evident from the manipulability and force ellipsoids, if it is easy to generate a tip velocity in a given direction then it is difficult to generate a force in that same direction, and vice versa (Figure 5.6). In fact, for a given robot configuration, the principal axes of the manipulability ellipsoid May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

195 Chapter 5. Velocity Kinematics and Statics 177 x ̇ f 2 2 f ̇ x 1 1 ̇ x f 2 2 f x ̇ 1 1 Figure 5.6: Left-hand column: Manipulability ellipsoids at two different arm configu- rations. Right-hand column: The force ellipsoids for the same two arm configurations. and force ellipsoid are aligned, and the lengths of the principal semi-axes of the force ellipsoid are the reciprocals of the lengths of the principal semi-axes of the manipulability ellipsoid. At a singularity, the manipulability ellipsoid collapses to a line segment. The force ellipsoid, on the other hand, becomes infinitely long in a direction orthogonal to the manipulability ellipsoid line segment (i.e., the direction of the aligned links) and skinny in the orthogonal direction. Consider, for example, carrying a heavy suitcase with your arm. It is much easier if your arm hangs straight down under gravity (with your elbow fully straightened at a singularity), because the force you must support passes directly through your joints, therefore requiring no torques about the joints. Only the joint structure bears the load, not the muscles generating torques. The manipulability ellipsoid loses dimension at a singularity and therefore its area drops to zero, but the force ellipsoid’s area goes to infinity (assuming that the joints can support the load!). In this chapter we present methods for deriving the Jacobian for general open chains, where the configuration of the end-effector is expressed as T ∈ SE (3) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

196 178 5.1. Manipulator Jacobian V in the fixed base frame or the end- and its velocity is expressed as a twist effector body frame. We also examine how the Jacobian can be used for velocity and static analysis, including identifying kinematic singularities and determining the manipulability and force ellipsoids. Later chapters on inverse kinematics, motion planning, dynamics, and control make extensive use of the Jacobian and related notions introduced in this chapter. Manipulator Jacobian 5.1 In the 2R planar open chain example, we saw that, for any joint configuration ̇ and joint velocity vector v θ are linearly related , the tip velocity vector θ tip ̇ θ ), i.e., v = via the Jacobian matrix J ( θ ) J θ . The tip velocity v ( depends tip tip on the coordinates of interest for the tip, which in turn determine the specific can be taken form of the Jacobian. For example, in the most general case v tip to be a six-dimensional twist, while, for pure orienting devices such as a wrist, v is usually taken to be the angular velocity of the end-effector frame. Other tip v choices for lead to different formulations for the Jacobian. We begin with tip the general case where v is taken to be a six-dimensional twist V . tip All the derivations below are mathematical expressions of the same simple θ of the robot, the idea, embodied in Equation (5.2): given the configuration ̇ = 1 and all ( θ ), which is column i of J ( θ ), is the twist V when 6-vector θ J i i other joint velocities are zero. This twist is determined in the same way as the joint screw axes were determined in the previous chapter, using a point on q i joint axis i for revolute joints. The only difference is that the screw axes of the Jacobian depend on the joint variables whereas the screw axes for the forward θ kinematics of Chapter 4 were always for the case θ = 0. The two standard types of Jacobian that we will consider are: the space ̇ J ) corresponds ( θ ) satisfying V θ = J ( ( θ ) , where each column θ Jacobian J si s s s s ; and the body Jacobian to a screw axis expressed in the fixed space frame { } ̇ θ ) satisfying V ) corresponds to a screw = J θ J θ ) ( θ , where each column J ( ( bi b b b { b } . We start with the space Jacobian. axis expressed in the end-effector frame Space Jacobian 5.1.1 In this section we derive the relationship between an open chain’s joint velocity ̇ vector θ and the end-effector’s spatial twist V . We first recall a few basic prop- s n × n R A,B are erties from linear algebra and linear differential equations: (i) if ∈ − 1 n 1 − 1 n × − A AB = ; (ii) if A ∈ R B ) both invertible then ( is constant and θ ( t ) Aθ − Aθ 1 Aθ − Aθ Aθ ̇ ̇ ; (iii) ( e e ) A θ = e θ = . then d ( e is a scalar function of ) t /dt = Ae May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

197 Chapter 5. Velocity Kinematics and Statics 179 n Consider an -link open chain whose forward kinematics is expressed in the following product of exponentials form: θ ] S [ [ S θ ] θ S [ ] n n 2 2 1 1 (5.5) M. e ··· e ) = ( θ T ,...,θ e 1 n − 1 ̇ V ] = The spatial twist TT is given by [ V , where s s ( ( ) ) d d θ ] S [ θ ] S S [ [ S [ ] θ ] [ S θ ] θ 1 1 n 1 2 1 n 2 n n ̇ T = ··· + M + M e e ··· e e ··· e dt dt S [ θ [ S ] θ ] θ [ S ] θ S [ ] [ S ] θ n 2 n 1 2 n 1 n 1 1 ̇ ̇ e ] [ θ ··· e + M e ··· e + ··· e θ S ] M S = [ 2 1 2 1 Also, − 1 1 − [ S ] θ − θ [ S − ] 1 n 1 n = e M T e . ··· − 1 ̇ TT we obtain Calculating θ ] S [ − θ ] S [ − θ ] [ S S ] θ [ θ ] S [ θ − [ S ] 2 1 1 1 1 1 2 1 1 1 2 2 ̇ ̇ ̇ ] e + e [ S . ] e θ θ S [ ] + ··· e S ] = [ θ e V [ + e 2 s 3 3 1 1 2 The above can also be expressed in vector form by means of the adjoint mapping: ̇ ̇ ̇ + ( ) S + Ad θ (5.6) θ ··· θ S = V + Ad ( S ) [ S θ [ S ] θ ] θ S [ ] 3 2 3 1 1 s 2 1 2 2 1 1 1 e e e ︸︷︷︸ ︷︷ ︸ ︷︷ ︸ ︸ ︸ J 1 s J J s s 2 3 Observe that is a sum of n spatial twists of the form V s ̇ ̇ + J V ( θ ) = θ J ··· + J ( θ ) + θ , (5.7) s 1 1 2 s n s sn n θ θ ) = ( ω J ( θ ) ,v where each ( ( )) depends explictly on the joint values θ ∈ R si si si for i = 2 ,...,n . In matrix form,   ̇ θ 1 [ ]   . . ) ··· J J ( J ) ( θ θ = V   s 1 sn 2 s s . (5.8) ̇ θ n ̇ θ ) θ. J = ( s J The matrix ( θ ) is said to be the Jacobian in fixed (space) frame coordinates, s or more simply the space Jacobian . Definition 5.1. Let the forward kinematics of an n -link open chain be expressed in the following product of exponentials form: S ] θ [ [ S ] θ n 1 n 1 = e ··· T e M. (5.9) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

198 180 5.1. Manipulator Jacobian 6 × n n ̇ space Jacobian ) ∈ relates the joint rate vector θ θ ∈ R R to the The J ( s via spatial twist V s ̇ ( = θ ) J θ. (5.10) V s s i J The ( θ ) is th column of s θ ) = Ad J (5.11) , ) S ( ( θ ] S [ ] [ S θ i si 1 1 − i − i 1 1 e ··· e S ,...,n J i = 2 = , with the first column . for 1 1 s θ ( To understand the physical meaning behind the columns of ), observe J s S θ ] S [ [ θ ] 1 − i 1 1 − 1 i T th column is of the form Ad ··· e = that the e ), where S ( ; i i T − i 1 1 i − S recall that i th joint axis in terms of the fixed is the screw axis describing the i ( S ) is therefore the screw frame with the robot in its zero position. Ad T i − 1 i axis describing the i th joint axis after it undergoes the rigid body displacement . Physically this is the same as moving the first i − 1 joints from their zero T i − 1 θ θ ,...,θ ) of position to the current values ( . Therefore, the i th column J si − i 1 1 J ( θ ) is simply the screw vector describing joint axis i , expressed in fixed-frame s coordinates, as a function of the joint variables ,...,θ . θ 1 i − 1 J of In summary, the procedure for determining the columns J ) is similar ( θ s si S to the procedure for deriving the joint screws in the product of exponentials i [ θ ] [ ] S S θ n 1 1 n e ··· e θ M : each column J ) is the screw vector describing ( formula si joint axis , expressed in fixed-frame coordinates, but for arbitrary θ rather than i = 0. θ We now illustrate Example 5.2 (Space Jacobian for a spatial RRRP chain) . the procedure for finding the space Jacobian for the spatial RRRP chain of th column of J = ( ( θ ) by J Figure 5.7. Denote the i ). The [Ad ,v ω ] T s si si si − i 1 matrices are implicit in our calculations of the screw axes of the displaced joint axes. , ω • is constant and in the ˆz Observe that -direction: ω 1). 0 = (0 , 1 s s s 1 0). as the origin, Choosing q = (0 , 0 , v 1 1 s • ω as is also constant in the ˆz q -direction, so ω 1). Choose , = (0 , 0 2 s 2 2 s s L = c 0), where c ,L v s . Then , the point ( θ = cos θ = sin , s 1 1 1 1 1 1 s 2 1 1 − ω 0). × q , = ( L c s L , − 1 1 1 2 2 1 The direction of is always fixed in the ˆz -direction regardless of the • ω s 3 s θ + and θ s , so ω ,L c = (0 , 0 values of 1). Choosing q L = ( L + c , 1 3 2 3 12 1 1 2 1 1 s L ), it follows that s θ , 0), where c + = cos( θ θ + θ = sin( ), s 12 1 12 12 2 2 1 2 = ( L s + 0). v s , − L c − L c , L 1 1 1 3 12 2 1 12 2 s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

199 Chapter 5. Velocity Kinematics and Statics 181 L 2 θ θ L 3 1 1 θ 2 θ 4 ˆz ˆz ˆy ˆy ˆx ˆ x q 3 q 2 q 1 Figure 5.7: Space Jacobian for a spatial RRRP chain. ω • Since the final joint is prismatic, = (0 , 0 , 0), and the joint-axis direction 4 s is given by = (0 , v , 1). 0 s 4 The space Jacobian is therefore   0 0 0 0   0 0 0 0     1 0 1 1   . θ J ( ) = s   L s L + s 0 s L 0 2 12 1 1 1 1     − 0 0 c − L c − L c L 1 1 1 1 12 2 1 0 0 0 (Space Jacobian for a spatial RRPRRR chain) We now derive . Example 5.3 the space Jacobian for the spatial RRPRRR chain of Figure 5.8. The base frame is chosen as shown in the figure. The first joint axis is in the direction ω = • = (0 , 0 , 1). Choosing q 1 s 1 , 0 ,L 0). ), we get v , 0 = (0 ω , × q = (0 − 1 1 1 s 1 • The second joint axis is in the direction ω 0). Choosing , = ( − c s , − 1 2 s 1 q 0). = (0 , 0 ,L , ), we get v c L = − ω − × q , = ( L s s 1 2 2 2 1 1 1 1 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

200 182 5.1. Manipulator Jacobian θ θ 4 1 θ 2 θ θ 3 5 q 1 θ 6 q w L 2 L 1 } s { Figure 5.8: Space Jacobian for the spatial RRPRRR chain. ω • The third joint is prismatic, so = (0 , 0 , 0). The direction of the pris- s 3 matic joint axis is given by     c − s 0 1 2     c 1 c = v − , )Rot(ˆx ) ,θ = Rot(ˆz θ . 1 2 3 s 2 1 0 s − 2 • Now consider the wrist portion of the chain. The wrist center is located at the point       θ c 0 0 L ( − )s + 3 2 1 2       θ + L 0 )c + ( L θ c − θ = ) + Rot(ˆz ,θ = q )Rot(ˆx , . 3 2 2 3 1 2 1 2 w L 0 − ( L L + θ )s 1 3 2 1 2 Observe that the directions of the wrist axes depend on θ , θ , and the 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

201 Chapter 5. Velocity Kinematics and Statics 183 preceding wrist axes. These are     0 − s s 2 1     0 c s )Rot(ˆx ω ,θ − , = Rot(ˆz ) θ = , 1 2 2 1 4 s 1 c 2     1 − c c + s s − c 1 2 4 1 4     0 − c s s − c c , = Rot(ˆz ,θ )Rot(ˆz θ − ) )Rot(ˆx ,θ ω = , 1 4 1 4 2 4 s 5 1 2 0 s s 4 2   0   1 ) θ ω , − θ ,θ − , )Rot(ˆx )Rot(ˆz ,θ = Rot(ˆz )Rot(ˆx 1 5 6 2 s 4 0   s c − (s s c ) + s c s + c 1 4 2 1 1 2 5 5 4   ) c c − s s (c − c s s c . = 2 1 1 2 5 5 4 4 1 c c s − s − c 4 5 5 2 2 The space Jacobian can now be computed and written in matrix form as follows: [ ] ω ω ω 0 ω ω 4 s 2 6 s s s 1 s 5 ) = ( J θ . s ω q v − ω × − − × × q − ω ω × q 0 q s 5 6 s w s 4 s 2 2 w w s 3 Note that we were able to obtain the entire Jacobian directly, without having to explicitly differentiate the forward kinematic map. 5.1.2 Body Jacobian In the previous section we derived the relationship between the joint rates and 1 − ̇ [ ] = V , the end-effector’s twist expressed in fixed-frame coordinates. Here TT s − 1 ̇ T we derive the relationship between the joint rates and [ V , the end- T ] = b effector twist in end-effector-frame coordinates. For this purpose it will be more convenient to express the forward kinematics in the alternative product of exponentials form: [ B θ ] θ ] [ B B ] θ [ 2 2 n 1 1 n ··· e ( θ ) = Me T e . (5.12) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

202 184 5.1. Manipulator Jacobian ̇ Computing T , ( ) d B [ θ ] B [ ] θ θ ] [ B 1 n n − − 1 n n 1 1 ̇ e ··· Me e T = dt ( ) d [ ] θ B θ [ [ B ] θ B ] 1 1 n n − n − n 1 1 + Me ··· e + ··· e dt ] [ B θ B ] [ θ n n 1 1 ̇ θ [ B ] ··· e Me = n n [ θ ] B θ B ] [ B ] θ [ n n n 1 n − 1 − 1 1 ̇ ··· ] e [ B + θ ··· e Me + 1 n − 1 − n B [ θ [ θ ] B [ B ] θ ] 1 2 n n 2 1 ̇ Me ] ··· B [ e θ e . + 1 1 Also, θ 1 − [ B − ] 1 − − [ B ] θ 1 1 n n e M ··· e . = T − 1 ̇ T , T Evaluating B θ ] [ B [ − θ ] n n n n ̇ ̇ B e [ B + ··· + ] e θ ] θ V ] = [ [ b n n n − 1 1 − n [ B θ ] θ ] − − [ B B ] θ [ [ B θ ] 2 2 2 2 n n n n ̇ [ B ··· e θ ] e + e e ··· 1 1 or, in vector form, ̇ ̇ ̇ V B = + Ad θ ) B ( . θ (5.13) ··· + θ + Ad B ( ) ] [ B − θ θ ] B [ − θ ] − [ B n n n b n n − 1 1 1 − n 1 n n 2 2 e e ··· e ︸︷︷︸ ︷︷ ︸ ︸ ︸ ︷︷ ︸ J J J bn b,n − 1 b 1 V The twist can therefore be expressed as a sum of n body twists: b ̇ ̇ ̇ J , θ ( θ ) J θ V + ··· + J (5.14) + θ ( θ ) = 1 n − bn bn 1 1 n b b − 1 ,v ) = ( θ J ω where each ( θ ) ( for ( θ )) depends explictly on the joint values θ bi bi bi = 1 ,...,n − 1. In matrix form, i   ̇ θ 1 [ ]   . ̇ . J ( ( θ ) J ) ··· J θ V = J ( θ ) = θ. (5.15)   1 b bn 1 − bn b b . ̇ θ n The matrix J ( θ ) is the Jacobian in the end-effector- (or body-) frame coordi- b nates and is called, more simply, the body Jacobian . Definition 5.4. Let the forward kinematics of an n -link open chain be expressed in the following product of exponentials form: B ] θ [ [ B ] θ n 1 n 1 = Me ··· T e . (5.16) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

203 Chapter 5. Velocity Kinematics and Statics 185 6 × n n ̇ body Jacobian R J ( θ relates the joint rate vector ) θ ∈ R ∈ to the The b V ω ,v ) via end-effector twist = ( b b b ̇ = ( θ ) V θ. (5.17) J b b th column of J The ( θ ) is i b ( θ ) = Ad (5.18) , ) B ( J θ ] B [ − ] bi − [ B θ i +1 i +1 i n n ··· e e i = n − 1 ,..., 1, with J . = B for bn n J θ ): each column A physical interpretation can be given to the columns of ( b ( θ ) = ( ω J ( θ ) ,v , expressed in ( θ )) of J i ( θ ) is the screw vector for joint axis b bi bi bi the coordinates of the end-effector frame rather than those of the fixed frame. ( ) is similar to the proce- J The procedure for determining the columns of θ b dure for deriving the forward kinematics in the product of exponentials form ] θ ] B θ [ B [ 1 1 n n e ··· , the only difference being that each of the end-effector-frame Me = 0. J θ ) are expressed for arbitrary θ rather than θ ( joint screws bi Visualizing the Space and Body Jacobian 5.1.3 Another, perhaps simpler, way to derive the formulas for the i th column of the i space Jacobian (5.11) and the th column of the body Jacobian (5.18) comes from inspecting the 5R robot in Figure 5.9. Let’s start with the third column, , of the space Jacobian using the left-hand column of Figure 5.9. J s 3 S The screw corresponding to joint axis 3 is written as in { s } when the robot 3 is at its zero configuration. Clearly the joint variables θ have no , θ θ , and 4 5 3 ̇ θ , because they do impact on the spatial twist resulting from the joint velocity 3 { . So we fix those joint variables at zero, making s not displace axis 3 relative to } θ . If = 0 and θ is arbitrary then B the robot from joint 2 outward a rigid body 2 1 θ ′ ] S [ 2 2 ′ e T at { } s = is at the same position and orientation relative the frame ss B to { s } when θ is also arbitrary then the frame = θ θ = 0. Now, if as frame 2 1 1 ] S θ ] S [ θ ′′ [ 1 1 2 2 ′′ e e { T s at } = is at the same position and orientation relative to ss B as frame { s } when θ represents the screw relative to = θ S = 0. Thus 2 1 3 ′′ J for arbitrary joint angles θ s and θ , though, is the screw . The column { } s 3 1 2 s { . The mapping that changes the frame of representation of S relative to } 3 ′′ , precisely ] S ], i.e., ] = [Ad J = [Ad is [Ad } } s { { from to s θ [ S [ S ] θ ] 3 s 3 T T 1 2 2 1 ′′ ′′ e e ss ss Equation (5.11) for joint i = 3. Equation (5.11) is the generalization of the i = 2 ,...,n . reasoning above for any joint J Now let’s derive the third column, , of the body Jacobian by inspecting 3 b the right-hand column of Figure 5.9. The screw corresponding to joint 3 is written B when the robot is at its zero configuration. Clearly the joint in { b } 3 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

204 186 5.1. Manipulator Jacobian } { b B S 3 3 { s } ′ θ ] [ B 4 4 ′ e T = b } { bb θ θ 2 4 ] [ θ S ′ 2 2 ′ T = e { s } ss ′′ B [ θ B [ θ ] ] 4 4 5 5 ′′ T e = e b { } bb θ 2 θ 5 θ θ 1 4 ] θ θ ′′ [ S [ S ] 1 1 2 2 ′′ { T s } = e e ss Figure 5.9: A 5R robot. (Left-hand column) Derivation of J , the third column of 3 s the space Jacobian. (Right-hand column) Derivation of , the third column of the J b 3 body Jacobian. θ have no impact on the body twist resulting from the , θ variables , and θ 3 1 2 ̇ θ , because they do not displace axis 3 relative to joint velocity b } . So we fix { 3 B from the base to those joint variables at zero, making the robot a rigid body ′ θ [ B ] 4 4 ′ b θ } at T is = 0 and = e θ is arbitrary, then the frame joint 4. If { bb 4 5 ′′ θ { b } at the new end-effector frame. Now if is also arbitrary, then the frame 5 B ] θ [ θ [ B ] 4 4 5 5 ′′ = e T e is the new end-effector frame. The column J is simply the bb 3 b ′′ { } } . Since B b is expressed in { b screw axis of joint 3 expressed in , we have 3 J B = [Ad ] b T 3 3 ′′ b b 1 − ] B = [Ad 3 T ′′ bb . B ] = [Ad [ ] θ − [ − B B θ ] 3 4 5 5 4 e e 1 − 1 − − 1 T . This formula = T T where we have made use of the fact that ( ) T 1 2 2 1 for J = 3. Equation (5.18) is the i is precisely Equation (5.18) for joint 3 b generalization of the reasoning above for any joint i = 1 ,...,n − 1. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

205 Chapter 5. Velocity Kinematics and Statics 187 Relationship between the Space and Body Jacobian 5.1.4 s { b } , the forward } and the end-effector frame by { Denoting the fixed frame by kinematics can be written as θ ). The twist of the end-effector frame can be T ( sb written in terms of the fixed- and end-effector-frame coordinates as − 1 ̇ V T T ] = [ , sb s sb 1 − ̇ T V ] = [ T , b sb sb V V and V ). The twists related by V V = Ad ( with ( V = Ad ) and V b T T s s b s s b sb bs V and are also related to their respective Jacobians via b ̇ ) ( V θ = J θ, (5.19) s s ̇ θ. = ( θ ) V (5.20) J b b Equation (5.19) can therefore be written ̇ Ad ( V θ. ) = J ( (5.21) θ ) T s b sb Applying [Ad ] to both sides of Equation (5.21) and using the general property T bs [Ad ][Ad ] = [Ad ] of the adjoint map, we obtain XY X Y ̇ Ad (Ad V )) = Ad ( = Ad ( V ) = V q θ ) ( ) . J ( T T T b T b T b s bs sb bs sb bs ̇ ̇ , it follows that J ) are ( θ Since we also have V θ for all = θ θ J ( ( θ ) and J ) b s b b related by J (5.22) ( θ ) = Ad . ) ( J θ ( θ )) = [Ad ( J ] T s T b s bs bs The space Jacobian can in turn be obtained from the body Jacobian via J (5.23) ( θ ) = Ad . ) ( J θ ( θ )) = [Ad ( J ] T b T s b sb sb The fact that the space and body Jacobians, and the space and body twists, are similarly related by the adjoint map should not be surprising since each column of the space or body Jacobian corresponds to a twist. An important implication of Equations (5.22) and (5.23) is that J ) and ( θ b ( θ ) always have the same rank; this is shown explicitly in Section 5.3 on J s singularity analysis. 5.1.5 Alternative Notions of the Jacobian The space and body Jacobians derived above are matrices that relate joint rates to the twist of the end-effector. There exist alternative notions of the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

206 188 5.1. Manipulator Jacobian Jacobian that are based on a representation of the end-effector configuration q . Such representations are particularly using a minimum set of coordinates SE (3). For relevant when the task space is considered to be a subspace of example, the configuration of the end-effector of a planar robot could be treated 3 SE ) R as instead of as an element of x,y,θ (2). q = ( ∈ When using a minimum set of coordinates, the end-effector velocity is not but by the time derivative of the coordinates ̇ q , and the Jaco- given by a twist V ̇ is sometimes called the q = J J ( θ ) bian θ in the velocity kinematics ̇ analytic a a Jacobian as opposed to the geometric Jacobian in space and body form, 3 described above. 6 (3) task space, a typical choice of the minimal coordinates q ∈ R For an SE includes three coordinates for the origin of the end-effector frame in the fixed frame and three coordinates for the orientation of the end-effector frame in the fixed frame. Example coordinates for the orientation include the Euler angles (see Appendix B) and exponential coordinates for rotation. Example 5.5 . (Analytic Jacobian with exponential coordinates for rotation) in In this example, we find the relationship between the geometric Jacobian J b J that uses exponential coordinates the body frame and an analytic Jacobian a = ˆ ωθ to represent the orientation. (Recall that ‖ ˆ ω ‖ = 1 and θ ∈ [0 ,π ].) r n First, consider an open chain with joints and the body Jacobian ̇ = J θ, ( V ) θ b b 6 × n ( θ ) ∈ R where J = . The angular and linear velocity components of V b b ω ) can be written explicitly as ( ,v b b [ [ ] ] ω ( θ J ) b ω ̇ ̇ V = θ = ( = J θ ) θ, b b v ( ) J θ b v where J J is the 3 × n matrix corresponding to the top three rows of J and b ω v n matrix corresponding to the bottom three rows of is the 3 . × J b 6 ∈ R Now suppose that our minimal set of coordinates is given by q = q 3 r,x ), where x ∈ R ( is the position of the origin of the end-effector frame and 3 r ωθ ∈ R is the exponential coordinate representation for the rotation. The = ˆ 3 The term “geometric Jacobian” has also been used to describe the relationship between joint rates and a representation of the end-effector velocity that combines the rate of change of the position coordinates of the end-effector (which is neither the linear portion of a body twist nor the linear portion of a spatial twist) and a representation of the angular velocity. Unlike a body or spatial twist, which depends only on the body or space frame, respectively, this “hybrid” notion of a spatial velocity depends on the definition of both frames. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

207 Chapter 5. Velocity Kinematics and Statics 189 v v in the by a rotation that gives x coordinate time derivative ̇ is related to b b fixed coordinates: ̇ ( ( θ ) v ̇ = R x = θ ) J θ, ( θ ) R v sb sb b r [ [ˆ ω ] θ ] ( = e where ) = e R . θ sb r ω by is related to the body angular velocity The time-derivative ̇ b = A ( r ω r, ) ̇ b where ‖ r ‖ sin ‖− r ‖ cos ‖ r ‖ 1 − 2 r [ . ] r [ ] + ( r ) = I − A 2 3 r ‖ r ‖ ‖ ‖ (The derivation of this formula is explored in Exercise 5.10.) Provided that the : r ) is invertible, ̇ r can be obtained from ω A ( matrix b − 1 − 1 ̇ ( r ) ω = = A A ̇ ( r ) J θ. ( θ ) r ω b Putting these together, we obtain [ ] [ ][ ] − 1 r r ( ̇ A 0 ) ω b = q = ̇ , (5.24) x R ̇ 0 v sb b i.e., the analytic Jacobian is related to the body Jacobian J J by b a ] [ [ ] ][ − − 1 1 A r 0 0 ( J ) ( θ ) r A ( ) ω ) ( = . ) = (5.25) θ ( J J θ b a ( θ ) 0 R R ( θ ) ( θ ) J 0 sb v sb Looking Ahead to Inverse Velocity Kinematics 5.1.6 In the above sections we asked the question “What twist results from a given set of joint velocities?” The answer, written independently of the frame in which the twists are represented, was given by ̇ J ( θ ) θ. V = Often we are interested in the inverse question: given a desired twist V , what ̇ θ are needed? This is a question of inverse velocity kinematics, joint velocities 6.3. Briefly, if J ( θ ) is square (so that which is discussed in more detail in Section the number of joints n is equal to six, the number of elements of a twist) and of − 1 ̇ J full rank then = ( θ ) V . If n 6 = 6 or the robot is at a singularity, however, θ then J ( θ ) is not invertible. In the case n < 6, arbitrary twists V cannot be achieved – the robot does not have enough joints. If n > 6 then we call the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

208 190 5.2. Statics of Open Chains V places six constraints on the robot redundant . In this case, a desired twist 6 freedoms correspond to internal motions of − joint rates, and the remaining n the robot that are not evident in the motion of the end-effector. For example, if you consider your arm from your shoulder to your palm as a seven-joint open chain, when you place your palm at a fixed configuration in space (e.g., on the surface of a table), you still have one internal degree of freedom corresponding to the position of your elbow. Statics of Open Chains 5.2 Using our familiar principle of conservation of power, we have power at the joints = (power to move the robot) + (power at the end-effector) and, considering the robot to be at static equilibrium (no power is being used to move the robot), we can equate the power at the joints to the power at the 4 end-effector, T T ̇ = F τ V , θ b b ̇ is the column vector of the joint torques. Using the identity V where = J , ( θ ) τ θ b b we get T J , τ ( θ ) F = b b relating the joint torques to the wrench written in the end-effector frame. Sim- ilarly, T J = τ ( θ ) F s s in the fixed space frame. Independently of the choice of the frame, we can simply write T = ( θ ) F . (5.26) τ J −F is applied to the end-effector when the robot is at If an external wrench θ equilibrium with joint values τ , Equation (5.26) calculates the joint torques 5 F needed to generate the opposing wrench , keeping the robot at equilibrium. This is important in force control of a robot, for example. One could also ask the opposite question, namely, what is the end-effector T J 6 invertible is a 6 × wrench generated by a given set of joint torques? If − T θ matrix, then clearly J ( = ) τ . If the number of joints n is not equal to six F T J is not invertible, and the question is not well posed. then 4 ̇ We are considering the limiting case as θ goes to zero, consistent with our assumption that the robot is at equilibrium. 5 If the robot has to support itself against gravity to maintain static equilibrium, the torques τ must be added to the torques that offset gravity. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

209 Chapter 5. Velocity Kinematics and Statics 191 6) then, even if the end-effector is embedded in If the robot is redundant ( n > concrete, the robot is not immobilized and the joint torques may cause internal motions of the links. The static equilibrium assumption is no longer satisfied, and we need to include dynamics to know what will happen to the robot. × T n 6 R ≤ 6 and J n n then embedding the end-effector in If ∈ has rank 6, no matter what τ we choose, the n < concrete will immobilize the robot. If robot cannot − n wrench directions defined by actively generate forces in the 6 T , the null space of J T T θ )) = {F | J J ( ( ) F = 0 } , Null( θ since no actuators act in these directions. The robot can, however, resist ar- T J θ )) without moving, bitrary externally applied wrenches in the space Null( ( owing to the lack of joints that would allow motions due to these forces. For n example, consider a motorized rotating door with a single revolute joint ( = 1) and an end-effector frame at the door knob. The door can only actively gener- ate a force at the knob that is tangential to the allowed circle of motion of the knob (defining a single direction in the wrench space), but it can resist arbitrary wrenches in the orthogonal five-dimensional wrench space without moving. Singularity Analysis 5.3 The Jacobian allows us to identify postures at which the robot’s end-effector loses the ability to move instantaneously in one or more directions. Such a posture is called a kinematic singularity , or simply a singularity . Math- J ( θ ) fails to be of ematically, a singular posture is one in which the Jacobian J ( θ ), whose maximal rank. To understand why, consider the body Jacobian b J ,...,n , i = 1 columns are denoted . Then bi   ̇ θ 1   . [ ] .   . ( θ ) J J ) θ ( ··· J = V   bn 1 bn − b 1 b   ̇ θ 1 n − ̇ θ n ̇ ̇ ̇ θ ( θ ) ··· θ = + J + J J + θ ( θ ) . 1 1 1 − bn bn 1 − n b n Thus, the tip frame can achieve twists that are linear combinations of the J . bi As long as n ≥ 6, the maximum rank that J ) can attain is six. Singular ( θ b θ θ at which the rank of J ) drops below ( postures correspond to those values of b the maximum possible value; at such postures the tip frame loses the ability to generate instantaneous spatial velocities in in one or more dimensions. This May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

210 192 5.3. Singularity Analysis loss of mobility at a singularity is accompanied by the ability to resist arbitrary wrenches in the direction corresponding to the lost mobility. The mathematical definition of a kinematic singularity is independent of the choice of body or space Jacobian. To see why, recall the relationship between J J θ ): J ( θ ) = Ad ) and ( J ( ( θ )) = [Ad θ ] J ( θ ) or, more explicitly, ( b T b s s T b sb sb [ ] R 0 sb θ ) = J ( ( ) . θ J s b R p ] R [ sb sb sb ] is always invertible. This can be estab- We now claim that the matrix [Ad T sb lished by examining the linear equation [ ] ][ R x 0 sb . = 0 R ] R p y [ sb sb sb = y = 0, implying that the matrix [Ad Its unique solution is ] is invertible. x T sb Since multiplying any matrix by an invertible matrix does not change its rank, it follows that J θ ( θ ) = rank J , ( rank ) b s as claimed; singularities of the space and body Jacobian are one and the same. Kinematic singularities are also independent of the choice of fixed frame and end-effector frame. Choosing a different fixed frame is equivalent to simply relocating the robot arm, which should have absolutely no effect on whether a particular posture is singular. This obvious fact can be verified by referring to Figure 5.10(a). The forward kinematics with respect to the original fixed frame is denoted T θ ), while the forward kinematics with respect to the relocated ( ′ fixed frame is denoted T θ ) = PT ( θ ), where P ∈ SE (3) is constant. Then the ( ′ 1 ′ − ′ ′ ̇ J θ ), is obtained from ( T θ ) ( T ), denoted T body Jacobian of . A simple ( b calculation reveals that 1 1 ′ − 1 − 1 ′ − − ̇ ̇ ̇ P T )( ) = ( T ) = T T ( T T, P ′ J ( θ ) = J i.e., ( θ ), so that the singularities of the original and relocated robot b b arms are the same. To see that singularities are independent of the end-effector frame, refer to Figure 5.10(b) and suppose the forward kinematics for the original end-effector frame is given by T ( θ ) while the forward kinematics for the relocated end-effector ′ T frame is ( θ ) = T ( θ ) Q , where Q ∈ SE (3) is constant. This time, looking at the space Jacobian – recall that singularities of J ( θ ) coincide with those of J ) – ( θ s b ′ ′ ). A simple calculation reveals that ( θ let T J ( θ ) denote the space Jacobian of s 1 − ′ 1 − ′ 1 − 1 − ̇ ̇ ̇ Q = ( ) T T ( TQ ) = TT T )( . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

211 Chapter 5. Velocity Kinematics and Statics 193 ) ( T θ Q P θ T ) ( ( ) θ T T θ )= θ PT ( ( ) T θ )= T ( θ ) Q ( (b) (a) Figure 5.10: Kinematic singularities are invariant with respect to the choice of fixed and end-effector frames. (a) Choosing a different fixed frame, which is equivalent to relocating the base of the robot arm; (b) choosing a different end-effector frame. ′ That is, J θ ) = J ), so that the kinematic singularities are invariant with ( θ ( s s respect to the choice of end-effector frame. In the remainder of this section we consider some common kinematic singu- larities that occur in six-dof open chains with revolute and prismatic joints. We now know that either the space or body Jacobian can be used for our analysis; we use the space Jacobian in the examples below. Case I: Two Collinear Revolute Joint Axes The first case we consider is one in which two revolute joint axes are collinear (see Figure 5.11(a)). Without loss of generality these joint axes can be labeled 1 and 2. The corresponding columns of the Jacobian are [ ] [ ] ω ω s 1 2 s ) = ( J θ and ( J ) = . θ s 1 2 s q − ω × ω − × q 1 s 1 2 2 s ω Since the two joint axes are collinear, we must have ± ω ; let us assume = s 1 s 2 the positive sign. Also, ω , the set × ( q J − q = ) = 0 for i = 1 , 2. Then J s 2 1 s 2 si 1 { J ) must θ ,J ( J ,...,J cannot be linearly independent, and the rank of } 1 6 2 s s s s be less than six. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

212 194 5.3. Singularity Analysis ˆz ˆy q q 2 3 q 1 ˆx (a) (b) Figure 5.11: (a) A kinematic singularity in which two joint axes are collinear. (b) A kinematic singularity in which three revolute joint axes are parallel and coplanar. Case II: Three Coplanar and Parallel Revolute Joint Axes The second case we consider is one in which three revolute joint axes are par- 5.11(b)). allel and also lie on the same plane (three coplanar axes: see Figure Without loss of generality we label these as joint axes 1, 2, and 3. In this case we choose the fixed frame as shown in the figure; then [ ] ω ω ω ··· s 1 1 s 1 s ( θ ) = J . s − 0 × q ··· − ω q × ω 1 3 2 1 s s q q and Since are points on the same unit axis, it is not difficult to verify that 3 2 the first three columns cannot be linearly independent. Case III: Four Revolute Joint Axes Intersecting at a Common Point Here we consider the case where four revolute joint axes intersect at a common point (Figure 5.12). Again, without loss of generality, label these axes from 1 to 4. In this case we choose the fixed-frame origin to be the common point of intersection, so that q = 0, and therefore = ··· = q 4 1 ] [ ω ω ω ··· ω s s 2 s 3 s 4 1 . θ J ) = ( s 0 0 ··· 0 0 The first four columns clearly cannot be linearly independent; one can be writ- ten as a linear combination of the other three. Such a singularity occurs, for example, when the wrist center of an elbow-type robot arm is directly above the shoulder. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

213 Chapter 5. Velocity Kinematics and Statics 195 S 1 S 2 S 3 S 4 Figure 5.12: A kinematic singularity in which four revolute joint axes intersect at a common point. Case IV: Four Coplanar Revolute Joints Here we consider the case in which four revolute joint axes are coplanar. Again, without loss of generality, label these axes from 1 to 4. Choose a fixed frame x – y -plane; in this case the unit vector such that the joint axes all lie on the 3 ω R ∈ in the direction of joint axis i is of the form si   ω six   ω = ω . siy si 0 3 q ∈ Similarly, any reference point lying on joint axis i is of the form R i   q ix   q q = , iy i 0 and subsequently   0   0 × q . = = − ω v si si i q − ω q ω iy siy six ix May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

214 196 5.4. Manipulability ( θ ) are The first four columns of the space Jacobian J s   ω ω ω ω 2 s s x 1 s 3 x x s 4 x   ω ω ω ω y s y s s 3 2 y s 4 1 y     0 0 0 0     0 0 0 0     0 0 0 0 ω q q ω q ω q − ω q − ω q q ω q − ω ω − y s 3 y x 3 x 1 1 s 3 x y 3 y 1 s 4 y s 4 x 2 y s 4 x x 4 y 2 x s 1 s 2 x s 2 y and cannot be linearly independent since they only have three nonzero compo- nents. Case V: Six Revolute Joints Intersecting a Common Line The final case we consider is six revolute joint axes intersecting a common line. Choose a fixed frame such that the common line lies along the ˆz-axis, and select the intersection between this common line and joint axis i as the reference point 3 ∈ R q for axis i ; each q ), and is thus of the form q ,q = (0 , 0 i i iz i − = × q = ( ω q , ω ω q − , 0) , v i iz six si siy si iz i ,..., 6. The space Jacobian J for = 1 θ ) thus becomes ( s   ω ω ω ω ω ω 4 s 2 x s s 3 x 1 s x x 6 s 5 x x s   ω ω ω ω ω ω y y s 2 y 6 s 3 s y s 4 y s s 5 y 1     ω ω ω ω ω ω s z s 3 z s s 4 z s 2 5 z z s 6 z 1   ,   q q ω q ω ω ω q ω q ω q 2 3 z y s 4 y s 4 z 2 s 5 y z 5 z s s z y s 6 z 1 y 3 y 1 6     − ω q − ω q − ω q q ω ω − q q ω − − 4 x 2 4 z x x s 5 x 2 5 z z 3 s 6 x 1 6 z s z x s 3 z 1 s s 0 0 0 0 0 0 which is clearly singular. 5.4 Manipulability In the previous section we saw that, at a kinematic singularity, a robot’s end- effector loses the ability to translate or rotate in one or more directions. A kinematic singularity presents a binary proposition – a particular configuration is either kinematically singular or it is not – and it is reasonable to ask if a nonsingular configuration is “close” to being singular. The answer is yes; in fact, one can even determine the directions in which the end-effector’s ability to move is diminished, and to what extent. The manipulability ellipsoid allows one May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

215 Chapter 5. Velocity Kinematics and Statics 197 to visualize geometrically the directions in which the end-effector moves with least effort or with greatest effort. Manipulability ellipsoids are illustrated for a 2R planar arm in Figure 5.3. The Jacobian is given by Equation (5.1). q ∈ For a general -joint open chain and a task space with coordinates n m m n , the manipulability ellipsoid corresponds to the end-effector R , where ≤ ̇ ̇ ‖ satisfying θ ‖ = 1, a unit sphere in the n -dimensional θ velocities for joint rates 6 J Assuming joint-velocity space. is invertible, the unit joint-velocity condition can be written T ̇ ̇ θ θ 1 = − 1 T − 1 = ( ( J J q ̇ q ) ̇ ) 1 T − − T J = ̇ ̇ q J q T T − 1 T − 1 ̇ q q q ) ( JJ = ̇ ̇ q. (5.27) = ̇ A T m × m A = JJ If ∈ R J is full rank (i.e., of rank m is square, ), the matrix 1 − A . symmetric, and positive definite, as is Consulting a textbook on linear algebra, we see that for any symmetric m 1 m × m − positive-definite A ∈ , the set of vectors ̇ q ∈ R R satisfying − 1 T ̇ ̇ q = 1 q A m v and λ be the eigen- defines an ellipsoid in the -dimensional space. Letting i i A , the directions of the principal axes of the ellipsoid vectors and eigenvalues of √ λ , as illustrated in Fig- v and the lengths of the principal semi-axes are are i i V of the ellipsoid is proportional to the ure 5.13. Furthermore, the volume product of the semi-axis lengths: √ √ √ T . λ ) ··· λ V = is proportional to det( A ) = λ det( JJ m 2 1 in J J (either in the end-effector frame or J For the geometric Jacobian b s the fixed frame), we can express the 6 × n Jacobian as ] [ θ ( J ) ω , θ ( ) = J ( ) J θ v 6 A two-dimensional ellipsoid is usually referred to as an “ellipse,” and an ellipsoid in more than three dimensions is often referred to as a “hyperellipsoid,” but here we use the term ellipsoid independently of the dimension. Similarly, we refer to a “sphere” independently of the dimension, instead of using “circle” for two dimensions and “hypersphere” for more than three dimensions. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

216 198 5.4. Manipulability v v 2 1 √ √ λ 1 λ 2 √ λ 3 v 3 1 T 3 − , where the q q = 1 in the ̇ q space R An ellipsoid visualization of ̇ ̇ Figure 5.13: A λ of A and the principal semi-axis lengths are the square roots of the eigenvalues i v . directions of the principal semi-axes are the eigenvectors i where comprises the top three rows of J and J the bottom three rows of J v ω J . It makes sense to separate the two because the units of angular velocity and linear velocity are different. This leads to two three-dimensional manipulability ellipsoids, one for angular velocities and one for linear velocities. These ma- nipulability ellipsoids have principal semi-axes aligned with the eigenvectors of T A = J J A , with lengths given by the square roots of the eigenvalues, where ω ω T for the linear J J = for the angular velocity manipulability ellipsoid and A v v velocity manipulability ellipsoid. When calculating the linear-velocity manipulability ellipsoid, it generally J makes more sense to use the body Jacobian instead of the space Jacobian J , s b since we are usually interested in the linear velocity of a point at the origin of the end-effector frame rather than that of a point at the origin of the fixed-space frame. Apart from the geometry of the manipulability ellipsoid, it can be useful to assign a single scalar measure defining how easily the robot can move at a given posture. One measure is the ratio of the longest and shortest semi-axes of the manipulability ellipsoid, √ √ λ ( A ) ( λ ) A max max √ ) = A ( μ = , 1 ≥ 1 λ ( A ) ) A ( λ min min T = JJ A where μ ) is low (i.e., close to 1) then the manipulability ( A . When 1 ellipsoid is nearly spherical or isotropic , meaning that it is equally easy to move in any direction. This situation is generally desirable. As the robot approaches μ ( A ) goes to infinity. a singularity, however, 1 A similar measure μ ), which is known as the ( A ) is just the square of μ A ( 1 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

217 Chapter 5. Velocity Kinematics and Statics 199 T of the matrix JJ A , condition number = λ ( A ) max ( A μ ) = ≥ 1 . 2 A ) λ ( min Again, smaller values (close to 1) are preferred. The condition number of a ma- trix is commonly used to characterize the sensitivity of the result of multiplying that matrix by a vector to small errors in the vector. A final measure is simply proportional to the volume of the manipulability ellipsoid, √ √ ··· λ λ ) A det( . = A ) = μ ( 2 1 3 In this case, unlike the first two measures, a larger value is better. Just like the manipulability ellipsoid, a force ellipsoid can be drawn for joint T satisfying ‖ τ ‖ = 1. Beginning from τ = J torques ( θ ) F , we arrive at similar τ results to those above, except that now the ellipsoid satisfies T T T − 1 1 = f = B f JJ f, f T − 1 − 1 = where = ( A ) JJ . For the force ellipsoid, the matrix B plays the B same role as A in the manipulability ellipsoid; it is the eigenvectors and the square roots of eigenvalues of B that define the shape of the force ellipsoid. A Since eigenvectors of any invertible matrix = are also eigenvectors of B 1 − A , the principal axes of the force ellipsoid are aligned with the principal axes 1 − A B of the manipulability ellipsoid. Furthermore, since the eigenvalues of = associated with each principal axis are the reciprocals of the corresponding eigenvalues of A , the lengths of the principal semi-axes of the force ellipsoid √ / are given by 1 λ , where λ . Thus the force ellipsoid are the eigenvalues of A i i is obtained from the manipulability ellipsoid simply by stretching the manipu- i by a factor 1 /λ lability ellipsoid along each principal axis . Furthermore, since i the volume V of the manipulability ellipsoid is proportional to the product of A √ the semi-axes, λ of the force ellipsoid is propor- λ V ··· , and the volume 2 B 1 √ / λ inde- λ is constant ··· , the product of the two volumes tional to 1 V V B 2 1 A pendently of the joint variables θ . Therefore, positioning the robot to increase the manipulability-ellipsoid volume measure μ ) simultaneously reduces the ( A 3 μ ( B ). This also explains the observation made force-ellipsoid volume measure 3 at the start of the chapter that, as the robot approaches a singularity, V goes A to zero while V goes to infinity. B May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

218 200 5.5. Summary 5.5 Summary n Let the forward kinematics of an • -link open chain be expressed in the following product of exponentials form: S [ ] θ ] θ S [ n n 1 1 M. e ··· e ) = T ( θ n n × 6 ̇ ∈ ) θ relates the joint rate vector R θ ∈ R ( to The space Jacobian J s ̇ the spatial twist V V = J ) is given by ( θ ) , via θ . The i th column of J θ ( s s s s ( ) = Ad θ J , ) S ( ] S [ θ [ θ ] S si i 1 i 1 i − − 1 1 ··· e e i = 2 , with the first column J for ,...,n = S J . The screw vector 1 1 s si is expressed in space-frame coordinates, with the joint values θ for joint i assumed to be arbitrary rather than zero. Let the forward kinematics of an n -link open chain be expressed in the • following product of exponentials form: θ ] B [ [ B θ ] n 1 1 n T ) = θ ( ··· . Me e 6 × n n ̇ ∈ R J θ ( relates the joint rate vector The body Jacobian θ ∈ R ) to b ̇ V th column = ( ω i ,v . The ) via V θ = J the end-effector body twist ( θ ) b b b b b J ( θ ) is given by of b , ( θ ) = Ad ) B ( J − [ B ] θ θ ] bi i B [ − +1 i i +1 n n e ··· e i = n − 1 ,..., 1, with J is = B for . The screw vector J i for joint bi bn n expressed in body-frame coordinates, with the joint values θ assumed to be arbitrary rather than zero. • The body and space Jacobians are related via J , ( θ ) = [Ad ) θ ] J ( b s T sb ) θ ) = [Ad ( ] J ( θ J , b s T bs where ). = T ( T θ sb • Consider a spatial open chain with n one-dof joints that is assumed to be n τ denote the vector of the joint torques in static equilibrium. Let R ∈ 6 and forces and F ∈ R be the wrench applied at the end-effector, in either and F are related by space- or body-frame coordinates. Then τ T T τ = ( θ ) F F = J . ( θ ) J b s b s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

219 Chapter 5. Velocity Kinematics and Statics 201 A kinematically singular configuration for an open chain, or more simply • n ∈ at which the rank of θ R a kinematic singularity, is any configuration the Jacobian is not maximal. For six-dof spatial open chains consisting of revolute and prismatic joints, some common singularities include (i) two collinear revolute joint axes; (ii) three coplanar and parallel revolute joint axes; (iii) four revolute joint axes intersecting at a common point; (iv) four coplanar revolute joints; and (v) six revolute joints intersecting a common line. • The manipulability ellipsoid describes how easily the robot can move in different directions. For a Jacobian J , the principal axes of the manip- T JJ and the corre- ulability ellipsoid are defined by the eigenvectors of sponding lengths of the principal semi-axes are the square roots of the eigenvalues. The force ellipsoid describes how easily the robot can generate forces in • different directions. For a Jacobian J , the principal axes of the force − T 1 JJ ellipsoid are defined by the eigenvectors of ( and the corresponding ) lengths of the principal semi-axes are the square roots of the eigenvalues. • Measures of the manipulability and force ellipsoids include the ratio of the longest principal semi-axis to the shortest; the square of this measure; and the volume of the ellipsoid. The first two measures indicate that the robot is far from being singular if they are small (close to 1). 5.6 Software Software functions associated with this chapter are listed below. Jb = JacobianBody(Blist,thetalist) n × 6 B θ ∈ R ( Computes the body Jacobian J given a list of joint screws ) ex- i b pressed in the body frame and a list of joint angles. Js = JacobianSpace(Slist,thetalist) 6 × n ( θ ) ∈ R Computes the space Jacobian J ex- given a list of joint screws S i s pressed in the fixed space frame and a list of joint angles. 5.7 Notes and References One of the key advantages of the PoE formulation is in the derivation of the Jacobian; the columns of the Jacobian are simply the (configuration-dependent) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

220 202 5.8. Exercises b ˆy } ˆy b b { b ˆx b { } ˆy s o ˆx b ˆx s } { s A rolling wheel. Figure 5.14: screws for the joint axes. Compact closed-form expressions for the columns of the Jacobian are also obtained because taking derivatives of matrix exponentials is particularly straightforward. There is extensive literature on the singularity analysis of 6R open chains. In addition to the three cases presented in this chapter, other cases are examined in [122] and in exercises at the end of this chapter, including the case when some of the revolute joints are replaced by prismatic joints. Many of the mathematical techniques and analyses used in open chain singularity analysis can also be used to determine the singularities of parallel mechanisms, which are the subject of Chapter 7. The concept of a robot’s manipulability was first formulated in a quantitative way by Yoshikawa [195]. There is now a vast literature on the manipulability analysis of open chains, see, e.g., [75, 134]. 5.8 Exercises Exercise 5.1 A wheel of unit radius is rolling to the right at a rate of 1 rad/s (see Figure 5.14; the dashed circle shows the wheel at = 0). t V (a) Find the spatial twist t ) as a function of t . ( s (b) Find the linear velocity of the { b } -frame origin expressed in { s } -frame coordinates. The 3R planar open chain of Figure 5.15(a) is shown in its zero Exercise 5.2 position. (a) Suppose that the tip must apply a force of 5 N in the ˆx } -direction of the { s s frame, with zero component in the ˆy -direction and zero moment about s the ˆz axis. What torques should be applied at each joint? s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

221 Chapter 5. Velocity Kinematics and Statics 203 ) y ( x, L 4 θ 4 θ 3 L 3 L 1 m 2 1 m L θ 2 1 ˆy ˆy s s ◦ 1 m 45 θ 1 ˆx s ˆx s (b) (a) (a) A 3R planar open chain. The length of each link is 1 m. (b) A 4R Figure 5.15: planar open chain. -direction. (b) Suppose that now the tip must apply a force of 5 N in the ˆy s What torques should be applied at each joint? Exercise 5.3 Answer the following questions for the 4R planar open chain of Figure 5.15(b). (a) For the forward kinematics of the form [ S ] θ θ ] [ S θ [ S ] [ S θ ] 1 1 2 2 4 3 3 4 e ) = θ ( T e e M, e 3 M SE (2) and each S . = ( write down ω ,v R ,v ∈ ) ∈ xi zi i yi (b) Write down the body Jacobian. (c) Suppose that the chain is in static equilibrium at the configuration θ = 1 θ 0) and a moment = 0 ,θ , = π/ 2 ,θ 10 = − π/ 2 and that a force f = (10 , 4 3 2 and = (0 0 , m f , m are expressed with 10) are applied to the tip (both respect to the fixed frame). What are the torques experienced at each joint? (d) Under the same conditions as (c), suppose that a force f = ( − 10 , 10 , 0) − and a moment = (0 , 0 , m 10), also expressed in the fixed frame, are applied to the tip. What are the torques experienced at each joint? (e) Find all kinematic singularities for this chain. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

222 204 5.8. Exercises ˆz b1 finger 2 finger 1 ˆz b2 ˆz b ˆy b1 } } b { { b 1 ˆy { b } ˆx 2 b2 b ˆx b1 LL ˆz s { } s ˆy s ˆx s Figure 5.16: Two fingers grasping a can. Figure 5.16 shows two fingers grasping a can. Frame { b } is Exercise 5.4 { attached to the center of the can. Frames } and { b b } are attached to the 1 2 f = ( f ,f ,f ) is the can at the two contact points as shown. The force 1 ,y 1 1 ,z ,x 1 { } coordinates. Similarly, b force applied by fingertip 1 to the can, expressed in 1 ) is the force applied by fingertip 2 to the can, expressed in f f ,f ,f = ( 2 2 ,y 2 2 ,z ,x { b } coordinates. 2 (a) Assume that the system is in static equilibrium, and find the total wrench applied by the two fingers to the can. Express your result in { b } F b coordinates. F is an arbitrary external wrench applied to the can ( F (b) Suppose that ext ext { is also expressed in frame- } coordinates). Find all F that cannot be b ext resisted by the fingertip forces. Exercise 5.5 Referring to Figure 5.17, a rigid body, shown at the top right, ̇ rotates about the point ( ) with angular velocity θ = 1. L,L (a) Find the position of point P on the moving body relative to the fixed reference frame { } in terms of θ . s P (b) Find the velocity of point in terms of the fixed frame. T , as seen from the fixed frame (c) What is { b } , the configuration of frame sb { s } ? (d) Find the twist of in body coordinates. T sb (e) Find the twist of T in space coordinates. sb (f) What is the relationship between the twists from (d) and (e)? ̇ (g) What is the relationship between the twist from (d) and P from (b)? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

223 Chapter 5. Velocity Kinematics and Statics 205 θ ˆy b d } { b P ˆx b L ˆy s s } { ˆx s O L Figure 5.17: A rigid body rotating in the plane. ̇ (h) What is the relationship between the twist from (e) and P from (b)? Figure 5.18 shows a design for a new amusement park ride. A Exercise 5.6 { b } . The fixed frame rider sits at the location indicated by the moving frame s } is attached to the top shaft as shown. The dimensions indicated in the { R = 10 m and L = 20 m, and the two joints each rotate at a constant figure are angular velocity of 1 rad/s. (a) Suppose t = 0 at the instant shown in the figure. Find the linear velocity v . Express your and angular velocity ω t of the rider as functions of time b b { } coordinates. answer in frame- b p be the linear coordinates expressing the position of the rider in { s } . (b) Let p ( t ). Find the linear velocity ̇ Exercise 5.7 The RRP robot in Figure 5.19 is shown in its zero position. (a) Write down the screw axes in the space frame. Evaluate the forward ◦ ◦ = (90 90 kinematics when , 1). Hand-draw or use a computer to show , θ the arm and the end-effector frame in this configuration. Obtain the space J Jacobian for this configuration. s (b) Write down the screw axes in the end-effector body frame. Evaluate the ◦ ◦ = (90 forward kinematics when , 90 θ , 1) and confirm that you get the same result as in part (a). Obtain the body Jacobian J for this configu- b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

224 206 5.8. Exercises wa ll ˆz s ̇ θ 1 { s } wa ll ˆy s ˆx s L ̇ θ 2 R ˆz b ˆy b ˆx { b } b Figure 5.18: A new amusement park ride. 3 θ 2 θ 3 ˆy b θ 1 ˆx b 2 ˆz b } { b { s } ˆz s ˆy s ˆx s Figure 5.19: RRP robot shown in its zero position. ration. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

225 Chapter 5. Velocity Kinematics and Statics 207 θ 3 θ 2 ˆz b ˆz s θ 1 ˆy { b } s s } { ˆx ˆy s b L L L ˆx b Figure 5.20: RPR robot. The RPR robot of Figure 5.20 is shown in its zero position. The Exercise 5.8 s and { b } . fixed and end-effector frames are respectively denoted } { 3 ( (a) Find the space Jacobian ) for arbitrary configurations θ ∈ R J . θ s (b) Assume the manipulator is in its zero position. Suppose that an external 3 ∈ R force is applied to the { b } frame origin. Find all the directions in f f = 0. which τ can be resisted by the manipulator with Exercise 5.9 Find the kinematic singularities of the 3R wrist given the forward kinematics [ˆ ω ω θ ] [ˆ ω ] θ θ [ˆ ] 1 1 3 3 2 2 e e = , R e √ √ , = (0 , 1), ˆ ω = (1 / ω where ˆ 0 , 2 , 0 , 1 / 0). 2), and ˆ ω 0 = (1 , 1 2 3 n Exercise 5.10 -link open chain we derive the analytic In this exercise, for an Jacobian corresponding to the exponential coordinates on SO (3). × n (a) Given an A ( t ) parametrized by t that is also differentiable n matrix ) t ( A × t ( X , its exponential t is then an n e n matrix ) = with respect to that is always nonsingular. Prove the following: ∫ 1 − 1 s − A ( t ) s ) A ( t ̇ ̇ A ( t ) e e ds, X = X 0 ∫ 1 1 ) − − t s A ( t ) s A ( ̇ ̇ t e ( A e ) = XX ds. 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

226 208 5.8. Exercises (Hint: The formula ∫ t d ) t A + A B As ) ( ( t − s | ds e = e Be  =0 d 0 may be useful.) 3 [ ( t )] r and R (b) Use the result above to show that, for t ) = e r ( t ) ∈ , the R ( T ̇ , is related to ̇ ω R R r by angular velocity in the body frame, [ ] = b = A ω r ) ̇ r, ( b ‖ r ‖ sin ‖− r ‖ cos ‖ r ‖ 1 − 2 ] r ] + . [ r [ ) = r ( A − I 3 2 r ‖ ‖ r ‖ ‖ (c) Derive the corresponding formula relating the angular velocity in the space T ̇ , to ̇ RR frame, [ ω r . ] = s Exercise 5.11 The spatial 3R open chain of Figure 5.21 is shown in its zero p be the coordinates of the origin of { b } expressed in { s } . position. Let (a) In its zero position, suppose we wish to make the end-effector move with linear velocity ̇ = (10 , 0 , 0). What are the required input joint velocities p ̇ ̇ ̇ θ , θ , and ? θ 3 1 2 ◦ ◦ . = 0 ,θ = 45 (b) Suppose that the robot is in the configuration θ ,θ 45 = − 3 2 1 Assuming static equilibrium, suppose that we wish to generate an end- effector force = (10 , 0 , 0), where f f is expressed with respect to the b b end-effector frame } . What are the required input joint torques τ b ,τ { , 1 2 and τ ? 3 (c) Under the same conditions as in (b), suppose that we now seek to generate m is expressed with respect = (10 , 0 , 0), where m an end-effector moment b b { b . What are the required input joint torques to the end-effector frame } ,τ ,τ ? τ 1 3 2 (d) Suppose that the maximum allowable torques for each joint motor are τ 5. ‖≤ ‖ , ‖ τ ‖≤ ‖≤ 20 , and ‖ τ 10 3 2 1 In the zero position, what is the maximum force that can be applied by the tip in the end-effector-frame ˆx-direction? Exercise 5.12 The RRRP chain of Figure 5.22 is shown in its zero position. { Let b } expressed in { s } . p be the coordinates of the origin of J = ( θ ) when θ . (a) Determine the body Jacobian θ L = 0 ,θ = = π/ 2 ,θ 4 3 2 1 b ̇ ̇ ̇ ̇ = 1. = θ θ = 0 ,θ (b) Find ̇ = π/ 2 ,θ = = L and p θ θ = when θ θ = 4 2 3 4 3 1 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

227 Chapter 5. Velocity Kinematics and Statics 209 LL ˆz b ˆy b θ 2 L ˆx b b } { θ 3 θ 1 2 L ˆz s ˆy s ˆx s s { } Figure 5.21: A spatial 3R open chain. θ θ 4 3 ˆz b L ˆy b } { b ˆx b L θ θ 2 1 ˆz s ˆy s ˆx s } { s Figure 5.22: An RRRP spatial open chain. Exercise 5.13 For the 6R spatial open chain of Figure 5.23, (a) Determine its space Jacobian ( θ ). J s (b) Find its kinematic singularities. Explain each singularity in terms of the alignment of the joint screws and of the directions in which the end-effector loses one or more degrees of freedom of motion. Exercise 5.14 Show that a six-dof spatial open chain is at a kinematic sin- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

228 210 5.8. Exercises LLL ˆz s ˆz b ˆy ˆx ˆy s ˆx s b b } { b Singularities of a 6R open chain. Figure 5.23: Prismatic joint axis Revolute joint axis A kinematic singularity involving prismatic and revolute joints. Figure 5.24: gularity when any two of its revolute joint axes are parallel, and any prismatic joint axis is normal to the plane spanned by the two parallel revolute joint axes (see Figure 5.24). The spatial PRRRRP open chain of Figure 5.25 is shown in Exercise 5.15 its zero position. (a) At the zero position, find the first three columns of the space Jacobian. (b) Find all configurations for which the first three columns of the space Ja- cobian become linearly dependent. θ = = θ = θ = θ (c) Suppose that the chain is in the configuration 1 2 5 3 ◦ θ = 0, θ = 90 . Assuming static equilibrium, suppose that a pure force 4 6 f is expressed in terms of the end-effector frame, = (10 , 0 , 10), where f b b is applied to the origin of the end-effector frame. Find the torques τ , ,τ 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

229 Chapter 5. Velocity Kinematics and Statics 211 ˆz L s L L { s } θ 2 ˆy s L ˆx θ L s 3 L θ 4 L ˆz b θ 1 θ 5 θ 6 ˆx ˆy b b b } { A spatial PRRRRP open chain. Figure 5.25: θ θ 2 5 ˆz s θ 3 ˆz } s { b θ θ 4 1 } b { ˆy b ˆy ˆx s s ˆx b 100 N θ 6 L Figure 5.26: A PRRRRR spatial open chain. τ experienced at the first three joints. and 3 Exercise 5.16 Consider the PRRRRR spatial open chain of Figure 5.26, shown in its zero position. The distance from the origin of the fixed frame to the origin of the end-effector frame at the home position is . L (a) Determine the first three columns of the space Jacobian J . s (b) Determine the last two columns of the body Jacobian J . b L is the home position a singularity? (c) For what value of (d) In the zero position, what joint forces and torques τ must be applied in order to generate a pure end-effector force of 100 N in the − ˆz -direction? b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

230 212 5.8. Exercises ˆz s } { s ˆy s θ 3 ˆx s L θ 2 L ˆz b ◦ } b { 45 L LL ˆy b θ ˆx 6 θ b 1 θ 4 θ 5 L A PRRRRP robot. Figure 5.27: Exercise 5.17 The PRRRRP robot of Figure 5.27 is shown in its zero position. J θ ( (a) Find the first three columns of the space Jacobian ). s ̇ θ = (1 , 0 , 1 , − (b) Assuming the robot is in its zero position and , 2 , 0), find 1 the spatial twist V . s (c) Is the zero position a kinematic singularity? Explain your answer. Exercise 5.18 The six-dof RRPRPR open chain of Figure 5.28 has a fixed frame { } and an end-effector frame { b } attached as shown. At its zero position, s joint axes 1, 2, and 6 lie in the ˆy–ˆz-plane of the fixed frame, and joint axis 4 is aligned along the fixed-frame ˆx-axis. (a) Find the first three columns of the space Jacobian J ( θ ). s ̇ . θ = (1 , 0 , 1 , − 1 , 2 , 0). Find the spatial twist V (b) At the zero position, let s (c) Is the zero position a kinematic singularity? Explain your answer. The spatial PRRRRP open chain of Figure is shown in Exercise 5.19 5.29 its zero position. J ( (a) Determine the first four columns of the space Jacobian θ ). s (b) Determine whether the zero position is a kinematic singularity. (c) Calculate the joint forces and torques required for the tip to apply the following end-effector wrenches: (i) F 0). = (0 , 1 , − 1 , 1 , 0 , s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

231 Chapter 5. Velocity Kinematics and Statics 213 L L θ 3 θ 5 θ 2 θ 4 L θ 6 L L ˆz b θ 1 L ˆy b ˆz s ˆx ◦ b } { b 45 ˆy s ˆx s s { } An RRPRPR open chain shown at its zero position. Figure 5.28: θ 11 4 ˆz b ˆz s s } { ˆy θ 3 s ˆy 1 b ˆx b θ θ ˆx } { b 5 6 s ◦ 1 45 θ 2 1 θ 1 Figure 5.29: A spatial PRRRRP open chain with a skewed joint axis. (ii) F 1). = (1 , − 1 , 0 , 1 , 0 , − s Exercise 5.20 The spatial RRPRRR open chain of Figure 5.30 is shown in its zero position. } and tool (end-effector) frame { t } as shown, express 0 (a) For the fixed frame { the forward kinematics in the product of exponentials form ] θ [ S θ θ [ [ S ] θ S [ S ] θ [ S ] θ [ S ] ] 3 2 5 1 3 5 4 6 6 1 4 2 e e T e ( θ e ) = e e M. (b) Find the first three columns of the space Jacobian ( θ ). J s ′ { 0 } is moved to another location { 0 (c) Suppose that the fixed frame } as shown in the figure. Find the first three columns of the space Jacobian May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

232 214 5.8. Exercises L L 2 1 θ 4 θ 3 ˆz L 1 } { t θ 5 ˆy θ 2 L θ ˆx 0 6 θ 1 ˆz ˆy ˆz ˆy ˆx } { 0 ˆx { } 0 A spatial RRPRRR open chain. Figure 5.30: ) with respect to this new fixed frame. ( J θ s (d) Determine whether the zero position is a kinematic singularity and, if so, provide a geometric description in terms of the joint screw axes. Exercise 5.21 Figure 5.31 shows an RRPRRR exercise robot used for stroke patient rehabilitation. M (a) Assume the manipulator is in its zero position. Suppose that ∈ SE (3) 0 c { 0 } to frame { c } is the displacement from frame M (3) is the ∈ SE and ct } { } to frame { displacement from frame c . Express the forward kinematics t T in the form 0 t θ ] A [ θ [ A ] ] θ A [ [ A θ ] θ ] A [ θ ] [ A 6 3 3 6 5 1 4 2 2 4 5 1 e e M M e e e = . e T 0 c ct 0 t A A , Find . , and A 5 2 4 ◦ and all the other joint variables are fixed at zero. = 90 (b) Suppose that θ 2 ̇ ̇ ̇ ̇ ̇ ̇ θ , , , θ 0 θ 1), and find θ , , , θ 0 , Set the joint velocities to ( θ , ) = (1 , 0 , 1 6 5 3 2 1 4 } V the spatial twist { 0 coordinates. in frame- s (c) Is the configuration described in part (b) a kinematic singularity? Explain your answer. (d) Suppose that a person now operates the rehabilitation robot. At the configuration described in part (b), a wrench F is applied to the elbow F is applied to the last link. Both F and elbow link, and a wrench tip elbow F = are expressed in frame { 0 } coordinates and are given by F elbow tip May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

233 215 Chapter 5. Velocity Kinematics and Statics θ 1 θ 5 θ 6 θ 4 θ 2 θ 3 (a) Rehabilitation robot ARMin III [123]. Figure courtesy of ETH Z ̈urich. ˆy t L ˆz ˆx ˆz 0 t t θ 1 } { t ˆx 0 ˆy 0 ˆz c { } 0 L c } { ˆx Elbow link c ˆy c Last link L L θ θ 5 6 L L θ 2 L L θ 4 L L θ 3 (b) Kinematic model of the ARMin III. Figure 5.31: The ARMin III rehabilitation robot. (1 , 0 , 0 , 0 , 0 , 1) and F 0). Find the joint forces and torques = (0 , 1 , 0 , 1 , 1 , tip May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

234 216 5.8. Exercises that must be applied for the robot to maintain static equilibrium. τ n -link open chain, with reference frames attached Exercise 5.22 Consider an to each link. Let [ S ] θ ] [ θ S 1 1 k k = 1 ··· , k e M ,...,n e = T k 0 k { k . Let J be the forward kinematics up to link frame ( θ ) be the space Jacobian } s ) has columns T for J as shown below: ( θ J ; 0 si s n ] [ ( J J ) θ ··· ( θ ) θ ( ) = J . 1 sn s s − 1 ̇ Let [ T coordinates. V T ] = } 0 { be the twist of link frame { k } in fixed frame k 0 k k 0 (a) Derive explicit expressions for V and V . 2 3 V (b) On the basis of your results from (a), derive a recursive formula for +1 k ̇ . , J θ in terms of ,...,J V , and +1 s,k s k 1 Exercise 5.23 Write a program that allows the user to enter the link lengths L of a 2R planar robot (Figure 5.32) and a list of robot configurations and L 1 2 )) and plots the manipulability ellipse θ (each defined by the joint angles ( ,θ 1 2 at each of those configurations. The program should plot the arm (as two line segments) at each configuration and the manipulability ellipse centered at the endpoint of the arm. Choose the same scaling for all the ellipses so that they can be easily visualized (e.g., the ellipse should usually be shorter than the arm but not so small that you cannot easily see it). The program should also print μ ,μ , and μ for each configuration. the three manipulability measures 3 1 2 L = L = 1 and plot the arm and its manipulability ellipse at the (a) Choose 2 1 ◦ ◦ ◦ ◦ ◦ ◦ ◦ ◦ , (60 − , 60 10 ) , (135 160 , 90 , ) , and (190 20 , four configurations ( ) ). At which of these configurations does the manipulability ellipse appear most isotropic? Does this agree with the manipulability measures calculated by the program? (b) Does the ratio of the length of the major axis of the manipulability ellipse and the length of the minor axis depend on ? On θ ? Explain your θ 2 1 answers. ◦ ◦ L = 1. Hand-draw the following: the arm at ( − 45 = , 90 (c) Choose ); L 2 1 ̇ ̇ θ the endpoint linear velocity vector arising from = 1 rad/s and θ = 0; 2 1 ̇ ̇ the endpoint linear velocity vector arising from = 0 and θ θ = 1 rad/s; 2 1 and the vector sum of these two vectors to get the endpoint linear velocity ̇ ̇ when = 1 rad/s. = 1 rad/s and θ θ 2 1 Exercise 5.24 Modify the program in the previous exercise to plot the force May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

235 Chapter 5. Velocity Kinematics and Statics 217 ˆx 2 θ L 2 2 L 1 θ 1 ˆx 1 Left: The 2R robot arm. Right: The arm at four different configura- Figure 5.32: tions. ellipse. Demonstrate it at the same four configurations as in the first part of the previous exercise. Exercise 5.25 The kinematics of the 6R UR5 robot are given in Section 4.1.2. J (a) Give the numerical space Jacobian when all joint angles are π/ 2. Sep- s arate the Jacobian matrix into an angular velocity portion J (the joint ω rates act on the angular velocity) and a linear velocity portion (the J v joint rates act on the linear velocity). (b) For this configuration, calculate the directions and lengths of the prin- cipal semi-axes of the three-dimensional angular-velocity manipulability ellipsoid (based on J ) and the directions and lengths of the principal ω semi-axes of the three-dimensional linear-velocity manipulability ellipsoid (based on J ). v (c) For this configuration, calculate the directions and lengths of the principal semi-axes of the three-dimensional moment (torque) force ellipsoid (based J ) and the directions and lengths of the principal semi-axes of the on ω J ). three-dimensional linear force ellipsoid (based on v The kinematics of the 7R WAM robot are given in Section 4.1.3. Exercise 5.26 J when all joint angles are π/ 2. Sep- (a) Give the numerical body Jacobian b arate the Jacobian matrix into an angular-velocity portion J (the joint ω rates act on the angular velocity) and a linear-velocity portion (the J v joint rates act on the linear velocity). (b) For this configuration, calculate the directions and lengths of the prin- cipal semi-axes of the three-dimensional angular-velocity manipulability ellipsoid (based on J ) and the directions and lengths of the principal ω semi-axes of the three-dimensional linear-velocity manipulability ellipsoid (based on J ). v (c) For this configuration, calculate the directions and lengths of the principal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

236 218 5.8. Exercises semi-axes of the three-dimensional moment (torque) force ellipsoid (based on J ) and the directions and lengths of the principal semi-axes of the ω three-dimensional linear force ellipsoid (based on J ). v Exercise 5.27 Examine the software functions for this chapter in your favorite programming language. Verify that they work in the way that you expect. Can you make them more computationally efficient? May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

237 Chapter 6 Inverse Kinematics n For a general ( θ ), degree-of-freedom open chain with forward kinematics T n R , the inverse kinematics problem can be stated as follows: given a ho- ∈ θ ∈ SE (3), find solutions θ that satisfy T mogeneous transform θ ) = X . To X ( highlight the main features of the inverse kinematics problem, let us examine the two-link planar open chain of Figure 6.1(a) as a motivational example. Con- sidering only the position of the end-effector and ignoring its orientation, the forward kinematics can be expressed as ] [ ] [ θ x cos θ L + L ) cos( θ + 1 1 2 2 1 (6.1) . = θ L + L y sin( θ sin + θ ) 2 1 2 1 1 Assuming L , the set of reachable points, or the workspace, is an annulus > L 2 1 L L L of inner radius and outer radius L . Given some end-effector + − 2 1 2 1 position ( x,y ), it is not hard to see that there will be either zero, one, or two solutions depending on whether ( ) lies in the exterior, boundary, or x,y interior of this annulus, respectively. When there are two solutions, the angle at the second joint (the “elbow” joint) may be positive or negative. These two solutions are sometimes called “lefty” and “righty” solutions, or “elbow-up” and “elbow-down” solutions. θ ) is also not difficult. ,θ Finding an explicit solution ( ) for a given ( x,y 1 2 For this purpose, we will find it useful to introduce the two-argument arctan- gent function atan2( y,x ), which returns the angle from the origin to a point − 1 ( ( y/x ), but whereas ) in the plane. It is similar to the inverse tangent tan x,y 1 − 1 1 − − y/ tan ( − ( − x ), and therefore tan y/x ) is equal to tan only returns angles 2), the atan2 function returns angles in the range ( in the range ( π/ 2 ,π/ − − π,π ]. For this reason, atan2 is sometimes called the four-quadrant arctangent. 219

238 220 ) y x, ( L 2 θ L 2 1 L 2 2 2 + x y θ 1 β θ 2 γ α L 1 θ 1 rkspace Wo (a) A workspace, and lefty and righty (b) Geometric solution. configurations. Figure 6.1: Inverse kinematics of a 2R planar open chain. We also recall the law of cosines, 2 2 2 c + b = − 2 ab cos C, a is the a , and c are the lengths of the three sides of a triangle and C b , where c . interior angle of the triangle opposite the side of length 6.1(b), angle β , restricted to lie in the interval [0 ,π ], can Referring to Figure be determined from the law of cosines, 2 2 2 2 L L = − 2 L y L + cos β , x + 2 1 1 2 from which it follows that ( ) 2 2 2 2 − + L L y − x 2 1 1 − β = cos . L L 2 1 2 Also from the law of cosines, ) ( 2 2 2 2 − L y x + L + 2 1 − 1 √ . = cos α 2 2 + y 2 L x 1 The angle γ is determined using the two-argument arctangent function, γ = atan2( ). With these angles, the righty solution to the inverse kinematics is y,x θ β = γ − α, θ − = π 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

239 Chapter 6. Inverse Kinematics 221 and the lefty solution is θ γ + α, θ π. = = − β 2 1 2 2 + y If lies outside the range [ L ] then no solution exists. − L x ,L L + 1 2 1 2 This simple motivational example illustrates that, for open chains, the in- verse kinematics problem may have multiple solutions; this situation is in con- trast with the forward kinematics, where a unique end-effector displacement T . In fact, three-link planar open chains have an in- θ exists for given joint values x,y ) lying in the interior of the workspace; finite number of solutions for points ( in this case the chain possesses an extra degree of freedom and is said to be kinematically redundant. In this chapter we first consider the inverse kinematics of spatial open chains with six degrees of freedom. At most a finite number of solutions exists in this case, and we consider two popular structures – the PUMA and Stanford robot arms – for which analytic inverse kinematic solutions can be easily obtained. For more general open chains, we adapt the Newton–Raphson method to the inverse kinematics problem. The result is an iterative numerical algorithm which, pro- vided that an initial guess of the joint variables is sufficiently close to a true solution, converges quickly to that solution. 6.1 Analytic Inverse Kinematics We begin by writing the forward kinematics of a spatial six-dof open chain in the following product of exponentials form: θ ] S [ θ [ S ] ] θ S [ [ S θ ] θ ] S [ S [ ] θ 5 3 2 2 4 1 4 1 5 3 6 6 M. e e e e e θ T ) = e ( X ∈ SE (3), the inverse kinematics problem is Given some end-effector frame 6 X θ to find solutions satisfying T ( θ ) = R . In the following subsections we ∈ derive analytic inverse kinematic solutions for the PUMA and Stanford arms. 6.1.1 6R PUMA-Type Arm We first consider a 6R arm of the PUMA type. Referring to Figure 6.2, when the arm is placed in its zero position: (i) the two shoulder joint axes intersect orthogonally at a common point, with joint axis 1 aligned in the ˆz -direction and 0 − ˆy -direction; (ii) joint axis 3 (the elbow joint) lies in joint axis 2 aligned in the 0 the ˆx -plane and is aligned parallel with joint axis 2; (iii) joint axes 4, 5, and –ˆy 0 0 6 (the wrist joints) intersect orthogonally at a common point (the wrist center) to form an orthogonal wrist and, for the purposes of this example, we assume May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

240 222 6.1. Analytic Inverse Kinematics ˆz 0 p z a 3 θ 3 a 2 ˆy θ 0 2 r θ 1 p x p y ˆx 0 Inverse position kinematics of a 6R PUMA-type arm. Figure 6.2: ˆz d 0 1 ˆy 0 ˆy 0 p y p y r r α φ d 1 θ 1 p x p x ˆx ˆx 0 θ 0 1 d 1 (a) Elbow arm (b) Kinematic diagram. with offset. Figure 6.3: A 6R PUMA-type arm with a shoulder offset. -, and ˆx -, ˆy -directions, respectively. that these joint axes are aligned in the ˆz 0 0 0 , respectively. The arm may also have a and a The lengths of links 2 and 3 are 2 3 an offset at the shoulder (see Figure 6.3). The inverse kinematics problem for PUMA-type arms can be decoupled into inverse-position and inverse-orientation subproblems, as we now show. We first consider the simple case of a zero-offset PUMA-type arm. Referring to Figure 6.2 and expressing all vectors in terms of fixed-frame coordinates, 3 p R p by p denote the components of the wrist center ∈ ). Projecting ,p ,p = ( y z x p onto the ˆx -plane, it can be seen that –ˆy 0 0 θ . = atan2( p ) ,p x y 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

241 Chapter 6. Inverse Kinematics 223 ˆz 0 Singular configuration of the zero-offset 6R PUMA-type arm. Figure 6.4: Note that a second valid solution for is given by θ 1 p θ ,p π, ) + = atan2( x y 1 π − θ . As long as θ ,p when the original solution for 6 = 0 is replaced by p 2 x 2 y p = 0 the arm is in a singular both these solutions are valid. When = p x y configuration (see Figure 6.4), and there are infinitely many possible solutions θ for . 1 If there is an offset d 6 = 0 as shown in Figure 6.3, then in general there will 1 , the righty and lefty solutions (Figure 6.3). As seen from θ be two solutions for 1 √ 2 2 φ − α where φ = atan2( p θ ,p the figure, ) and α = atan2( d = , ). r − d y x 1 1 1 The second solution is given by √ ( ) 2 2 2 θ π ) + atan2 = − + atan2( p ,d p + p ,p − d . 1 y 1 x y x 1 and θ for the PUMA-type arm now reduces to the inverse Determining angles θ 3 2 kinematics problem for a planar two-link chain: 2 2 2 2 2 − a a − + p − r d z 3 2 1 θ cos = 3 a a 2 2 3 2 2 2 2 2 2 a − a − p + − d p + p z y x 3 2 1 = D. = 2 a a 2 3 Using D defined above, θ is given by 3 ( ) √ 2 = atan2 ± θ − D 1 ,D 3 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

242 224 6.1. Analytic Inverse Kinematics Four possible inverse kinematics solutions for the 6R PUMA-type arm Figure 6.5: with shoulder offset. θ and can be obtained in a similar fashion as 2 ( ) √ 2 2 = atan2 r p d , θ − ) − c a a s + ,a atan2 ( z 2 3 3 3 3 2 1 √ ) ( 2 2 2 p + p − d ) , , p c = atan2 − atan2 ( a a s + ,a 2 3 3 3 z 3 y x 1 correspond to = sin θ = cos and c θ where s θ . The two solutions for 3 3 3 3 3 the well-known elbow-up and elbow-down configurations for the two-link planar arm. In general, a PUMA-type arm with an offset will have four solutions to the inverse position problem, as shown in Figure 6.5; the postures in the upper panel are lefty solutions (elbow-up and elbow-down), while those in the lower panel are righty solutions (elbow-up and elbow-down). We now solve the inverse orientation problem of finding ( ,θ ,θ ) given the θ 4 5 6 end-effector orientation. This problem is completely straightforward: having ,θ ,θ ), the forward kinematics can be manipulated into the form θ found ( 1 2 3 S − ] θ θ [ S ] ] θ 1 [ S S ] θ [ [ − [ S − ] θ θ − [ S ] 1 2 2 3 6 6 3 5 4 1 4 5 e = e e e XM e e , (6.2) , where the right-hand side is now known, and the , and -components of S S ω 4 5 i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

243 Chapter 6. Inverse Kinematics 225 ˆz 0 a 2 d 3 p z θ 2 r s p y ˆy 0 d 1 θ 1 p x ˆx 0 Figure 6.6: The first three joints of a Stanford-type arm. S are 6 = (0 ω , 0 , 1) , 4 ω = (0 , 1 , 0) , 5 , = (1 , 0 ω 0) . 6 Denoting the (3) component of the right-hand side of Equation (6.2) by R , SO θ ) can be determined as the solution to ,θ ,θ the wrist joint angles ( 4 6 5 Rot(ˆz ,θ ,θ )Rot(ˆy )Rot(ˆx ,θ R, ) = 6 5 4 B. which correspond exactly to the ZYX Euler angles, derived in Appendix 6.1.2 Stanford-Type Arms If the elbow joint in a 6R PUMA-type arm is replaced by a prismatic joint, as shown in Figure 6.6, we then have an RRPRRR Stanford-type arm. Here we 6.6; the inverse consider the inverse position kinematics for the arm of Figure orientation kinematics is identical to that for the PUMA-type arm and so is not repeated here. θ can be found in similar fashion to the PUMA-type The first joint variable 1 θ are not both zero). The arm: p ,p ) (provided that p and p = atan2( x x y y 1 variable is then found from Figure 6.6 to be θ 2 , = atan2( s,r ) θ 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

244 226 6.2. Numerical Inverse Kinematics 2 2 2 r where + p p . Similarly to the case of the PUMA-type and s = p = − d 1 z y x arm, a second solution for θ θ is given by and 1 2 , = π + atan2( p θ ,p ) x y 1 − atan2( s,r ) . π = θ 2 The translation distance is found from the relation θ 3 2 2 2 a s ) ( = r + + θ 2 3 as √ √ 2 2 2 2 2 = θ + ( = . + p + a s p p − d − ) r 3 1 2 z y x Ignoring the negative square root solution for θ , we obtain two solutions to 3 the inverse position kinematics as long as the wrist center does not intersect p the ˆz -axis of the fixed frame. If there is an offset then, as in the case of the 0 PUMA-type arm, there will be lefty and righty solutions. Numerical Inverse Kinematics 6.2 Iterative numerical methods can be applied if the inverse kinematics equations do not admit analytic solutions. Even in cases where an analytic solution does exist, numerical methods are often used to improve the accuracy of these solu- tions. For example, in a PUMA-type arm, the last three axes may not exactly intersect at a common point, and the shoulder joint axes may not be exactly or- thogonal. In such cases, rather than throw away any analytic inverse kinematic solutions that are available, such solutions can be used as the initial guess in an iterative numerical procedure for solving the inverse kinematics. There exist a variety of iterative methods for finding the roots of a nonlinear equation, and our aim is not to discuss these in detail – any text on numerical analysis will cover these methods in depth – but rather to develop ways in which to transform the inverse kinematics equations so that they become amenable to existing numerical methods. We will make use of an approach fundamental to nonlinear root-finding, the Newton–Raphson method. Also, methods of opti- mization are needed in situations where an exact solution may not exist and we seek the closest approximate solution; or, conversely, an infinity of inverse kinematics solutions exists (i.e., if the robot is kinematically redundant) and we seek a solution that is optimal with respect to some criterion. We now therefore discuss the Newton–Raphson method for nonlinear root-finding and also the first-order necessary conditions for optimization. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

245 Chapter 6. Inverse Kinematics 227 Newton–Raphson Method 6.2.1 ( To solve the equation θ g ) = 0 numerically for a given differentiable function 0 R , assume g is an initial guess for the solution. Write the Taylor → R : θ 0 ) at θ expansion of and truncate it at first order: g ( θ ∂g 0 0 0 θ ( ) + θ ) = ( θ g )( θ − θ g ) + higher-order terms (h.o.t) . ( ∂θ Keeping only the terms up to first order, set g ( θ ) = 0 and solve for θ to obtain ) ( − 1 ∂g 0 0 0 ( θ g ( θ ) ) . θ − = θ ∂θ Using this value of as the new guess for the solution and repeating the above, θ we get the following iteration: ) ( − 1 ∂g k +1 k k k = ) − θ ( g ( θ θ ) . θ ∂θ The above iteration is repeated until some stopping criterion is satisified, e.g., k k +1 k / − g ( θ | g ) | ( | g ( θ ) ) |≤  for some user-prescribed threshold value  . θ The same formula applies for the case when g is multi-dimensional, i.e., n n g → R : , in which case R   ∂g ∂g 1 1 θ ( ) ··· ( θ ) ∂θ ∂θ 1 n ∂g   . . n n × . . . . . R ∈ ( ) = θ   . . . ∂θ ∂g ∂g n n ( ( ··· θ θ ) ) ∂θ ∂θ 1 n The case where the above matrix fails to be invertible is discussed in Sec- tion 6.2.2. 6.2.2 Numerical Inverse Kinematics Algorithm x Suppose we express the end-effector frame using a coordinate vector governed = f ( by the forward kinematics ), a nonlinear vector equation mapping the n θ x n m f joint coordinates to the R m → R end-effector coordinates. Assume that : is differentiable, and let x be the desired end-effector coordinates. Then g ( θ ) d for the Newton-Raphson method is defined as g ( θ ) = x ), and the goal is − f ( θ d such that θ to find joint coordinates d ( θ ) = . g − f ( θ ) = 0 x d d d May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

246 228 6.2. Numerical Inverse Kinematics − f ( θ ) x d ∂f ) ( θ slop e= − 0 ∂θ θ f ) − ( x 0 d θ θ θ θ 0 d 1 1 − ∂f f θ ( ∆ ( x = − ) ( θ )) θ 0 d 0 ∂θ Figure 6.7: The first step of the Newton–Raphson method for nonlinear root-finding and . In the first step, the slope − ∂f/∂θ is evaluated at the point θ x for a scalar 0 1 1 0 − f ( θ θ )). In the second step, the slope is evaluated at the point ( θ ,x ,x )) − f ( θ ( d d and eventually the process converges to . Note that an initial guess to the left of θ d − ( θ ) would be likely to result in convergence to the other root of the plateau of x f d f ( θ ), and an initial guess at or near the plateau would result in a large initial x − d | | and the iterative process might not converge at all. ∆ θ 0 which is “close to” a solution θ θ , the kinematics can Given an initial guess d be expressed as the Taylor expansion ∣ ∣ ∂f 0 0 ∣ ) = f ( θ − ) + θ θ = f ( θ (6.3) ( , + h.o.t. ) x d d d ∣ ︷︷ ︸ ︸ ∂θ 0 θ ︸ ︷︷ ︸ ∆ θ 0 ) J ( θ × n 0 m 0 R J ( θ is the coordinate Jacobian evaluated at θ ) . Truncating where ∈ the Taylor expansion at first order, we can approximate Equation (6.3) as 0 0 θ )∆ (6.4) = x ) − f ( θ . ( θ J d 0 ( θ n ) is square ( m = Assuming that ) and invertible, we can solve for ∆ θ as J ( ) 1 − 0 0 θ θ = ) J x (6.5) − f ( θ ( ) ∆ . d If the forward kinematics is linear in θ , i.e., the higher-order terms in Equa- 1 0 1 exactly satisfies + ∆ θ θ x = = f ( θ θ ). tion (6.3) are zero, then the new guess d θ , as is usually the case, the new guess If the forward kinematics is not linear in 1 0 should still be closer to the root than θ θ , and the process is then repeated, 0 1 2 } ,θ θ ,θ producing a sequence ,... { converging to θ 6.7). (Figure d May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

247 Chapter 6. Inverse Kinematics 229 6.7, if there are multiple inverse kinematics solutions, As indicated in Figure the iterative process tends to converge to the solution that is “closest” to the 0 initial guess . You can think of each solution as having its own basin of θ attraction. If the initial guess is not in one of these basins (e.g., the initial guess is not sufficiently close to a solution), the iterative process may not converge. In practice, for computational efficiency reasons, Equation (6.4) is often 1 0 − ( θ solved without directly calculating the inverse J ). More efficient tech- b = x . For example, for Ax niques exist for solving a set of linear equations for , the LU decomposition of A can be used to solve invertible square matrices A with fewer operations. In MATLAB, for example, the syntax for x x = A\b − 1 for x without computing A . solves Ax = b is not invertible, either because it is not square or because it is singular, J If − 1 then J in Equation (6.5) does not exist. Equation (6.4) can still be solved − 1 J (or approximately solved) for ∆ by replacing in Equation (6.5) with the θ † J Moore–Penrose . For any equation of the form Jy = z , where pseudoinverse m × n n m ∈ y ∈ R R , and z ∈ J , , the solution R ∗ † J = z y falls into one of two categories: ∗ ∗ = y Jy • The solution z and, for any solution y exactly exactly satisfies ∗ Jy = z , we have ‖ y . In other words, among all so- ‖ ≤ ‖ y ‖ satisfying ∗ lutions, minimizes the two-norm. There can be an infinite number of y y to = z if the robot has more joints n than end-effector solutions Jy , i.e., the Jacobian is “fat.” m J coordinates ∗ that exactly satisfies Jy • z then y If there is no minimizes the two- y = ∗ n Jy ‖ z ‖ ≤ ‖ Jy − z ‖ for any y ∈ R norm of the error, i.e., . This − J < m case corresponds to rank n than , i.e., the robot has fewer joints m end-effector coordinates J ) or it is at a singularity. (a “tall” Jacobian Many programming languages provide functions to calculate the pseudoin- verse; for example, the usage in MATLAB is y = pinv(J) * z In the case where J is full rank (rank m for n > m or rank n for n < m ), i.e., the robot is not at a singularity, the pseudoinverse can be calculated as T T − 1 † † = ) J n > m if J is fat, ( (called a right inverse since JJ JJ = I ) J † T − 1 T † ) J J J = ( if J is tall, n < m (called a left inverse since J J J = I ) . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

248 230 6.2. Numerical Inverse Kinematics Replacing the Jacobian inverse with the pseudoinverse, Equation (6.5) be- comes ( ) 0 † 0 ) ( x = − f ( θ J ) θ . (6.6) θ ∆ d J ) < m then the solution ∆ θ calculated in Equation (6.6) may not If rank( exactly satisfy Equation (6.4), but it satisfies this condition as closely as possible then the solution is the smallest joint variable in a least-squares sense. If n > m change (in the two-norm sense) that exactly satisfies Equation (6.4). Equation (6.6) suggests using the Newton–Raphson iterative algorithm for finding : θ d m 0 n ∈ (a) Initialization : Given θ x ∈ R R , set i = 0. and an initial guess d i = x ‖ − f ( θ (b) Set ). While ‖ e e >  for some small  : d +1 i † i i ( J = θ θ • ) e . θ + Set i . • Increment To modify this algorithm to work with a desired end-effector configuration T ∈ SE (3) instead of as a coordinate vector x , we can replace represented as sd d 6 × n the coordinate Jacobian ∈ R J with the end-effector body Jacobian J . b i θ Note, however, that the vector − f ( = x ), representing the direction from e d the current guess (evaluated through the forward kinematics) to the desired i end-effector configuration, cannot simply be replaced by − T ); the pseu- ( θ T sb sd 6 J . To find the right analogy, should act on a body twist V doinverse of ∈ R b b i = − f ( θ x ) as a velocity vector which, if followed for e we should think of d i θ x ) to unit time, would cause a motion from f . Similarly, we should look ( d V which, if followed for unit time, would cause a motion from for a body twist b i ( θ ) to the desired configuration T . T sb sd V To find this , we first calculate the desired configuration in the body frame, b − 1 i i i θ T ) = ( θ ( ) T = T ( θ T ) T . sd bd sd bs sb Then V is determined using the matrix logarithm, b i ) T V ( θ [ ] = log . b bd This leads to the following inverse kinematics algorithm, which is analogous to the above coordinate-vector algorithm: 0 n T (a) and an initial guess θ Initialization ∈ R : Given , set i = 0. sd ) ( − 1 i >  : V , ( θ ] = log ) T  (b) Set [ . While ‖ ω for small ‖ T >  or ‖ v ‖ b v b sd ω ω v b sb May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

249 Chapter 6. Inverse Kinematics 231 ne ro afte { } b teration Ri N- 1m ˆy θ 2 { goal } 1m screw axis l initia θ 1 g uess ˆx (Left) A 2R robot. (Right) The goal is to find the joint angles yielding Figure 6.8: ◦ ◦ corresponding to the end-effector frame { = 30 goal and θ . The initial = 90 } θ 2 1 ◦ ◦ 30 guess is (0 ). After one Newton–Raphson iteration, the calculated joint angles , ◦ ◦ . , 79 . 18 are (34 ). The screw axis that takes the initial frame to the goal frame (by 23 means of the curved dashed line) is also indicated. † i i +1 i θ • + J θ Set ( θ = ) V . b b Increment i . • An equivalent form can be derived in the space frame, using the space Ja- cobian = [Ad ( θ ) and the spatial twist V J . ] V s T s b sb For this numerical inverse kinematics method to converge, the initial guess 0 should be sufficiently close to a solution θ . This condition can be satisfied θ d by starting the robot from an initial home configuration where both the actual end-effector configuration and the joint angles are known and ensuring that the requested end-effector position changes slowly relative to the frequency of T sd the calculation of the inverse kinematics. Then, for the rest of the robot’s run, 0 at the previous timestep serves as the initial guess the calculated θ θ for the d new at the next timestep. T sd (Planar 2R robot) . Now we apply the body Jacobian Newton– Example 6.1 6.8. Each link Raphson inverse kinematics algorithm to the 2R robot in Figure is 1 m in length, and we would like to find the joint angles that place the tip of ◦ ◦ ) = (0 . 366 m , 1 . 366 m), which corresponds to θ ) = (30 the robot at ( , 90 x,y d and   366 − 0 . . 5 0 . 866 0 0 −   0 1 5 366 0 . 866 − 0 . .   T = sd   0 1 0 0 0 0 1 0 as shown by the frame { goal } in Figure 6.8. The forward kinematics, expressed May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

250 232 6.3. Inverse Velocity Kinematics in the end-effector frame, is given by     0 0       0 0 1 0 0 2           1 1 0 1 0 0       . B = , B M = = , 2 1       0 0 0 0 1 0         1 2 0 0 0 1 0 0 ◦ 0 , 30 θ ), and we specify an error tolerance Our initial guess at the solution is = (0 ◦ − 4 . 001 rad (or 0 . 057  ) and of  = 10 m (100 microns). The progress = 0 ω v of the Newton–Raphson method is illustrated in the table below, where only the ( ,v ω ,v are given since the robot’s )-components of the body twist V zb yb xb b motion is restricted to the x – y -plane: ( θ ‖ , θ v ) ( x,y ) V ‖ ‖ = ( ω i ,v ω ,v ‖ ) yb b zb b 2 b 1 xb ◦ 571 00 , 30 . 00 (0 ) (1 . 866 , 0 0 500) (1 . . , 0 . 498 , 1 . 858) 1 . 571 1 . 924 . ◦ ◦ . 23 , , 79 . 18 1 ) (0 . 429 (34 1 . 480) (0 . 115 , − 0 . 074 , 0 . 108) 0 . 115 0 . 131 ◦ ◦ 2 (29 , 90 . 22 98 ) (0 . 363 , 1 . 364) ( − 0 . 004 , 0 . 000 , − 0 . 004) 0 . 004 0 . 004 . ◦ ◦ 000 (30 , 3 . 00 00 ) (0 . 366 , 1 . 366) (0 . 000 , 0 . . , 0 . 000) 0 . 000 0 . 000 90 The iterative procedure converges to within the tolerances after three it- erations. Figure 6.8 shows the initial guess, the goal configuration, and the configuration after one iteration. Notice that the first v calculated is positive, xb − ˆx -direction of the initial even though the origin of the goal frame is in the b guess. The reason is that the constant body velocity V that takes the initial b guess to } in one second is a rotation about the screw axis indicated in the { goal figure. Inverse Velocity Kinematics 6.3 One solution for controlling a robot so that it follows a desired end-effector ( t ) is to calculate the inverse kinematics trajectory T ( k ∆ t ) at each discrete θ sd d ̇ k , then control the joint velocities θ as follows timestep ( ) ̇ − θ t ( k ∆ t ) θ θ (( k − 1)∆ t ) = / ∆ d k − 1)∆ t,k ∆ t ]. This amounts to a feedback controller during the time interval [( since the desired new joint angles θ ) are being compared with the most ( k ∆ t d 1)∆ θ (( k − recently measured actual joint angles t ) in order to calculate the required joint velocities. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

251 Chapter 6. Inverse Kinematics 233 Another option that avoids the computation of inverse kinematics is to cal- ̇ ̇ θ θ = V directly from the relationship , culate the required joint velocities J d V and J are expressed with respect to the where the desired end-effector twist d same frame: † ̇ ) ( θ θ = J . (6.7) V d − 1 ̇ ) can be chosen to be T The desired twist ( t V t ) T ( t ) (the body twist of the ( d sd sd 1 − ̇ desired trajectory at time ( t ) T ) or t T ( ) (the spatial twist), depending on t sd sd whether the body Jacobian or space Jacobian is used; however small velocity errors are likely to accumulate over time, resulting in increasing position error. Thus, a position feedback controller should choose ( t V ) so as to keep the end- d effector following T ( t ) with little position error. Feedback control is discussed sd in Chapter 11. n > 6 joints, of the ( n − 6)-dimensional In the case of a redundant robot with † J θ ) set of joint velocities satisfying Equation (6.7), the use of the pseudoinverse ( √ T ̇ ̇ ̇ ̇ θ θ . = θ ‖ minimizing the two-norm θ ‖ returns joint velocities The use of the pseudoinverse in Equation (6.7) implicitly weights the cost of each joint velocity identically. We could instead give the joint velocities different weights; for example, the velocity at the first joint, which moves a lot of the robot’s mass, could be weighted more heavily than the velocity at the last joint, which moves little of the robot’s mass. As we will see later, the kinetic energy of a robot can be written 1 T ̇ ̇ θ M ( θ ) θ, 2 where θ ) is the symmetric, positive-definite, configuration-dependent mass M ( ( ) can be used as a weighting function matrix of the robot. The mass matrix θ M ̇ in the inverse velocity kinematics, and the goal is to find the θ that minimizes ̇ . θ J θ = V the kinetic energy while also satisfying ) ( d ̇ Another possibility is to find the θ that causes the robot to minimize a ̇ ( θ ) while satisfying J ( θ ) configuration-dependent potential energy function θ = h h ( θ ) could be the gravitational potential energy, or an artificial . For example, V d potential function whose value increases as the robot approaches an obstacle. h ( θ ) is Then the rate of change of d dh ( θ ) dθ T ̇ ( θ ) = θ, h = ∇ h ( θ ) dt dθ dt θ ∇ ( θ ) points in the direction of maximum ascent of h where h ). ( More generally, we may wish to minimize the sum of the kinetic energy and the rate of change of the potential energy: 1 T T ̇ ̇ ̇ h θ M ( θ ) θ, θ + ∇ min ( θ ) ̇ 2 θ May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

252 234 6.4. A Note on Closed Loops ̇ . From the first-order necessary conditions θ = V ) θ ( J subject to the constraint d for optimality (Appendix D) T ̇ M θ + ∇ h, = J λ ̇ J V θ, = d ̇ and λ can be derived as follows: θ the optimal − 1 ̇ I + ( θ − GJ ) M = G ∇ h, V d − 1 V + λ BJM = B ∇ h, d n m m × m × G where R ∈ and R are defined by ∈ B T − 1 1 − = ( ) JM , B J T T 1 T − 1 − − 1 − 1 ) M JM J = M = J J ( B. G T τ = J Recalling the static relation F from the previous chapter, the Lagrange multiplier λ D) can be interpreted as a wrench in task space. (see Appendix − 1 λ V = + BJM , can be B ∇ h , the first term, B V Moreover, in the expression d d V interpreted as a dynamic force generating the end-effector velocity while the d − 1 second term, ∇ h , can be interpreted as the static wrench counteracting BJM gravity. If the potential function h ( θ ) is zero or unspecified, the kinetic-energy- minimizing solution is 1 − T 1 − 1 − T ̇ ( M J = ) θ JM V J , d − − 1 1 T − T 1 M J JM ) ( J is the weighted pseudoinverse according to the mass where matrix M ( θ ). 6.4 A Note on Closed Loops ,t ] is a closed loop if A desired end-effector trajectory over a time interval [0 f ). It should be noted that numerical methods for calculating (0) = T t ( T sd f sd inverse kinematics for redundant robots, at either the configuration or velocity levels, are likely to yield motions that are not closed loops in the joint space, ( θ (0) 6 = θ i.e., t ). If closed-loop motions in joint space are required, an extra f set of conditions on the inverse kinematics must be satisfied. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

253 Chapter 6. Inverse Kinematics 235 Summary 6.5 n ( θ ), θ ∈ R • , in the T Given a spatial open chain with forward kinematics inverse kinematics problem one seeks to find, for a desired end-effector = SE (3), solutions θ that satisfy X configuration T ( θ ). Unlike X ∈ the forward kinematics problem, the inverse kinematics problem can pos- sess multiple solutions, or no solutions in the event that lies outside X the workspace. For a spatial open chain with joints and an X in the n n workspace, = 6 typically leads to a finite number of inverse kinematic solutions while n > 6 leads to an infinite number of solutions. • The inverse kinematics can be solved analytically for the six-dof PUMA- type robot arm, a popular 6R design consisting of a 3R orthogonal axis wrist connected to a 2R orthogonal axis shoulder by an elbow joint. • Stanford-type arms also admit analytic inverse kinematics solutions. These arms are obtained by replacing the elbow joint in the generalized 6R PUMA-type arm by a prismatic joint. Geometric inverse kinematic al- gorithms similar to those for PUMA-type arms have been developed. Iterative numerical methods are used in cases where analytic inverse kine- • matic solutions are unavailable. These methods typically involve solv- ing the inverse kinematics equations using an iterative procedure like the Newton–Raphson method, and they require an initial guess at the joint variables. The performance of the iterative procedure depends to a large extent on the quality of the initial guess and, in the case where there are several possible inverse kinematic solutions, the method finds the solution that is “closest” to the initial guess. Each iteration is of the form † i +1 i i ̇ = + θ ( θ θ ) V , J † J ) and ( θ ) is the pseudoinverse of the Jacobian J ( θ where V is the twist i that takes θ T ) to T ( in one second. sd 6.6 Software Software functions associated with this chapter are listed below. [thetalist,success] = IKinBody(Blist,M,T,thetalist0,eomg,ev) This function uses iterative Newton–Raphson to calculate the inverse kinematics given the list of joint screws expressed in the end-effector frame, the end- B i effector home configuration M , the desired end-effector configuration T , an May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

254 236 6.7. Notes and References 0 θ  initial guess at the joint angles and  , and the tolerances on the final v ω error. If a solution is not found within a maximum number of iterations, then success is false. [thetalist,success] = IKinSpace(Slist,M,T,thetalist0,eomg,ev) IKinBody , except that the joint screws S are expressed in the This is similar to i space frame and the tolerances are interpreted in the space frame, also. Notes and References 6.7 The inverse kinematics of the most general 6R open chain is known to have up to 16 solutions; this result was proved by Lee and Liang [87] and by Raghavan and Roth [143]. Procedures for finding closed-form inverse kinematics solutions to somewhat more general six-dof open chains than those treated in this chapter 122]; these procedures use solutions to a collection of some are described in [129, basic screw-theoretic subproblems, called the Paden–Kahan subproblems, e.g., finding the angle of rotation for a zero-pitch screw motion between a pair of given points. Iterative numerical procedures for finding all 16 solutions of a general 6R open chain are reported in [104]. A comprehensive summary of inverse kinematics methods for kinematically redundant robot arms are discussed in [26]. Many of these methods rely on results and solution techniques from least-squares optimization, and for this reason we provide a brief review of the basics of optimization in Appendix D; a classic reference for optimization is [98]. The repeatability (or cyclicity) condi- tions for a general class of inverse kinematic redundancy resolution schemes are examined in [162]. 6.8 Exercises Exercise 6.1 Write a program that solves the analytical inverse kinematics L = 3, for a planar 3R robot with link lengths L = 1, given the = 2, and L 3 2 1 desired position ( x,y ) and orientation θ of a frame fixed to the tip of the robot. Each joint has no joint limits. Your program should find all the solutions (how many are there in the general case?), give the joint angles for each, and draw the robot in these configurations. Test the code for the case of ( x,y,θ ) = (4 , 2 , 0). Exercise 6.2 Solve the inverse position kinematics (you do not need to solve the orientation kinematics) of the 6R open chain shown in Figure 6.9. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

255 Chapter 6. Inverse Kinematics 237 Figure 6.9: A 6R open chain. L L θ 2 θ 4 θ 3 ˆz ˆz } T { { T } ˆy ˆy θ 6 ˆx ˆx θ √ 5 θ 1 L 2 L ˆz s ˆy s ˆx s } { s Figure 6.10: A 6R open chain. Exercise 6.3 Find the inverse kinematics solutions when the end-effector ′ T frame of the 6R open chain shown in Figure 6.10 is set to { T { } as shown. } The orientation of { T } at the zero position is the same as that of the fixed frame ′ -axis. , and { T { } is the result of a pure translation of { T } along the ˆy } s s The RRP open chain of Figure is shown in its zero position. Exercise 6.4 6.11 Joint axes 1 and 2 intersect at the fixed frame origin, and the end-effector frame p origin , 1 , 0) when the robot is in its zero position. is located at (0 (a) Suppose that θ when the end-effector frame = 0. Solve for θ θ and 2 3 1 √ 5 − 6 origin is at ( , p 3). , (b) If joint 1 is not fixed to zero but instead allowed to vary, find all the inverse kinematic solutions ( θ given in (a). ,θ p ,θ ) for the 3 2 1 Exercise 6.5 The four-dof robot of Figure 6.12 is shown in its zero position. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

256 238 6.8. Exercises θ 1 θ 2 π 6 ˆz p ˆy ˆx θ 3 =1 L Figure 6.11: An RRP open chain. . Given the end-effector position p = ( p Joint 1 is a screw joint of pitch ,p ) ,p h z y x α [ˆz] e , where ˆz = (0 R and orientation , 0 , 1) and α ∈ [0 , 2 π ], find the inverse = and θ kinematics solution ( . ,θ α ,θ p ) as a function of ,θ 4 3 2 1 Figure Exercise 6.6 6.13(a) shows a surgical robot, which can be modeled as an RRPRRP open chain as shown in Figure 6.13(b). (a) In the general case, how many inverse kinematic solutions will exist for a given end-effector frame? (b) Consider points A and B on the surgical robot shown in Figure 6.13(b). ,z Given coordinates ( ) and ( x ,y ,y ,z ) for the points A and B x B A A A B B θ θ , in the fixed frame, find the joint variables . You , θ θ , θ , and 1 4 5 2 3 θ should find an explicit formula for ( ,θ ,θ ) while for ( θ ,θ ) you can 1 4 3 5 2 just describe the procedure. In this exercise you are asked to draw a plot of a scalar x − f ( θ ) Exercise 6.7 d versus a scalar θ (similar to Figure 6.7) with two roots. Draw it so that, for 0 some initial guess , the iterative process actually jumps over the closest root θ and eventually converges to the further root. Hand-draw the plot and show the iteration process that results in convergence to the further root. Comment on the basins of attraction of the two roots in your plot. Exercise 6.8 Use Newton–Raphson iterative numerical root finding to per- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

257 Chapter 6. Inverse Kinematics 239 θ 1 L L θ 2 θ 4 θ 3 L L ˆz s s } { ˆz b L ˆx b { } ˆy s s ˆx ˆy b b Figure 6.12: An open chain with a screw joint. form two steps in finding the root of [ ] 2 x − 4 ) = ( g x,y 2 y 9 − 0 0 when your initial guess is ( ) = (1 , 1). Write the general form of the gradient ,y x )) and compute the results of the first two iterations. You x,y (for any guess ( can do this by hand or write a program. Also, give all the correct roots, not just the one that would be found from your initial guess. How many are there? Modify the function IKinBody to print out the results of each Exercise 6.9 Newton–Raphson iteration, in a table similar to that for the 2R robot example in Section 6.2. Show the table produced when the initial guess for the 2R robot ◦ ◦ ◦ 30 of Figure 6.8 ) and the goal configuration corresponds to (90 is (0 , 120 , ). Exercise 6.10 6.14 is The 3R orthogonal axis wrist mechanism of Figure shown in its zero position, with joint axes 1 and 3 collinear. (a) Given a desired wrist orientation R ∈ SO (3), derive an iterative numerical procedure for solving its inverse kinematics. (b) Perform a single iteration of Newton–Raphson root-finding using body- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

258 240 6.8. Exercises (a) da Vinci S Surgical System instrument arm, c 2016 Intuitive Surgical, Inc. © L 2 θ θ 2 1 ◦ L 45 2 L L 2 L 2 2 L θ 3 B point ˆz 0 L ˆy 0 A point ˆx 0 θ 6 { } 0 θ θ 2 L 4 5 end-effector { 6 } (b) RRPRRP robot at zero position. Figure 6.13: Surgical robot and kinematic model. frame numerical inverse kinematics. First write down the forward kine- matics and Jacobian for general configurations of the wrist. Then apply θ π/ = θ 6, = 0, θ your results for the specific case of an initial guess of = 1 2 3 with a desired end-effector frame at   1 1 √ √ 0 − 2 2 1 1   √ √ 0 SO . (3) ∈ R = 2 2 0 1 0 Exercise 6.11 The 3R nonorthogonal chain of Figure 6.15 is shown in its zero position. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

259 Chapter 6. Inverse Kinematics 241 θ 1 ˆz ˆy ˆx θ 2 θ 3 Figure 6.14: A 3R wrist. θ 1 θ 2 ] [ π Rp 4 ˆz s π = } T { θ 3 4 01 ˆy s ˆx s } s { L Figure 6.15: A 3R nonorthogonal chain. (a) Derive a numerical procedure for solving the inverse position kinematics; that is, given some end-effector position p as indicated in the figure, find ( θ ). ,θ ,θ 2 1 3 (b) Given an end-effector orientation R ∈ SO (3), find all inverse kinematic May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

260 242 6.8. Exercises θ ,θ ,θ solutions ( ). 2 3 1 Exercise 6.12 θ Use the function of the IKinSpace to find joint variables d UR5 (Section 4.1.2) satisfying   5 . 0 − 0 1 0   0 0 0 − 1 . 1   . θ ) = = ( T T d sd   − 0 0 . 1 1 0 1 0 0 0 ◦  = 0 . 001 rad (0.057 0001 ) and  The distances are in meters. Use = 0 . v ω 0 θ , choose all joint angles as 0.1 rad. If T (0.1 mm). For your initial guess sd is outside the workspace, or if you find that your initial guess is too far from a IKinBody using another T . solution to converge, you may demonstrate sd Note that numerical inverse kinematics is intended to find a solution close to the initial guess. Since your initial guess is not close to a solution (and remember that there are generally multiple solutions), the procedure may thrash about before finding a solution far from the initial guess. This solution may not respect joint limits. You can post-process the solution so that all joint angles , π ). are in the range [0 2 Exercise 6.13 to find joint variables θ Use the function of the IKinBody d 4.1.3) satisfying WAM (Section   5 1 0 0 0 .   0 1 0 0   . ) = = T ( T θ d sd   4 . 0 0 1 0 1 0 0 0 ◦ Distances are in meters. Use = 0 . 001 rad (0.057  ) and  0001 (0.1 mm). = 0 . v ω 0 θ For your initial guess , choose all joint angles as 0.1 rad. If T is outside the sd workspace, or if you find that your initial guess is too far from a solution to IKinBody using another T converge, you may demonstrate . sd Note that numerical inverse kinematics is intended to find a solution close to the initial guess. Since your initial guess is not close to a solution (and remember that there are generally multiple solutions), the procedure may thrash about before finding a solution far away from the initial guess. This solution may not respect joint limits. You can post-process the solution so that all joint angles ). , 2 π are in the range [0 Exercise 6.14 The fundamental theorem of linear algebra (FTLA) states that, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

261 Chapter 6. Inverse Kinematics 243 m × n A ∈ , given a matrix R ⊥ T ) = range( null( , A A ) ⊥ T ) = range( , ) null( A A n ) denotes the null space of (i.e., the subspace of R of vectors A where null( A Ax = 0), range( A x A (i.e., that satisfy ) denotes the range or column space of ⊥ m A ), and range( A ) R denotes the the subspace of spanned by the columns of m ) (i.e., the set of all vectors in R that are orthogonal complement to range( A A orthogonal to every vector in range( )). In this problem you are asked to use the FTLA to prove the existence of Lagrange multipliers (see Appendix D) for the equality-constrained optimization n : R R → problem. Let f , assumed differentiable, be the objective function to be minimized. The vector must satisfy the equality constraint g ( x ) = 0 for x n m g → R : . given differentiable R ∗ is a local minimum. Let x ( t ) be any arbitrary curve on the Suppose that x g ( x ) = 0 (implying that g ( x ( t )) = 0 for all t ) surface parametrized implicitly by ∗ ∗ x such that . Further, assume that x x is a regular point of the surface. (0) = g )) = 0 at x ( t Taking the time derivative of both sides of t = 0 then leads to ( ∂g ∗ x ( ) ̇ x (0) = 0 . (6.8) ∂x ∗ x (0) = x is a local minimum it follows that At the same time, because f ( x ( t )) (viewed as an objective function in t = 0, implying ) has a local minimum at t that ∣ ∣ d ∂f ∗ ∣ x ( t ( (6.9) f )) . ( x = ) ̇ x (0) = 0 ∣ ∂x dt =0 t x ( t ) on the surface defined Since (6.8) and (6.9) must hold for all arbitrary curves g ( ) = 0, use the FTLA to prove the existence of a Lagrange multiplier by x ∗ m R such that the first-order necessary condition, λ ∈ ∂g ∗ T ∗ ∗ f ∇ ( x ) + , ) λ ( = 0 x ∂x holds. Exercise 6.15 − 1 , B , C , and D , if A (a) For matrices A exists show that ] [ [ ] − 1 − 1 − 1 − 1 A F + EG EG − A D , = − 1 − 1 − G F G C B − 1 − 1 − 1 where G D , E = A = B D , and F = CA − CA . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

262 244 6.8. Exercises (b) Use the above result to solve for the first-order necessary conditions for the equality-constrained optimization problem 1 T T c x + x min Qx n R ∈ x 2 n × n b , where Q ∈ R subject to Hx = is symmetric and positive definite m × n is some matrix of maximal rank and ∈ R H m . See Appendix D. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

263 Chapter 7 Kinematics of Closed Chains Any kinematic chain that contains one or more loops is called a . closed chain Several examples of closed chains were encountered in Chapter 2, from the planar four-bar linkage to spatial mechanisms like the Stewart–Gough platform and the Delta robot (Figure parallel 7.1). These mechanisms are examples of mechanisms : closed chains consisting of fixed and moving platforms connected by a set of “legs.” The legs themselves are typically open chains but sometimes can also be other closed chains (like the Delta robot in Figure 7.1(b)). In this chapter we analyze the kinematics of closed chains, paying special attention to parallel mechanisms. The Stewart–Gough platform is used widely as both a motion simulator and a six-axis force–torque sensor. When used as a force–torque sensor, the six prismatic joints experience internal linear forces whenever any external force is applied to the moving platform; by measuring these internal linear forces one can estimate the applied external force. The Delta robot is a three-dof mechanism whose moving platform moves in such a way that it always remains parallel to the fixed platform. Because the three actuators are all attached to the three revolute joints of the fixed platform, the moving parts are relatively light; this allows the Delta to achieve very fast motions. Closed chains admit a much greater variety of designs than open chains, and their kinematic and static analysis is consequently more complicated. This com- plexity can be traced to two defining features of closed chains: (i) not all joints are actuated, and (ii) the joint variables must satisfy a number of loop-closure constraint equations, which may or may not be independent depending on the 245

264 246 R } { b b i B i R d i p S S S a } s { i R A i S (a) Stewart–Gough platform. (b) Delta robot. Figure 7.1: Two popular parallel mechanisms. configuration of the mechanism. The presence of unactuated (or passive) joints, together with the fact that the number of actuated joints may deliberately be designed to exceed the mechanism’s kinematic degrees of freedom – such mech- anisms are said to be redundantly actuated – not only makes the kinematics analysis more challenging but also introduces new types of singularities not present in open chains. Recall also that, for open chains, the kinematic analysis proceeds in a more or less straightforward fashion, with the formulation of the forward kinematics (e.g., via the product of exponentials formalism) followed by that of the inverse kinematics. For general closed chains it is usually difficult to obtain an explicit X = T ( θ ), where X ∈ set of equations for the forward kinematics in the form n (3) is the end-effector frame and θ ∈ R SE are the joint coordinates. The more effective approaches exploit, as much as possible, any kinematic symmetries and other special features of the mechanism. In this chapter we begin with a series of case studies involving some well- known parallel mechanisms and eventually build up a repertoire of kinematic analysis tools and methodologies for handling more general closed chains. Our focus will be on parallel mechanisms that are exactly actuated, i.e., the number of actuated degrees of freedom is equal to the number of degrees of freedom May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

265 Chapter 7. Kinematics of Closed Chains 247 of the mechanism. Methods for the forward and inverse position kinematics of parallel mechanisms are discussed; this is followed by the characterization and derivation of the constraint Jacobian, and the Jacobians of both the inverse and forward kinematics. The chapter concludes with a discussion of the different types of kinematic singularities that arise in closed chains. Inverse and Forward Kinematics 7.1 One general observation that can be made for serial mechanisms versus par- allel mechanisms is the following: for serial chains, the forward kinematics is generally straightforward while inverse kinematics may be complex (e.g., there may be multiple solutions or no solution). For parallel mechanisms, the inverse kinematics is often relatively straightforward (e.g., given the configuration of a platform, it may not be hard to determine the joint variables), while the forward kinematics may be quite complex: an arbitrarily chosen set of joint valuees may be infeasible or it may correspond to multiple possible configurations of the platform. RPR planar parallel mech- We now continue with two case studies, the 3 × SPS Stewart–Gough platform. The anism and its spatial counterpart, the 3 × analysis of these two mechanisms draws upon some simplification techniques that result in a reduced form of the governing kinematic equations, which in turn can be applied to the analysis of more general parallel mechanisms. 7.1.1 3 × RPR Planar Parallel Mechanism × RPR parallel mechanism The first example we consider is the 3-dof planar 3 } 7.2. A fixed frame } and body frame { b s are assigned to the shown in Figure { platform as shown. The three prismatic joints are typically actuated while the six revolute joints are passive. Denote the lengths of each of the three legs by i , s = 1 , 2 , 3. The forward kinematics problem is to determine, for given values i of s = ( s ,s ,s ), the body frame’s position and orientation. Conversely, the 2 3 1 inverse kinematics problem is to determine s T from ∈ SE (2). sb Let p be the vector from the origin of the s } frame to the origin of the { b } { frame. Let denote the angle measured from the ˆx -axis of the { s } frame to φ s the ˆx 3, as -axis of the { b } frame. Further, define the vectors a , , b 2 , d , , i = 1 i b i i shown in the figure. From these definitions, clearly (7.1) d − a , = p + b i i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

266 248 7.1. Inverse and Forward Kinematics d 3 a 3 a 2 s { } b 3 p b { } a 1 d 2 b 2 b 1 d 1 3 RPR planar parallel mechanism. Figure 7.2: × for = 1 2 , 3. Let , i [ ] p x = p in s } -frame coordinates , { p y [ ] a ix -frame coordinates in { s = a , } i a iy [ ] d ix = d in { s } -frame coordinates , i d iy [ ] b ix -frame coordinates . = b } in { b i b iy ) for a Note that ( ) and ( b 3 are all constant, and that, with ,b , ,a i = 1 , 2 iy ix iy ix b ,b the exception of the ( -frame coor- ), all other vectors are expressed in { s } iy ix { must } -frame coordinates, b s dinates. To express Equation (7.1) in terms of i { s } -frame coordinates. This is straightforward: defining be expressed in ] [ − sin φ cos φ = , R sb φ φ sin cos it follows that [ [ ] ] [ ] ] [ d p a b ix ix x ix = + , R − sb d a p b iy iy iy y May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

267 Chapter 7. Kinematics of Closed Chains 249 2 2 2 , 3. Also, since s i = 1 = d , , we have + d 2 for iy ix i 2 2 = ( p ) + b a cos φ − b φ sin s − iy ix x ix i 2 a + b , sin φ + b + ( cos (7.2) − p ) φ iy ix y iy i for , 2 , 3. = 1 Formulated as above, the inverse kinematics is trivial to compute: given values for ( p ) can be directly calculated from ,p ,s ,φ ), the leg lengths ( s ,s 1 y x 2 3 s the above equations (negative values of will not be physically realizable in i most cases and can be ignored). In contrast, the forward kinematics problem of determining the body frame’s position and orientation ( p ) from the leg ,φ ,p y x s ) is not trivial. The following tangent half-angle substitution ,s lengths ( ,s 3 1 2 , where transforms the three equations in (7.2) into a system of polynomials in t φ t = tan , 2 2 t sin φ = , 2 t 1 + 2 1 − t φ = cos . 2 t 1 + After some algebraic manipulation, the system of polynomials (7.2) can even- tually be reduced to a single sixth-order polynomial in ; this effectively shows t × RPR mechanism may have up to six forward kinematics solutions. that the 3 Showing that all six mathematical solutions are physically realizable requires further verification. Figure 7.3(a) shows the mechanism at a singular configuration, where each leg length is identical and as short as possible. This configuration is a sin- gularity, because extending the legs from this symmetric configuration causes the platform to rotate either clockwise or counterclockwise; we cannot predict 1 which. 7.3. Figure 7.3(b) Singularities are covered in greater detail in Section shows two solutions to the forward kinematics when all leg lengths are identical. 7.1.2 Stewart–Gough Platform We now examine the inverse and forward kinematics of the 6 × SPS Stewart– Gough platform of Figure 7.1(a). In this design the fixed and moving platforms are connected by six serial SPS structures, with the spherical joints passive and the prismatic joints actuated. The derivation of the kinematic equations is 1 A third possibility is that the extending legs crush the platform! May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

268 250 7.1. Inverse and Forward Kinematics ) (b (a ) × RPR at a singular configuration. From this configuration, Figure 7.3: (a) The 3 extending the legs may cause the platform to snap to a counterclockwise rotation or a clockwise rotation. (b) Two solutions to the forward kinematics when all prismatic joint extensions are identical. close to that for the 3 s } and RPR planar mechanism discussed above. Let × { denote the fixed and body frames, respectively, and let d be the vector } { b i directed from joint A 7.1(a), we , i = 1 ,..., 6. Referring to Figure to joint B i i make the following definitions: 3 s = p in ∈ R } -frame coordinates, p { 3 ∈ R { = a } in a -frame coordinates, s i i 3 ∈ = b in { b } -frame coordinates, b R i i 3 { ∈ = d d in R s } -frame coordinates, i i R SO (3) is the orientation of { b } as seen from { s } . ∈ In order to derive the kinematic constraint equations, note that, vectorially, , i − a d = p + b = 1 ,..., 6 . i i i Writing the above equations explicitly in { s } -frame coordinates yields , i d + Rb − a p = 1 ,..., 6 . = i i i i s , we have by Denoting the length of leg i 2 T T ) d , d ) = ( p + a − − a Rb = s ( p + Rb i i i i i i i for i = 1 ,..., 6. Note that a are all known constant vectors. Writing and b i i the constraint equations in this form, the inverse kinematics becomes straight- forward: given p and R , the six leg lengths s can be determined directly from i the above equations. The forward kinematics is not as straightforward: given each leg length s , i 3 = 1 ,..., 6, we must solve for p ∈ R i and R ∈ SO (3). These six constraint equa- T tions, together with six further constraints imposed by the condition R = I , R constitute a set of 12 equations in 12 unknowns (three for p , nine for R ). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

269 Chapter 7. Kinematics of Closed Chains 251 { } b θ m ψ p φ n ψ 1 p − θ − 1 m φ 1 − n θ ψ m 2 − 2 − p φ − n 2 { s } ψ 1 φ 1 θ 1 leg3 leg2 leg1 Figure 7.4: A general parallel mechanism. 7.1.3 General Parallel Mechanisms × RPR mechanism and the Stewart–Gough platform, we were For both the 3 able to exploit certain features of the mechanism that resulted in a reduced set of equations; for example, the fact that the legs of the Stewart–Gough platform can be modeled as straight lines considerably simplified the analysis. In this section we briefly consider the case when the legs are general open chains. Consider such a parallel mechanism, as shown in Figure 7.4; here the fixed and moving platforms are connected by three open chains. Let the configuration of the moving platform be given by T . Denote the forward kinematics of the sb n m θ ( T ( φ ), and T ( ψ ), respectively, where ), ∈ R T , φ ∈ R three chains by , θ 3 1 2 p T R ψ . The loop-closure conditions can be written T ) = = T φ ( θ ) = and ( ∈ 2 sb 1 ( T ). Eliminating T , we get ψ 3 sb T (7.3) ( θ ) = T , ( φ ) 2 1 (7.4) φ ) = T ( ψ ) . T ( 2 3 Equations (7.3) and (7.4) each consist of 12 equations (nine for the rotation component and three for the position component), six of which are independent: T from the rotation matrix constraint R R = I , the nine equations for the rotation component can be reduced to a set of three independent equations. Thus there are 24 constraint equations, 12 of which are independent, with n + m + p unknown 12 degrees of freedom. variables. The mechanism therefore has = n + m + p − d In the forward kinematics problem, given d values for the joint variables ( θ,φ,ψ ), Equations (7.3) and (7.4) can be solved for the remaining joint vari- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

270 252 7.2. Differential Kinematics ables. Generally this is not trivial and multiple solutions are likely. Once the joint values for any one of the open chain legs are known, the forward kinematics of that leg can then be evaluated to determine the forward kinematics of the closed chain. In the inverse kinematics problem, given the body-frame displacement T ∈ sb (3), we set T = T and solve Equations (7.3) and (7.4) for the = T SE = T 2 3 1 θ,φ,ψ joint variables ( ). As suggested by these case studies, for most parallel mechanisms there are features of the mechanism that can be exploited to elim- inate some of these equations and simplify them to a more computationally amenable form. 7.2 Differential Kinematics We now consider the differential kinematics of parallel mechanisms. Unlike the case for open chains, in which the objective is to relate the input joint velocities to the twist of the end-effector frame, the analysis for closed chains is complicated by the fact that not all the joints are actuated. Only the actuated joints can be prescribed input velocities; the velocities of the remaining passive joints must then be determined from the kinematic constraint equations. These passive joint velocities are usually required in order to eventually determine the twist of the closed chain’s end-effector frame. For open chains, the Jacobian of the forward kinematics is central to the velocity and static analysis. For closed chains, in addition to the forward kine- matics Jacobian, the Jacobian defined by the kinematic constraint equations – – also plays a central role in the constraint Jacobian we will call this the velocity and static analysis. Usually there are features of the mechanism that can be exploited to simplify and reduce the procedure for obtaining the two Jacobians. We illustrate this with a case study of the Stewart–Gough platform, and show that the Jacobian of the inverse kinematics can be obtained straight- forwardly via static analysis. The velocity analysis for more general parallel mechanisms is then detailed. Stewart–Gough Platform 7.2.1 Earlier we saw that the inverse kinematics for the Stewart–Gough platform can be solved analytically. That is, given the body-frame orientation R ∈ SO (3) 3 6 ∈ R and position , the leg lengths s ∈ R p can be obtained analytically in the functional form s = g ( R,p ). In principle one could differentiate this equation May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

271 Chapter 7. Kinematics of Closed Chains 253 and manipulate it into the form G ( R,p ) V ̇ , (7.5) = s s 6 6 ∈ V s R R denotes the leg velocities, is the spatial twist, and where ̇ ∈ s 6 × 6 R ( R,p is the Jacobian of the inverse kinematics. In most cases this G ∈ ) procedure will require considerable algebraic manipulation. Here we take a different approach, based on the conservation of power prin- T for open chains. The F = ciple used to determine the static relationship τ J static relationship for closed chains can be expressed in exactly the same form. We illustrate this with an analysis of the Stewart–Gough platform. In the absence of external forces, the only forces applied to the moving platform occur at the spherical joints. In what follows, all vectors are expressed { } -frame coordinates. Let s in = ˆ f n τ i i i 3 , where ˆ n be the three-dimensional linear force applied by leg ∈ R i is a unit i vector indicating the direction of the applied force and ∈ R is the magnitude τ i m generated by f of the linear force. The moment is i i × r m , f = i i i 3 where r denotes the vector from the { s } -frame origin to the point of R ∈ i in this case). Since i application of the force (the location of spherical joint neither the spherical joint at the moving platform nor the spherical joint at the fixed platform can resist any torques about them, the force f must be along i the line of the leg. Therefore, instead of calculating the moment m using the i spherical joint at the moving platform, we can calculate the moment using the spherical joint at the fixed platform: m = q × f , i i i 3 where R denotes the vector from the fixed-frame origin to the base joint q ∈ i × . Since i is constant, expressing the moment as q of leg q f is preferable. i i i f m and Combining ), the into the six-dimensional wrench F ,f = ( m i i i i i F on the moving platform is given by resultant wrench s ] [ 6 6 ∑ ∑ r × n ˆ i i = F τ = F i i s ˆ n i i =1 i =1   τ ] [ 1 × q − ··· − ˆ n ˆ × n q   . 6 6 1 1 . =   . ˆ n ˆ n ··· 6 1 τ 6 T − J = τ, s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

272 254 7.2. Differential Kinematics where is the spatial Jacobian of the forward kinematics, with inverse given J s by [ ] T ˆ n q × q − ··· − ˆ n × 6 1 6 1 − 1 . = J s n ˆ ··· n ˆ 1 6 7.2.2 General Parallel Mechanisms Because of its kinematic structure, the Stewart–Gough platform lends itself par- ticularly well to a static analysis, as each of the six joint forces are directed along their respective legs. The Jacobian (or more precisely, the inverse Jacobian) can therefore be derived in terms of the screws associated with each straight-line leg. In this subsection we consider more general parallel mechanisms where the static analysis is less straightforward. Using the previous three-legged spatial parallel mechanism of Figure 7.4 as an illustrative example, we derive a procedure for determining the forward kinematics Jacobian that can be generalized to other types of parallel mechanisms. 7.4 consists of two platforms connected by three The mechanism of Figure legs with n , and p joints, respectively. For simplicity, we will take m = n = m , = 5, so that the mechanism has d = n + m + p − 12 = 3 degrees of freedom p (generalizing what follows to different types and numbers of legs is completely straightforward). For the fixed and body frames indicated in the figure, we can write the forward kinematics for the three chains as follows: ] S [ [ θ ] [ S ] θ S θ 1 5 2 1 5 2 θ e ,...,θ ··· e ,θ ) = ( M e , T 1 5 1 1 2 P ] [ φ ] φ P [ P ] φ [ 2 1 5 2 1 5 e e ) = ,...,φ , ··· e ,φ M φ ( T 2 2 2 1 5 ] Q [ ψ ] Q [ ψ ψ ] [ Q 5 1 5 2 2 1 ,...,ψ e ,ψ . M ··· e ψ ( ) = e T 3 3 2 1 5 The kinematic loop constraints can be expressed as T ( θ ) = T (7.6) ( φ ) , 1 2 T (7.7) ( φ ) = T . ( ψ ) 3 2 Since these constraints must be satisfied at all times, we can express their time derivatives in terms of their spatial twists, using − 1 − 1 ̇ ̇ = T T , T T (7.8) 2 1 2 1 1 − − 1 ̇ ̇ = T T T T (7.9) . 3 2 3 2 1 − ̇ ], where T T Since ’s end-effector frame, = [ V i is the spatial twist of chain V i i i i the above identities can also be expressed in terms of the forward kinematics May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

273 Chapter 7. Kinematics of Closed Chains 255 Jacobian for each chain: ̇ ̇ ( θ = J J ( φ ) θ φ, (7.10) ) 1 2 ̇ ̇ ) J φ = J ψ ( ( ) φ ψ, (7.11) 2 3 which can be rearranged as   ̇ [ ] θ J ( φ − 0 ) θ J ( ) 1 2 ̇   = 0 (7.12) . φ 0 ) ) ( J φ ( J − ψ 3 2 ̇ ψ Now we rearrange the 15 joints into those that are actuated and those that are passive. Assume without loss of generality that the three actuated joints 3 ,φ are ( ,ψ and the vector θ q ). Define the vector of the actuated joints ∈ R 1 a 1 1 12 R as q ∈ of the passive joints p     θ 2 θ 1   .   . φ q = , q , =   1 a p . ψ 1 φ 5 15 = ( q R ,q . Equation (7.12) can now be rearranged into ) ∈ and we have q p a the form [ ] [ ] q ̇ a ( ( H ) q ) q H , (7.13) = 0 a p ̇ q p or, equivalently, ̇ + H ̇ q q = 0 , (7.14) H a p a p × 3 12 × 12 12 R and H ∈ ∈ R is invertible, we have H where . If H p p a − 1 = − H ̇ q . H q (7.15) ̇ a a p p So, assuming that H is invertible, once the velocities of the actuated joints p are given, then the velocities of the remaining passive joints can be obtained uniquely via Equation (7.15). It still remains to derive the forward kinematics Jacobian with respect to 3 × 6 V ( ∈ R ) J the actuated joints, i.e., to find satisfying V , where = J q ( q ) ̇ q a s a s a is the spatial twist of the end-effector frame. For this purpose we can use the forward kinematics for any of the three open chains: for example, in terms of ̇ , and from Equation (7.15) we can write ( θ ) J chain 1, = V θ s 1 T ̇ (7.16) = g ̇ , θ q a 2 2 T ̇ = g θ (7.17) ̇ q , a 3 3 T ̇ = g θ ̇ q , (7.18) 4 a 4 T ̇ (7.19) = g ̇ , θ q a 5 5 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

274 256 7.3. Singularities 3 ) ∈ R where each , for i = 2 ,..., 5, can be obtained from Equation (7.15). g q ( i T Defining the row vector e = [1 0 0], the differential forward kinematics for 1 chain 1 can now be written   T e 1   T   ̇ g θ 2   1   T ̇   g . (7.20) θ ) J = V ( φ   s 1 1 3   T ̇   ψ g 1 4 T g 5 T ̇ ̇ ̇ = [ θ ( q ) ̇ q , and since ̇ ) in J Since we are seeking q V = ( J φ q ψ ], from the 1 a 1 a a s 1 a above it now follows that   T e 1 T   g ) ( q 2     T q ( g ) ) = J J ( q ( ,...,q q ) (7.21) ;   3 a 1 1 5   T   ( g q ) 4 T g ( q ) 5 this equation could also have been derived using either chain 2 or chain 3. q Given values for the actuated joints , we still need to solve for the pas- a q from the loop-constraint equations. Eliminating in advance as sive joints p many elements of q as possible will obviously simplify matters. The second p point to note is that H cannot be ( q ) may become singular, in which case ̇ q p p q . Configurations in which H ( q ) becomes singular correspond obtained from ̇ p a to , which are discussed in the next section. actuator singularities 7.3 Singularities Characterizing the singularities of closed chains involves many more subtleties than for open chains. In this section we highlight the essential features of closed- chain singularities via two planar examples: a four-bar linkage (see Figure 7.5) and a five-bar linkage (see Figure 7.6). On the basis of these examples we clas- sify closed-chain singularities into three basic types: , actuator singularities configuration space singularities , and end-effector singularities . We begin with the four-bar linkage of Figure 7.5. Recall from Chapter 2 that its C-space is a one-dimensional curve embedded in a four-dimensional ambient space (each dimension is parametrized by one of the four joints). Projecting the C-space onto the joint angles ( θ,φ ) leads to the bold curve shown in Figure 7.5. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

275 Chapter 7. Kinematics of Closed Chains 257 φ π L 2 L 1 L 3 θ φ θ π − π 0 L 4 − π Figure 7.5: (Left) A planar four-bar linkage and (right) its one-dimensional C-space, – φ space. Also shown on the right are five sample config- represented in bold in the θ urations (bold dots), three of which are near bifurcation points and two of which are far removed from a bifurcation point. In terms of and φ , the kinematic loop constraint equations for the four-bar θ linkage can be expressed as ) ( ( ) β γ − 1 − 1 √ (7.22) , cos ± = tan φ 2 2 α β + α where = 2 L (7.23) L α − 2 L θ, L cos 3 1 3 4 β = − 2 L (7.24) L θ, sin 3 1 2 2 2 2 L L = θ. − L (7.25) cos γ L − L + 2 L − 1 4 1 3 4 2 The existence and uniqueness of solutions to the equations above depend on the 2 2 2 β ,...,L γ . ≤ α L + . In particular, a solution will fail to exist if link lengths 1 4 Figure 7.5 depicts the feasible configurations for the choice of link lengths L = 1 L both range from = 4 and L φ = L and = 2. For this set of link lengths, θ 4 2 3 0 to 2 π . A distinctive feature of Figure 7.5 is the presence of bifurcation points where branches of the curve meet. As the mechanism approaches these config- urations, it has a choice of which branch to follow. Figure 7.5 shows sample May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

276 258 7.3. Singularities A planar five-bar linkage. Figure 7.6: configurations on the different branches near, and also far from, the bifurcation points. We now turn to the five-bar linkage of Figure 7.6. The kinematic loop- constraint equations can be written + cos θ (7.26) + ··· + L , cos( θ L + θ ) = + θ θ L 3 2 1 5 4 1 4 1 sin θ + ··· + (7.27) sin( θ + θ + θ + θ L ) = 0 , L 3 1 4 4 2 1 1 where we have eliminated in advance the joint variable θ from the loop-closure 5 conditions. Writing these two equations in the form ( θ f ,...,θ : ) = 0, where f 4 1 4 2 R → R , the configuration space can be regarded as a two-dimensional surface 4 in R . Like the bifurcation points of the four-bar linkage, self-intersections of the surface can also occur. At such points the constraint Jacobian loses rank. For the five-bar linkage, any point θ at which ( ) ∂f ( θ ) (7.28) < 2 rank ∂θ corresponds to what we call a configuration space singularity . Figure 7.7 illustrates the possible configuration space singularities of the five-bar linkage. Notice that so far we have made no mention of which joints of the five-bar linkage are actuated, or where the end-effector frame is placed. The notion of a configuration space singularity is completely independent of the choice of actuated joints or where the end-effector frame is placed. We now consider the case when two joints of the five-bar linkage are actu- ated. Referring to Figure 7.8, the two revolute joints fixed to ground are the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

277 Chapter 7. Kinematics of Closed Chains 259 Configuration space singularities of the planar five-bar linkage. Figure 7.7: Figure 7.8: Actuator singularities of the planar five-bar linkage, where in each case the two actuated joints are shaded gray. The singularity on the left is nondegenerate, while the singularity on the right is degenerate. . actuated joints. Under normal operating conditions, the motions of the actu- ated joints can be independently controlled. Alternatively, locking the actuated joints should immobilize the five-bar linkage and turn it into a rigid structure. For the nondegenerate actuator singularity shown in the left-hand panel of Figure 7.8, rotating the two actuated joints oppositely and outward will pull the mechanism apart; rotating them oppositely and inward would either crush the inner two links or cause the center joint to unpredictably buckle upward or downward. For the degenerate actuator singularity shown on the right, even when the actuated joints are locked in place the inner two links are free to rotate. The reason for classifying these singularities as actuator singularities is May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

278 260 7.3. Singularities End-effector singularity of the planar five-bar linkage. Figure 7.9: that, by relocating the actuators to a different set of joints, such singularities can be eliminated. For both the degenerate and nondegenerate actuator singularities of the five-bar linkage, relocating one actuator to one of the other three passive joints eliminates the singularity. Visualizing the actuator singularities of the planar five-bar linkage is straight- forward enough but, for more complex spatial closed chains, visualization may be difficult. Actuator singularities can be characterized mathematically by the rank of the constraint Jacobian. As before, write the kinematic loop constraints in differential form: [ ] ] [ ̇ q a H q ) H ( q ) ( = q ) ̇ H q ( = 0 , (7.29) a p q ̇ p a p ∈ R q is the vector of the a actuated joints and q is the vector of ∈ R where p a p × ( a + p ) ∈ R the p passive joints. It follows that H ( q ) and that H p ( q ) is a p × p matrix. With the above definitions, we have the following: is an H . Distinguishing be- ( q ) < p then • If rank actuator singularity q p tween degenerate and nondegenerate singularities involves additional mathematical subtleties and relies on second-order derivative information; we do not pursue this further here. • If rank H ( q ) < p then q is a configuration space singularity . Note that under this condition ( q ) is also singular (the converse is not true, H p however). The configuration space singularities can therefore be regarded as the intersection of all possible actuator singularities obtained over all possible combinations of actuated joints. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

279 Chapter 7. Kinematics of Closed Chains 261 The final class of singularities depends on the choice of end-effector frame. For the five-bar linkage, let us ignore the orientation of the end-effector frame – y location of the end-effector frame. Figure 7.9 x and focus exclusively on the for a given choice of end-effector singularity shows the five-bar linkage in an end-effector location. Note that velocities along the dashed line are not possible in this configuration, similarly to the case of singularities for open chains. To see why these velocities are not possible, consider the effective 2R open chain created by the rightmost joint, the link connecting it to the platform, the joint on the platform, and the effective link connecting the platform joint to the end- effector frame. Since the two links of the 2R robot are aligned, the end-effector frame can have no component of motion along the direction of the links. End-effector singularities are independent of the choice of actuated joints. They can be mathematically characterized as follows. Choose any valid set of q such that the mechanism is not at an actuator singularity. actuated joints a Write the forward kinematics in the form f ( q (7.30) ) = T . sb a f One can then check for rank deficiencies in the Jacobian of , as was done for open chains, to determine the presence of an end-effector singularity. 7.4 Summary Any kinematic chain that contains one or more loops is called a closed • . Parallel mechanisms are a class of closed chains that are char- chain acterized by two platforms – one moving and one stationary – connected by several legs; the legs are typically open chains, but can themselves be closed chains. The kinematic analysis of closed chains is complicated com- pared with that of open chains because only a subset of joints is actuated and because the joint variables must satisfy a number of loop-closure con- straint equations which may or may not be independent, depending on the configuration of the mechanism. • For a parallel mechanism with equal numbers of actuators and degrees of freedom, the inverse kinematics problem involves finding, from the given position and orientation of the moving platform, the joint coordinates of the actuated joints. For well-known parallel mechanisms like the planar 3 × RPR and the spatial Stewart–Gough platform, the inverse kinematics admits unique solutions. • For a parallel mechanism with equal numbers of actuators and degrees of freedom, the forward kinematics problem involves finding the position and May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

280 262 7.5. Notes and References orientation of the moving platform given coordinates for all the actuated × RPR and joints. For well-known parallel mechanisms like the planar 3 the spatial Stewart–Gough platform, the forward kinematics usually ad- mits multiple solutions. In the case of the most general Stewart–Gough platform, a maximum of 40 solutions is possible. • The differential kinematics of a closed chain relates the velocities of the actuated joints to the linear and angular velocities of the moving plat- n one- form’s end-effector frame. For an m -dof closed chain consisting of m n − m q and respectively denote the vector ∈ R ∈ R dof joints, let q p a of actuated and passive joints. The kinematic loop-closure constraints H ̇ q H can then be expressed in differential form as ̇ q = 0, where + a p a p ( − ( n − m ) × m ) m − n ( n × m ) ∈ R R ∈ H and are configuration-dependent H p a − 1 ; the differential for- q ̇ H = − H q is invertible then ̇ matrices. If H a a p p p q V ( q = ,q ward kinematics can then be expressed in the form ) ̇ J , a p a m 6 × ( q is ,q V ) ∈ R is the twist of the end-effector frame and where J p a a configuration-dependent Jacobian matrix. For closed chains like the Stewart–Gough platform, the differential forward kinematics can also be obtained from a static analysis by exploiting the fact that, just as for open chains, the wrench applied by the end-effector is related to the F T F τ = joint forces or torques τ by . J • Singularities for closed chains can be classified into three types: (i) config- uration space singularities at self-intersections of the configuration space surface (also called bifurcation points for one-dimensional configuration spaces); (ii) nondegenerate actuator singularities, when the actuated joints cannot be independently actuated, and degenerate actuator singularities when locking all joints fails to make the mechanism a rigid structure; and (iii) end-effector singularities when the end-effector loses one or more degrees of freedom of motion. Configuration space singularities are in- dependent of the choice of actuated joints, while actuator singularities depend on which joints are actuated. End-effector singularities depend on the placement of the end-effector frame but do not depend on the choice of actuated joints. 7.5 Notes and References A comprehensive reference for all aspects of parallel robots is [116]; [117] pro- vides a more compact summary but with more recent references. One of the major outstanding problems in parallel mechanism kinematics in the 1990s was May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

281 Chapter 7. Kinematics of Closed Chains 263 the question of how many forward kinematics solutions can exist for the gen- eral 6–6 platform consisting of six SPS legs (with the prismatic joints actuated) connecting a fixed platform to a moving platform. Raghavan and Roth [143] showed that there can be at most 40 solutions, while Husty [62] developed an algorithm for finding all 40 solutions. Singularities of closed chains have also received considerable attention in the literature. The terminology for closed-chain singularities used in this chapter was introduced in [136]; in particular, the distinction between degenerate and nondegenerate actuator singularities derives in part from similar terminology used in Morse theory to identify those critical points where the Hessian is sin- gular (i.e., degenerate). The 3 UPU mechanism, which is addressed in the × exercises in both Chapter 2 and the current chapter, can exhibit rather unusual singularity behavior; a more detailed singularity analysis of this mechanism can be found in [52, 35]. 7.6 Exercises A 2 B 3 ˆy B 2 P ˆx B 1 ˆy A A 1 3 O ˆx Figure 7.10: 3 × RPR planar parallel mechanism. Exercise 7.1 RPR planar parallel mechanism of Figure 7.10 the × In the 3 2 prismatic joints are actuated. Define ∈ R to be the vector from the fixed- a i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

282 264 7.6. Exercises , = 1 , 2 , 3, expressed in fixed-frame coordinates. frame origin O to joint A i i 2 ∈ to be the vector from the moving-platform-frame origin P to Define b R i i , , 2 , 3, defined in terms of the moving-platform-frame coordinates. joint B = 1 i (a) Solve the inverse kinematics. (b) Derive a procedure to solve the forward kinematics. (c) Is the configuration shown in the figure an end-effector singularity? Ex- plain your answer by examining the inverse kinematics Jacobian. Is this also an actuator singularity? × RPR planar parallel mechanism in Figure 7.11(a), Exercise 7.2 For the 3 b be the angle measured from the s } -frame ˆx-axis to the { φ } -frame ˆx-axis, let { 2 ∈ R } be the vector from the { s and p { b } -frame origin, -frame origin to the 2 s } -frame coordinates. Let a { ∈ R expressed in be the vector from the { s } - i frame origin to the three joints fixed to ground, = 1 , 2 , 3 (note that two of i 2 s } -frame coordinates. Let b the joints are overlapping), expressed in ∈ R { be i { } -frame origin to the three joints attached to the moving the vector from the b i = 1 , platform, , 3 (note that two of the joints are overlapping), expressed in 2 { b } -frame coordinates. The three prismatic joints are actuated, and the leg lengths are θ , as shown. , θ θ , and 2 3 1 ) and ( φ,p ,θ θ ,θ (a) Derive a set of independent equations relating ( ). 1 2 3 (b) What is the maximum possible number of forward kinematics solutions? = (1 , 0 (c) Assuming static equilibrium, given joint forces − 1) applied at τ , θ ,θ ,θ joints ( ), find the planar wrench ( m ,f ,f ) in the end-effector 3 bx 2 by 1 bz { } . b frame (d) Now construct a mechanism with three connected 3 × RPR parallel mech- 7.11(b). How many degrees of freedom does anisms as shown in Figure this mechanism have? Exercise 7.3 For the 3 × RRR planar parallel mechanism shown in Figure 7.12, 2 φ be the orientation of the end-effector frame and p ∈ R let be the vector p 2 expressed in fixed-frame coordinates. Let ∈ R a be the vector a expressed in i i 2 ∈ R fixed-frame coordinates and be the vector b expressed in the moving b i i body-frame coordinates. (a) Derive a set of independent equations relating ( φ,p ) and ( θ ). ,θ ,θ 2 3 1 (b) What is the maximum possible number of inverse and forward kinematic solutions for this mechanism? Exercise 7.4 Figure 7.13 p , shows a six-bar linkage in its zero position. Let ( x -frame coordinates, ) be the position of the { b } -frame origin expressed in { s } p y May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

283 Chapter 7. Kinematics of Closed Chains 265 ˆ y 2 b { } θ 1 ˆ x ˆ y 1 } s { p ˆ x 1 θ 2 ◦ 45 θ 3 (a) 3 RPR planar parallel mechanism. × (b) Truss. 3 × Figure 7.11: RPR planar parallel mechanism and truss structure. be the orientation of the φ b } frame. The inverse kinematics problem and let { is defined as that of finding the joint variables ( ) given ( p ). ,p ,φ θ,ψ y x (a) In order to solve the inverse kinematics problem, how many equations are needed? Derive these equations. (b) Assume that joints A, D, and E are actuated. Determine whether the configuration shown in Figure 7.13 is an actuator singularity by analyzing an equation of the form [ ] [ ] ̇ q a H H = 0 , a p q ̇ p where q is the vector of the actuated joints and q is the vector of the p a passive joints. (c) Suppose instead that joints A, B, and D are actuated. Find the forward kinematics Jacobian J -frame from V } = J s ̇ q { , where V is the twist in s a s a a coordinates and ̇ q is the vector of actuated joint rates. a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

284 266 7.6. Exercises ˆ y a 3 ˆ x θ p 3 b a 3 2 a 1 θ 1 φ b 2 b 1 θ 2 3 × RRR planar parallel mechanism. Figure 7.12: E F D 1 111 ψ ˆ ψ y ψ 3 2 1 1 ˆ x { b } 1 end-effector C θ 3 1 ˆ y θ θ 2 1 ˆ x s } { B A 1 Figure 7.13: A six-bar-linkage. Exercise 7.5 Consider the 3 × PSP spatial parallel mechanism of Figure 7.14. (a) How many degrees of freedom does this mechanism have? ,φ R ) be the orientation of the body frame = Rot(ˆz ,θ )Rot(ˆy (b) Let )Rot(ˆx ,ψ sb May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

285 Chapter 7. Kinematics of Closed Chains 267 3 R p { = ( x,y,z ) ∈ , and let b be the vector from the { s } -frame origin } sb { b } -frame origin (both R -frame and p } are expressed in to the s { sb sb 2 = 1 b 3, are defined as shown in the , d , , i coordinates). The vectors a , , i i i figure. Derive a set of independent kinematic constraint equations relating ( θ,φ,ψ,x,y,z ) and the defined vectors. (c) Given values for ( x,y,z ), is it possible to solve for the vertical prismatic joint values s 3? If so, derive an algorithm , where s , = ‖ d 2 ‖ for i = 1 , i i i for doing so. ◦ 120 Prismatic ˆz joint } b { end-effector ˆ x ˆ y b i Spherical joint d i p sb Prismatic joint } s { ˆz ◦ 120 ˆ x ◦ 60 a i ˆ y stationary Figure 7.14: 3 × PSP spatial parallel manipulator. Exercise 7.6 The Eclipse mechanism of Figure is a six-dof parallel mech- 7.15 ◦ ± 90 anism whose moving platform can tilt by with respect to ground and can ◦ also rotate by 360 about the vertical axis. Assume that the six sliding joints are actuated. (a) Derive the forward and inverse kinematics. How many forward kinematic solutions are there for general nonsingular configurations? (b) Find and classify all singularities of this mechanism. Exercise 7.7 For the Delta robot of Figure 7.1(b), obtain the following: (a) the forward kinematics, (b) the inverse kinematics, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

286 268 7.6. Exercises VerticalColumn Prismaticjoint Spherical B 2 Revolute Joint Joint C 2 C 1 w v B 1 A 2 u C 3 B 3 z y x O A 1 A 3 CircularGuide SlidealongtheCircularGuide Figure 7.15: The Eclipse mechanism. J (assume that the revolute joints at the fixed base are (c) the Jacobian a actuated). (d) Identify all actuator singularities of the Delta robot. Exercise 7.8 × UPU platform of Figure 7.16, the axes of the universal In the 3 joints are attached to the fixed and moving platforms in the sequence indicated, i.e., axis 1 is attached orthogonally to the fixed base, while axis 4 is attached orthogonally to the moving platform. Obtain the following: (a) the forward kinematics, (b) the inverse kinematics, (c) the Jacobian J (assume that the revolute joints at the fixed base are a actuated). (d) Identify all actuator singularities of this robot. (e) If you can, build a mechanical prototype and see whether the mechanism behaves as predicted by your analysis. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

287 269 Chapter 7. Kinematics of Closed Chains B 3 ˆx B 2 ˆz { 1 } ˆy h d 2 3 d 2 B 3 1 4 A 3 A 2 ˆyˆx 0 } { d 1 ˆz h 1 A 1 2 1 Figure 7.16: The 3 × UPU mechanism. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

288 270 7.6. Exercises May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

289 Chapter 8 Dynamics of Open Chains In this chapter we study once again the motions of open-chain robots, but this time taking into account the forces and torques that cause them; this is the robot dynamics subject of . The associated dynamic equations – also referred to as the equations of motion – are a set of second-order differential equations of the form ̈ ̇ θ, θ M θ + h ( ( = θ ) , (8.1) τ ) n n R is the vector of joint variables, τ ∈ R θ is the vector of joint forces where ∈ n × n mass matrix ) θ ( M is a symmetric positive-definite R , and ∈ and torques, n ̇ h θ ) ∈ R ( are forces that lump together centripetal, Coriolis, gravity, and θ, ̇ θ and friction terms that depend on θ . One should not be deceived by the apparent simplicity of these equations; even for “simple” open chains, e.g., those with joint axes that are either orthogonal or parallel to each other, θ ) and M ( ̇ θ, ( ) can be extraordinarily complex. h θ Just as a distinction was made between a robot’s forward and inverse kine- forward and in- matics, it is also customary to distinguish between a robot’s . The forward problem is the problem of determining the verse dynamics ̈ ̇ given the state ( θ, θ ) and the joint forces and torques, θ robot’s acceleration ) ( 1 − ̇ ̈ ( θ ) M τ − h ( θ, θ θ ) = , (8.2) and the inverse problem is finding the joint forces and torques corresponding τ to the robot’s state and a desired acceleration, i.e., Equation (8.1). A robot’s dynamic equations are typically derived in one of two ways: by a direct application of Newton’s and Euler’s dynamic equations for a rigid body (often called the Newton–Euler formulation ) or by the Lagrangian dy- namics formulation derived from the kinetic and potential energy of the robot. 271

290 272 8.1. Lagrangian Formulation The Lagrangian formalism is conceptually elegant and quite effective for robots with simple structures, e.g., with three or fewer degrees of freedom. The calcula- tions can quickly become cumbersome for robots with more degrees of freedom, however. For general open chains, the Newton–Euler formulation leads to effi- cient recursive algorithms for both the inverse and forward dynamics that can also be assembled into closed-form analytic expressions for, e.g., the mass matrix ) and the other terms in the dynamics equation (8.1). The Newton–Euler ( θ M formulation also takes advantage of tools we have already developed in this book. In this chapter we study both the Lagrangian and Newton–Euler dynamics formulations for an open-chain robot. While we usually express the dynamics in terms of the joint space variables θ , it is sometimes convenient to express it in terms of the configuration, twist, and rate of change of the twist of the end- effector. This is the task-space dynamics, studied in Section 8.6. Sometimes robots are subject to a set of constraints on their motion, such as when the robot makes contact with a rigid environment. This leads to a formulation of the constrained dynamics (Section 8.7), whereby the space of joint torques and forces is divided into a subspace that causes motion of the robot and a subspace that causes forces against the constraints. The URDF file format for specifying 8.8. Finally, some practical robot inertial properties is described in Section issues that arise in the derivation of robot dynamics, such as the effect of motor gearing and friction, are described in Section 8.9. Lagrangian Formulation 8.1 8.1.1 Basic Concepts and Motivating Examples The first step in the Lagrangian formulation of dynamics is to choose a set of n q independent coordinates R that describes the system’s configuration. The ∈ coordinates are called generalized coordinates . Once generalized coordi- q n nates have been chosen, these then define the generalized forces f ∈ R . The forces f and the coordinate rates ̇ q are dual to each other in the sense that the T inner product ̇ q corresponds to power. A Lagrangian function L ( q, ̇ q ) is then f ) minus the potential energy K q, ̇ q ( defined as the overall system’s kinetic energy P ( q ), L ( q, ̇ q ) = K ( q, ̇ q ) −P ( q ) . The equations of motion can now be expressed in terms of the Lagrangian as follows: ∂ L ∂ L d − , (8.3) = f ∂ ̇ ∂q q dt May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

291 Chapter 8. Dynamics of Open Chains 273 m 2 L 2 θ 2 ˆy θ π/ = 2 2 m 1 L 1 g θ 1 ˆx θ =0 1 θ = (0 ,π/ 2). Figure 8.1: (Left) A 2R open chain under gravity. (Right) At Euler–Lagrange equations with These equations are also referred to as the 1 external forces The derivation can be found in dynamics texts. . We illustrate the Lagrangian dynamics formulation through two examples. m constrained to move on In the first example, consider a particle of mass a vertical line. The particle’s configuration space is this vertical line, and a natural choice for a generalized coordinate is the height of the particle, which we denote by the scalar variable x ∈ . Suppose that the gravitational force m g R f acts downward, and an external force is applied upward. By Newton’s second law, the equation of motion for the particle is ̈ − g = f m x. (8.4) m We now apply the Lagrangian formalism to derive the same result. The kinetic 2 m ̇ x , and the Lagrangian is / 2, the potential energy is m gx energy is 1 2 m x ) = K ( x, ̇ x ) −P ( x ) = L ( x, ̇ x ̇ − m gx. (8.5) 2 The equation of motion is then given by d ∂ L ∂ L f = m + − (8.6) g, x = m ̈ ∂x x ∂ dt ̇ which matches Equation (8.4). We now derive the dynamic equations for a planar 2R open chain moving in the presence of gravity (Figure 8.1). The chain moves in the ˆx–ˆy-plane, with gravity g acting in the − ˆy-direction. Before the dynamics can be derived, the mass and inertial properties of all the links must be specified. To keep things simple the two links are modeled as point masses and m concentrated at m 1 2 1 The external force f is zero in the standard form of the Euler–Lagrange equations. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

292 274 8.1. Lagrangian Formulation the ends of each link. The position and velocity of the link-1 mass are then given by [ ] [ ] x L cos θ 1 1 1 = , y sin θ L 1 1 1 [ ] [ ] x ̇ L − θ sin 1 1 1 ̇ = , θ 1 ̇ y θ cos L 1 1 1 while those of the link-2 mass are given by [ ] [ ] x ) cos θ + + L L cos( θ θ 2 1 1 2 1 2 = , y sin θ + L L sin( θ ) + θ 2 2 1 1 1 2 [ ] ][ [ ] ̇ ̇ x θ − L ) sin θ θ − L + sin( θ θ + θ sin( ) − L 2 2 1 2 1 2 1 2 1 1 = . ̇ θ L cos( θ ) + θ ̇ cos L + cos( θ y + θ L ) θ 2 1 1 2 2 2 2 1 1 2 θ = ( θ We choose the joint coordinates ,θ ) as the generalized coordinates. 2 1 T ̇ τ ,τ θ τ τ = ( The generalized forces ) then correspond to joint torques (since 2 1 ̇ ( θ, θ corresponds to power). The Lagrangian ) is of the form L 2 ∑ ̇ ( ) = ( θ L θ, K , −P ) (8.7) i i i =1 where the link kinetic energy terms K are K and 2 1 1 1 2 2 2 2 ̇ x ( ̇ + ̇ y L m ) = m θ = K 1 1 1 1 1 1 1 2 2 1 2 2 m ) ( ̇ x y + ̇ K = 2 2 2 2 2 ( ) 1 2 2 2 2 2 2 ̇ ̇ ̇ ̇ L θ + L L L ) + 2 θ m = + 2( L cos L + L L cos θ ) ( θ θ + θ , 2 1 2 1 2 2 2 2 1 2 1 1 2 2 2 2 P are and P and the link potential energy terms 2 1 = m gy = P gL , sin θ m 1 1 1 1 1 1 θ P m . gy )) = m θ g ( L + sin = θ + L sin( 2 1 1 1 2 2 2 2 2 The Euler–Lagrange equations (8.3) for this example are of the form ∂ L ∂ L d 2 , i . , − = 1 (8.8) = τ i ̇ dt ∂θ θ ∂ i i The dynamic equations for the 2R planar chain follow from explicit evaluation of the right-hand side of (8.8) (we omit the detailed calculations, which are straightforward but tedious): May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

293 Chapter 8. Dynamics of Open Chains 275  ( ) 2 2 2 ̈ L m + 2 L + L τ cos θ L + L m θ ) = (  1 1 2 1 1 2 2 1 2 1     2 2 ̈ ̇ ̇ ̇  m θ cos − m θ L + L ( sin θ ) (2 L θ + L θ θ + ) L  2 2 1 2 2 1 1 2 2 2 2 2 2  (8.9) + + m m gL + ( cos( θ ) + θ m ) , L g cos θ 1 1 2 1 2 2 1 2    2 2 2 ̇ ̈ ̈  θ + m L ( L L θ L ) sin θ m = cos θ τ + m L θ + L  1 2 2 2 2 1 2 2 2 1 2 2 1 2 2    . ) gL + cos( θ m + θ 2 1 2 2 We can gather terms together into an equation of the form ̇ ̈ (8.10) , ( ) ( θ, θ θ ) + g c θ ) θ + τ = M ( ︷︷ ︸ ︸ ̇ θ ) θ, ( h with ] [ 2 2 2 2 θ L m ( L L + + 2 L L cos + + L m ) m ( L L ) cos θ 2 2 2 1 1 1 2 2 2 2 1 1 2 θ M , ( ) = 2 2 ( cos θ L + m L L ) m L 2 2 1 2 2 2 2 [ ] 2 ̇ ̇ ̇ − L L m sin θ (2 θ ) θ + θ 2 2 1 2 1 2 2 ̇ ( ) = θ, θ c , 2 ̇ L L m θ sin θ 2 2 1 2 1 [ ] m ) + m θ ) L + g cos θ ( + m θ gL cos( 2 2 1 1 2 2 1 1 g ( θ ) = , cos( θ + gL m ) θ 2 2 2 1 ̇ ( c ( θ, ) is the symmetric positive-definite mass matrix, θ ) is the vector M θ where ( θ ) is the vector containing containing the Coriolis and centripetal torques, and g the gravitational torques. These reveal that the equations of motion are linear ̈ ̇ , quadratic in θ , and trigonometric in θ . This is true in general for serial in θ chains containing revolute joints, not just for the 2R robot. ̈ ̇ θ ) θ, θ + c ( The M θ ) terms in Equation (8.10) could have been derived by ( a f writing a are written for each point mass, where the accelerations m = i i i i , by differentiating the expressions for ( ̇ x ) given , ̇ y in terms of ) and ( ̇ x θ , ̇ y 2 1 2 1 above:       2 ̈ ̇ f ̈ s c x − L θ L θ − x 1 1 1 1 1 1 1 1 2       ̈ ̇ f ̈ y = (8.11) f , = m m = θ c L + s θ − L 1 y 1 1 1 1 1 1 1 1 1 1 f z ̈ 0 1 z 1   2 2 ̈ ̈ ̇ ̇ ̈ ̇ − + )s s c θ − L θ ( θ θ L + ( θ − ) L c L θ − 1 12 1 12 2 2 2 1 1 1 1 2 1 1 2 2   ̈ ̈ ̇ ̈ ̇ ̇ (8.12) , = f m c ( − + s θ )c ) θ s + + L θ ( θ L L + θ L − θ 2 2 1 1 1 2 2 2 12 1 12 2 1 1 1 1 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

294 276 8.1. Lagrangian Formulation where s + θ ), etc. Defining r as the vector from joint 1 to indicates sin( θ 11 2 12 1 , r as the vector from joint 1 to m m , and r as the vector from joint 2 to 22 1 12 2 { m } attached to joints 1 and 2 can be , the moments in world-aligned frames i 2 f = r . (Note that joint 1 × expressed as f + r × × m r and m = f 2 22 12 1 2 11 1 2 , but joint 2 only needs to provide m m must provide torques to move both and 2 1 m τ and τ are just the third elements of torque to move .) The joint torques 2 1 2 axes out of the page, respectively. and m m , i.e., the moments about the ˆz 2 i 1 x,y ) coordinates, the accelerations of the masses are written simply as In ( x y ). This is because the , second time-derivatives of the coordinates, e.g., ( ̈ ̈ 2 2 ) are not in an θ ˆx-ˆy frame is an inertial frame. The joint coordinates ( ,θ 1 2 inertial frame, however, so accelerations are expressed as a sum of terms that ̈ , and quadratic of the θ are linear in the second derivatives of joint variables, T ̇ ̇ θ , as seen in Equations (8.11) and (8.12). first derivatives of joint variables, θ 2 ̇ are called centripetal terms, and quadratic Quadratic terms containing θ i ̇ ̇ ̈ Coriolis θ 6 = j, are called ,i terms. In other words, terms containing θ = 0 θ j i does not mean zero acceleration of the masses, due to the centripetal and Coriolis terms. To better understand the centripetal and Coriolis terms, consider the arm θ = ,θ θ ) = (0 ,π/ 2), i.e., cos θ ) = 1, sin at the configuration ( θ θ + = sin( 1 2 1 2 1 1 ̈ θ from Equa- + θ m ) = 0. Assuming ) of θ = 0, the acceleration ( ̈ x cos( , ̈ y 2 2 1 2 2 tion (8.12) can be written [ ] [ ] [ ] 2 ̇ 0 ̈ x L − θ 2 1 1 + . = 2 2 ̇ ̇ ̇ ̇ y ̈ L 2 − θ θ − L L − θ θ 2 2 1 2 2 2 1 2 ︷︷ ︸ ︸ ︸ ︷︷ ︸ Coriolis terms centripetal terms 2 2 ̇ ̇ L a Figure 8.2 shows the centripetal acceleration = ( − θ − L , θ ) when 1 cent1 2 1 1 2 ̇ ̇ ̇ a θ = (0 , − L = 0, the centripetal acceleration ) when θ = 0, and the θ 2 2 cent2 1 2 ̇ ̇ ̇ ̇ 2 L − , a Coriolis acceleration = (0 θ are positive. θ ) when both θ θ and cor 2 1 1 2 2 As illustrated in Figure 8.2, each centripetal acceleration pulls m toward a 2 cent i 2 i to keep m joint rotating about the center of the circle defined by joint i . 2 Therefore a creates zero torque about joint i a . The Coriolis acceleration cent cor i in this example passes through joint 2, so it creates zero torque about joint 2 but it creates negative torque about joint 1; the torque about joint 1 is negative m gets closer to joint 1 (due to joint 2’s motion). Therefore the inertia because 2 due to m about the ˆz -axis is dropping, meaning that the positive momentum 1 2 ̇ about joint 1 drops while joint 1’s speed theta is constant. Therefore joint 1 1 must apply a negative torque, since torque is defined as the rate of change of 2 Without this centripetal acceleration, and therefore centripetal force, the mass m would 2 fly off along a tangent to the circle. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

295 Chapter 8. Dynamics of Open Chains 277 a a t1 cen t1 cen a t2 cen a t2 cen a co r ̈ when θ = (0 ,π/ 2) and Figure 8.2: θ Accelerations of m = 0. (Left) The centripetal 2 2 2 ̇ ̇ ̇ m when = 0. (Middle) The centripetal ) of θ θ L − , θ − = ( acceleration L a 2 2 2 1 cent1 1 1 2 ̇ ̇ m = 0. (Right) When both joints are θ θ when ) of − L a acceleration = (0 , 2 1 cent2 2 2 ̇ rotating with > 0, the acceleration is the vector sum of a , and the θ a , cent2 cent1 i ̇ ̇ θ ). θ , L 2 = (0 − Coriolis acceleration a 2 1 cor 2 ̇ θ angular momentum. Otherwise would increase as m gets closer to joint 1, 2 1 just as a skater’s rotation speed increases as she pulls in her outstretched arms while doing a spin. General Formulation 8.1.2 n -link open We now describe the Lagrangian dynamics formulation for general n chains. The first step is to select a set of generalized coordinates ∈ R for θ the configuration space of the system. For open chains all of whose joints are actuated, it is convenient and always possible to choose θ to be the vector of the n τ . If θ ∈ is a revolute R joint values. The generalized forces will be denoted i will correspond to a torque, while if θ joint then is a prismatic joint then τ τ i i i will correspond to a force. θ has been chosen and the generalized forces τ identified, the next step Once ̇ is to formulate the Lagrangian θ, L θ ) as follows: ( ̇ ̇ L θ ) = K ( θ, θ, ( ) −P ( θ ) , (8.13) θ ̇ K ( θ, ) is the potential energy of the overall θ ) is the kinetic energy and P ( θ where system. For rigid-link robots the kinetic energy can always be written in the form n n ∑ ∑ 1 1 T ̇ ̇ ̇ ̇ K ) = θ ( θ ) θ ) θ θ ( m (8.14) M θ, θ ( = j i ij 2 2 i j =1 =1 n where ); a con- ( θ ) is the ( i,j )th element of the n × m mass matrix M ( θ ij structive proof of this assertion is provided when we examine the Newton–Euler formulation. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

296 278 8.1. Lagrangian Formulation The dynamic equations are analytically obtained by evaluating the right- hand side of d L ∂ ∂ L = τ − (8.15) ,...,n. = 1 , i i ̇ ∂θ dt ∂ θ i i With the kinetic energy expressed as in Equation (8.14), the dynamics can be written explicitly as n n n ∑ ∑ ∑ ∂ P ̈ ̇ ̇ θ θ + (8.16) ) ,...,n, = 1 , i τ = ( m Γ + ( θ ) θ θ j i ij j ijk k ∂θ i =1 j j =1 k =1 ( where the Γ ), known as the Christoffel symbols of the first kind , are θ ijk defined as follows: ( ) ∂m 1 ∂m ∂m ij ik jk ) = Γ + . (8.17) ( θ − ijk ∂θ ∂θ 2 ∂θ j k i This shows that the Christoffel symbols, which generate the Coriolis and cen- ̇ θ θ, tripetal terms ( ), are derived from the mass matrix M ( c ). θ As we have already seen, the equations (8.16) are often gathered together in the form ̈ ̇ ̈ ̇ θ ) ) θ + c ( θ, τ θ ) + g ( θ ) or M ( θ ) = θ + h ( θ, M θ ( , where g ) is simply ∂ P /∂θ . θ ( We can see explicitly that the Coriolis and centripetal terms are quadratic in the velocity by using the form T ̈ ̇ ̇ ) θ θ + τ θ = Γ( θ ) M θ + g ( ( ) , (8.18) θ T ̇ ̇ ) n × n matrix and the product θ θ n Γ( θ ) is an × θ should be interpreted where Γ( as follows:   T ̇ ̇ θ θ ) θ ( Γ 1 T ̇ ̇   θ θ ( Γ θ ) 2   T ̇ ̇ θ Γ( θ ) θ = ,   . .   . T ̇ ̇ ) θ θ ( Γ θ n . θ ) is an n × n matrix with ( j,k )th entry Γ where Γ ( i ijk It is also common to see the dynamics written as ̇ ̇ ̈ ) ( ) M θ + C ( θ, τ θ θ = θ + g ( θ ) , May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

297 Chapter 8. Dynamics of Open Chains 279 n × n ̇ )th entry R ( θ ) is called the Coriolis matrix , with ( i,j ∈ C where θ, n ∑ ̇ ̇ θ, θ ) = ( c Γ θ ( θ ) . (8.19) ij ijk k k =1 passivity property The Coriolis matrix is used to prove the following (Propo- 8.1), which can be used to prove the stability of certain robot control sition 11.4.2.2. laws, as we will see in Section n × n ̇ ̇ − Proposition 8.1. C The matrix θ, 2 θ ) ∈ R M ( θ is skew symmetric, ) ( × n n ̇ ̇ R ) where M is the mass matrix, ( M ( θ ) its time derivative, and C ( θ, θ θ ) ∈ ∈ n × n is the Coriolis matrix as defined in Equation (8.19) . R ̇ C )th component of M − 2 i,j is The ( Proof. n ∑ ∂m ∂m ∂m ∂m ij kj ik ij ̇ ̇ ̇ ̇ ̇ − θ θ θ − + θ m 2 ( θ, θ ̇ ( ) θ c − ) = k k k k ij ij ∂θ ∂θ ∂θ ∂θ k i k j =1 k n ∑ ∂m ∂m kj ik ̇ ̇ = . − θ θ k k ∂θ ∂θ j i k =1 By switching the indices i j , it can be seen that and ̇ ̇ m θ ) − 2 c ̇ ( θ, , θ ) = − ( ̇ m )) ( θ ) − 2 c θ ( θ, ( ij ji ji ij T ̇ ̇ 2 C ) M = − ( thus proving that ( M − 2 C − ) as claimed. 8.1.3 Understanding the Mass Matrix 1 T ̇ ̇ ( M The kinetic energy is a generalization of the familiar expression ) θ θ θ 2 1 T mv v ) is positive for a point mass. The fact that the mass matrix M ( θ 2 T ̇ ̇ ̇ ( θ ) θ θ > 0 for all definite, meaning that θ 6 = 0, is a generalization of the fact M that the mass of a point mass is always positive, m > 0. In both cases, if the velocity is nonzero, the kinetic energy must be positive. On the one hand, for a point mass with dynamics expressed in Cartesian f = m ̈ coordinates as , the mass is independent of the direction of acceleration, x and the acceleration ̈ x is always “parallel” to the force, in the sense that ̈ x is a scalar multiple of f . A mass matrix M ( θ ), on the other hand, presents a ̈ different effective mass in different acceleration directions, and is not generally θ ̇ τ even when a scalar multiple of θ = 0. To visualize the direction dependence T ̈ ̈ ̈ θ = 1 } | θ { of the effective mass, we can map a unit ball of joint accelerations θ through the mass matrix M ( θ ) to generate a joint force–torque ellipsoid when May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

298 280 8.1. Lagrangian Formulation ̈ τ [ ] θ 2 2 . 0 − 5 . 0 5 − 1 M ( θ )= 51 − . 0 . 5 ◦ θ =0 1 ◦ =90 θ 2 ] [ 31 )= θ ( M 11 τ ̈ 1 θ 1 ̈ θ 2 τ 2 [ ] . 0 . 8 − 0 11 1 − M θ )= ( 111 . 01 0 . − ̈ θ 1 τ 1 ] [ . 270 . 13 1 M )= θ ( ◦ 131 0 . θ =0 1 ◦ =150 θ 2 ̈ (Bold lines) A unit ball of accelerations in Figure 8.3: θ maps through the mass ( ) to a torque ellipsoid that depends on the configuration of the 2R arm. matrix θ M These torque ellipsoids may be interpreted as mass ellipsoids. The mapping is shown ◦ ◦ ◦ ◦ ) and (0 , 150 , ). (Dotted lines) A unit ball in τ for two arm configurations: (0 90 1 − M ( θ ) to an acceleration ellipsoid. maps through ̇ θ = 0). An example is shown in Figure 8.3 the mechanism is at rest ( for m 8.1, with = L the 2R arm of Figure = m L = = 1, at two different 2 1 1 2 ◦ ◦ ◦ ◦ θ , 90 ). The torque ) and ( θ ,θ ) = (0 joint configurations: ( , 150 ) = (0 ,θ 1 2 2 1 ellipsoid can be interpreted as a direction-dependent mass ellipsoid: the same ̈ joint acceleration magnitude θ ‖ ‖ ‖ τ ‖ requires different joint torque magnitudes depending on the acceleration direction. The directions of the principal axes of the mass ellipsoid are given by the eigenvectors v ) and the lengths of M ( θ i λ . The of the principal semi-axes are given by the corresponding eigenvalues i ̈ τ θ is only a scalar multiple of acceleration when τ is along a principal axis of the ellipsoid. It is easier to visualize the mass matrix if it is represented as an effective mass May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

299 Chapter 8. Dynamics of Open Chains 281 of the end-effector, since it is possible to feel this mass directly by grabbing and moving the end-effector. If you grabbed the endpoint of the 2R robot, depending on the direction you applied force to it, how massy would it feel? Let us denote ), and the velocity of the θ the effective mass matrix at the end-effector as Λ( V = ( ̇ x, y ). We know that the kinetic energy of the robot must end-effector as ̇ be the same regardless of the coordinates we use, so 1 1 T T ̇ ̇ ) θ θ = M (8.20) V θ Λ( θ ) V. ( 2 2 ̇ ( θ ) satisfying V = J ( θ ) Assuming the Jacobian θ is invertible, Equation (8.20) J can be rewritten as follows: − 1 T T − 1 ( V ) V M = ( J J V V ) Λ T − T − 1 V J MJ ( = ) V. In other words, the end-effector mass matrix is 1 − T − θ J θ ) M ( θ ) J ) = θ ( ( ) . (8.21) Λ( Figure 8.4 shows the end-effector mass ellipsoids, with principal-axis directions given by the eigenvectors of Λ( θ ) and principal semi-axis lengths given by its eigenvalues, for the same two 2R robot configurations as in Figure 8.3. The endpoint acceleration ( ̈ x, ̈ y ) is a scalar multiple of the force ( f ) applied at ,f y x the endpoint only if the force is along a principal axis of the ellipsoid. Unless 0 is a scalar and ) is of the form cI , where Λ( θ I is the identity matrix, the c > mass at the endpoint feels different from a point mass. The change in apparent endpoint mass as a function of the configuration of the robot is an issue for robots used as haptic displays. One way to reduce the sensation of a changing mass to the user is to make the mass of the links as small as possible. Note that the ellipsoidal interpretations of the relationship between forces and accelerations defined here are only relevant at zero velocity, where there are no Coriolis or centripetal terms. 8.1.4 Lagrangian Dynamics vs. Newton–Euler Dynamics In the rest of this chapter, we focus on the Newton–Euler recursive method for calculating robot dynamics. Using the tools we have developed so far, the Newton–Euler formulation allows computationally efficient computer implemen- tation, particularly for robots with many degrees of freedom, without the need May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

300 282 8.1. Lagrangian Formulation f y ̈ y [ ] 10 1 − ( )= θ Λ 00 . 5 f x ] [ 10 ◦ Λ( θ )= ̈ x =0 θ 1 02 ◦ =90 θ 2 ̈ y f ◦ y =0 θ 1 ] [ ◦ 4 − 1 . 73 =150 θ 2 Λ( θ )= 1 − . 732 f x x ̈ [ ] . 35 0 40 . − 1 Λ ( )= θ 8 350 . 0 . (Bold lines) A unit ball of accelerations in ( ̈ x, ̈ y ) maps through the Figure 8.4: θ ) to an end-effector force ellipsoid that depends on the end-effector mass matrix Λ( ◦ ◦ ,θ ), a force in the configuration of the 2R arm. For the configuration ( θ , 90 ) = (0 1 2 f -direction -direction exactly feels both masses m f and m , while a force in the 2 y 1 x − 1 . (Dotted lines) A unit ball in f feels only m maps through Λ ( θ ) to an acceleration 2 ◦ ◦ symbols for ( θ ) indicate an example endpoint force ,θ ellipsoid. The ) = (0 × , 150 2 1 ( f 35), showing that ,f . ) = (1 , 0) and its corresponding acceleration ( ̈ x, ̈ y ) = (0 . 4 , 0 y x the force and acceleration at the endpoint are not aligned. for differentiation. The resulting equations of motion are, and must be, identical with those derived using the energy-based Lagrangian method. The Newton–Euler method builds on the dynamics of a single rigid body, so we begin there. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

301 Chapter 8. Dynamics of Open Chains 283 Dynamics of a Single Rigid Body 8.2 8.2.1 Classical Formulation Consider a rigid body consisting of a number of rigidly connected point masses, ∑ and the total mass is m = i where point mass m has mass . Let r m = i i i i in a body frame ) be the fixed location of mass i ,y { b } , where the origin ( ,z x i i i of this frame is the unique point such that ∑ m r = 0 . i i i . If some other point happens to This point is known as the center of mass b should be moved be inconveniently chosen as the origin, then the frame } { ∑ ) r to the center of mass at (1 m / m (in the inconvenient frame) and the r i i i i recalculated in the center-of-mass frame. Now assume that the body is moving with a body twist = ( ω V ,v ), and b b b let , in the inertial ( t ) be the time-varying position of m r , initially located at p i i i { frame . Then b } , × v ̇ + ω = p p i i b b d d = ̇ p + ̈ v × ω p × p ω + b i b i i b dt dt v . + ̇ ω ) × p p + ω × = ̇ ( v ω + × b b b i i b b Substituting r on the right-hand side and using our skew-symmetric for p i i notation (see Equation (3.30)), we get 2 ̈ v p + [ ̇ ω . ] r r + [ ω = ̇ ] v ] + [ ω b b i i i b b b f is = m m ̈ p for a point mass, the force acting on Taking as a given that i i i i 2 = ( ̇ v + [ ̇ ω ] r f ω m ] v + [ ω ] + [ r ) , b b b i i i b i b which implies a moment m . = [ r f ] i i i F : The total force and moment acting on the body is expressed as the wrench b [ ] ] [ ∑ m m b i i ∑ F = = . b f f b i i ∑ and m , keep in mind that r = 0 m f To simplify the expressions for i b b i i ∑ 3 T m a [ r ] ] = 0) and, for a,b ∈ R [ , [ a ] = − (and therefore a ] , and , [ a ] b = − [ b i i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

302 284 8.2. Dynamics of a Single Rigid Body T ] = ([ b ][ a ][ a . Focusing on the linear dynamics, [ b ]) ∑ 2 + [ m ) ( ̇ v r + [ ̇ ω = ] r ] f ω ω ] v + [ b b i b b i i b b i 0 ∑ ∑ ∑ * 0 :      ][ ) − r [ m = m ω ( ̇ v ] m + [ [ r ω ] ̇ ω ω + ] v b b i i b i b i i b b       i i i  ∑ m v ( ̇ = ) + [ ω v ] b b i b i ( ̇ v = + [ m (8.22) ] v . ) ω b b b m [ ω The velocity product term ] v = 0, a arises from the fact that, for ω 6 b b b 6 constant v = 0 corresponds to a changing linear velocity in an inertial frame. b Now focusing on the rotational dynamics, ∑ 2 m ) m r [ r + [ ̇ ]( ̇ v ] = ω ω ] r + [ + [ ω v ] i b i b b b b i i b i 0 ∑ ∑ * 0  :     r ] ̇ v [ + m m v ω [ r ] ][ = b i b i b i i       i i  ∑ 2 m ) [ r r ]([ ̇ ω ] + r ] + [ ω b i i i i b i ∑ ) ( 2 T T = r ω ] − ̇ ω ] m [ r r ] − [ ω [ ] [ b b i b i i i i ∑ ) ( 2 2 ω [ m ̇ ω − [ ] r ][ r ] − ω = i i b b i b i ( ) ( ) ∑ ∑ 2 2 ] = ̇ ω + [ ω [ r − ] m [ r ] m − ω i i i b b b i i i I ̇ ω + [ = ] I (8.23) ω , ω b b b b b ∑ 2 3 × 3 is the body’s m . [ r rotational inertia matrix ] = ∈ R − where I b i i i Equation (8.23) is known as Euler’s equation for a rotating rigid body. In Equation (8.23), note the presence of a term linear in the angular accel- I , just as ω eration, , and a term quadratic in the angular velocities, [ ω ] I ω ̇ b b b b b we saw for the mechanisms in Section I is symmetric and positive 8.1. Also, b definite, just like the mass matrix for a mechanism, and the rotational kinetic energy is given by the quadratic 1 T ω I . ω = K b b b 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

303 Chapter 8. Dynamics of Open Chains 285 M ( θ is constant whereas the mass matrix One difference is that I ) changes b with the configuration of the mechanism. , we get I Writing out the individual entries of b   ∑ ∑ ∑ 2 2 z + z ( x ) − y m m x m y − i i i i i i i i i ∑ ∑ ∑ 2 2   y m m m − z x x − + z y ) ( I = i i i i i i i b i i ∑ ∑ ∑ 2 2 ( z z m − x x y + y m − ) m i i i i i i i i i   I I I xy xz xx   I I I = . yz xy yy I I I zz yz xz , using the The summations can be replaced by volume integrals over the body B differential volume element dV m replaced by a mass , with the point masses i ρ ( x,y,z ): density function ∫  2 2   + z ( y ρ ( x,y,z ) dV = I ) xx    B  ∫    2 2   ) dV = x I + z ( ) ρ ( x,y,z yy    B  ∫    2 2   ) ( x,y,z I + = ) y ρ x ( dV zz  B ∫ (8.24)    xyρ − = x,y,z I ) ( dV xy    B  ∫      − dV ) x,y,z ( xzρ = I xz    B  ∫      = ( I yzρ x,y,z ) dV. − yz B I is determined exclusively by the shape of If the body has uniform density, b the rigid body (see Figure 8.5). I , the Given an inertia matrix principal axes of inertia are given by b the eigenvectors and eigenvalues of I . Let v ,v ,v be the eigenvectors of 2 1 3 b I λ ,λ ,λ be the corresponding eigenvalues. Then the principal axes and 1 3 b 2 v ,v ,v , and the scalar moments of inertia of inertia are in the directions of 1 2 3 about these axes, the principal moments of inertia , are λ 0. One ,λ > ,λ 3 1 2 principal axis maximizes the moment of inertia among all axes passing through the center of mass, and another minimizes the moment of inertia. For bodies with symmetry, often the principal axes of inertia are apparent. They may not be unique; for a uniform-density solid sphere, for example, any three orthogonal axes intersecting at the center of mass constitute a set of principal axes, and May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

304 286 8.2. Dynamics of a Single Rigid Body ˆz ˆz ˆz c h r h ˆy b ˆx a ˆy ˆy ˆx ˆx w circular cylinder: ellipsoid: rectangular parallelepiped: 2 volume = πr volume = h , volume = 4 πabc/ 3, abc , 2 2 2 2 2 2 + h I ) / 12, I 5, = m (3 r = + h m ) / 12, ( / = m ( b w + c I ) xx xx xx 2 2 2 2 2 2 + h h ) / 12, I 5, = m (3 r = + m ( ) / 12, I / = m ( a ` + c I ) yy yy yy 2 2 2 2 2 ` m + w I ) / 12 I / = m r 5 / 2 I ) = = ( a m + b ( zz zz zz Figure 8.5: The principal axes and the inertia about the principal axes for uniform- density bodies of mass . Note that the ˆx and ˆy principal axes of the cylinder are not m unique. the minimum principal moment of inertia is equal to the maximum principal moment of inertia. If the principal axes of inertia are aligned with the axes of } , the off- { b I are all zero, and the eigenvalues are the scalar moments of diagonal terms of b I , and , I about the ˆx-, ˆy-, and ˆz-axes, respectively. In this case, inertia I zz xx yy the equations of motion (8.23) simplify to   ω ω I ) ̇ ω −I + ( I zz x yy xx y z   ω I ω ̇ ω ) + ( I −I = (8.25) , m xx z yy x zz y b ω ω −I ) I + ( ω I ̇ x z xx zz y yy where ω to be aligned = ( ω } ,ω b ,ω { ). When possible, we choose the axes of z y b x with the principal axes of inertia, in order to reduce the number of nonzero entries in I and to simplify the equations of motion. b Examples of common uniform-density solid bodies, their principal axes of inertia, and the principal moments of inertia obtained by solving the inte- grals (8.24), are given in Figure 8.5. An inertia matrix I described by can be expressed in a rotated frame { c } b R . Denoting this inertia matrix as I the rotation matrix , and knowing that c bc the kinetic energy of the rotating body is independent of the chosen frame, we May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

305 Chapter 8. Dynamics of Open Chains 287 have 1 1 T T I ω ω ω ω = I c b c b c b 2 2 1 T ( ) ω ω ) ( I R R = b bc c c bc 2 1 T T ω R ( . = ω I ) R bc c b bc c 2 In other words, T = R I (8.26) I . R bc c b bc { b } are not aligned with the principal axes of inertia then we can If the axes of { } , diagonalize the inertia matrix by expressing it instead in the rotated frame c R . correspond to the eigenvalues of where the columns of I b bc Sometimes it is convenient to represent the inertia matrix in a frame at a point not at the center of mass of the body, for example at a joint. Steiner’s theorem can be stated as follows. Theorem 8.2. The inertia matrix I , but at a about a frame aligned with { b } q point = ( q q ,q calculated at the ,q I ) in { b } , is related to the inertia matrix z y x b center of mass by T T I qq + m ( q I qI − (8.27) = ) , q b I is the 3 × where identity matrix and m is the mass of the body. 3 Steiner’s theorem is a more general statement of the parallel-axis theorem, which states that the scalar inertia I about an axis parallel to, but a distance d d I from, an axis through the center of mass is related to the scalar inertia cm about the axis through the center of mass by 2 . = (8.28) + I d I m d cm Equations (8.26) and (8.27) are useful for calculating the inertia of a rigid body consisting of component rigid bodies. First we calculate the inertia matri- ces of the n component bodies in terms of frames at their individual centers of mass. Then we choose a common frame { common } (e.g., at the center of mass of the composite rigid body) and use Equations (8.26) and (8.27) to express each inertia matrix in this common frame. Once the individual inertia matrices are expressed in { common } , they can be summed to get the inertia matrix I common for the composite rigid body. In the case of motion confined to the ˆx–ˆy-plane, where ω ) and = (0 , 0 ,ω z b the inertia of the body about the ˆz-axis through the center of mass is given May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

306 288 8.2. Dynamics of a Single Rigid Body , the spatial rotational dynamics (8.23) reduces to the planar I by the scalar zz rotational dynamics , ̇ I ω m = z zz z and the rotational kinetic energy is 1 2 I = ω . K zz z 2 Twist–Wrench Formulation 8.2.2 The linear dynamics (8.22) and the rotational dynamics (8.23) can be written in the following combined form: [ [ ] ][ ] [ ][ ][ ] m 0 I ̇ ω 0 ω [ ] I 0 ω b b b b b b = + , (8.29) f 0 m I ̇ v ω [ 0 ] I 0 m v b b b b I × 3 identity matrix. With the benefit of hindsight, and also where is the 3 T ] v = v × v = 0 and [ v ] making use of the fact that [ = − [ v ], we can write v Equation (8.29) in the following equivalent form: [ ][ ] ] [ ][ ][ ] [ 0 ̇ ω ω I I m 0 [ ω ] ] [ v b b b b b b b = + 0 [ ω m ] ̇ v I f 0 m I v 0 b b b b [ ][ [ ] ] [ ][ ] T 0 I ω ̇ [ ω ] 0 I 0 ω b b b b b = − (8.30) . 0 m I ̇ v ω ] [ v ] [ I 0 m v b b b b Written this way, each term can now be identified with six-dimensional spa- tial quantities as follows: ω ) can be respectively identified with the ,v (a) The vectors ( ) and ( m ,f b b b b V , and body wrench body twist F b b [ [ ] ] ω m b b V = F , = . (8.31) b b v f b b 6 × 6 ∈ R spatial inertia matrix G (b) The is defined as b [ ] I 0 b (8.32) = . G b I m 0 As an aside, the kinetic energy of the rigid body can be expressed in terms of the spatial inertia matrix as 1 1 1 T T T I G ω v + . V m ω (8.33) V v = kinetic energy = b b b b b b b b 2 2 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

307 Chapter 8. Dynamics of Open Chains 289 6 spatial momentum ∈ R P is defined as (c) The b [ [ ] ][ ] ω I 0 I ω b b b b P = = G V (8.34) = . b b b m v m 0 I v b b P in Equation (8.30) is left-multiplied by Observe that the term involving b the matrix [ ] T ω ] 0 [ b . (8.35) − ] ω ] [ v [ b b We now explain the origin and geometric significance of this matrix. First, recall 3 R ω ∈ can be calculated, using the that the cross product of two vectors ,ω 1 2 skew-symmetric matrix notation, as follows: ω (8.36) × ω . ] = [ ω ] ][ ω ] [ − [ ω ω ][ 2 2 1 2 1 1 The matrix in (8.35) can be thought of as a generalization of the cross-product operation to six-dimensional twists. Specifically, given two twists = ( ω ,v ) V 1 1 1 ω = ( and V ,v ), we perform a calculation analogous to (8.36): 2 2 2 [ ][ [ ] ][ ] ω v [ ] v ] ω [ v ] ω [ ω ] [ v 1 1 2 2 2 2 1 1 ] = V ][ V V [ − ][ ] V [ − 2 1 1 2 0 0 0 0 0 0 0 0 [ ] [ ω v ][ ω ] ] − [ ω ω ][ ω [ ] [ ω − ] v 2 1 2 2 2 1 1 1 = 0 0 [ ] ′ ′ [ ω ] v , = 0 0 which can be written more compactly in vector form as ][ ] ] [ [ ′ [ ω ω ] 0 ω 1 2 = . ′ v ω ] [ v v ] [ 1 1 2 V and V is called the This generalization of the cross product to two twists 1 2 Lie bracket V and V . of 1 2 Definition 8.3. Given two twists V Lie = ( ω ), the ,v ,v ) and V ω = ( 2 1 1 2 1 2 V V and V bracket , written either as [ad ), is defined as V ] of ( or ad V 2 2 V 1 2 1 1 follows: [ ][ ] [ 0 ω ω ] 2 1 6 = [ad R ∈ (8.37) , ) ] V = ad V ( V V 2 2 1 1 ] [ v v ] [ ω 1 1 2 where [ ] [ ω ] 0 6 × 6 ∈ R (8.38) . ] = [ad V ω ] [ v ] [ May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

308 290 8.2. Dynamics of a Single Rigid Body V Given a twist ) and a wrench F = ( m,f ), define the = ( ω,v Definition 8.4. mapping ] [ ] [ ] [ T ] ω [ ω ] 0 − f m m v − [ ] [ T T F = ( ad ] = F ) = [ad (8.39) . V V ] − [ ω ] f f ω ] [ [ v Using the notation and definitions above, the dynamic equations for a single rigid body can now be written as T ̇ G F ) V P − ad = ( b b b b V b T ̇ G = V − [ad . ] G (8.40) V b V b b b b Note the analogy between Equation (8.40) and the moment equation for a ro- tating rigid body: T = I . m ω ω − [ ω I ] (8.41) ̇ b b b b b b Equation (8.41) is simply the rotational component of (8.40). 8.2.3 Dynamics in Other Frames The derivation of the dynamic equations (8.40) relies on the use of a center-of- mass frame { b } . It is straightforward to express the dynamics in other frames, however. Let’s call one such frame { a } . Since the kinetic energy of the rigid body must be independent of the frame of representation, 1 1 T T G V V = V G V b b a a a b 2 2 1 T = ([Ad ] V G ) [Ad ] V T a T b a ba ba 2 1 T T V ; V [Ad G [Ad ] ] = T a b T a ba ba ︷︷ ︸ ︸ 2 G a for the adjoint representation Ad (see Definition 3.20). In other words, the G spatial inertia matrix in { a } is related to G by b a T (8.42) ] G G [Ad = [Ad . ] T T a b ba ba This is a generalization of Steiner’s theorem. G , the equations of motion (8.40) in the Using the spatial inertia matrix a } frame can be expressed equivalently in the { a } frame as { b T ̇ , = V − [ad (8.43) ] G G V F a V a a a a a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

309 Chapter 8. Dynamics of Open Chains 291 F are the wrench and twist written in the { a } frame. (See and where V a a Exercise 8.3.) Thus the form of the equations of motion is independent of the frame of representation. Newton–Euler Inverse Dynamics 8.3 n -link open chain con- We now consider the inverse dynamics problem for an n n ̇ ∈ θ ∈ R R , nected by one-dof joints. Given the joint positions θ , velocities n ̈ ∈ R , the objective is to calculate the right-hand side of the and accelerations θ dynamics equation ̇ ̈ θ ) = θ + h ( θ, τ θ ) M ( . The main result is a recursive inverse dynamics algorithm consisting of a forward and a backward iteration stage. In the former, the positions, velocities, and accelerations of each link are propagated from the base to the tip while in the backward iterations the forces and moments experienced by each link are propagated from the tip to the base. 8.3.1 Derivation { i } is attached to the center of mass of each link A body-fixed reference frame , i = 1 ,...,n . The base frame is denoted { 0 } , and a frame at the end-effector i { is denoted } . This frame is fixed in { n } . n + 1 When the manipulator is at the home position, with all joint variables zero, as we denote the configuration of frame in { i } j M { ∈ SE (3), and the } i,j { i } in the base frame { 0 } using the shorthand M configuration of = M . ,i 0 i M and M With these definitions, can be calculated as i − − 1 1 ,i i,i 1 − − 1 . M and M M = M M = M i − i,i 1 1 ,i 1 − − i i i 1 i − i The screw axis for joint i } , is A , expressed in the link frame . This same { i screw axis is expressed in the space frame 0 } as S , where the two are related { i by − 1 ( S ) . = Ad A i i M i j for arbitrary Defining SE (3) to be the configuration of frame { T } in { i } ∈ i,j joint variables θ then T given } 1 − ( θ i ), the configuration of { i } relative to { i ,i i 1 − 1 − ) = the joint variable θ T ( θ T , and ( θ ) are calculated as i 1 − i,i i i − 1 ,i i − θ θ ] A [ ] [ A i i i i θ e . ) = M M T ( and T e ) = θ ( 1 1 − i,i ,i 1 − i i ,i i − i,i − 1 i We further adopt the following notation: May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

310 292 8.3. Newton–Euler Inverse Dynamics { i , expressed in frame- { i } coordinates, is denoted (a) The twist of link frame } = ( ω ,v V ). i i i (b) The wrench transmitted through joint to link frame { i } , expressed in i } coordinates, is denoted F frame- = ( m { ,f i ). i i i 6 × 6 (c) Let G , expressed relative denote the spatial inertia matrix of link i ∈ R i { to link frame . Since we are assuming that all link frames are situated } i at the link center of mass, G has the block-diagonal form i [ ] 0 I i G = , (8.44) i I m 0 i I denotes the 3 × where i and m is the 3 rotational inertia matrix of link i i link mass. With these definitions, we can recursively calculate the twist and acceleration of each link, moving from the base to the tip. The twist V of link i is the sum i i − 1, but expressed in of the twist of link i } , and the added twist due to the { ̇ θ joint rate : i ̇ = V A + [Ad θ (8.45) ] V . i i 1 i − T i 1 i,i − ̇ V can also be found recursively. Taking the time derivative The accelerations i of Equation ( 8.45), we get ( ) d ̈ ̇ ̇ [Ad V (8.46) . ] A V = + [Ad θ V ] + T 1 − i i i i T 1 − i i,i − 1 − i,i 1 dt To calculate the final term in this equation, we express and A as T − i 1 i,i ] [ [ ] R p ω i,i − 1 A T = = and . i 1 − i,i 1 v 0 Then ]) ([ ( ) d d 0 R i,i − 1 [Ad V V ] = i − 1 T 1 i − i,i − 1 [ p ] R R dt dt − 1 i,i 1 − i,i ] [ ̇ − θ ω ] R [ 0 1 i − i,i V = 1 − i ̇ ̇ ̇ θ ω ][ p [ R − [ ω − θ R ] R ] θ v [ − ] − i,i − i,i − 1 1 1 i i i i,i ] [ ] [ ̇ ω ] 0 [ θ − 0 R i − 1 i,i V = 1 i − ̇ ̇ p [ R ] R [ v θ ω ] θ − ] − [ 1 − i,i − 1 i,i i i ︸ ︷︷ ︸ ︸ ︷︷ ︸ ] [Ad [ad − ] T ̇ 1 − i,i A θ i i − [ad = ] V ̇ i A θ i i ̇ = [ad . ] A θ V i i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

311 Chapter 8. Dynamics of Open Chains 293 join t a xis i ̇ V i V i } i { F i T ( ) − F Ad i +1 T i +1 ,i Figure 8.6: Free-body diagram illustrating the moments and forces exerted on link i . Substituting this result into Equation (8.46), we get ̈ ̇ ̇ ̇ = A V ] + [Ad θ V A + [ad ] θ (8.47) , i i − i 1 i T i V i i,i − i 1 i i.e., the acceleration of link is the sum of three components: a component due ̈ θ to the joint acceleration i − 1 , a component due to the acceleration of link i { i } , and a velocity-product component. expressed in Once we have determined all the link twists and accelerations moving out- ward from the base, we can calculate the joint torques or forces by moving inward from the tip. The rigid-body dynamics (8.40) tells us the total wrench ̇ given . Furthermore, the total wrench acting on that acts on link and i V V i i link i F is the sum of the wrench transmitted through joint i and the wrench i applied to the link through joint + 1 (or, for link n , the wrench applied to the i { n + 1 } ), expressed in the link by the environment at the end-effector frame i . Therefore, we have the equality frame T T ̇ (8.48) ); F ( Ad G − ( G F V ) = − V ad +1 i i i i i i T V i +1 ,i i see Figure 8.6. Solving from the tip toward the base, at each link i we solve for the only unknown in Equation (8.48): F has only one degree . Since joint i i F are provided “for free” by the of freedom, five dimensions of the six-vector i structure of the joint, and the actuator only has to provide the scalar force or torque in the direction of the joint’s screw axis: T . F τ (8.49) A = i i i Equation (8.49) provides the torques required at each joint, solving the inverse dynamics problem. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

312 294 8.4. Dynamic Equations in Closed Form Newton-Euler Inverse Dynamics Algorithm 8.3.2 { Initialization to the base, frames { 1 } to { n } to the centers 0 Attach a frame } } } { n } , and a frame { n + 1 1 at the end-effector, fixed in the of mass of links { to 1 } n { frame to be the configuration of { i − . Define } in { i } when θ = 0. M i i,i 1 − A be the screw axis of joint i expressed in { i } Let G 6 spatial be the 6 × , and i i i . Define V expressed to be the twist of the base frame { 0 } inertia matrix of link 0 3 0 } coordinates. (This quantity is typically zero.) Let g ∈ R in be the gravity { ̇ vector expressed in base-frame coordinates, and define = ( ̇ ω V , ̇ v ). ) = (0 , − g 0 0 0 (Gravity is treated as an acceleration of the base in the opposite direction.) F ) to be the wrench applied to the environment Define = F ,f = ( m tip tip +1 n tip { n + 1 } . by the end-effector, expressed in the end-effector frame ̇ ̈ Given θ, do θ , for i = 1 to n Forward iterations θ, ] − [ A θ i i T e , = (8.50) M i,i − 1 − 1 i,i ̇ ( V ) + A = Ad V θ (8.51) , i i − 1 i T i i,i − 1 ̇ ̈ ̇ ̇ θ ( V A + = Ad ) + ad V ) ( A (8.52) . θ i i − i i V 1 i T i i,i − 1 i = n to 1 do Backward iterations For i T T ̇ , ) ( F F ) + G G ( V (8.53) = Ad ad − V i +1 i i i i i T V i ,i +1 i T (8.54) F τ . A = i i i 8.4 Dynamic Equations in Closed Form In this section we show how the equations in the recursive inverse dynamics algorithm can be organized into a closed-form set of dynamics equations τ = ̇ ̈ θ θ + c ( θ, ) θ ) + g ( θ ). M ( Before doing so, we prove our earlier assertion that the total kinetic energy 1 T ̇ ̇ M K θ ( ) θ θ . We do so by noting that of the robot can be expressed as K = K 2 can be expressed as the sum of the kinetic energies of each link: n ∑ 1 T G (8.55) , V V = K i i i 2 =1 i where V is the spatial inertia matrix is the twist of link frame { i } and G i i i as defined by Equation (8.32) (both are expressed in link-frame- { i } of link T ) denote the forward kinematics from the base ( θ ,...,θ coordinates). Let 1 0 i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

313 Chapter 8. Dynamics of Open Chains 295 J } { i } 0 to link frame { ( θ ) denote the body Jacobian obtained frame , and let ib − 1 ̇ T from n T × matrix; we turn it into a 6 . Note that J i as defined is a 6 × ib 0 i i 0 − i columns with zeros. With this matrix by filling in all entries of the last n , we can write J definition of ib ̇ = J ,...,n. V θ ) ( θ, i = 1 ib i The kinetic energy can then be written ( ) n ∑ 1 T T ̇ ̇ θ θ ( J ) J ( θ ) G (8.56) θ. = K i ib ib 2 i =1 The term inside the parentheses is precisely the mass matrix M ( θ ): n ∑ T J θ (8.57) ) = M . ( ( ) G θ J ( θ ) ib i ib =1 i We now return to the original task of deriving a closed-form set of dynamic equations. We start by defining the following stacked vectors:   V 1   . n 6 . = (8.58) , R ∈ V   . V n   F 1   . n 6 . = F ∈ R . (8.59)   . F n May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

314 296 8.4. Dynamic Equations in Closed Form Further, define the following matrices:   0 ··· 0 A 1   0 0 ··· A 2   n × n 6 A = ∈ , R (8.60)   . . . . . . . .   . . . . ··· ··· A 0 n   G 0 ··· 0 1   G 0 ··· 0 2   6 × n 6 n R , ∈ (8.61) = G   . . . . . . . .   . . . . 0 ··· ··· G n   0 ··· 0 [ad ] V 1   ] ··· 0 [ad 0 V 2   6 6 n × n ] = [ad , ∈ R (8.62)   . . . V . . . . .   . . . . ] [ad ··· ··· 0 V n   ··· 0 0 ] [ad ̇ A θ 1 1   ] ··· 0 0 [ad ̇ [ ] θ A 2 2   6 n 6 × n , ∈ (8.63) R = ad   ̇ . . . . θ A . . . .   . . . . ] 0 ··· ··· [ad ̇ θ A n n   0 0 0 ··· 0   0 0 ··· 0 [Ad ] T 21     n 6 × 6 n ] ··· 0 0 [Ad 0 T 32 ( ) = θ W ∈ . R (8.64)     . . . . . . . . . .   . . . . . [ ] ··· 0 Ad 0 0 T n,n − 1 We write W ( θ ) to emphasize the dependence of W on θ . Finally, define the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

315 Chapter 8. Dynamics of Open Chains 297 following stacked vectors:   ) Ad ( V T 0 10   0   6 n , (8.65) R ∈ V =   . base .   . 0   ̇ V ) ( Ad T 0 10   0   n 6 ̇ ∈ , R (8.66) V =   base . .   . 0   0 .   .   n 6 . . ∈ (8.67) R = F   tip   0 T F ( ) Ad +1 n T +1 ,n n 6 6 n × n × n 6 n R G ∈ and R A ∈ are constant block-diagonal matrices, Note that A contains only the kinematic parameters while G contains only the in which mass and inertial parameters for each link. With the above definitions, our earlier recursive inverse dynamics algorithm can be assembled into the following set of matrix equations: ̇ + W θ ) V + A = θ ( V V , (8.68) base ̈ ̇ ̇ ̇ ( θ ) , V + A V θ − [ad (8.69) = V ]( W ( θ ) V + V W ) + ̇ base base A θ T T ̇ + ) F + G = V − [ad F ] ( GV θ F (8.70) , W tip V T A τ . (8.71) = F n ( θ ) has the property that W ) = 0 (such a matrix is said to be ( θ The matrix W nilpotent of order n ), and one consequence verifiable through direct calculation − n 1 − 1 − 1 ). Defining = + W ( θ )+ ··· + W is that ( −W )) ( θ I L ( θ ) = ( I −W ( θ )) θ ( , I it can further be verified via direct calculation that   0 ··· 0 0 I   [Ad I ··· 0 ] 0 T 21     6 n × 6 n ] [Ad [Ad 0 ] I ··· T T 32 31 θ ) = L ( (8.72) R ∈ .     . . . . . . . . . .   . . . . . ··· [Ad I ] ] [Ad ] [Ad T T T 1 n n n 3 2 on We write ( θ ) to emphasize the dependence of L L θ . The earlier matrix May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

316 298 8.5. Forward Dynamics of Open Chains equations can now be reorganized as follows: ) ( ̇ = A θ θ + V L ( , (8.73) V ) base ( ) ̈ ̇ ̇ + [ad A V θ + [ad = (8.74) , ] W ( θ ) V L ( θ V ] V ) + ̇ ̇ base base θ A A θ ( ) T T ̇ L G = V − [ad F ( θ GV + F (8.75) ) , ] tip V T A τ F . (8.76) = F If the robot applies an external wrench at the end-effector, this can be tip included into the dynamics equation T ̈ ̇ g τ θ + c ( θ, M ( ) + = ( θ ) + J θ ( θ ) F ) , (8.77) θ tip J ( θ ) denotes the Jacobian of the forward kinematics expressed in the where F , and same reference frame as tip T T ) = A GL L M ( θ ) ( ( θ ) A , (8.78) θ ( ) T T T ̇ ̇ c L ( ( θ ) θ, GL ( θ ) [ad (8.79) θ, G ] W ( θ ) + [ad A ] θ ) = −A L ( θ ) ̇ V A θ T T ̇ ( L A ( θ ) GL ( θ ) ) = V θ . (8.80) g base Forward Dynamics of Open Chains 8.5 The forward dynamics problem involves solving T ̈ ̇ h θ = τ ( t ) − M ( ( θ θ ) − J ) ( θ ) F (8.81) θ, tip ̈ ̇ , given θ , F θ , τ , and the wrench for θ applied by the end-effector (if ap- tip ̇ plicable). The term h ( θ, θ ) can be computed by calling the inverse dynamics ̈ ( θ = 0 and F ) can be computed = 0. The inertia matrix M algorithm with θ tip using Equation (8.57). An alternative is to use calls of the inverse dynamics n M θ ) column by column. In each of the n calls, set g = 0, ( algorithm to build ̈ ̇ F = 0. In the first call, the column vector θ θ is all zeros except = 0, and tip ̈ is all zeros except for a 1 in the for a 1 in the first row. In the second call, θ τ second row, and so on. The i th call is the i th column vector returned by the of M ( θ ), and after n calls the n × n matrix M ( θ ) is constructed. ̇ With θ ), h ( θ, ( θ ), and F we can use any efficient algorithm for solving M tip ̈ ̈ M = b , for Equation (8.81), which is of the form θ . θ The forward dynamics can be used to simulate the motion of the robot given its initial state, the joint forces–torques τ ( t ), and an optional external wrench May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

317 Chapter 8. Dynamics of Open Chains 299 ,t ( t ∈ [0 ), for ]. First define the function ForwardDynamics returning the F t f tip solution to Equation (8.81), i.e., ̇ ̈ θ = θ,τ, F ( ) . θ, ForwardDynamics tip ̇ θ , q = = q θ , the second-order dynamics (8.81) can be Defining the variables 2 1 converted to two first-order differential equations, ̇ = q q , 2 1 ,τ, . ( q ,q ForwardDynamics F ) ̇ q = 2 1 2 tip The simplest method for numerically integrating a system of first-order differ- n q ( q,t ), = ∈ R q , is the first-order Euler iteration f ential equations of the form ̇ ( q + δt ) = q ( t ) + δtf ( q ( t ) ,t ) , t where the positive scalar denotes the timestep. The Euler integration of the δt robot dynamics is thus t t + δt ) = q q ( ( ) + q δt, ( t ) 2 1 1 δt. ( t + δt ) = q ) q t ) + ForwardDynamics ( q F ,q ,τ, ( 2 tip 2 2 1 ̇ q (0) = Given a set of initial values for (0) and q (0), the above equa- (0) = θ θ 1 2 tions can be iterated forward in time to obtain the motion θ ( t ) = q ) numeri- ( t 1 cally. Euler Integration Algorithm for Forward Dynamics ̇ (0) and • θ (0), the input torques θ ( t ) and Inputs : The initial conditions τ ( wrenches at the end-effector t ) for t ∈ [0 ,t F ], and the number of f tip integration steps N . ̇ : Set the timestep δt = t [0] = /N , and set θ [0] = θ (0), • θ Initialization f ̇ θ (0). Iteration : For k • N − 1 do = 0 to ̈ ̇ [ k ] = ForwardDynamics ( θ [ k ] , [ θ θ k ] ,τ ( kδt ) , F , ( kδt )) tip ̇ k + 1] = θ [ k ] + θ [ [ k ] δt, θ ̇ ̇ ̈ k + 1] = ] θ [ k ] + θ θ [ k [ δt. ̇ ̇ kδt θ ( kδt ) = θ [ k ], • θ ( Output ) = : The joint trajectory θ [ k ], k = 0 ,...,N . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

318 300 8.6. Dynamics in the Task Space The result of the numerical integration converges to the theoretical result as the number of integration steps N goes to infinity. Higher-order numerical integration schemes, such as fourth-order Runge–Kutta, can yield a closer ap- proximation with fewer computations than the simple first-order Euler method. Dynamics in the Task Space 8.6 In this section we consider how the dynamic equations change under a trans- formation to coordinates of the end-effector frame (task-space coordinates). To keep things simple we consider a six-degree-of-freedom open chain with joint- space dynamics 6 6 ̈ ̇ ) θ + h ( θ, τ θ = , θ M R ( , τ ∈ R θ . (8.82) ) ∈ F . The twist We also ignore, for the time being, any end-effector forces tip ̇ = ( ω,v θ by V ) of the end-effector is related to the joint velocity ̇ J ( θ ) θ, V = (8.83) with the understanding that and J ( θ ) are always expressed in terms of the V ̇ is then same reference frame. The time derivative V ̇ ̈ ̇ ̇ ( θ ) V θ + J ( θ ) = θ. (8.84) J θ where J ( θ ) is invertible, we have At configurations − 1 ̇ θ J V , (8.85) = 1 − − 1 − 1 ̈ ̇ ̇ J (8.86) θ JJ = V − V . J ̇ ̈ and Substituting for θ in Equation (8.82) leads to θ ( ) 1 1 − − 1 − − 1 ̇ ̇ V J ( JJ θ ) V V − + h τ θ,J J = M ) . (8.87) ( T T − 1 − T − 1 − T J Let ) denote ( ) . Pre-multiply both sides by J J = ( to get J 1 − T − 1 − − T T − 1 − ̇ ̇ MJ J = V − MJ τ J V J JJ (8.88) − T 1 − θ,J ( h V ) . J + − T Expressing J τ as the wrench F , the above can be written ̇ ( θ ) F V + η = Λ( θ, V ) , (8.89) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

319 Chapter 8. Dynamics of Open Chains 301 where − T − 1 J θ ) J θ M , (8.90) ( ) = Λ( − 1 − − 1 T ̇ ( θ,J ) = V V ) − Λ( h ) J JJ θ, ( V . (8.91) η θ These are the dynamic equations expressed in end-effector frame coordinates. F If an external wrench is applied to the end-effector frame then, assuming the actuators provide zero forces and torques, the motion of the end-effector frame is governed by these equations. Note that ( θ ) must be invertible (i.e., there must be a one-to-one mapping J between joint velocities and end-effector twists) in order to derive the task- ) and space dynamics above. Also note the dependence of Λ( ( θ, V ) on θ . In θ η θ by a dependence on the end- general, we cannot replace the dependence on X because there may be multiple solutions to the inverse effector configuration kinematics, and the dynamics depends on the specific joint configuration θ . 8.7 Constrained Dynamics n -joint robot is subject to a set of k holonomic Now consider the case where the or nonholonomic Pfaffian velocity constraints of the form k × n ̇ ( θ = 0 , A A θ ) ∈ R ( θ ) . (8.92) (See Section for an introduction to Pfaffian constraints.) Such constraints 2.4 can come from loop-closure constraints; for example, the motion of an end- k = 5 constraints due to the effector rigidly holding a door handle is subject to hinges of the door. As another example, a robot writing with a pen is subject to a single constraint that keeps the height of the tip of the pen above the paper at zero. In any case, we assume that the constraints do no work on the robot, due to the constraints satisfy τ i.e., the generalized forces con T ̇ θ = 0 . τ con τ This assumption means that must be a linear combination of the columns con T T k ( θ ), i.e., τ , since these are the generalized = A of ( θ ) λ for some A ∈ R λ con ̇ θ is subject to the constraints (8.92): forces that do no work when k T T T ̇ ̇ λ ) ( θ θ = λ A A ( θ ) ) θ = 0 for all λ ∈ R ( . For the writing-robot example, the assumption that the constraint is workless means that there can be no friction between the pen and the paper. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

320 302 8.7. Constrained Dynamics T A θ ) λ to the equations of motion, we can Adding the constraint forces ( ̈ + n + k unknowns { write the θ,λ } k n constrained equations of motion in the n k { τ,λ } (the inverse dynamics): unknowns (the forward dynamics) or in the + T ̈ ̇ ) τ = + h ( θ, M θ ) + A ( ( θ ) λ, (8.93) θ θ ̇ ) ( θ θ , (8.94) A = 0 T is a set of Lagrange multipliers and A θ ( where λ λ are the joint forces and ) torques that act against the constraints. From these equations, it should be n − velocity freedoms and k “force freedoms” – the clear that the robot has k T θ ) λ . (For the writing A ( robot is free to create any generalized force of the form robot, there is an equality constraint: the robot can only apply pushing forces into the paper and table, not pulling forces.) + Often it is convenient to reduce the equations in n + k unknowns to n n k n unknowns, without explicitly calculating the Lagrange multipliers equations in . To do this, we can solve for λ in terms of the other quantities and substitute λ our solution into Equation (8.93). Since the constraints are satisfied at all times, the time rate of change of the constraints satisfies ̈ ̇ ̇ . ) θ + A ( θ ) ( A = 0 θ (8.95) θ Assuming that M ( θ ) and A ( θ ) are full rank, we can solve Equation (8.93) for ̈ ̇ , substitute into Equation (8.95), and omit the dependences on θ θ θ for and conciseness, to get T 1 − ̇ ̇ θ AM τ − + − A ( λ ) = 0 . (8.96) A h ̇ ̈ ̇ θ A A θ , after some manipulation, we obtain Using − = 1 T − 1 − − 1 ̈ = ( ( A AM ) ( τ − h ) − A λ θ ) . (8.97) AM Combining Equation (8.97) with Equation (8.93) and manipulating further, we get ̈ = P ( M + θ Pτ h ) (8.98) where T − 1 T − 1 − 1 − = A ) P ( AM AM I (8.99) A and where I is the n × n identity matrix. The n × n matrix P ( θ ) is of rank θ n , and maps the joint generalized forces τ to P ( k ) τ , projecting away the − generalized force components that act on the constraints while retaining the generalized forces that do work on the robot. The complementary projection, I − P ( θ ), maps τ to ( I − P ( θ )) τ , the joint forces that act on the constraints and do no work on the robot. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

321 Chapter 8. Dynamics of Open Chains 303 P Using ), we can solve the inverse dynamics by evaluating the right-hand ( θ ( ) τ is a set of generalized joint forces P side of Equation (8.98). The solution θ ̈ that achieves the feasible components of the desired joint acceleration . To θ T ( θ ) λ without changing the this solution we can add any constraint forces A acceleration of the robot. We can rearrange Equation (8.98) into the form − 1 ̈ P ( P = M , θ ) τ − h (8.100) ̈ ̈ θ θ where 1 − T 1 − 1 − T − 1 − I P A = ( AM PM M A = ) (8.101) A. M ̈ θ n × n k ) projection matrix P projects away the components ( θ ) ∈ R − n The rank-( ̈ θ ̈ θ that violate the constraints, leaving only the of an arbitrary joint acceleration ̈ θ ) P θ that satisfy the constraints. To solve the forward dynamics, ( components ̈ θ ̈ P θ is we evaluate the right-hand side of Equation (8.100), and the resulting ̈ θ the set of joint accelerations. In Section we discuss the related topic of hybrid motion–force control, in 11.6 which the goal at each instant is to simultaneously achieve a desired acceleration ̈ satisfying A = 0 (consisting of n − k motion freedoms) and a desired force θ against the k constraints. In that section we use the task-space dynamics to represent the task-space end-effector motions and forces more naturally. 8.8 Robot Dynamics in the URDF 4.2 As described in Section and illustrated in the UR5 Universal Robot Descrip- tion Format file, the inertial properties of link i are described in the URDF by mass , origin (the position and orientation of the center-of- the link elements i ), and inertia , which specifies mass frame relative to a frame attached at joint the six elements of the symmetric rotational inertia matrix on or above the di- agonal. To fully write the robot’s dynamics, for joint i we need in addition the joint element origin , specifying the position and orientation of link i ’s joint frame relative to link ( − 1)’s joint frame when θ = 0, and the element axis , i i which specifies the axis of motion of joint i . We leave to the exercises the trans- lation of these elements into the quantities needed for the Newton–Euler inverse dynamics algorithm. 8.9 Actuation, Gearing, and Friction Until now we have been assuming the existence of actuators that directly provide commanded forces and torques. In practice there are many types of actuators May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

322 304 8.9. Actuation, Gearing, and Friction (e.g., electric, hydraulic, and pneumatic) and mechanical power transformers (e.g., gearheads), and the actuators can be located at the joints themselves or remotely, with mechanical power transmitted by cables or timing belts. Each combination of these has its own characteristics that can play a significant role in the “extended dynamics” mapping the actual control inputs (e.g., the current requested of amplifiers connected to electric motors) to the motion of the robot. In this section we provide an introduction to some of the issues associated with one particular, and common, configuration: geared DC electric motors located at each joint. This is the configuration used in the Universal Robots UR5, for example. Figure 8.7 shows the electrical block diagram for a typical n -joint robot driven by DC electric motors. For concreteness, we assume that each joint is revolute. A power supply converts the wall AC voltage to a DC voltage to power the amplifier associated with each motor. A control box takes user input, for example in the form of a desired trajectory, as well as position feedback from encoders located at each joint. Using the desired trajectory, a model of the robot’s dynamics, and the measured error in the current robot state relative to the desired robot state, the controller calculates the torque required of each actuator. Since DC electric motors nominally provide a torque proportional to the current through the motor, this torque command is equivalent to a current command. Each motor amplifier then uses a current sensor (shown as external to the amplifier in Figure 8.7, but in reality internal to the amplifier) to continually 3 adjust the voltage across the motor to try to achieve the requested current. The motion of the motor is sensed by the motor encoder, and the position information is sent back to the controller. The commanded torque is typically updated at around 1000 times per second (1 kHz), and the amplifier’s voltage control loop may be updated at a rate ten times that or more. Figure 8.8 is a conceptual representation of the motor and other components for a single axis. The motor has a single shaft extending from both ends of the motor: one end drives a rotary encoder, which measures the position of the joint, and the other end becomes the input to a gearhead. The gearhead increases the torque while reducing the speed, since most DC electric motors with an appropriate power rating provide torques that are too low to be useful for robotics applications. The purpose of the bearing is to support the gearhead output, freely transmitting torques about the gearhead axis while isolating the gearhead (and motor) from wrench components due to link i + 1 in the other five directions. The outer cases of the encoder, motor, gearhead, and bearing 3 The voltage is typically a time-averaged voltage achieved by the duty cycle of a voltage rapidly switching between a maximum positive voltage and a maximum negative voltage. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

323 Chapter 8. Dynamics of Open Chains 305 position feedback I I 1 1 motor 1 torque/ current amp 1 + encoder current sensor controller, commands dynamics I I 2 2 model motor 2 current amp 2 + encoder sensor user input . DC . voltage power . supply I I n n n motor current amp n + encoder sensor AC voltage A block diagram of a typical n -joint robot. The bold lines correspond Figure 8.7: to high-power signals while the thin lines correspond to communication signals. current position feedback encodermotorgearheadbearinglink +1 i Figure 8.8: The outer cases of the encoder, motor, gearhead, and bearing are fixed in link , while the gearhead output shaft supported by the bearing is fixed in link i i + 1. are all fixed relative to each other and to link i . It is also common for the motor to have some kind of brake, not shown. 8.9.1 DC Motors and Gearing A DC motor consists of a stator and a rotor that rotates relative to the stator. DC electric motors create torque by sending current through windings in a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

324 306 8.9. Actuation, Gearing, and Friction magnetic field created by permanent magnets, where the magnets are attached to the stator and the windings are attached to the rotor, or vice versa. A DC motor has multiple windings, some of which are energized and some of which are inactive at any given time. The windings that are energized are chosen as a function of the angle of the rotor relative to the stator. This “commutation” of the windings occurs mechanically using brushes (brushed motors) or electrically using control circuitry (brushless motors). Brushless motors have the advantage of no brush wear and higher continuous torque, since the windings are typically attached to the motor housing where the heat due to the resistance of the windings can be more easily dissipated. In our basic introduction to DC motor modeling, we do not distinguish between brushed and brushless motors. Figure 8.9 shows a brushed DC motor with an encoder and a gearhead. The torque τ , measured in newton-meters (N m), created by a DC motor is governed by the equation τ = k I, t where , measured in amps (A), is the current through the windings. The I constant k , measured in newton-meters per amp (N m/A), is called the torque t constant . The power dissipated as heat by the windings, measured in watts (W), is governed by 2 I P R, = heat where R is the resistance of the windings in ohms (Ω). To keep the motor wind- ings from overheating, the continuous current flowing through the motor must be limited. Accordingly, in continuous operation, the motor torque must be kept below a continuous-torque limit τ determined by the thermal properties of cont the motor. A simplified model of a DC motor, where all units are in the SI system, can be derived by equating the electrical power consumed by the motor = IV P elec in watts (W) to the mechanical power (also in W) and other power = τw P mech produced by the motor, dI 2 , + friction and other power-loss terms LI R + + IV I = τw dt where V is the voltage applied to the motor in volts (V), w is the angular speed of the motor in radians per second (1/s), and is the inductance due to the L windings in henries (H). The terms on the right-hand side are the mechanical power produced by the motor, the power lost to heating the windings due to the resistance of the wires, the power consumed or produced by energizing or de-energizing the inductance of the windings (since the energy stored in an 1 2 LI , and power is the time derivative of energy), and the power inductor is 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

325 Chapter 8. Dynamics of Open Chains 307 Encoder Commutator Magnet Windings Motor shaft Motor output pinion (input to gearhead) Torsional spring Planetary gearhead Brush Housing (magnetic return) Ball bearings Gearhead CommutatorWindings output shaft Magnet Shaft Brush Housing Figure 8.9: (Top) A cutaway view of a Maxon brushed DC motor with an encoder and gearhead. (Cutaway image courtesy of Maxon Precision Motors, Inc., maxonmo- torusa.com.) The motor’s rotor consists of the windings, commutator ring, and shaft. Each of the several windings connects different segments of the commutator, and as the motor rotates, the two brushes slide over the commutator ring and make contact with different segments, sending current through one or more windings. One end of the motor shaft turns the encoder, and the other end is input to the gearhead. (Bottom) A simplified cross-section of the motor only, showing the stator (brushes, housing, and magnets) in dark gray and the rotor (windings, commutator, and shaft) in light gray. τw by k , Iw lost to friction in bearings, etc. Dropping this last term, replacing t and dividing by I , we get the voltage equation dI (8.102) . V w + IR + L = k t dt May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

326 308 8.9. Actuation, Gearing, and Friction electrical constant k (with units Often Equation (8.102) is written with the e k , but in SI units (V s or N m/A) the of V s) instead of the torque constant t numerical values of the two are identical; they represent the same constant property of the motor. So we prefer to use k . t back electromotive w in Equation (8.102) is called the The voltage term k t force for short, and it is what differentiates a motor from being or back-emf simply a resistor and inductor in series. It also allows a motor, which we usually think of as converting electrical power to mechanical, to be run as a generator, converting mechanical power to electrical. If the motor’s electrical inputs are disconnected (so no current can flow) and the shaft is forced to turn by some k w across the motor’s external torque, you can measure the back-emf voltage t inputs. LdI/dt For simplicity, in the rest of this section we ignore the term. This as- sumption is exactly satisfied when the motor is operating at a constant current. With this assumption, Equation (8.102) can be rearranged to R V 1 τ, − − IR ) = ( V = w 2 k k k t t t 2 as a linear function of τ expressing the speed − R/k w ) for a con- (with a slope of t stant V . Now assume that the voltage across the motor is limited to the range [ − V ], , + V I ] and the current through the motor is limited to [ − I + , max max max max perhaps by the amplifier or power supply. Then the operating region of the mo- tor in the torque–speed plane is as shown in Figure 8.10. Note that the signs of τ are opposite in the second and fourth quadrants of this plane, and there- w and fore the product is negative. When the motor operates in these quadrants, τw it is actually consuming mechanical power, not producing mechanical power. The motor is acting like a damper. τ ≥ 0 ,w ≥ 0 ,τw Focusing on the first quadrant ( 0), the boundary of ≥ the operating region is called the . The no-load speed speed–torque curve at one end of the speed–torque curve is the speed at which V w /k = t 0 max the motor spins when it is powered by V but is providing no torque. In max this operating condition, the back-emf k is equal to the applied voltage, so w t there is no voltage remaining to create current (or torque). The stall torque at the other end of the speed–torque curve is achieved when = k τ V /R max t stall the shaft is blocked from spinning, so there is no back-emf. 8.10 also indicates the continuous operating region where . τ |≤ τ Figure | cont The motor may be operated intermittently outside the continuous operating region, but extended operation outside the continuous operating region raises the possibility that the motor will overheat. The motor’s rated mechanical power is P is = τ w w , where cont cont cont rated May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

327 Chapter 8. Dynamics of Open Chains 309 w + V max limit limit max lp hanica er= ow ec dm rate I w τ − nt cont co w 0 τ τ τ l stal nt co continuous limit operating region − max V I max + limit Figure 8.10: The operating region (dark gray) of a current- and voltage-limited DC electric motor, and its continuous operating region (light gray). the speed on the speed–torque curve corresponding to τ . Even if the motor’s cont rated power is sufficient for a particular application, the torque generated by a DC motor is typically too low to be useful. As mentioned earlier, gearing is therefore used to increase the torque while also decreasing the speed. For a gear ratio , the output speed of the gearhead is G w motor w = . gear G τ w = For an ideal gearhead, no power is lost in the torque conversion, so motor motor τ w , which implies that gear gear τ = Gτ . motor gear In practice, some mechanical power is lost due to friction and impacts between gear teeth, bearings, etc., so τ , = ηGτ motor gear where η ≤ 1 is the efficiency of the gearhead. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

328 310 8.9. Actuation, Gearing, and Friction w τ G = 2 The original motor operating region, and the operating region with a Figure 8.11: gear ratio = 2 showing the increased torque and decreased speed. G when Figure 8.11 shows the operating region of the motor from Figure 8.10 = 2 (with η = 1). The maximum torque doubles, the motor is geared by G while the maximum speed shrinks by a factor of two. Since many DC motors are capable of no-load speeds of 10 000 rpm or more, robot joints often have gear ratios of 100 or more to achieve an appropriate compromise between speed and torque. 8.9.2 Apparent Inertia The motor’s stator is attached to one link and the rotor is attached to another link, possibly through a gearhead. Therefore, when calculating the contribution of a motor to the masses and inertias of the links, the mass and inertia of the stator must be assigned to one link and the mass and inertia of the rotor must be assigned to the other link. Consider a stationary link 0 with the stator of the joint-1 gearmotor attached ̇ to it. The rotational speed of joint 1, the output of the gearhead, is . Therefore θ ̇ the motor’s rotor rotates at G θ . The kinetic energy of the rotor is therefore 1 1 2 2 2 ̇ ̇ = K I θ ( ) G I = , G θ rotor rotor ︸ ︷︷ ︸ 2 2 apparent inertia 2 I is is the rotor’s scalar inertia about the rotation axis and G where I rotor rotor the apparent inertia (often called the reflected inertia ) of the rotor about the axis. In other words, if you were to grab link 1 and rotate it manually, the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

329 Chapter 8. Dynamics of Open Chains 311 2 G inertia contributed by the rotor would feel as if it were a factor larger than its actual inertia, owing to the gearhead. I I While the inertia of the is typically much less than the inertia link rotor 2 rest of the link about the rotation axis, the apparent inertia I may be G rotor on the order of, or even larger than, I . link One consequence as the gear ratio becomes large is that the inertia seen by i joint becomes increasingly dominated by the apparent inertia of the rotor. In other words, the torque required of joint i becomes relatively more dependent ̈ on θ than on other joint accelerations, i.e., the robot’s mass matrix becomes i more diagonal. In the limit when the mass matrix has negligible off-diagonal components (and in the absence of gravity), the dynamics of the robot are decoupled – the dynamics at one joint has no dependence on the configuration or motion of the other joints. 8.1 with L = = L = As an example, consider the 2R arm of Figure m 2 1 1 = 1. Now assume that each of joint 1 and joint 2 has a motor of mass 1, m 2 with a stator of inertia 0.005 and a rotor of inertia 0.00125, and a gear ratio G (with η = 1). With a gear ratio G = 10, the mass matrix is [ ] 1 . 13 + 2 cos θ θ 4 . 01 + cos 2 2 ( M θ ) = . 01 + cos θ 1 . 13 1 . 2 With a gear ratio G = 100, the mass matrix is [ ] 13 + cos . θ 16 . 5 + 2 cos θ 1 2 2 ( ) = M θ . 13 + cos θ . 13 . 5 1 2 The off-diagonal components are relatively less important for this second robot. The available joint torques of the second robot are ten times that of the first robot so, despite the increases in the mass matrix elements, the second robot is capable of significantly higher accelerations and end-effector payloads. The top speed of each joint of the second robot is ten times less than that of the first robot, however. If the apparent inertia of the rotor is non-negligible relative to the inertia of the rest of the link, the Newton–Euler inverse dynamics algorithm must be modified to account for it. One approach is to treat the link as consisting of two separate bodies, the geared rotor driving the link and the rest of the link, each with its own center of mass and inertial properties (where the link inertial properties include the inertial properties of the stator of any motor mounted on the link). In the forward iteration, the twist and acceleration of each body is determined while accounting for the gearhead in calculating the rotor’s motion. In the backward iteration, the wrench on the link is calculated as the sum of two May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

330 312 8.9. Actuation, Gearing, and Friction T F ( ) − Ad ( i +1) T L t i axis join ( ) ,i +1 i L L lin i k axis roto r i gearhead i { } +1 R } i { r roto L i r moto stator F i L lin k − 1 i (b) (a) Figure 8.12: − 1 and i . (b) The (a) Schematic of a geared motor between links i , which is analogous to Figure 8.6. i free-body diagram for link wrenches: (i) the link wrench as given by Equation (8.53) and (ii) the reaction wrench from the distal rotor. The resultant wrench projected onto the joint τ by the gear ratio and adding ; dividing τ axis is then the gear torque gear gear to this the torque resulting from the acceleration of the rotor results in the required motor torque τ . The current command to the DC motor is then motor ). = τ ηk / ( I t motor com Newton–Euler Inverse Dynamics Algorithm Accounting 8.9.3 for Motor Inertias and Gearing We now reformulate the recursive Newton–Euler inverse dynamics algorithm 8.12 il- taking into account the apparent inertias as discussed above. Figure lustrates the setup. We assume massless gears and shafts and that the friction between gears as well as the friction between shafts and links is negligible. Initialization Attach a frame { 0 } to the to the base, frames { 1 } } to { n L L L n n } , and frames to { 1 } { to the centers of mass of centers of mass of links 1 to R R . Frame { n + 1 } rotors 1 to is attached to the end-effector, which is assumed n L to be the n fixed with respect to frame { . Define M M and } L i − 1) , 1) i i ( , ( i − R L L L configuration of { i − 1 } A in { i } = 0. Let and in { i } θ , respectively, when L i L i R i i { be the screw axis of joint } expressed in . Similarly, let R be the screw axis i L of rotor i expressed in { i } 6 spatial inertia matrix of link . Let G × be the 6 R i L i that includes the inertia of the attached stator and G be the 6 × 6 spatial i R May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

331 Chapter 8. Dynamics of Open Chains 313 and i is . The twists V . The gear ratio of motor inertia matrix of rotor G i 0 i L ̇ ̇ , and V F , V are defined in the same way as V F and the wrench n 0 0 +1 0 +1) ( n L L in Section 8.3.2. ̇ ̈ Given θ, , for i = 1 to n do θ, θ Forward iterations G ] θ R [ − i i i (8.103) , M e = T i , 1) i − ( , i − 1) i ( R L L R θ ] A [ − i i , (8.104) M e = T , ( i − 1) i , − i ( 1) i L L L L ̇ = Ad V ) + ( V G R θ (8.105) , T i i i i i 1) − ( R L 1) , i ( i − R L ̇ (8.106) , θ = Ad V A ( V ) + i i i T i 1) ( − L L 1) − i ( , i L L ̇ ̈ ̇ ̇ V G R ) + ad = Ad ( V ) ( θ + G R θ , (8.107) i T V i i i i i i i − 1) ( i R L , i 1) ( i − R R L ̇ ̈ ̇ ̇ V ) + ad θ ( ) A ( A V = Ad + (8.108) . θ V i i i i T i i ( − 1) i L L i , ( i − 1) L L L = n to 1 do Backward iterations For i T T ̇ F = Ad G ( F ) + G ( V ad V − ) i i i i i +1) ( i T V L L L L L L i ,i i +1) ( L L L T T ̇ (8.109) )) , V G ( ad − V ( G + Ad ( +1) i ( i +1) +1) i ( i ( +1) V T R R R R +1) ( i ( i +1) ,i R R L T = A τ F (8.110) , i gear i, i L τ i, gear T T ̇ = τ G ad ( )) (8.111) − V V . G R + ( motor i, i i i i V i R R R R i R G i occurring in the first F In the backward iteration stage, the quantity ( n +1) L step of the backward iteration is taken to be the external wrench applied to the { n + 1 end-effector (expressed in the F frame), with G set to zero; } L i n +1) ( L R denotes the wrench applied to link i via the motor i gearhead (expressed in the is the torque generated at the motor { } frame); τ τ gearhead; and i i L gear i, i, motor is the torque at rotor i . Note that if there is no gearing then no modification to the original Newton– Euler inverse dynamics algorithm is necessary; the stator is attached to one link and the rotor is attached to another link. Robots constructed with a motor at each axis and no gearheads are sometimes called direct-drive robots . Direct- drive robots have low friction, but they see limited use because typically the motors must be large and heavy to generate appropriate torques. No modification is needed to the Lagrangian approach to the dynamics to handle geared motors, provided that we can correctly represent the kinetic en- ergy of the faster-spinning rotors. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

332 314 8.9. Actuation, Gearing, and Friction τ τ τ τ τ τ fric fric fric fric fric fric ̇ ̇ ̇ ̇ ̇ ̇ θ θ θ θ θ θ (b ) (e) ) (a (c) (d ) (f ) Examples of velocity-dependent friction models. (a) Viscous friction, Figure 8.13: ̇ ̇ b τ θ . (b) Coulomb friction, τ ); = θ can take any value in sgn( b = τ static viscous fric fric fric ̇ = ,b ] at zero velocity. (c) Static plus viscous friction, τ )+ b b [ sgn( − θ static static static fric ̇ θ . (d) Static and kinetic friction, requiring τ to initiate motion and ≥| b | b static viscous fric ̇ . (e) Static, kinetic, = b > b sgn( then θ τ b ) during motion, where static kinetic kinetic fric and viscous friction. (f) A friction law exhibiting the Stribeck effect – at low velocities, the friction decreases as the velocity increases. 8.9.4 Friction The Lagrangian and Newton–Euler dynamics do not account for friction at the joints, but the friction forces and torques in gearheads and bearings may be significant. Friction is a complex phenomenon that is the subject of considerable current research; any friction model is a gross attempt to capture the average behavior of the micromechanics of contact. static friction term and a velocity-dependent Friction models often include a viscous friction term. The presence of a static friction term means that a nonzero torque is required to cause the joint to begin to move. The viscous fric- tion term indicates that the amount of friction torque increases with increasing velocity of the joint. See Figure 8.13 for some examples of velocity-dependent friction models. Other factors may contribute to the friction at a joint, including the loading of the joint bearings, the time the joint has been at rest, the temperature, etc. The friction in a gearhead often increases as the gear ratio increases. G 8.9.5 Joint and Link Flexibility In practice, a robot’s joints and links are likely to exhibit some flexibility. For example, the flexspline element of a harmonic drive gearhead achieves essentially zero backlash by being somewhat flexible. A model of a joint with harmonic drive gearing, then, could include a relatively stiff torsional spring between the motor’s rotor and the link to which the gearhead is attached. Similarly, links themselves are not infinitely stiff. Their finite stiffness is exhibited as vibrations along the link. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

333 Chapter 8. Dynamics of Open Chains 315 Flexible joints and links introduce extra states to the dynamics of the robot, significantly complicating the dynamics and control. While many robots are designed to be stiff in order to minimize these complexities, in some cases this is impractical owing to the extra link mass required to create the stiffness. Summary 8.10 τ Given a set of generalized coordinates and generalized forces θ • , the Euler–Lagrange equations can be written ∂ ∂L L d − , τ = ̇ dt ∂θ ∂ θ ̇ ̇ where θ ) = K ( θ, L θ ) −P ( θ ), K is the kinetic energy of the robot, and ( θ, is the potential energy of the robot. P • The equations of motion of a robot can be written in the following equiv- alent forms: ̈ ̇ ( τ ) = θ + h ( θ, M θ ) θ ̈ ̇ θ ) = θ + c ( θ, ( θ ) + g ( θ ) M T ̈ ̇ ̇ ) θ + = θ ) Γ( θ ( M θ + g ( θ ) θ ̈ ̇ ̇ θ ) ( θ + C ( θ, = θ ) M θ + g ( θ ) , ̇ where ) is the n × n symmetric positive-definite mass matrix, h ( θ, M θ ) is θ ( the sum of the generalized forces due to the gravity and quadratic velocity ̇ θ, c θ ) are quadratic velocity forces, ( ( θ ) are gravitational forces, terms, g θ ) is an n × n × n matrix of Christoffel symbols of the first kind obtained Γ( ̇ ) is the θ ) with respect to θ , and C ( θ, M θ ( n × n from partial derivatives of i,j )th entry is given by Coriolis matrix whose ( n ∑ ̇ ̇ . θ ) = θ ) θ ( Γ ( c θ, ijk k ij k =1 to the environ- F If the end-effector of the robot is applying a wrench tip T should be added to the right-hand side of the ( θ ) F ment, the term J tip robot’s dynamic equations. The symmetric positive-definite rotational inertia matrix of a rigid body • is   I I I xx xy xz   I I I = I , yy yz xy b I I I yz xz zz May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

334 316 8.10. Summary where ∫ ∫ 2 2 2 2 dV, z ( ) ρ ( x,y,z ) dV, I = = y ) ( x I + z + ) ρ ( x,y,z xx yy B B ∫ ∫ 2 2 x = + y I ) ρ ( x,y,z ) dV, I dV, = − ( ) xyρ ( x,y,z xy zz B B ∫ ∫ − ) ) xzρ ( x,y,z I dV, I x,y,z = − = ( yzρ dV, xz yz B B is the volume of the body, dV B is a differential volume element, and x,y,z ρ ( ) is the density function. I } is defined in a frame { b • at the center of mass, with axes aligned If b I is diagonal. with the principal axes of inertia, then b If b } is at the center of mass but its axes are not aligned with the principal • { { c axes of inertia, there always exists a rotated frame defined by the } T R such that I R = R rotation matrix I is diagonal. c bc bc b bc } If , the inertia • { b I at the center of mass then I is defined in a frame q b in a frame { q } aligned with { b } but displaced from the origin of { b } by 3 q in { b } coordinates, is ∈ R T T qI . m ( q = I − qq I ) + q b The spatial inertia matrix G } expressed in a frame { b • at the center of b mass is defined as the 6 × 6 matrix [ ] 0 I b . = G b I 0 m { a } at a configuration T In a frame relative to { b } , the spatial inertia ba matrix is T . = [Ad ] [Ad ] G G b T a T ba ba V and V is The Lie bracket of two twists • 2 1 ( ad V ) = [ad ] , V V 2 2 V 1 1 where [ ] [ ω ] 0 6 6 × ∈ R ] = [ad . V ] [ ω ] [ v • The twist–wrench formulation of the rigid-body dynamics of a single rigid body is T ̇ . V V − [ad G ] F G = b b V b b b b G F , V , and The equations have the same form if are all expressed in the same frame, regardless of the frame. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

335 Chapter 8. Dynamics of Open Chains 317 1 T • The kinetic energy of a rigid body is G V V , and the kinetic energy of b b b 2 1 T ̇ ̇ ( θ ) θ θ . M an open-chain robot is 2 The forward–backward Newton–Euler inverse dynamics algorithm is the • following: 0 } to the base, frames { Initialization: } to { n } to the Attach a frame { 1 1 } to { n } centers of mass of links { n +1 } at the end-effector, { , and a frame { } . Define M fixed in the frame n } to be the configuration of { i − 1 1 i,i − i } when θ = 0. Let { A , be the screw axis of joint i expressed in in i } { i i G be the 6 × and i . Define V to be the 6 spatial inertia matrix of link i 0 { twist of the base frame expressed in base-frame coordinates. (This 0 } 3 ∈ be the gravity vector expressed in quantity is typically zero.) Let g R ̇ } coordinates, and define , V base-frame- = (0 { − g 0 ). (Gravity is treated 0 as an acceleration of the base in the opposite direction.) Define F = +1 n F m ,f ) to be the wrench applied to the environment by the = ( tip tip tip { n + 1 } . end-effector expressed in the end-effector frame ̈ ̇ i Given θ , for θ, = 1 to n do Forward iterations: θ, θ ] A [ − i i e T = M , 1 i,i − 1 i,i − ̇ V = Ad ) + A ( V , θ i T 1 − i i i 1 i,i − ̇ ̈ ̇ ̇ V = Ad V ) + ad ( ( A A ) θ + θ . T i − 1 i V i i i i 1 − i,i i For i = n to 1 do Backward iterations: T T ̇ , ) ( F ( V ) + G G = Ad − ad V F +1 i i i i i i V T i +1 i ,i T = F A . τ i i i ̇ Let J ’s center- ( θ ) be the Jacobian relating • θ to the body twist V i in link i ib i } . Then the mass matrix M ( { ) of the manipulator can of-mass frame θ be expressed as n ∑ T J M ( . θ ) = ) ( θ ) G θ J ( ib i ib =1 i • The forward dynamics problem involves solving T ̈ ̇ ) M θ = τ ( t ) − h ( θ, ( θ ) − J θ ( θ ) F tip ̈ = θ , using any efficient solver of equations of the form Ax for b . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

336 318 8.10. Summary ̈ ̇ The robot’s dynamics • θ + h ( θ, ) θ ) can be expressed in the task space M ( θ as ̇ θ, V + η ( ) V ) , F θ = Λ( is the wrench applied to the end-effector, V is the twist of the end- where F , F J ( θ ) are all defined in the same frame. , and the Jacobian effector, and V ) and gravity and quadratic velocity forces θ The task-space mass matrix Λ( θ, V ) are η ( T − − 1 Λ( M ( θ θ ) = J ) , J − T − 1 − 1 ̇ h ( θ,J V ) = V ) − Λ( θ ) J JJ η θ, V . ( Define two × • n projection matrices of rank n − k n 1 T 1 − 1 − T − A AM θ A P ) ( − AM I ) = , ( 1 − T 1 − T 1 − 1 − ( = PM M A ) = ( AM θ M A I ) A, P − ̈ θ n × k ̇ , acting ( θ = 0, A ∈ R A Pfaffian constraints, k ) θ corresponding to the n + k constrained equations of motion on the robot. Then the T ̈ ̇ θ ) τ θ + h ( θ, = θ ) + A M ( θ ) λ, ( ̇ ( θ ) A θ = 0 can be reduced to the following equivalent forms by eliminating the La- : λ grange multipliers ̈ P ( M θ Pτ + h ) , = − 1 ̈ h θ = P P − M . ) ( τ ̈ ̈ θ θ P The matrix projects away the joint force–torque components that act on the constraints without doing work on the robot, and the matrix P ̈ θ projects away acceleration components that do not satisfy the constraints. • An ideal gearhead (one that is 100% efficient) with a gear ratio G multi- plies the torque at the output of a motor by a factor G and divides the speed by the factor , leaving the mechanical power unchanged. The in- G ertia of the motor’s rotor about its axis of rotation, as it appears at the 2 G output of the gearhead, is I . rotor May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

337 Chapter 8. Dynamics of Open Chains 319 Software 8.11 Software functions associated with this chapter are listed below. adV = ad(V) ]. Computes [ad V taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,g,Ftip, Mlist,Glist,Slist) n τ of the required Uses Newton–Euler inverse dynamics to compute the -vector ̈ ̇ , a list of transforms θ θ θ , g , F joint forces–torques given , M specifying , − 1 ,i i tip { i } relative to { i − 1 } when the configuration of the center-of-mass frame of link G the robot is at its home position, a list of link spatial inertia matrices , and a i list of joint screw axes expressed in the base frame. S i M = MassMatrix(thetalist,Mlist,Glist,Slist) Computes the mass matrix M θ ) given the joint configuration θ , a list of trans- ( M , and a list of joint screw , a list of link spatial inertia matrices G forms ,i i − i 1 S expressed in the base frame. axes i c = VelQuadraticForces(thetalist,dthetalist,Mlist,Glist,Slist) ̇ ̇ θ, c θ ) given the joint configuration θ , the joint velocities Computes θ , a list of ( transforms M , and a list of joint G , a list of link spatial inertia matrices ,i 1 i i − S screw axes expressed in the base frame. i grav = GravityForces(thetalist,g,Mlist,Glist,Slist) θ θ Computes g , the gravity vector g , a list of ( ) given the joint configuration M , and a list of joint , a list of link spatial inertia matrices G transforms ,i i − i 1 S expressed in the base frame. screw axes i JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist) T ( Computes θ J F applied by given the joint configuration θ , the wrench F ) tip tip the end-effector, a list of transforms M , a list of link spatial inertia matrices − 1 i ,i S expressed in the base frame. G , and a list of joint screw axes i i ddthetalist = ForwardDynamics(thetalist,dthetalist,taulist,g,Ftip, Mlist,Glist,Slist) ̇ ̈ Computes θ given the joint configuration θ , the joint velocities θ , the joint forces- torques τ , the gravity vector g , the wrench F applied by the end-effector, a tip M , a list of link spatial inertia matrices G list of transforms , and a list of ,i i i − 1 joint screw axes S expressed in the base frame. i [thetalistNext,dthetalistNext] = EulerStep(thetalist,dthetalist, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

338 320 8.12. Notes and References ddthetalist,dt) ̇ Computes a first-order Euler approximation to t + δt ) , { θ ( t + δt ) } given the ( θ ̇ ̈ ( ( θ θ t ), the joint accelerations ), the joint velocities θ ( t ), and joint configuration t . δt a timestep taumat = InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat, g,Ftipmat,Mlist,Glist,Slist) n N The variable thetamat matrix of robot joint variables θ , where the is an × th row corresponds to the n -vector of joint variables θ ( i ) at time t = ( i − 1) δt , t where is the timestep. The variables dthetamat , ddthetamat , and Ftipmat δt ̇ ̈ θ θ , and F similarly represent , as a function of time. Other inputs include the tip gravity vector g , a list of transforms M , a list of link spatial inertia matrices 1 ,i i − G S expressed in the base frame. This function , and a list of joint screw axes i i representing the joint forces-torques N computes an matrix taumat × τ ( t ) n required to generate the trajectory specified by θ ( t ) and F ). Note that it is ( t tip ̇ ̈ not necessary to specify θ ( t ) and accelerations . The velocities θ ( t ) should be δt θ t ). consistent with ( [thetamat,dthetamat] = ForwardDynamicsTrajectory(thetalist, dthetalist,taumat,g,Ftipmat,Mlist,Glist,Slist,dt,intRes) This function numerically integrates the robot’s equations of motion using Euler N × n matrices thetamat and dthetamat , where integration. The outputs are ̇ ) and n -vectors θ (( i − 1) δt the i θ (( i − 1) δt ). th rows correspond respectively to the ̇ θ θ (0), an N × n matrix of joint forces or The inputs are the initial state (0), τ ( t ), the gravity vector g , an N × n matrix of end-effector wrenches torques ( t ), a list of transforms M F , a list of link spatial inertia matrices G , a 1 ,i tip i i − list of joint screw axes S expressed in the base frame, the timestep δt , and the i number of integration steps to take during each timestep (a positive integer). 8.12 Notes and References An accessible general reference on rigid-body dynamics that covers both the Newton–Euler and Lagrangian formulations is [51]. A more classical reference that covers a wide range of topics in dynamics is [193]. A recursive inverse dynamics algorithm for open chains using the classical screw-theoretic machinery of twists and wrenches was first formulated by Feath- erstone (the collection of twists, wrenches, and the corresponding analogues of accelerations, momentum, and inertias, are collectively referred to as spatial vector notation); this formulation, as well as more efficient extensions based on articulated body inertias, are described in [46, 47]. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

339 Chapter 8. Dynamics of Open Chains 321 The recursive inverse dynamics algorithm presented in this chapter was first described in [133] and makes use of standard operators from the theory of Lie groups and Lie algebras. One of the important practical advantages of this ap- proach is that analytic formulas can be derived for taking first- and higher-order derivatives of the dynamics. This has important consequences for dynamics- based motion optimization: the availability of analytic gradients can greatly improve the convergence and robustness for motion optimization algorithms. These and other related issues are explored in, e.g., [88]. The task-space formulation was first initiated by Khatib [74], who referred to it as the operational space formulation. Note that the task-space formula- tion involves taking time derivatives of the forward kinematics Jacobian, i.e., ̇ ̇ θ J ( J ( θ ) can in fact be evaluated ). Using either the body or space Jacobian, analytically; this is explored in one of the exercises at the end this chapter. A brief history of the evolution of robot dynamics algorithms, as well as pointers to references on the more general subject of multibody system dynamics (of which robot dynamics can be considered a subfield) can be found in [48]. 8.13 Exercises Derive the formulas given in Figure 8.5 for: Exercise 8.1 (a) a rectangular parallelpiped; (b) a circular cylinder; (c) an ellipsoid. Consider a cast iron dumbbell consisting of a cylinder connecting Exercise 8.2 two solid spheres at either end of the cylinder. The density of the dumbbell is 3 7500 kg / m . The cylinder has a diameter of 4 cm and a length of 20 cm. Each sphere has a diameter of 20 cm. (a) Find the approximate rotational inertia matrix I at the in a frame { b } b center of mass with axes aligned with the principal axes of inertia of the dumbbell. (b) Write down the spatial inertia matrix G . b Exercise 8.3 Rigid-body dynamics in an arbitrary frame. (a) Show that Equation (8.42) is a generalization of Steiner’s theorem. (b) Derive Equation (8.43). Exercise 8.4 The 2R open-chain robot of Figure 8.14, referred to as a ro- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

340 322 8.13. Exercises θ 1 θ 1 ˆz s τ 1 τ 1 L 1 ˆy s L ˆx 1 s ˆz } { s s ˆz 1 } { b 1 ˆz 1 ˆy } { b s 1 m xˆ 1 1 yˆ ˆx rod 1 1 s τ s { } 2 xˆ 1 θ τ yˆ 2 2 1 L 2 θ 2 g g ˆz 2 L yˆ 2 2 ˆz 2 yˆ 2 { b } xˆ 2 2 xˆ 2 ZeroPosition ZeroPosition m 2 rod 2 b { } 2 2R rotational inverted pendulum. (Left) Its construction; (right) the Figure 8.14: model. tational inverted pendulum or Furuta pendulum, is shown in its zero position. Assuming that the mass of each link is concentrated at the tip and neglecting its thickness, the robot can be modeled as shown in the right-hand figure. As- m I = m and = 2, L sume that = L I = 1, g = 10, and the link inertias 2 1 2 1 2 1 { and ) are } (expressed in their respective link frames { b b } 1 2     4 0 0 0 0 0     0 4 0 0 4 0 = , I . = I 2 1 0 0 0 0 0 4 (a) Derive the dynamic equations and determine the input torques and τ τ 1 2 θ = = θ 4 and the joint velocities and accelerations are all zero. when π/ 2 1 M ( θ ) when θ (b) Draw the torque ellipsoid for the mass matrix = θ π/ = 4. 2 1 Prove the following Lie bracket identity (called the Jacobi iden- Exercise 8.5 V : , V tity) for arbitrary twists , V 3 1 2 ad . )) = 0 (ad V ( ( V (ad )) + ad )) + ad V (ad ( V 2 V V 3 V V 1 V 2 1 2 1 3 3 ̇ The evaluation of J ( θ ), the time derivative of the forward kine- Exercise 8.6 ̇ V in matics Jacobian, is needed in the calculation of the frame accelerations i the Newton–Euler inverse dynamics algorithm and also in the formulation of the task-space dynamics. Letting J ), we have ( θ ) denote the i th column of J ( θ i n ∑ d ∂J i ̇ θ J ( ) = θ . i j ∂θ dt j j =1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

341 Chapter 8. Dynamics of Open Chains 323 J (a) Suppose that ) is the space Jacobian. Show that ( θ { ∂J ad ( J i > j ) for i J j i = 0 for i ≤ . j ∂θ j J θ ) is the body Jacobian. Show that ( (b) Now suppose that { ∂J ( ad i < j ) for J i j J i = ≥ 0 for i . j ∂θ j ̇ Show that the time derivative of the mass matrix M ( θ ) can be Exercise 8.7 written explicitly as T T T T T T T ̇ L , A L ] A L = GLA + A M [ad Γ GL [ad ]Γ ̇ ̇ θ A A θ with the matrices as defined in the closed-form dynamics formulation. Exercise 8.8 Explain intuitively the shapes of the end-effector force ellipsoids in Figure 8.4 on the basis of the point masses and the Jacobians. Exercise 8.9 Consider a motor with rotor inertia I connected through rotor a gearhead of gear ratio I about the rota- to a load with scalar inertia G link tion axis. The load and motor are said to be inertia matched if, for any given torque at the motor, the acceleration of the load is maximized. The τ m acceleration of the load can be written Gτ m ̈ = θ . 2 I + G I link rotor √ ̈ = 0. / I θ/dG I d by solving Solve for the inertia-matching gear ratio rotor link Exercise 8.10 Give the steps that rearrange Equation (8.98) to get Equa- tion (8.100). Remember that ( θ ) is not full rank and cannot be inverted. P ̇ ̇ h ( θ, ) effi- θ ) = c ( θ, Exercise 8.11 θ ) + g ( θ Program a function to calculate ciently using Newton–Euler inverse dynamics. Give the equations that would convert the joint and link de- Exercise 8.12 scriptions in a robot’s URDF file to the data Mlist , Glist , and Slist , suitable for using with the Newton–Euler algorithm InverseDynamicsTrajectory . Exercise 8.13 The efficient evaluation of M ( θ ). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

342 324 8.13. Exercises (a) Develop conceptually a computationally efficient algorithm for determin- ing the mass matrix θ ) using Equation (8.57). M ( (b) Implement this algorithm. The function InverseDynamicsTrajectory requires the user Exercise 8.14 thetamat but also a time to enter not only a time sequence of joint variables dthetamat sequence of joint velocities ddthetamat . Instead, and accelerations the function could use numerical differencing to find approximately the joint velocities and accelerations at each timestep, using only . Write an thetamat alternative InverseDynamicsTrajectory function that does not require the user to enter dthetamat and ddthetamat . Verify that it yields similar results. Exercise 8.15 Dynamics of the UR5 robot. (a) Write the spatial inertia matrices G of the six links of the UR5, given i the center-of-mass frames and mass and inertial properties defined in the URDF in Section 4.2. 2 g = 9 . 81 m/s (b) Simulate the UR5 falling under gravity with acceleration in the ˆz -direction. The robot starts at its zero configuration and zero − s joint torques are applied. Simulate the motion for three seconds, with at least 100 integration steps per second. (Ignore the effects of friction and the geared rotors.) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

343 Chapter 9 Trajectory Generation During robot motion, the robot controller is provided with a steady stream of goal positions and velocities to track. This specification of the robot position . In some cases, the trajectory is as a function of time is called a trajectory completely specified by the task – for example, the end-effector may be required to track a known moving object. In other cases, as when the task is simply to move from one position to another in a given time, we have freedom to design trajectory the trajectory to meet these constraints. This is the domain of planning . The trajectory should be a sufficiently smooth function of time, and it should respect any given limits on joint velocities, accelerations, or torques. In this chapter we consider a trajectory as the combination of a , a path purely geometric description of the sequence of configurations achieved by the time scaling , which specifies the times when those configurations robot, and a are reached. We consider three cases: point-to-point straight-line trajectories in both joint space and task space; trajectories passing through a sequence of via points ; and minimum-time trajectories along specified paths taking timed actuator limits into consideration. Finding paths that avoid obstacles is left to Chapter 10. 9.1 Definitions A θ ( s ) maps a scalar path parameter s , assumed to be 0 at the start path of the path and 1 at the end, to a point in the robot’s configuration space Θ, θ : [0 , 1] → Θ. As s increases from 0 to 1, the robot moves along the path. Sometimes is taken to be time and is allowed to vary from time s = 0 to s the total motion time s = T , but it is often useful to separate the role of the 325

344 326 9.2. Point-to-Point Trajectories s geometric path parameter . A time scaling s ( t ) from the time parameter t to each time → ∈ [0 ,T ], s : [0 ,T ] s [0 , 1]. assigns a value t ( θ s ( t )), or θ ( t ) for Together, a path and a time scaling define a trajectory short. Using the chain rule, the velocity and acceleration along the trajectory can be written as dθ ̇ ̇ s, (9.1) θ = ds 2 dθ θ d 2 ̈ s (9.2) . ̇ θ + ̈ = s 2 ds ds To ensure that the robot’s acceleration (and therefore dynamics) is well defined, θ s each of ) and s ( t ) must be twice differentiable. ( Point-to-Point Trajectories 9.2 The simplest type of motion is from rest at one configuration to rest at another. We call this a point-to-point motion. The simplest type of path for point-to- point motion is a straight line. Straight-line paths and their time scalings are discussed below. 9.2.1 Straight-Line Paths A “straight line” from a start configuration θ θ to an end configuration start end could be defined in joint space or in task space. The advantage of a straight-line θ in joint space is simplicity: since joint limits typically θ path from to start end ≤ , the allowable joint configu- take the form θ i ≤ θ for each joint θ max i, i min i, in joint space, so the straight line between any rations form a convex set Θ free two endpoints in Θ also lies in Θ . The straight line can be written free free ( s ) = θ + s (9.3) θ θ − θ ) , s ∈ [0 , 1] ( end start start with derivatives dθ = θ , − θ (9.4) start end ds 2 d θ (9.5) = 0 . 2 ds Straight lines in joint space generally do not yield straight-line motion of the end-effector in task space. If task-space straight-line motions are desired, the start and end configurations can be specified by X in task space. If and X end start May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

345 Chapter 9. Trajectory Generation 327 θ ) (deg 2 180 θ d en 90 θ star t 9090180 − (deg ) θ 1 θ 2 90 − θ ) (deg θ 2 1 180 θ d en 90 θ t star − 9090180 θ ) (deg 1 90 − ◦ ◦ ◦ ◦ θ . (Top ≤ 180 Figure 9.1: , 0 (Left) A 2R robot with joint limits 0 ≤ θ ≤ ≤ 150 2 1 center) A straight-line path in joint space and (top right) the corresponding motion of the end-effector in task space (dashed line). The reachable endpoint configurations, subject to joint limits, are indicated in gray. (Bottom center) This curved line in joint space and (bottom right) the corresponding straight-line path in task space (dashed line) would violate the joint limits. X X and are represented by a minimum set of coordinates then a straight end start X ( 1]. Compared with the ) = X + s ( X − X line is defined as ) ,s ∈ [0 , s start start end case when joint coordinates are used, the following issues must be addressed: If the path passes near a kinematic singularity, the joint velocities may • become unreasonably large for almost all time scalings of the path. • Since the robot’s reachable task space may not be convex in X coordinates, some points on a straight line between two reachable endpoints may not be reachable (Figure 9.1). In addition to the issues above, if X and X are represented as elements start end May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

346 328 9.2. Point-to-Point Trajectories SE of (3) instead of as a minimum set of coordinates, then there is the question (3). A configuration of the form X + of how to define a “straight” line in SE start SE − X X ) does not generally lie in s (3). ( end start One option is to use the screw motion (simultaneous rotation about and translation along a fixed screw axis) that moves the robot’s end-effector from = X (0) to X X = X (1). To derive this X ( s ), we can write the start and end start s } frame as X end configurations explicitly in the and use our and { X s, end start s, subscript cancellation rule to express the end configuration in the start frame: 1 − X = X = X X . X ,s end , start end s, start s, end s, start − 1 Then log( X X ) is the matrix representation of the twist, expressed in end s, start s, start } the X { to X in unit time. The path can therefore frame, that takes start end be written as − 1 ) = X (9.6) exp(log( X X s , ( ) ) s X end start start where X is post-multiplied by the matrix exponential since the twist is start represented in the { start } frame, not the fixed world frame { s } . This screw motion provides a “straight-line” motion in the sense that the screw axis is constant. The origin of the end-effector does not generally follow a straight line in Cartesian space, since it is following a screw motion. It may be preferable to decouple the rotational motion from the translational motion. = ( R,p ), we can define the path Writing X s ) = p s + p ( p ( − p (9.7) ) , start end start T s R R ( exp(log( R ) = R (9.8) ) s ) start end start where the frame origin follows a straight line while the axis of rotation is constant 9.2 illustrates a screw path and a decoupled path for in the body frame. Figure X the same and X . end start 9.2.2 Time Scaling a Straight-Line Path A time scaling s ( t ) of a path should ensure that the motion is appropriately smooth and that any constraints on robot velocity and acceleration are satisfied. For a straight-line path in joint space of the form (9.3), the time-scaled joint ̈ ̇ θ = ̇ ( θ ), − θ θ velocities and accelerations are θ s = ̈ s ( θ − ) and end start start end respectively. For a straight-line path in task space parametrized by a minimum m ̇ ̈ ̇ ̈ θ θ , ∈ θ , and R X by X , set of coordinates X , and , simply replace X . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

347 Chapter 9. Trajectory Generation 329 w p e a r t c h s X star t decoupled rotation and translation X end A path following a constant screw motion versus a decoupled path where Figure 9.2: the frame origin follows a straight line and the angular velocity is constant. ̇ s ̈ s s 3 6 1 2 T T 2 0 t T t t T T Plots of s ( t ), ̇ s ( t ), and ̈ s ( t ) for a third-order polynomial time scaling. Figure 9.3: Polynomial Time Scaling 9.2.2.1 A convenient form for the time scaling s ( Third-order Polynomials ) is a t cubic polynomial of time, 3 2 t ) = a (9.9) + a . t + a s t ( + a t 3 2 0 1 T imposes the initial constraints (0) = ̇ s (0) = 0 A point-to-point motion in time s ( s ) = 1 and ̇ s ( T ) = 0. Evaluating Equation (9.9) and the terminal constraints T and its derivative 2 ̇ t ) = a (9.10) + 2 a ( t + 3 a s t 3 1 2 at t = 0 and t = T and solving the four constraints for a , we find ,...,a 3 0 3 2 = 0 = 0 , a = a , a − . , a = 3 2 1 0 3 2 T T ), and ̈ Plots of ( t ), ̇ s ( t s s ( t ) are shown in Figure 9.3. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

348 330 9.2. Point-to-Point Trajectories 2 3 s t = + a a t Substituting into Equation (9.3) yields 2 3 ) ( 3 2 t 2 3 t θ + t − (9.11) θ ) = , ) ( ( θ − θ end start start 2 3 T T ( ) 2 t t 6 6 ̇ , − t ) = − ) θ θ ( θ ( (9.12) end start 3 2 T T ( ) 6 t 12 ̈ θ . ) (9.13) θ − ( − ( t θ ) = start end 2 3 T T The maximum joint velocities are achieved at the halfway point of the motion, = t 2: T/ 3 ̇ = ) . θ ( θ θ − end max start T 2 t = 0 and The maximum joint accelerations and decelerations are achieved at t T : = ∣ ∣ ∣ ∣ ∣ ∣ ∣ ∣ 6 6 ̈ ̈ ∣ ∣ ∣ ∣ . θ = − , − θ ( − ) ) θ ( θ θ θ = min max end start end start ∣ ∣ ∣ ∣ 2 2 T T ̇ ̇ θ and maxi- | θ |≤ If there are known limits on the maximum joint velocities limit ̈ ̈ mum joint accelerations | θ |≤ θ , these bounds can be checked to see whether limit the requested motion time T is feasible. Alternatively, one could solve for T to find the minimum possible motion time that satisfies the most restrictive velocity or acceleration constraint. Because third-order time scaling does not con- Fifth-order Polynomials strain the endpoint path accelerations ̈ s (0) and ̈ ( T ) to be zero, the robot is s t = 0 and t = T . asked to achieve a discontinuous jump in acceleration at both jerk , the derivative of acceleration, which may cause This implies an infinite vibration of the robot. s (0) = ̈ s ( T ) = 0. One solution is to constrain the endpoint accelerations to ̈ Adding these two constraints to the problem formulation requires the addition of two more design freedoms in the polynomial, yielding a quintic polynomial of 5 s ( t ) = a . We can use the six terminal position, velocity, and + ··· + a time, t 5 0 acceleration constraints to solve uniquely for a ,...,a (Exercise 9.5), which 0 5 yields a smoother motion with a higher maximum velocity than a cubic time scaling. A plot of the time scaling is shown in Figure 9.4. 9.2.2.2 Trapezoidal Motion Profiles Trapezoidal time scalings are quite common in motor control, particularly for the motion of a single joint, and they get their name from their velocity profiles. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

349 Chapter 9. Trajectory Generation 331 s ̈ ̇ s s 15 10 √ 1 2 8 T T 3 0 T t t t T T s ( t ), ̇ s ( t ), and ̈ s ( t ) for a fifth-order polynomial time scaling. Figure 9.4: Plots of ̇ s s v 1 slop a e= e= − slop a t T − T t t Tt a a s Figure 9.5: t ) and ̇ Plots of ( t ) for a trapezoidal motion profile. s ( s = a of The point-to-point motion consists of a constant acceleration phase ̈ = t s = v of time t , followed by a constant velocity phase ̇ T − 2 t time , a v a followed by a constant deceleration phase ̈ s a of time t = . The resulting ̇ s − a s profile is a trapezoid and the profile is the concatenation of a parabola, linear segment, and parabola as a function of time (Figure 9.5). The trapezoidal time scaling is not as smooth as the cubic time scaling, but it has the advantage that if there are known constant limits on the joint velocities n n ̇ ̈ θ θ ∈ ∈ R and on the joint accelerations then the trapezoidal R limit limit v and a satisfying motion using the largest ̇ ( θ (9.14) − θ , ) v |≤ | θ limit start end ̈ θ a − θ (9.15) ) | |≤ ( θ limit start end is the fastest straight-line motion possible. (See Exercise 9.8.) 2 v If /a > 1, the robot never reaches the velocity v during the motion (Ex- ercise 9.10). The three-phase accelerate–coast–decelerate motion becomes a two-phase accelerate–decelerate “bang-bang” motion, and the trapezoidal pro- file ̇ s ( t ) in Figure 9.5 becomes a triangle. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

350 332 9.2. Point-to-Point Trajectories 2 v 1, the trapezoidal motion is fully specified by v , a , Assuming that /a ≤ T , and , but only two of these can be specified independently since they must t a T ) = 1 and v satisfy at s . It is unlikely that we would specify t indepen- ( = a a dently, so we can eliminate it from the equations of motion by the substitution v/a . The motion profile during the three stages (acceleration, coast, de- = t a , a , and T as follows: celeration) can then be written in terms of v v a, ) = (9.16) t , ̈ s ( t ≤ for 0 ≤ a s ( t ) = at, (9.17) ̇ 1 2 ; s at ( ) = (9.18) t 2 v v < t ≤ T − (9.19) ) = 0 , ̈ s ( t , for a a s t ) = v, (9.20) ̇ ( 2 v ) = s − ( t vt ; (9.21) 2 a v a, − ̈ ) = < t ≤ T , (9.22) s ( t − for T a ) s ) = a ( T − t t , (9.23) ( ̇ 2 2 2 2 avT − 2 v ) T a − ( t − ) = ( s t (9.24) . a 2 v , Since only two of , and T can be chosen independently, we have three a options: 2 Choose v and a such that v ≤ /a • 1, ensuring a three-stage trapezoidal profile, and solve s T ) = 1 (using Equation (9.24)) for T : ( 2 v + a = T . va v and a correspond to the highest possible joint velocities and acceler- If ations, this is the minimum possible time for the motion. • Choose v and T such that 2 ≥ vT > 1, ensuring a three-stage trapezoidal profile and that the top speed is sufficient to reach s = 1 in time T , and v solve s ( T ) = 1 for a : 2 v . a = − 1 vT May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

351 Chapter 9. Trajectory Generation 333 s ̇ v slop a e= e= − slop a 1234567 Tt Figure 9.6: ( t ) for an S-curve motion profile consisting of seven stages: (1) Plot of ̇ s constant positive jerk, (2) constant acceleration, (3) constant negative jerk, (4) con- stant velocity, (5) constant negative jerk, (6) constant deceleration, and (7) constant positive jerk. 2 a and T such that aT ≥ • Choose 4, ensuring that the motion is completed in time, and solve ( T ) = 1 for v : s ) ( √ √ 1 2 v = − aT aT − 4 a . 2 9.2.2.3 S-Curve Time Scalings Just as cubic polynomial time scalings lead to infinite jerk at the beginning and end of the motion, trapezoidal motions cause discontinuous jumps in ac- celeration at t ∈ { 0 ,t time ,T − t S-curve ,T } . A solution is a smoother a a scaling, a popular motion profile in motor control because it avoids vibrations or oscillations induced by step changes in acceleration. An S-curve time scaling 3 3 d s/dt = J until a desired accelera- consists of seven stages: (1) constant jerk s = tion ̈ is achieved; (2) constant acceleration until the desired ̇ s = v is being a approached; (3) constant negative jerk − J until ̈ s equals zero exactly at the time ̇ s reaches v ; (4) coasting at constant v ; (5) constant negative jerk − J ; (6) and ̇ constant deceleration ; and (7) constant positive jerk J until ̈ s a s reach − zero exactly at the time s reaches 1. The ̇ s ( t ) profile for an S-curve is shown in Figure 9.6. T Given some subset of , a , J , and the total motion time v , algebraic manipu- lation reveals the switching time between stages and conditions that ensure that all seven stages are actually achieved, similarly to the case of the trapezoidal motion profile. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

352 334 9.3. Polynomial Via Point Trajectories 3 via 2 via 3 via 2 via ˆy ˆy ˆx ˆx end end start start ) ) (a (b x,y Two paths in an ( Figure 9.7: ) space corresponding to piecewise-cubic trajectories interpolating four via points, including a start point and an end point. The velocities at the start and end are zero, and the velocities at vias 2 and 3 are indicated by the dashed tangent vectors. The shape of the path depends on the velocities specified at the via points. 9.3 Polynomial Via Point Trajectories If the goal is to have the robot joints pass through a series of via points at specified times, without a strict specification on the shape of path between consecutive points, a simple solution is to use polynomial interpolation to find θ joint histories t ) directly without first specifying a path θ ( s ) and then a time ( scaling ( t ) (Figure 9.7). s k Let the trajectory be specified by via points, with the start point occurring at T = 0 and the final point at T = T . Since each joint history is interpolated 1 k β to avoid a prolifera- individually, we focus on a single joint variable and call it tion of subscripts. At each via point i ∈{ 1 ,...,k } , the user specifies the desired ̇ ̇ 1 segments, ( β ) = and velocity β β ( T − ) = position β k . The trajectory has T i i i i T ∈ { ,...,k − 1 j is ∆ 1 = T and the duration of segment − T . The joint } j j +1 j j trajectory during segment is expressed as the third-order polynomial 2 3 (9.25) + ∆ t ) = a + a β t + a ∆ t ( + a ∆ t T ∆ j j j j 3 j 2 1 0 ≤ in terms of the time ∆ elapsed in segment j , where 0 ≤ ∆ t t ∆ T j . Segment j May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

353 Chapter 9. Trajectory Generation 335 y x y ̇ city sition lo ̇ x po ve t t T T T T T T T 4 1 2 3 4 2 3 T 1 The coordinate time histories for the cubic via-point interpolation of Figure 9.8: Figure 9.7(a). is subject to the four constraints ̇ ̇ ) = ) = β , ( β ( T , β T β j j j j ̇ ̇ β + ∆ T . ) = β β ( , T β ( T ) = + ∆ T j j j j +1 j j +1 ,...,a a yields Solving these constraints for 3 j 0 j = β , (9.26) a j 0 j ̇ = β , (9.27) a j 1 j ̇ ̇ β 3 T β − T − 3 β ∆ − 2 ∆ β j j j j +1 j j +1 a = , (9.28) j 2 2 T ∆ j ̇ ̇ T 2 β )∆ + ( β β β 2 − + j +1 j +1 j j j (9.29) . = a j 3 3 ∆ T j Figure 9.8 shows the time histories for the interpolation of Figure 9.7(a). In this two-dimensional ( x,y ,..., 4 occur at ) coordinate space the via points 1 = 2, and T = 0, T 1), = 1, times , 0), (0 T , = 3. The via points are at (0 T 4 1 3 2 (1 , 1), and (1 , 0) with velocities (0 , 0), (1 , 0), (0 , − 1), and (0 , 0). Two issues are worth mentioning: • The quality of the interpolated trajectories is improved by “reasonable” combinations of via-point times and via-point velocities. For example, if the user wants to specify a via-point location and time, but not the velocity, a heuristic could be used to choose a via velocity on the basis of May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

354 336 9.4. Time-Optimal Time Scaling the times and coordinate vectors to the via points before and after the via 9.7(b) is smoother in question. As an example, the trajectory of Figure 9.7(a). than the trajectory of Figure Cubic via-point interpolation ensures that velocities are continuous at via • points, but not accelerations. The approach is easily generalized to the use of fifth-order polynomials and specification of the accelerations at the via points, at the cost of increased complexity of the solution. If only two points are used (the start and end point), and the velocities at each are zero, the resulting trajectory is identical to the straight-line cubic polynomial time-scaled trajectory discussed in Section 9.2.2.1. There are many other methods for interpolating a set of via points. For example, B-spline interpolation is popular. In B-spline interpolation, the path may not pass exactly through the via points, but the path is guaranteed to be confined to the convex hull of the via points, unlike the paths in Figure 9.7. This can be important to ensure that joint limits or workspace obstacles are respected. 9.4 Time-Optimal Time Scaling θ ( s ) is fully specified by the task or an obstacle- In the case where the path 9.9), the trajectory planning problem reduces avoiding path planner (e.g., Figure ( t ). One could choose the time scaling to minimize s to finding a time scaling the energy consumed while meeting a time constraint, or to prevent spilling a glass of water that the robot is carrying. One of the most useful time scalings minimizes the time of motion along the path, subject to the robot’s actuator limits. Such time-optimal trajectories maximize the robot’s productivity. 9.2.2.2 can yield time-optimal While the trapezoidal time scalings of Section trajectories, this is only under the assumption of straight-line motions, constant maximum acceleration a , and constant maximum coasting velocity v . For most robots, because of state-dependent joint actuator limits and the state-dependent dynamics ̈ ̇ θ ) M θ + c ( θ, ( θ ) + g ( θ ) = τ, (9.30) the maximum available velocities and accelerations change along the path. In this section we consider the problem of finding the fastest possible time s ( t ) that respects the robot’s actuator limits. We write the limits on the scaling th actuator as i min max ̇ ̇ θ, τ θ ) ≤ τ . ≤ τ (9.31) ) ( θ, ( θ i i i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

355 Chapter 9. Trajectory Generation 337 ) ,y x ( c c =0 s s =1 R ˆy ˆx A path planner has returned a semicircular path of radius around Figure 9.9: R an obstacle in ( x,y ) space for a robot with two prismatic joints. The path can be , as x ( s ) = x represented in terms of a path parameter + R cos sπ s y ( s ) = and c − R sin sπ for y ∈ [0 , 1]. For a 2R robot, inverse kinematics would be used to s c s in joint coordinates. express the path as a function of The available actuator torque is typically a function of the current joint speed (see Section 8.9.1). For example, for a given maximum voltage of a DC motor, the maximum torque available from the motor drops linearly with the motor’s speed. ̇ θ, Before proceeding we recall that the quadratic velocity terms θ ) in Equa- ( c tion (9.30) can be written equivalently as T ̇ ̇ ̇ ( θ θ, Γ( θ ) c θ, ) = θ where Γ( ) is the three-dimensional tensor of Christoffel symbols constructed θ from partial derivatives of components of the mass matrix ( θ ) with respect to M . This form shows more clearly the quadratic dependence on velocities. Now, θ ̇ ̈ by ( dθ/ds ) ̇ s beginning with Equation (9.30), replacing θ θ by ( dθ/ds ) ̈ s + and 2 2 2 θ/ds ( , and rearranging, we get ) ̇ d s ( ) ( ) ( ) T 2 θ dθ d dθ dθ 2 s + M ( θ ( )) )) s ̈ ( θ s ( M = + ( g τ, + s Γ( θ ( s )) ( ̇ θ s )) 2 ︸ ︷︷ ︸ ds ds ds ds ︷︷ ︸ ︸ n g ( s ) ∈ R ︸ ︷︷ ︸ n ∈ m ( s ) R n ∈ c ( s ) R (9.32) expressed more compactly as the vector equation 2 ( s ) ̈ s + c ( s ) ̇ s m + g ( s ) = τ, (9.33) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

356 338 9.4. Time-Optimal Time Scaling ( s ) is the effective inertia of the robot when it is confined to the path where m 2 s ( s ) ̇ s s comprises the quadratic velocity terms, and g ( c ) is the gravitational ), θ ( torque. Similarly, the actuation constraints (9.31) can be expressed as a function of s : min max s, ̇ s ) ≤ τ (9.34) ≤ τ τ ( ( s, ̇ s ) . i i i i th component of Equation (9.33), we get Substituting the min 2 max ̇ s ) ≤ m (9.35) ( s ) ̈ s + c . ( τ ) ̇ s ( + g ) ( s ) ≤ τ s, s ( s, ̇ s i i i i i L s ( s, ̇ s ) and U sat- ( s, ̇ Let ) be the minimum and maximum accelerations ̈ s i i m ( s ), th component of Equation (9.35). Depending on the sign of i isfying the i we have three possibilities:  2 min ) s ( − − ) s ) ̇ s ( τ c s ( s, ̇ g  i  , s ) = s, ( s , L 0 > ) ( m if ̇  i i   m ) s ( i   max 2  ̇ s ) − c ( s ) ̇ s τ − g ( s ) ( s,  i  s, U ( ̇ ) = s  i   m s ( ) i max 2 ) ̇ s ( s, ̇ s ) − c ( s ) s τ − g ( i ) = if m ( s , ) < 0 , L ( s, ̇ s  i i   m ) ( s i   2 min  ( s ) s, ̇ s ) − c ( s ( ) ̇ s τ − g  i   s ) = U ̇ s, ( i   m s ( ) i   zero-inertia point m 9.4.4 . if ( s ) = 0 , we have a , discussed in Section i (9.36) Defining L ( s, ̇ s ) = max , L ) ( s, ̇ s ) and U ( s, ̇ s ) = min s U ̇ ( s, i i i i the actuator limits (9.35) can be written as the state-dependent time-scaling constraints s, ̇ s ) ≤ ̈ s ≤ ( ( s, ̇ s ) . (9.37) L U The time-optimal time-scaling problem can now be stated: s Given a path s ) ,s ∈ [0 , 1] , an initial state ( ( , and a final state , ̇ s 0) ) = (0 , θ 0 0 ( s , find a monotonically increasing twice-differentiable time scaling , ̇ s 0) ) = (1 , f f : [0 ,T ] s [0 , 1] that → (a) satisfies s (0) = ̇ s (0) = ̇ s ( T ) = 0 and s ( T ) = 1 , and (b) T along the path while respecting the actu- minimizes the total travel time ator constraints (9.37) . The problem formulation is easily generalized to the case of nonzero initial and final velocities along the path, ̇ s (0) > 0 and ̇ s ( T ) > 0. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

357 Chapter 9. Trajectory Generation 339 ̇ s g ̇ ) s s timescalin ( 0 s 1 s, ̇ s ) phase plane is a curve, with ̇ s ≥ 0 at all A time scaling in the ( Figure 9.10: 0) to the final position and times, connecting the initial path position and velocity (0 , 0). velocity (1 , ( The ̇ s ) Phase Plane 9.4.1 s, s, ̇ s The problem is easily visualized in the ( ) phase plane of the path-constrained robot, with running from 0 to 1 on a horizontal axis and ̇ s on a vertical s ) s ) is monotonically increasing, ̇ s ( t t ≥ 0 for all times t and for all axis. Since ( ∈ [0 s 1]. A time scaling of the path is any curve in the phase plane that moves , monotonically to the right from (0 0) to (1 , 0) (Figure 9.10). Not all such curves , satisfy the acceleration constraints (9.37), however. To see the effect of the acceleration constraints, at each ( s, ̇ s ) in the phase plane, we can plot the limits L ( s, ̇ s ) ≤ ̈ s ≤ U ( s, ̇ s ) as a cone constructed from ̇ s , L , and , as illustrated in two dimensions in Figure 9.11(a). If L ( s, ̇ s ) ≥ U ( s, ̇ s ), U the cone disappears – there are no actuator commands that can keep the robot on the path at this state. These states are indicated in gray inadmissible ) 9.11(a). For any , typically there is a single limit velocity ̇ s s in Figure s ( lim above which all velocities are inadmissible. The function ̇ s ( s ) is called the lim velocity limit curve . On the velocity limit curve, L ( s, ̇ s ) = U ( s, ̇ s ), and the cone reduces to a single vector. For a time scaling to satisfy the acceleration constraints, the tangent of the time-scaling curve must lie inside the feasible cone at all points on the curve. Figure 9.11(b) shows an example of an infeasible time scaling, which demands more deceleration than the actuators can provide at the state indicated. For a minimum-time motion, the “speed” ̇ must be as high as possible s at every s while still satisfying the acceleration constraints and the endpoint constraints. To see this, write the total time of motion T as ∫ T 1 T (9.38) = dt. 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

358 340 9.4. Time-Optimal Time Scaling s ̇ s ̇ ve lo city e limitcurv U ( s, ̇ s ) . s ̇ s, s L ) ( s 0 s 0 1 1 (b ) ) (a Figure 9.11: (a) Acceleration-limited motion cones at four different states. The ( s, upper ray of the cone is the sum of s ) plotted in the vertical direction (the change U ̇ s plotted in the horizontal direction (the change in position). The in velocity) and ̇ L ( s, ̇ s ) and ̇ s . The points in gray, bounded lower ray of the cone is constructed from s, L ̇ s ) ≥ U ( s, ̇ s ): the state is inadmissible and there ( by the velocity limit curve, have is no motion cone. On the velocity limit curve the cone is reduced to a single tangent vector. (b) The proposed time scaling is infeasible because the tangent to the curve is outside the motion cone at the state indicated. ds/ds = 1, and changing the limits of integration from Making the substitution T s ), we get 0 to (time) to 0 to 1 ( ∫ ∫ ∫ ∫ 1 1 T T dt ds 1 − s dt ( ds. = ̇ (9.39) ) ds = s = dt 1 T = ds ds 0 0 0 0 − 1 s ( s ) should be as small as possible, and Thus for time to be minimized, ̇ s ( s ) must be as large as possible, at all s , while still satisfying the therefore ̇ acceleration constraints (9.37) and the boundary constraints. This implies that the time scaling must always operate either at the limit s, ( ̇ s ) or at the limit L U s, ̇ s ), and our only choice is when to switch between ( these limits. A common solution is a bang-bang trajectory: maximum accel- eration U ( s, ̇ s ) followed by a switch to maximum deceleration L ( s, ̇ s ). (This is similar to the trapezoidal motion profile that never reaches the coasting velocity in Section 9.2.2.2.) In this case the time scaling is calculated by numerically v integrating U ( s, ̇ s ) forward in s from (0 , 0), integrating L ( s, ̇ s ) backward in s from (1 , 0), and finding the intersection of these curves (Figure 9.12(a)). The switch between maximum acceleration and maximum deceleration occurs at the intersection. In some cases, the velocity limit curve prevents a single-switch solution (Fig- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

359 Chapter 9. Trajectory Generation 341 s ̇ s ̇ lb ang-bang optima g timescalin U ( s, ̇ s ) ̇ s, ) s L ( non-optima l ∗ 0 s s 0 s 1 (b ) (a ) U ( s, ̇ s ) from Figure 9.12: (a) A time-optimal bang-bang time scaling integrates ∗ 0) and switches to L ( s, ̇ (0 ) at a switching point s , . Also shown is a non-optimal s time scaling with a tangent inside a motion cone. (b) Sometimes the velocity limit curve prevents a single-switch solution. ure 9.12(b)). These cases require an algorithm to find multiple switching points. 9.4.2 The Time-Scaling Algorithm Finding the optimal time scaling is reduced to finding the switches between ( s, ̇ s maximum acceleration L ( s, ̇ s ), maximizing U ) and maximum deceleration s, ̇ the “height” of the curve in the ( ) phase plane. s Time-scaling algorithm S {} and a switch counter i = 0. Set 1. Initialize an empty list of switches = s ( , ̇ s 0). ) = (0 , i i 2. Integrate the equation ̈ s = L ( s, ̇ s ) backward in time from (1 , 0) until L ( ̇ s ) > U ( s, ̇ s ) (the velocity limit curve is penetrated) or s = 0. Call s, F this phase plane curve . s = U ( s, ̇ 3. Integrate the equation ̈ ) forward in time from ( s ) until it , ̇ s s i i crosses F or until U ( s, ̇ s ) < L ( s, ̇ s ) (the velocity limit curve is penetrated). ) Call this curve A . If A s crosses F then increment i , set ( s ̇ , ̇ s s, ) to the ( i i i i value at which the crossing occurs, and append s to the list of switches i S . This is a switch from maximum acceleration to maximum deceleration. The problem is solved and S is the set of switches expressed in the path parameter. If instead the velocity limit curve is penetrated, let ( , ̇ s s ) lim lim be the point of penetration and proceed to the next step. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

360 342 9.4. Time-Optimal Time Scaling , 4. Perform a binary search on the velocity in the range [0 ̇ ] to find the ve- s lim ′ ′ = s ( s, ̇ s ) forward from ( s s , ̇ s such that the curve integrating ̈ ) L locity ̇ lim touches the velocity limit curve without penetrating it. The binary search = ̇ s s and ̇ is initiated with ̇ = 0. s low high lim (a) Set the test velocity halfway between ̇ s s + : ̇ s s = ( ̇ and ̇ test high low high ). s / 2. The test point is ( s ̇ , ̇ s ) test low lim (b) If the curve from the test point penetrates the velocity limit curve, s s . If instead the curve from the test point hits set ̇ equal to ̇ high test s = 0, set ̇ s . Return to step 4(a). equal to ̇ s ̇ test low s Continue the binary search until a specified tolerance. Let ( , ̇ s ) be tan tan the point where the resulting curve just touches the velocity limit curve tangentially (or comes closest to the curve without hitting it). The motion ( s, ̇ s ) = cone at this point is reduced to a single vector ( L s, ̇ s )), tangent ( U to the velocity limit curve. s = L ( s, ̇ s ) backwards from ( s ) until it intersects , ̇ s . 5. Integrate ̈ A i tan tan i , set ( s Increment , ̇ s ) value at the intersection, and label ) to the ( s, ̇ s i i A to the list the curve segment from ( s s , ̇ s ). Append as s s , ̇ ) to ( tan tan i i i i of switches S . This is a switch from maximum acceleration to maximum deceleration. 6. Increment and set ( s i , ̇ s to the list of switches ) to ( s s , ̇ s ). Append tan tan i i i . This is a switch from maximum deceleration to maximum acceleration. S Go to step 3. Figure 9.13 shows steps 2–6 of the time-scaling algorithm. (Step 2) Integra- s = L ( s, ̇ s ) backward from (1 , 0) until the velocity limit curve is reached. tion of ̈ s U (Step 3) Integration of ̈ ( s, ̇ s ) forward from (0 , 0) to the intersection = ′ s ( ̇ s ) ) with the velocity limit curve. (Step 4) Binary search to find ( s s , ̇ , lim lim lim ′ s = ( s, ̇ s ), integrated forward from ( s L , ̇ s from which ̈ ), touches the veloc- lim ity limit curve tangentially. (Step 5) Integration backward along ( s, ̇ s ) from L s ( , ̇ s ) to find the first switch from acceleration to deceleration. (Step 6) tan tan s ). , ̇ s s The second switch, from deceleration to acceleration, is at ( s ̇ , ) = ( tan 2 tan 2 (Step 3) Integration forward along U ( s, ̇ s ) from ( s ) results in intersection , ̇ s 2 2 F at ( s ), where a switch occurs from acceleration to deceleration. The , ̇ s with 3 3 S = { optimal time scaling consists of switches at . ,s ,s } s 3 2 1 9.4.3 A Variation on the Time-Scaling Algorithm Remember that each point ( s, ̇ s ) below the velocity limit curve has a cone of feasible motions, while each point on the velocity limit curve has a single feasible May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

361 Chapter 9. Trajectory Generation 343 s ̇ ̇ s s ( , s ) ̇ lim lim city lo ve e limitcurv A F F 0 ( s , ̇ s ) 0 0 0 s 0 s 1 1 = i , S = {} Step 2: i =0 , S =0 {} Step 3: s ̇ s ̇ , s ) s ̇ ( 1 1 3 , ) s ( ) s s ̇ , ̇ s ( tan tan tan tan 4 2 A 1 1 F F A A 0 0 s 0 s 0 s 1 1 1 } = S s =1 { , Step 5: i i , {} = S Step =0 4: 1 ̇ s s ̇ ̇ ( s ) , s 3 3 ) s ̇ ( , s 2 2 A A 1 1 A 2 A A F F 0 0 accdecaccdec s s s s s 0 s 2 1 0 s 3 2 1 1 1 Step 3: i =3 , } ,s ,s s { S = Step =2 i , 6: ,s } S = { s 1 2 3 2 1 Figure 9.13: The time-scaling algorithm. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

362 344 9.4. Time-Optimal Time Scaling A point on the velocity limit curve can only be a part of a time-optimal Figure 9.14: time scaling if the feasible motion vector at that point is tangential to the curve. A search along the velocity limit curve illustrated reveals that there are only two points on this particular curve (marked by dots) that can belong to a time-optimal time scaling. vector. The only points on the velocity limit curve that can be part of an optimal solution are those where the feasible motion vector is tangent to the velocity ̇ , limit curve; these are the points ( s s ) referred to above. Recognizing this, tan tan the binary search in step 4 of the time-scaling algorithm, which is essentially s ), can be replaced by explicit construction of the , ̇ searching for a point ( s tan tan velocity limit curve and a search for points on this curve satisfying the tangency condition. See Figure 9.14. Assumptions and Caveats 9.4.4 The description above covers the major points of the optimal time-scaling algo- rithm. A few assumptions were glossed over; they are made explicit now. Static posture maintenance. The algorithm, as described, assumes • that the robot can maintain its configuration against gravity at any state s, ̇ s = 0). This ensures the existence of valid time scalings, namely, time ( scalings that move the robot along the path arbitrarily slowly. For some robots and paths this assumption may be violated owing to weakness of the actuators. For example, some paths may require some momentum to carry the motion through configurations that the robot cannot maintain statically. The algorithm can be modified to handle such cases. • The algorithm assumes that at every s there is a Inadmissible states. ≤ s ( s ) unique velocity limit ̇ 0 such that all velocities ̇ s ) are ̇ s s ( > lim lim admissible and all velocities ̇ s > ̇ s ) are inadmissible. For some models ( s lim of actuator dynamics or friction this assumption may be violated – there may be isolated “islands” of inadmissible states. The algorithm can be modified to handle this case. • Zero-inertia points. The algorithm assumes that there are no zero- inertia points (Equation (9.36)). If m ) = 0 in (9.36) then the torque ( s i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

363 Chapter 9. Trajectory Generation 345 i has no dependence on the acceleration ̈ , and the provided by actuator s velocity i . constraint on ̇ th actuator constraint in (9.35) directly defines a s m with one or more zero components in s ), the velocity limit s At a point ( curve is defined by the minimum of (a) the velocity constraints defined values satisfying L by the zero-inertia components and (b) the ̇ ( s, ̇ s ) = s i ( s, ̇ s ) for the other components. For the algorithm as described, singular U i arcs of zero-inertia points on the velocity limit curve may lead to rapid s = U ( s, ̇ s ) and ̈ s = L ( switching between ̈ ̇ s ). In such cases, choosing an s, acceleration tangent to the velocity limit curve and lying between ( s, ̇ s ) U L ( ̇ s ), preserves time optimality without causing chattering of the and s, controls. It is worth noting that the time-scaling algorithm generates trajectories with discontinuous acceleration, which could lead to vibrations. Beyond this, inaccu- racies in models of robot inertial properties and friction make direct application of the time-scaling algorithm impractical. Finally, since a minimum-time time scaling always saturates at least one actuator, if the robot moves off the planned trajectory, there may be no torque left for corrective action by a feedback con- troller. Despite these drawbacks, the time-scaling algorithm provides a deep under- standing of the true maximum capabilities of a robot following a path. 9.5 Summary A trajectory θ ( t ), θ : [0 • ] → Θ, can be written as θ ( s ( t )), i.e., as the ,T composition of a path θ ( s ), θ : [0 , 1] → Θ, and a time scaling s ( t ), s : [0 ,T → [0 , 1]. ] s A straight-line path in joint space can be written s ) = θ + ( ( θ • − θ start end θ s ∈ [0 , 1]. A similar form holds for straight-line paths in a minimum ), start SE (3), where set of task-space coordinates. A “straight-line” path in X = ( R,p ), can be decoupled to a Cartesian path and a rotation path: p ( s ) = p (9.40) + s ( p , − p ) start end start T ) = R R exp(log( R ( s R (9.41) ) s ) . end start start 2 3 ( t ) = a can be used to time + a t + a t A cubic polynomial + a t s • 3 0 2 1 scale a point-to-point motion with zero initial and final velocities. The T t = 0 and t = acceleration undergoes a step change (an infinite jerk) at . Such an impulse in jerk can cause vibration of the robot. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

364 346 9.6. Software 5 2 3 4 A quintic polynomial • + a can be used t + a s t t + a ( t ) = + a t t a + a 5 4 2 1 0 3 to time-scale a point-to-point motion with zero initial and final velocities and accelerations. The jerk is finite at all times. • The trapezoidal motion profile is a popular time scaling in point-to-point control, particularly the control of a single motor. The motion consists of three phases: constant acceleration, constant velocity, and constant deceleration, resulting in a trapezoid in ̇ t ). Trapezoidal motion involves s ( step changes in acceleration. The S-curve motion profile is also popular in point-to-point control of a • motor. It consists of seven phases: (1) constant positive jerk, (2) constant acceleration, (3) constant negative jerk, (4) constant velocity, (5) constant negative jerk, (6) constant deceleration, and (7) constant positive jerk. • Given a set of via points including a start state, a goal state, and other via states through which the robot’s motion must pass, as well as the times at which these states should be reached, a series of cubic-polynomial T i time scalings can be used to generate a trajectory ( t ) interpolating the θ via points. To prevent step changes in acceleration at the via points, a series of quintic polynomials can be used instead. Given a robot path θ ( s ), the dynamics of the robot, and limits on the • actuator torques, the actuator constraints can be expressed in terms of ( s, ̇ s ) as the vector inequalities L ( ̇ s ) ≤ ̈ s ≤ U ( s, ̇ s ) . s, The time-optimal time scaling s t ) is such that the “height” of the curve in ( s s, s ) phase plane is maximized while satisfying the ( (0) = ̇ ̇ (0) = ̇ s ( T ) = s 0, s ( T ) = 1, and the actuator constraints. The optimal solution always op- ( erates at maximum acceleration s, ̇ s ) or maximum deceleration L ( s, ̇ s ). U 9.6 Software Software functions associated with this chapter are listed below. s = CubicTimeScaling(Tf,t) and the total time of motion s ( t ) for a cubic time scaling, given t Computes T . f s = QuinticTimeScaling(Tf,t) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

365 Chapter 9. Trajectory Generation 347 s ( ) for a quintic time scaling, given t and the total time of motion Computes t . T f traj = JointTrajectory(thetastart,thetaend,Tf,N,method) Computes a straight-line trajectory in joint space as an N × n matrix, where rows is an n each of the N -vector of the joint variables at an instant in time. θ and the N th row is θ . The elapsed time between each The first row is start end T / row is ( N − 1). The parameter method equals either 3 for a cubic time f scaling or 5 for a quintic time scaling. traj = ScrewTrajectory(Xstart,Xend,Tf,N,method) Computes a trajectory as a list of (3) matrices, where each matrix repre- N SE sents the configuration of the end-effector at an instant in time. The first matrix X , and the motion is along a constant screw axis. , the N th matrix is X is start end T / ( The elapsed time between each matrix is − 1). The parameter method N f equals either 3 for a cubic time scaling or 5 for a quintic time scaling. traj = CartesianTrajectory(Xstart,Xend,Tf,N,method) Computes a trajectory as a list of N SE (3) matrices, where each matrix repre- sents the configuration of the end-effector at an instant in time. The first matrix X , the N th matrix is is , and the origin of the end-effector frame fol- X end start lows a straight line, decoupled from the rotation. The elapsed time between T ( / each matrix is N − 1). The parameter method equals either 3 for a cubic f time scaling or 5 for a quintic time scaling. 9.7 Notes and References In 1985, Bobrow et al. [15] and Shin and McKay [168] published papers nearly simultaneously that independently derived the essence of the time-optimal time- scaling algorithm outlined in Section 9.4. A year earlier, Hollerbach addressed the restricted problem of finding dynamically feasible time-scaled trajectories for uniform time scalings where the time variable t is replaced by ct for c > 0 [59]. The original papers of Bobrow et al. and Shin and McKay were followed by a number of papers refining the methods by addressing zero-inertia points, sin- gularities, algorithm efficiency, and even the presence of constraints and obsta- cles [138, 173, 163, 164, 166, 167, 139, 140]. In particular, a computationally 165, efficient method for finding the points ( s ), where the optimal time scal- , ̇ s tan tan ing touches the velocity limit curve, is described in [138, 173]. This algorithm is used to improve the computational efficiency of the time-scaling algorithm; see, for example, the description and supporting open-source code in [139, 140]. In this chapter the binary search approach in step 4 of the time-scaling algorithm May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

366 348 9.8. Exercises (2 , 1) ˆy 0) (0 (4 , 0) , ˆx , − (2 1) An elliptical path. Figure 9.15: follows the presentation in [15] because of its conceptual simplicity. Other research has focused on numerical methods such as dynamic pro- gramming or nonlinear optimization to minimize cost functions such as actu- ator energy. One early example of work in this area is by Vukobratovi ́c and Kir ́canski [191]. 9.8 Exercises x,y )-plane. The path starts Exercise 9.1 Consider an elliptical path in the ( 0) , 1), (4 , 0), (2 , − 1), and back to (0 , , at (0 0) and proceeds clockwise to (2 s ∈ [0 , 1]. (Figure 9.15). Write the path as a function of Exercise 9.2 X = ( x,y,z ) is given by x = cos 2 πs , A cylindrical path in 1 1 2 z = 2 s , s ∈ [0 , 1], and its time scaling is s ( t ) = y πs = sin 2 , 2]. + t , ,t ∈ [0 t 4 8 ̇ ̈ Write down X and X . Exercise 9.3 Consider a path from X (0) = X ∈ ∈ SE (3) to X (1) = X end start (3) consisting of motion along a constant screw axis. The path is time scaled SE ̇ t ). Write down the twist V and acceleration by some s at any point on the ( V s and ̈ s . path given ̇ Consider a straight-line path Exercise 9.4 ( s ) = θ ∈ + s ( θ ,s − θ ) θ start end start [0 , 1] from θ 3). The motion starts and ends at = (0 , 0) to θ π,π/ = ( end start ̇ ̇ θ | , | rest. The feasible joint velocities are θ | ≤ 2 rad/s and the feasible joint | 1 2 2 ̈ ̈ accelerations are using a | , | | θ T |≤ 0 . 5 rad/s θ . Find the fastest motion time 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

367 Chapter 9. Trajectory Generation 349 cubic time scaling that satisfies the joint velocity and acceleration limits. Exercise 9.5 ( T ) = Find the fifth-order polynomial time scaling that satisfies s s (0) = ̇ (0) = ̈ s (0) = ̇ s s T ) = ̈ s ( T ) = 0. 1 and ( As a function of the total time of motion T , find the times at Exercise 9.6 s of the fifth-order polynomial point-to-point time scaling which the acceleration ̈ is a maximum or a minimum. Exercise 9.7 If you want to use a polynomial time scaling for point-to-point motion with zero initial and final velocities, accelerations, and jerks, what would be the minimum order of the polynomial? Exercise 9.8 Prove that the trapezoidal time scaling, using the maximum and velocity a , minimizes the time of motion T . allowable acceleration v Plot by hand the acceleration profile ̈ Exercise 9.9 ( t ) for a trapezoidal time s scaling. Exercise 9.10 If v and a are specified for a trapezoidal time scaling of a 2 v 1 is a necessary condition for the robot to reach the /a ≤ robot, prove that maximum velocity v during the path. If and T are specified for a trapezoidal time scaling, prove Exercise 9.11 v that vT > 1 is a necessary condition for the motion to be able to complete in T . Prove that vT ≤ 2 is a necessary condition for a three-stage trapezoidal time motion. Exercise 9.12 a and T are specified for a trapezoidal time scaling, prove If 2 aT ≥ 4 is a necessary condition to ensure that the motion completes in that time. Exercise 9.13 Consider the case where the maximum velocity v is never reached in a trapezoidal time scaling. The motion becomes a bang-bang motion: a for time T/ 2 followed by constant deceleration − a for constant acceleration time T/ 2. Write down the position s ( t ), velocity ̇ s ( t ), and acceleration ̈ s ( t ) for May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

368 350 9.8. Exercises both phases, in analogy to Equations (9.16)–(9.24). Exercise 9.14 ( t ) for an S-curve time Plot by hand the acceleration profile ̈ s scaling. A seven-stage S-curve is fully specified by the time (the Exercise 9.15 t J t duration of a constant positive or negative jerk), the time (the duration of a constant positive or negative acceleration), the time t (the duration of constant v T , the jerk J , the acceleration velocity), the total time , and the velocity v . a Of these seven quantities, how many can be specified independently? A nominal S-curve has seven stages, but it can have fewer if Exercise 9.16 certain inequality constraints are not satisfied. Indicate which cases are possible with fewer than seven stages. Sketch by hand the ̇ s ( t ) velocity profiles for these cases. If the S-curve achieves all seven stages and uses a jerk Exercise 9.17 , an J acceleration a , and a velocity v , what is the constant-velocity coasting time t v in terms of v , a , J , and the total motion time T ? Exercise 9.18 Write your own via-point cubic-polynomial interpolation tra- jectory generator program for a two-dof robot. A new position and velocity specification is required for each joint at 1000 Hz. The user specifies a sequence of via-point positions, velocities, and times, and the program generates an array consisting of the joint angles and velocities at every millisecond from time t = 0 t = T , the total duration of the movement. For a test case with at to time least three via points (one at the start and and one at the end, both with zero velocity, and at least one more via point), plot (a) the path in the joint angle space (similar to Figure 9.7), and (b) the position and velocity of each joint as a function of time (these plots should look similar to Figure 9.8). Exercise 9.19 Via points with specified positions, velocities, and accelerations can be interpolated using fifth-order polynomials of time. For a fifth-order polynomial segment between via points j and j + 1, of duration ∆ T , , with β j j ̇ ̈ ̇ ̈ , β β , specified, solve for the coefficients of the fifth-order β , and β β , +1 +1 j j +1 j j j polynomial (which is similar to Equations (9.26)–(9.29)). A symbolic math May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

369 Chapter 9. Trajectory Generation 351 ̇ s A BC abc 0 s 0 1 Figure 9.16: A, B, and C are candidate integral curves, originating from the dots = 0. Two of the integral s indicated, while a, b, and c are candidate motion cones at ̇ curves and two of the motion cones are incorrect. solver will simplify the problem. By hand or by computer, plot a trapezoidal motion profile in Exercise 9.20 s, the ( s )-plane. ̇ Figure 9.16 shows three candidate motion curves in the ( s ̇ Exercise 9.21 )- s, s = 0 (a, b, and c). plane (A, B, and C) and three candidate motion cones at ̇ Two of the three curves and two of the three motion cones cannot be correct for any robot dynamics. Indicate which are incorrect and explain your reasoning. Explain why the remaining curve and motion cone are possibilities. Exercise 9.22 Under the assumptions of Section 9.4.4, explain why the time- 9.4.2 9.13) is correct. In particular, scaling algorithm of Section (see Figure (a) explain why, in the binary search of step 4, the curve integrated forward s from ( , ̇ s ) must either hit (or run tangent to) the velocity limit curve test lim or hit the ̇ s = 0 axis (and does not hit the curve F , for example); (b) explain why the final time scaling can only touch the velocity limit curve tangentially; and (c) explain why the acceleration switches from minimum to maximum at points where the time scaling touches the velocity limit curve. Exercise 9.23 Explain how the time-scaling algorithm should be modified to handle the case where the initial and final velocities, at s = 0 and s = 1, are May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

370 352 9.8. Exercises nonzero. Exercise 9.24 Explain how the time-scaling algorithm should be modified if the robot’s actuators are too weak to hold it statically at some configurations of the path (the static-posture-maintenance assumption is violated), but the assumptions on inadmissible states and zero-inertia points are satisfied. Valid time scalings may no longer exist. Under what condition(s) should the algo- rithm terminate and indicate that no valid time scaling exists? (Under the 9.4.4 the original algorithm always finds a solution and assumptions of Section therefore does not check for failure cases.) What do the motion cones look like at states ( = 0) where the robot cannot hold itself statically? s, ̇ s Create a computer program that plots the motion cones in Exercise 9.25 the ( s )-plane for a 2R robot in a horizontal plane. The path is a straight s, ̇ θ ,θ line in joint space from ( ) = (0 , 2). Use the dynamics from π/ 2 ,π/ 0) to ( 1 2 g = 0), then rewrite the dynamics in terms of s, ̇ s, Equation (8.9) (with s instead ̈ ̇ ̈ ̇ of θ . The actuators can provide torques in the range − τ θ, ≤ − b θ, θ i i, limit ̇ ≤ τ b > , where − b τ 0 indicates the velocity dependence of the torque. θ i i, i limit The cones should be drawn at a grid of points in ( ̇ s ). To keep the figure s, manageable, normalize each cone ray to the same length. Exercise 9.26 We have been assuming forward motion on a path, ̇ s > 0. What if we allowed backward motion on a path, ̇ s < 0? This exercise involves s, s )-plane, including both drawing motion cones and an integral curve in the ( ̇ positive and negative values of ̇ s . Assume that the maximum acceleration is ( s, ̇ s ) = U > 0 (constant over the ( s, ̇ s )-plane) and the maximum deceleration U U L s, ̇ s ) = L = − ( . You can assume, for example, that U = 1 and L = − 1. is (a) For any constant s , draw the motion cones at the five points where ̇ s takes , the values , − 1 , 0 2 1 , 2 } . {− (b) Assume the motion starts at ( s, ̇ s ) = (0 , 0) and follows the maximum acceleration for time t . Then it follows the maximum deceleration L for U time 2 t . Then it follows U for time t . Sketch by hand the integral curve. (The exact shape does not matter, but the curve should have the correct features.) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

371 Chapter 10 Motion Planning Motion planning is the problem of finding a robot motion from a start state to a goal state that avoids obstacles in the environment and satisfies other constraints, such as joint limits or torque limits. Motion planning is one of the most active subfields of robotics, and it is the subject of entire books. The purpose of this chapter is to provide a practical overview of a few common techniques, using robot arms and mobile robots as the primary example systems (Figure 10.1). The chapter begins with a brief overview of motion planning. This is followed by foundational material including configuration space obstacles and graph search. We conclude with summaries of several different planning methods. Overview of Motion Planning 10.1 A key concept in motion planning is configuration space, or C-space for short. C corresponds to a unique configuration q of the Every point in the C-space robot, and every configuration of the robot can be represented as a point in C-space. For example, the configuration of a robot arm with joints can be n n joint positions, free C-space = ( θ ,...,θ represented as a list of ). The q n 1 C consists of the configurations where the robot neither penetrates an obstacle free nor violates a joint limit. In this chapter, unless otherwise stated, we assume that is an n -vector and q n C ⊂ R . With some generalization, the concepts of this chapter apply to that non-Euclidean C-spaces such as C = SE (3). The control inputs available to drive the robot are written as an -vector m m ∈ U ⊂ R u , where m = n for a typical robot arm. If the robot has second- 353

372 354 10.1. Overview of Motion Planning (Left) A robot arm executing an obstacle-avoiding motion plan. The Figure 10.1: motion plan was generated using MoveIt! [180] and visualized using rviz in ROS (the Robot Operating System). (Right) A car-like mobile robot executing parallel parking. order dynamics, such as that for a robot arm, and the control inputs are forces (equivalently, accelerations), the of the robot is defined by its configuration state n = ̇ q,v ∈ X . For q = ( R x , typically we write v ) q . If we can and velocity, ∈ x is simply the configuration q . treat the control inputs as velocities, the state q ( x ) indicates the configuration q corresponding to the state The notation , x and X = { x | q ( x ) ∈C . } free free The equations of motion of the robot are written x = f ( x,u ) (10.1) ̇ or, in integral form, ∫ T x ( T ) = x (0) + (10.2) f ( x ( t ) ,u ( t )) dt. 0 10.1.1 Types of Motion Planning Problems With the definitions above, a fairly broad specification of the motion planning problem is the following: x (0) = x Given an initial state and a desired final state x , find a time goal start and a set of controls u : [0 ,T T → U such that the motion (10.2) satisfies ] x ( T ) = x . and q ( x ( t )) ∈C ] for all t ∈ [0 ,T free goal It is assumed that a feedback controller (Chapter 11) is available to ensure that the planned motion x ( t ), t ∈ [0 ,T ], is followed closely. It is also assumed that an accurate geometric model of the robot and environment is available to evaluate C during motion planning. free There are many variations of the basic problem; some are discussed below. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

373 Chapter 10. Motion Planning 355 Path planning versus motion planning. The path planning problem is a subproblem of the general motion planning problem. Path planning is the s ) ,s q [0 , 1], ( purely geometric problem of finding a collision-free path ∈ (0) = q to a goal configuration from a start configuration q (1) = q q , goal start without concern for the dynamics, the duration of motion, or constraints on the motion or on the control inputs. It is assumed that the path returned by the path planner can be time scaled to create a feasible tra- jectory (Chapter 9). This problem is sometimes called the piano mover’s problem , emphasizing the focus on the geometry of cluttered spaces. m = n versus m < n . If there are fewer control inputs m Control inputs: n than degrees of freedom , then the robot is incapable of following many = 3 (the paths, even if they are collision-free. For example, a car has n position and orientation of the chassis in the plane) but m = 2 (forward– backward motion and steering); it cannot slide directly sideways into a parking space. Online versus offline. A motion planning problem requiring an immediate re- sult, perhaps because obstacles appear, disappear, or move unpredictably, calls for a fast, online, planner. If the environment is static then a slower offline planner may suffice. Optimal versus satisficing. In addition to reaching the goal state, we might want the motion plan to minimize (or approximately minimize) a cost J , e.g., ∫ T J = ,u L ( t ) x ( t )) dt. ( 0 L = 1 yields a time-optimal motion while For example, minimizing with T ) u L minimizing with t = u ( t ) yields a “minimum-effort” motion. ( Exact versus approximate. We may be satisfied with a final state x ( T ) that . is sufficiently close to x , e.g., ‖ x ( T ) − x <  ‖ goal goal With or without obstacles. The motion planning problem can be challeng- ing even in the absence of obstacles, particularly if m < n or optimality is desired. 10.1.2 Properties of Motion Planners Planners must conform to the properties of the motion planning problem as outlined above. In addition, planners can be distinguished by the following properties. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

374 356 10.1. Overview of Motion Planning If the robot is being asked Multiple-query versus single-query planning. to solve a number of motion planning problems in an unchanging envi- ronment, it may be worth spending the time building a data structure C . This data structure can then be searched that accurately represents free to solve multiple planning queries efficiently. Single-query planners solve each new problem from scratch. An anytime planner is one that continues to look for “Anytime” planning. better solutions after a first solution is found. The planner can be stopped at any time, for example when a specified time limit has passed, and the best solution returned. A motion planner is said to be Completeness. if it is guaranteed to complete find a solution in finite time if one exists, and to report failure if there is no feasible motion plan. A weaker concept is resolution completeness . A planner is resolution complete if it is guaranteed to find a solution if one exists at the resolution of a discretized representation of the problem, such as the resolution of a grid representation of C . Finally, a planner free is probabilistically complete if the probability of finding a solution, if one exists, tends to 1 as the planning time goes to infinity. Computational complexity. The computational complexity refers to charac- terizations of the amount of time the planner takes to run or the amount of memory it requires. These are measured in terms of the description of the planning problem, such as the dimension of the C-space or the number of vertices in the representation of the robot and obstacles. For example, the n , the dimension of the time for a planner to run may be exponential in C-space. The computational complexity may be expressed in terms of the average case or the worst case. Some planning algorithms lend themselves easily to computational complexity analysis, while others do not. 10.1.3 Motion Planning Methods There is no single planner applicable to all motion planning problems. Below is a broad overview of some of the many motion planners available. Details are left to the sections indicated. Complete methods (Section 10.3). These methods focus on exact repre- sentations of the geometry or topology of C , ensuring completeness. free For all but simple or low-degree-of-freedom problems, these representa- tions are mathematically or computationally prohibitive to derive. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

375 Chapter 10. Motion Planning 357 These methods discretize C into a grid and Grid methods (Section 10.4). free q to a grid point in the goal region. search the grid for a motion from start Modifications of the approach may discretize the state space or control space or they may use multi-scale grids to refine the representation of C free near obstacles. These methods are relatively easy to implement and can return optimal solutions but, for a fixed resolution, the memory required to store the grid, and the time to search it, grow exponentially with the number of dimensions of the space. This limits the approach to low- dimensional problems. Sampling methods (Section 10.5). A generic sampling method relies on a random or deterministic function to choose a sample from the C-space X ; a or state space; a function to evaluate whether the sample is in free function to determine the “closest” previous free-space sample; and a lo- cal planner to try to connect to, or move toward, the new sample from the previous sample. This process builds up a graph or tree representing feasible motions of the robot. Sampling methods are easy to implement, tend to be probabilistically complete, and can even solve high-degree-of- freedom motion planning problems. The solutions tend to be satisficing, not optimal, and it can be difficult to characterize the computational com- plexity. Virtual potential fields (Section Virtual potential fields create forces 10.6). on the robot that pull it toward the goal and push it away from obstacles. The approach is relatively easy to implement, even for high-degree-of- freedom systems, and fast to evaluate, often allowing online implementa- tion. The drawback is local minima in the potential function: the robot may get stuck in configurations where the attractive and repulsive forces cancel but the robot is not at the goal state. Nonlinear optimization (Section 10.7). The motion planning problem can be converted to a nonlinear optimization problem by representing the path or controls by a finite number of design parameters, such as the coefficients of a polynomial or a Fourier series. The problem is to solve for the design parameters that minimize a cost function while satisfying constraints on the controls, obstacles, and goal. While these methods can produce near- optimal solutions, they require an initial guess at the solution. Because the objective function and feasible solution space are generally not convex, the optimization process can get stuck far away from a feasible solution, let alone an optimal solution. Smoothing (Section 10.8). Often the motions found by a planner are jerky. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

376 358 10.2. Foundations A smoothing algorithm can be run on the result of the motion planner to improve the smoothness. A major trend in recent years has been toward sampling methods, which are easy to implement and can handle high-dimensional problems. Foundations 10.2 Before discussing motion planning algorithms, we establish concepts used in many of them: configuration space obstacles, collision detection, graphs, and graph search. 10.2.1 Configuration Space Obstacles Determining whether a robot at a configuration is in collision with a known q environment generally requires a complex operation involving a CAD model of the environment and robot. There are a number of free and commercial software packages that can perform this operation, and we will not delve into them here. For our purposes, it is enough to know that the workspace obstacles partition the configuration space C into two sets, the free space C and the obstacle free C . Joint limits are treated as obstacles in the , where space = C ∪C C free obs obs configuration space. With the concepts of and C , the path planning problem reduces to C free obs C the problem of finding a path for a point robot among the obstacles . If obs C into separate the obstacles break , and q and connected components free start q do not lie in the same connected component, then there is no collision-free goal path. The explicit mathematical representation of a C-obstacle can be exceedingly complex, and for that reason C-obstacles are rarely represented exactly. Despite this, the concept of C-obstacles is very important for understanding motion planning algorithms. The ideas are best illustrated by examples. 10.2.1.1 A 2R Planar Arm 10.2 shows a 2R planar robot arm, with configuration q = ( θ ), among ,θ Figure 2 1 obstacles A, B, and C in the workspace. The C-space of the robot is represented ≤ θ < 2 π . Remember from ≤ θ by a portion of the plane with 0 < 2 π , 0 1 2 Chapter 2, however, that the topology of the C-space is a torus (or doughnut) θ = 2 π is connected to the edge θ since the edge of the square at = 0; similarly, 1 1 2 = 2 π is connected to θ = 0. The square region of R is obtained by slicing θ 2 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

377 Chapter 10. Motion Planning 359 π 2 d en d en A 10 θ 2 10 C B t star 7 A 7 θ B 2 θ 1 4 A A C 4 star t 0 θ π 0 2 1 Figure 10.2: (Left) The joint angles of a 2R robot arm. (Middle) The arm navigating among obstacles A, B, and C. (Right) The same motion in C-space. Three intermediate points, 4, 7, and 10, along the path are labeled. the surface of the doughnut twice, at θ = 0, and laying it flat on = 0 and θ 2 1 the plane. The C-space on the right in Figure 10.2 shows the workspace obstacles A, B, and C represented as C-obstacles. Any configuration lying inside a C-obstacle corresponds to penetration of the obstacle by the robot arm in the workspace. A free path for the robot arm from one configuration to another is shown in both the workspace and C-space. The path and obstacles illustrate the topology of the C C-space. Note that the obstacles break into three connected components. free 10.2.1.2 A Circular Planar Mobile Robot Figure 10.3 shows a top view of a circular mobile robot whose configuration is 2 x,y ) ∈ R given by the location of its center, ( . The robot translates (moves with- out rotating) in a plane with a single obstacle. The corresponding C-obstacle is obtained by “growing” (enlarging) the workspace obstacle by the radius of the mobile robot. Any point outside this C-obstacle represents a free config- uration of the robot. Figure 10.4 shows the workspace and C-space for two obstacles, indicating that in this case the mobile robot cannot pass between the two obstacles. 10.2.1.3 A Polygonal Planar Mobile Robot That Translates Figure 10.5 shows the C-obstacle for a polygonal mobile robot translating in the presence of a polygonal obstacle. The C-obstacle is obtained by sliding the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

378 360 10.2. Foundations ) ( x, y ) y x, ( ˆy ˆy ˆx ˆx (b (a ) ) Figure 10.3: (a) A circular mobile robot (open circle) and a workspace obstacle (gray triangle). The configuration of the robot is represented by ( x,y ), the center of the robot. (b) In the C-space, the obstacle is “grown” by the radius of the robot x,y ) configuration outside the bold line is and the robot is treated as a point. Any ( collision-free. Figure 10.4: The “grown” C-space obstacles corresponding to two workspace obsta- cles and a circular mobile robot. The overlapping boundaries mean that the robot cannot move between the two obstacles. robot along the boundary of the obstacle and tracing the position of the robot’s reference point. 10.2.1.4 A Polygonal Planar Mobile Robot That Translates and Rotates Figure 10.6 illustrates the C-obstacle for the workspace obstacle and triangular mobile robot of Figure 10.5 if the robot is now allowed to rotate. The C-space May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

379 Chapter 10. Motion Planning 361 ˆy ˆy ) y ( x, x, ) ( y ˆx ˆx ) (a (b ) Figure 10.5: (a) The configuration of a triangular mobile robot, which can translate but not rotate, is represented by the ( x,y ) location of a reference point. Also shown is a workspace obstacle in gray. (b) The corresponding C-space obstacle (bold outline) is obtained by sliding the robot around the boundary of the obstacle and tracing the position of the reference point. 2 1 is now three dimensional, given by ( ) ∈ R × S . The three-dimensional x,y,θ C-obstacle is the union of two-dimensional C-obstacle slices at angles θ ∈ [0 , 2 π ). Even for this relatively low-dimensional C-space, an exact representation of the C-obstacle is quite complex. For this reason, C-obstacles are rarely described exactly. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

380 362 10.2. Foundations θ ) y x, ( ˆ ˆ θ θ ˆx ˆy ˆx ˆy Figure 10.6: (Top) A triangular mobile robot that can both rotate and translate, rep- resented by the configuration ( x,y,θ ). (Left) The C-space obstacle from Figure 10.5(b) when the robot is restricted to = 0. (Right) The full three-dimensional C-space ob- θ ◦ increments. stacle shown in slices at 10 Distance to Obstacles and Collision Detection 10.2.2 B and a configuration q , let Given a C-obstacle ( q, B ) be the distance between d the robot and the obstacle, where d ( q, B ) > 0 (no contact with the obstacle), d ( B ) = 0 (contact), q, 0 ( d ) < q, (penetration) . B The distance could be defined as the Euclidean distance between the two closest points of the robot and the obstacle, respectively. A distance-measurement algorithm is one that determines d ( q, B ). A collision–detection routine d ( q, B ) ≤ 0 for any C- determines whether i obstacle B . A collision-detection routine returns a binary result and may or i may not utilize a distance-measurement algorithm at its core. One popular distance-measurement algorithm is the Gilbert–Johnson–Keerthi (GJK) algorithm, which efficiently computes the distance between two convex bodies, possibly represented by triangular meshes. Any robot or obstacle can be treated as the union of multiple convex bodies. Extensions of this algorithm are May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

381 Chapter 10. Motion Planning 363 A lamp represented by spheres. The approximation improves as the Figure 10.7: number of spheres used to represent the lamp increases. Figure from [61] used with permission. used in many distance-measurement algorithms and collision-detection routines for robotics, graphics, and game-physics engines. A simpler approach is to approximate the robot and obstacles as unions of overlapping spheres. Approximations must always be conservative – the approximation must cover all points of the object – so that if a collision-detection , then we are guaranteed that the actual q routine indicates a free configuration geometry is collision-free. As the number of spheres in the representation of the robot and obstacles increases, the closer the approximations come to the actual geometry. An example is shown in Figure 10.7. q represented by k spheres of radius R Given a robot at centered at r ), ( q i i = 1 ,...,k , and an obstacle B i ` spheres of radius B centered represented by j at , j = 1 ,...,` , the distance between the robot and the obstacle can be b j calculated as d ( q, B ) = min . ‖ r B ( q ) − b − ‖− R i i j j i,j Apart from determining whether a particular configuration of the robot is in collision, another useful operation is determining whether the robot collides during a particular motion segment. While exact solutions have been developed for particular object geometries and motion types, the general approach is to sample the path at finely spaced points and to “grow” the robot to ensure that if two consecutive configurations are collision-free for the grown robot then the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

382 364 10.2. Foundations root a a a 2.5 1.1 3 5 1 2 2 cd cd b b b c d 4 8 3 8 f g e e e h (a) (b) (c) (a) A weighted digraph. (b) A weighted undirected graph. (c) A tree. Figure 10.8: The leaves are shaded gray. volume swept out by the actual robot between the two configurations is also collision-free. Graphs and Trees 10.2.3 Many motion planners explicitly or implicitly represent the C-space or state . A graph consists of a collection of nodes N and a collection space as a graph E , where each edge e connects two nodes. In motion planning, a node of edges typically represents a configuration or state while an edge between nodes and n 1 indicates the ability to move from n n n without penetrating an obstacle to 2 1 2 or violating other constraints. A graph can be either directed or undirected . In an undirected graph, each edge is bidirectional: if the robot can travel from n to n then it can also 2 1 n travel from to n for short, each edge allows . In a directed graph, or digraph 1 2 travel in only one direction. The same two nodes can have two edges between them, allowing travel in opposite directions. weighted or unweighted Graphs can also be . In a weighted graph, each edge has a positive cost associated with traversing it. In an unweighted graph each edge has the same cost (e.g., 1). Thus the most general type of graph we consider is a weighted digraph. A tree is a digraph in which (1) there are no cycles and (2) each node has at most one parent node (i.e., at most one edge leading to the node). A tree has one node with no parents and a number of leaf nodes with no child . root A digraph, undirected graph, and tree are illustrated in Figure 10.8. N × N A nodes, any graph can be represented by a matrix N Given , where R ∈ a to node of the matrix represents the cost of the edge from node i element ij j ; a zero or negative value indicates no edge between the nodes. Graphs and May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

383 Chapter 10. Motion Planning 365 trees can be represented more compactly as a list of nodes, each with links to its neighbors. 10.2.4 Graph Search Once the free space is represented as a graph, a motion plan can be found by searching the graph for a path from the start to the goal. One of the most ∗ powerful and popular graph search algorithms is (pronounced “A star”) A search. ∗ A 10.2.4.1 Search ∗ A search algorithm efficiently finds a minimum-cost path on a graph when The the cost of the path is simply the sum of the positive edge costs along the path. N = { 1 ,...,N Given a graph described by a set of nodes , where node 1 is } ∗ , the A the start node, and a set of edges algorithm makes use of the following E data structures: • OPEN of the nodes from which exploration is still to be done, a sorted list CLOSED of nodes for which exploration has already taken place; and a list • cost[node1,node2] encoding the set of edges, where a positive a matrix value corresponds to the cost of moving from node1 to node2 (a negative value indicates that no edge exists); • an array of the minimum cost found so far to reach past_cost[node] node node from the start node; and • a search tree defined by an array parent[node] , which contains for each a link to the node preceding it in the shortest path found so far from node the start node to that . node To initialize the search, the matrix cost is constructed to encode the edges, the list OPEN is initialized to the start node 1, the cost to reach the start node ( past_cost[1] ) is initialized to 0, and past_cost[node] for node ∈{ 2 ,...,N } is initialized to infinity (or a large number), indicating that currently we have no idea of the cost of reaching those nodes. At each step of the algorithm, the first node in OPEN is removed from OPEN and called current . The node current is also added to CLOSED . The first node in OPEN is one that minimizes the total estimated cost of the best path to the goal that passes through that node. The estimated cost is calculated as May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

384 366 10.2. Foundations est_total_cost[node] = past_cost[node] + heuristic_cost_to_go(node) ≥ 0 is an optimistic (underestimating) where heuristic_cost_to_go(node) . For many path planning node estimate of the actual cost-to-go to the goal from problems, an appropriate choice for the heuristic is the straight-line distance to the goal, ignoring any obstacles. is a list sorted according to the estimated total cost, inserting Because OPEN entails a small computational price. a new node at the correct location in OPEN If the node current is in the goal set then the search is finished and the parent links. If not, for each neighbor nbr of path is reconstructed from the in the graph which is not also in current , the tentative_past_cost CLOSED cost[current] + cost[current,nbr] . If nbr past for is calculated as tentative_past_cost < past_cost[nbr] , nbr then past_cost[nbr] can be reached with less cost than previously known, so is set to tentative_past_cost and parent[nbr] is set to current . The node nbr is then added (or moved) in OPEN according to its estimated total cost. The algorithm then returns to the beginning of the main loop, removing the first node from and calling it current . If OPEN is empty then there is no OPEN solution. ∗ The algorithm is guaranteed to return a minimum-cost path, as nodes A are only checked for inclusion in the goal set when they have the minimum current total estimated cost of all nodes. If the node is in the goal set then is zero and, since all edge costs are positive, heuristic_cost_to_go(current) we know that any path found in the future must have a cost greater than past_cost[current] . Therefore the path to current must be a or equal to shortest path. (There may be other paths of the same cost.) If the heuristic “cost-to-go” is calculated exactly, considering obstacles, then ∗ will explore from the minimum number of nodes necessary to solve the A problem. Of course, calculating the cost-to-go exactly is equivalent to solving the path planning problem, so this is impractical. Instead, the heuristic cost-to- go should be calculated quickly and should be as close as possible to the actual cost-to-go to ensure that the algorithm runs efficiently. Using an optimistic cost-to-go ensures an optimal solution. ∗ A algorithm is an example of the general class of The best-first searches, which always explore from the node currently deemed “best” by some measure. ∗ The A search algorithm is described in pseudocode in Algorithm 10.1. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

385 Chapter 10. Motion Planning 367 ∗ A Algorithm 10.1 search. ←{ } OPEN 1 1: ← 0, past_cost[node] ← infinity for node ∈{ 2 ,...,N 2: past_cost[1] } while is not empty do 3: OPEN current first node in OPEN , remove from OPEN 4: ← add 5: to CLOSED current 6: current is in the goal set then if return current 7: SUCCESS and the path to 8: end if for each nbr of current not in CLOSED 9: do 10: ← past_cost[current]+cost[current,nbr] tentative_past_cost past cost < past cost[nbr] then if 11: tentative 12: past_cost[nbr] ← tentative_past_cost 13: parent[nbr] current ← put (or move) nbr OPEN according to 14: in sorted list past_cost[nbr] + ← est_total_cost[nbr] heuristic_cost_to_go(nbr) 15: end if 16: end for end while 17: return 18: FAILURE Other Search Methods 10.2.4.2 Dijkstra’s algorithm. If the heuristic cost-to-go is always estimated as • ∗ zero then always explores from the OPEN node that has been reached A with minimum past cost. This variant is called Dijkstra’s algorithm, which ∗ preceded historically. Dijkstra’s algorithm is also guaranteed to find A a minimum-cost path but on many problems it runs more slowly than ∗ A owing to the lack of a heuristic look-ahead function to help guide the search. Breadth-first search. • E has the same cost, Dijkstra’s If each edge in algorithm reduces to breadth-first search. All nodes one edge away from the start node are considered first, then all nodes two edges away, etc. The first solution found is therefore a minimum-cost path. ∗ • A search. If the heuristic cost-to-go is overestimated by Suboptimal ∗ η > 1, the A multiplying the optimistic heuristic by a constant factor search will be biased to explore from nodes closer to the goal rather than nodes with a low past cost. This may cause a solution to be found more May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

386 368 10.3. Complete Path Planners quickly but, unlike the case of an optimistic cost-to-go heuristic, the so- ∗ A lution will not be guaranteed to be optimal. One possibility is to run with an inflated cost-to-go to find an initial solution, then rerun the search η until the time allotted for the search with progressively smaller values of η has expired or a solution is found with = 1. Complete Path Planners 10.3 . Complete path planners rely on an exact representation of the free C-space C free These techniques tend to be mathematically and algorithmically sophisticated, and impractical for many real systems, so we do not delve into them in detail. One approach to complete path planning, which we will see in modified form in Section 10.5, is based on representing the complex high-dimensional by a one-dimensional space roadmap R with the following properties: C free ′ . From every point q ∈ C (a) , a free path to a point q Reachability ∈ R free can be found trivially (e.g., a straight-line path). . For each connected component of , there is one con- Connectivity C (b) free . nected component of R q With such a roadmap, the planner can find a path between any two points start by simply finding paths from q in the same connected component of C and goal free ′ ′ ′ to q q ∈ R , from a point q , and from to ∈ R q to a point q goal start start start goal ′ and R . If a path can be found trivially between q q on the roadmap start goal q , the roadmap may not even be used. goal C While constructing a roadmap of is complex in general, some problems free admit simple roadmaps. For example, consider a polygonal robot translating 10.5, the among polygonal obstacles in the plane. As can be seen in Figure C-obstacles in this case are also polygons. A suitable roadmap is the weighted undirected , with nodes at the vertices of the C-obstacles and visibility graph edges between the nodes that can “see” each other (i.e., the line segment between the vertices does not intersect an obstacle). The weight associated with each edge is the Euclidean distance between the nodes. ∗ Not only is this a suitable roadmap , but it allows us to use an A search R to find a shortest path between any two configurations in the same connected component of C , as the shortest path is guaranteed either to be a straight free line from to a node to q q q or to consist of a straight line from goal start start ′ ′ , and a path along the ∈ R , a straight line from a node q q q ∈ R to goal start goal ′ ′ from q to 10.9). Note that the shortest straight edges of q R (Figure start goal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

387 Chapter 10. Motion Planning 369 goal start (a) (b) (c) (d) (e) (f) (a) The start and goal configurations for a square mobile robot (ref- Figure 10.9: erence point shown) in an environment with a triangular and a rectangular obstacle. (b) The grown C-obstacles. (c) The visibility graph roadmap of C . (d) The full R free graph consists of plus nodes at q , along with the links connecting these and q R start goal nodes to the visible nodes of R . (e) Searching the graph results in the shortest path, shown in bold. (f) The robot is shown traversing the path. path requires the robot to graze the obstacles, so we implicitly treat C as free including its boundary. Grid Methods 10.4 ∗ A requires a discretization of the search space. The A search algorithm like simplest discretization of C-space is a grid. For example, if the configuration n -dimensional and we desire k grid points along each dimension, the space is n k grid points. C-space is represented by ∗ The A algorithm can be used as a path planner for a C-space grid, with the following minor modifications: • The definition of a “neighbor” of a grid point must be chosen: is the robot constrained to move in axis-aligned directions in configuration space or can it move in multiple dimensions simultaneously? For example, for a two-dimensional C-space, neighbors could be 4-connected (on the cardi- nal points of a compass: north, south, east, and west) or 8-connected May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

388 370 10.4. Grid Methods 10.10(a). If diagonal motions are (diagonals allowed), as shown in Figure allowed, the cost to diagonal neighbors should be penalized appropriately. For example, the cost to a north, south, east or west neighbor could be 1, √ 2. If integers are desired, while the cost to a diagonal neighbor could be for efficiency of the implementation, the approximate costs 5 and 7 could be used. • If only axis-aligned motions are used, the heuristic cost-to-go should be , not the Euclidean distance. The based on the Manhattan distance Manhattan distance counts the number of “city blocks” that must be traveled, with the rule that diagonals through a block are not possible (Figure 10.10(b)). A node nbr is added to • only if the step from current to nbr is OPEN collision-free. (The step may be considered collision-free if a grown version nbr does not intersect any obstacles.) of the robot at • Other optimizations are possible, owing to the known regular structure of the grid. ∗ An grid-based path planner is resolution-complete: it will find a solution A if one exists at the level of discretization of the C-space. The path will be a shortest path subject to the allowed motions. Figure 10.10(c) illustrates grid-based path planning for the 2R robot example of Figure 10.2. The C-space is represented as a grid with k = 32, i.e., there is a ◦ ◦ 2 . 25 resolution of 360 for each joint. This yields a total of 32 32 = 11 = 1024 / grid points. The grid-based planner, as described, is a single-query planner: it solves q each path planning query from scratch. However, if the same will be goal used in the same environment for multiple path planning queries, it may be worth preprocessing the entire grid to enable fast path planning. This is the wavefront planner, illustrated in Figure 10.11. Although grid-based path planning is easy to implement, it is only appropri- ate for low-dimensional C-spaces. The reason is that the number of grid points, and hence the computational complexity of the path planner, increases expo- nentially with the number of dimensions n k = 100 . For instance, a resolution n in a C-space with = 3 dimensions leads to k = 1 million grid nodes, while n n = 5 leads to 10 billion grid nodes and n = 7 leads to 100 trillion nodes. An alternative is to reduce the resolution k along each dimension, but this leads to a coarse representation of C-space that may miss free paths. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

389 Chapter 10. Motion Planning 371 π 2 goal n 4-connected θ Euclidea 2 n Manhatta t star 8-connected 0 θ 2 π 0 1 ) ) (c ) (b (a Figure 10.10: (a) A 4-connected grid point and an 8-connected grid point for a space = 2. (b) Grid points spaced at unit intervals. The Euclidean distance between the n √ two points indicated is 5 while the Manhattan distance is 3. (c) A grid representation of the C-space and a minimum-length Manhattan-distance path for the problem of Figure 10.2. 9 9 8 8 4 7 10 6 6 5 5 3 4 10 7 2 7 4 3 11 3 8 9 2 3 13 6 12 7 8 2 1 14 12 7 13 0 1 2 1 5 6 4 3 2 13 2 1 3 8 2 11 12 12 2 4 11 3 3 10 9 4 4 8 5 7 9 10 6 10 3 Figure 10.11: A wavefront planner on a two-dimensional grid. The goal configuration is given a score of 0. Then all collision-free 4-neighbors are given a score of 1. The process continues, breadth-first, with each free neighbor (that does not have a score already) assigned the score of its parent plus 1. Once every grid cell in the connected component of the goal configuration is assigned a score, planning from any location in the connected component is trivial: at every step, the robot simply moves “downhill” to a neighbor with a lower score. Grid points in collision receive a high score. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

390 372 10.4. Grid Methods original cell subdivision 1 subdivision 2 subdivision 3 original cell subdivision 1 subdivision 2 subdivision 3 Figure 10.12: At the original C-space cell resolution, a small obstacle (indicated by the dark square) causes the whole cell to be labeled an obstacle. Subdividing the cell once shows that at least three-quarters of the cell is actually free. Three levels of subdivision results in a representation using ten total cells: four at subdivision level 3, three at subdivision level 2, and three at subdivision level 1. The cells shaded light gray are the obstacle cells in the final representation. The subdivision of the original cell is shown in the lower panel as a tree, specifically a quadtree, where the leaves of the tree are the final cells in the representation. 10.4.1 Multi-Resolution Grid Representation One way to reduce the computational complexity of a grid-based planner is to use a multi-resolution grid representation of C . Conceptually, a grid point is free considered an obstacle if any part of the rectilinear cell centered on the grid point touches a C-obstacle. To refine the representation of the obstacle, an obstacle cell can be subdivided into smaller cells. Each dimension of the original cell n is split in half, resulting in 2 subcells for an n -dimensional space. Any cells that are still in contact with a C-obstacle are then subdivided further, up to a specified maximum resolution. The advantage of this representation is that only the portions of C-space near obstacles are refined to high resolution, while those away from obstacles are represented by a coarse resolution. This allows the planner to find paths using short steps through cluttered spaces while taking large steps through wide open space. The idea is illustrated in Figure 10.12, which uses only 10 cells to represent an obstacle at the same resolution as a fixed grid that uses 64 cells. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

391 Chapter 10. Motion Planning 373 ̇ q q Sample trajectories emanating from three initial states in the phase Figure 10.13: space of a dynamic system with q ∈ . If the initial state has ̇ q > 0, the trajectory can- R q ) instantaneously. Similarly, not move to the left (corresponding to negative motion in q < if the initial state has ̇ 0, the trajectory cannot move to the right instantaneously. n quadtree , as For = 2, this multi-resolution representation is called a n = 4 cells. For n = 3, each obstacle cell each obstacle cell subdivides into 2 n = 8 cells, and the representation is called an subdivides into 2 octree . The multi-resolution representation of C can be built in advance of the free search or incrementally as the search is being performed. In the latter case, if current to nbr the step from is found to be in collision, the step size can be halved until the step is free or the minimum step size is reached. 10.4.2 Grid Methods with Motion Constraints The above grid-based planners operate under the assumption that the robot can go from one cell to any neighboring cell in a regular C-space grid. This may not be possible for some robots. For example, a car cannot reach, in one step, a “neighbor” cell that is to the side of it. Also, motions for a fast-moving robot arm should be planned in the state space, not just C-space, to take the arm dynamics into account. In the state space, the robot arm cannot move in certain directions (Figure 10.13). Grid-based planners must be adapted to account for the motion constraints of the particular robot. In particular, the constraints may result in a directed graph. One approach is to discretize the robot controls while still making use of a grid on the C-space or state space, as appropriate. Details for a wheeled mobile robot and a dynamic robot arm are described next. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

392 374 10.4. Grid Methods ω ω ω v v v diff-driv er e ca r unicycl obot Figure 10.14: Discretizations of the control sets for unicycle, diff-drive, and car-like robots. Grid-Based Path Planning for a Wheeled Mobile Robot 10.4.2.1 13.3, the controls for simplified models of unicycle, diff- As described in Section v,ω drive, and car-like robots are ( ), i.e., the forward–backward linear velocity and the angular velocity. The control sets for these mobile robots are shown in Figure 10.14. Also shown are proposed discretizations of the controls, as dots. Other discretizations could be chosen. Using the control discretization, we can use a variant of Dijkstra’s algorithm 10.2). to find short paths (Algorithm The search expands from q by integrating forward each control for a time start ∆ t , creating new nodes for the paths that are collision-free. Each node keeps track of the control used to reach the node as well as the cost of the path to the node. The cost of the path to a new node is the sum of the cost of the previous current node, , plus the cost of the action. Integration of the controls does not move the mobile robot to exact grid points. Instead, the C-space grid comes into play in lines 9 and 10. When a node is expanded, the grid cell it sits in is marked “occupied.” Subsequently, any node in this occupied cell will be pruned from the search. This prevents the search from expanding nodes that are close by nodes reached with a lower cost. No more than MAXCOUNT nodes, where MAXCOUNT is a value chosen by the user, are considered during the search. The time ∆ t should be chosen so that each motion step is “small.” The size of the grid cells should be chosen as large as possible while ensuring that t will move the mobile robot outside its integration of any control for a time ∆ current grid cell. The planner terminates when current lies inside the goal region, or when there are no more nodes left to expand (perhaps because of obstacles), or when MAXCOUNT nodes have been considered. Any path found is optimal for the choice of cost function and other parameters to the problem. The planner actually May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

393 Chapter 10. Motion Planning 375 Grid-based Dijkstra planner for a wheeled mobile robot. Algorithm 10.2 ←{ q 1: } OPEN start past_cost[ 0 ] ← 2: q start counter 3: 1 ← while is not empty and counter < MAXCOUNT do 4: OPEN current ← first node in OPEN 5: OPEN , remove from 6: current is in the goal set then if return current 7: SUCCESS and the path to 8: end if if current is not in a previously occupied C-space grid cell then 9: mark grid cell occupied 10: 11: counter ← counter + 1 12: for each control in the discrete control set do 13: integrate control forward a short time ∆ t from current to q new 14: the path to q if is collision-free then new 15: cost of the path to q compute new 16: q in OPEN , sorted by cost place new 17: parent[ q current ] ← new 18: end if end for 19: end if 20: end while 21: return FAILURE 22: runs faster in somewhat cluttered spaces, as the obstacles help to guide the exploration. Some examples of motion plans for a car are shown in Figure 10.15. 10.4.2.2 Grid-Based Motion Planning for a Robot Arm One method for planning the motion for a robot arm is to decouple the problem into a path planning problem followed by a time scaling of the path: (a) Apply a grid-based or other path planner to find an obstacle-free path in C-space. (b) Time scale the path to find the fastest trajectory that respects the robot’s dynamics, as described in Section 9.4, or use any less aggressive time scaling. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

394 376 10.4. Grid Methods goal start goal start (Left) A minimum-cost path for a car-like robot where each action has Figure 10.15: identical cost, favoring a short path. (Right) A minimum-cost path where reversals are penalized. Penalizing reversals requires a modification to Algorithm 10.2. Since the motion planning problem is broken into two steps (path planning fol- lowed by time scaling), the resultant motion will not be time-optimal in general. Another approach is to plan directly in the state space. Given a state ( ̇ q ) q, ( of the robot arm, let ̇ q ) represent the set of accelerations that are feasible A q, on the basis of the limited joint torques. To discretize the controls, the set A ( q, ̇ q ) is intersected with a grid of points of the form n ∑ , ca ˆe i i i =1 is an integer, a where > 0 is the acceleration step size in the ̈ q c -direction, and i i ˆe is a unit vector in the i th direction (Figure 10.16). i As the robot moves, the acceleration set ( q, ̇ q ) changes but the grid remains A fixed. Because of this, and assuming a fixed integration time ∆ at each “step” t in a motion plan, the reachable states of the robot (after any integral number of steps) are confined to a grid in state space. To see this, consider a single joint angle of the robot, , and assume for simplicity zero initial velocity, ̇ q (0) = 0. q 1 1 The velocity at timestep k takes the form t, ̇ ( k ) = ̇ q ∆ ( k − 1) + c ( k ) a q 1 1 1 c ) is any value in a finite set of integers. By induction, the velocity at where k ( is an integer. The position any timestep must be of the form a ∆ t , where k k v 1 v at timestep k takes the form 1 2 t k ) = q q ( k − 1) + ̇ q ( ( k − 1)∆ + a c ( k ) . (∆ t ) 1 1 1 1 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

395 Chapter 10. Motion Planning 377 q ̈ 2 a 1 a 2 ̇ q ) ( q, A q ̈ 1 The instantaneously available acceleration set A ( q, ̇ q ) for a two-joint Figure 10.16: a , gives the discretized in ̈ q and robot, intersected with a grid spaced at a q in ̈ 2 1 1 2 control actions (shown as larger dots). Substituting the velocity from the previous equation, we find that the position 2 is an integer. k k (∆ t ) a at any timestep must be of the form 2 + q (0), where / 1 p p 1 To find a trajectory from a start node to a goal set, a breadth-first search can be employed to create a search tree on the state space nodes. When exploration is made from a node ( q, ̇ q ) in the state space, the set A ( q, ̇ q ) is evaluated to find the discrete set of control actions. New nodes are created by integrating the control actions for time ∆ . A node is discarded if the path to it is in collision t or if it has been reached previously (i.e., by a trajectory taking the same or less time). Because the joint angles and angular velocities are bounded, the state space grid is finite and therefore it can be searched in finite time. The planner is resolution-complete and returns a time-optimal trajectory, subject to the reso- lution specified in the control grid and timestep ∆ t . a The control-grid step sizes must be chosen small enough that A ( q, ̇ q ), for i any feasible state ( q, ̇ q ), contains a representative set of points of the control grid. Choosing a finer grid for the controls, or a smaller timestep ∆ , creates a t finer grid in the state space and a higher likelihood of finding a solution amidst obstacles. It also allows the choice of a smaller goal set while keeping points of the state space grid inside the set. Finer discretization comes at a computational cost. If the resolution of the control discretization is increased by a factor r in each dimension (i.e., each a is i τ a , the computation /r ), and the timestep size is divided by a factor reduced to i time spent growing the search tree for a given robot motion duration increases May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

396 378 10.5. Sampling Methods nτ r n is the number of joints. For example, increasing the by a factor , where = 2 and decreasing the timestep by a factor r control-grid resolution by a factor 3 × 4 = 4 for a three-joint robot results in a search that is likely to take 2 = 4096 τ times longer to complete. The high computational complexity of the planner makes it impractical beyond a few degrees of freedom. The description above ignores one important issue: the feasible control set A ( q, ̇ q ) changes during a timestep, so the control chosen at the beginning of the timestep may no longer be feasible by the end of the timestep. For that reason, ̃ a conservative approximation ( q, ̇ q ) ⊂ A ( q, ̇ A ) should be used instead. This q set should remain feasible over the duration of a timestep regardless of which control action is chosen. How to determine such a conservative approximation ̃ A ( q, ̇ q ) is beyond the scope of this chapter, but it has to do with bounds on how rapidly the arm’s mass matrix M ( q ) changes with q and how fast the robot is ̃ moving. At low speeds ̇ t , the conservative set q A ( q, ̇ q ) is and short durations ∆ A ( q, ̇ q ). very close to 10.5 Sampling Methods Each grid-based method discussed above delivers optimal solutions subject to the chosen discretization. A drawback of these approaches, however, is their high computational complexity, making them unsuitable for systems having more than a few degrees of freedom. A different class of planners, known as sampling methods, relies on a random or deterministic function to choose a sample from the C-space or state space: a function to evaluate whether a sample or motion is in ; a function to X free determine nearby previous free-space samples; and a simple local planner to try to connect to, or move toward, the new sample. These functions are used to build up a graph or tree representing feasible motions of the robot. Sampling methods generally give up on the resolution-optimal solutions of a grid search in exchange for the ability to find satisficing solutions quickly in high-dimensional state spaces. The samples are chosen to form a roadmap or search tree that quickly approximates the free space X using fewer samples free than would typically be required by a fixed high-resolution grid, where the number of grid points increases exponentially with the dimension of the search space. Most sampling methods are probabilistically complete: the probability of finding a solution, when one exists, approaches 100% as the number of samples goes to infinity. Two major classes of sampling methods are rapidly exploring random trees (RRTs) and probabilistic roadmaps (PRMs). The former use a tree represen- tation for single-query planning in either C-space or state space, while PRMs May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

397 Chapter 10. Motion Planning 379 are primarily C-space planners that create a roadmap graph for multiple-query planning. The RRT Algorithm 10.5.1 The RRT algorithm searches for a collision-free motion from an initial state X x . It is applied to kinematic problems, where the state to a goal set x start goal is simply the configuration q , as well as to dynamic problems, where the state as outlined includes the velocity. The basic RRT grows a single tree from x start in Algorithm 10.3. Algorithm 10.3 RRT algorithm. T x initialize search tree 1: with start T is less than the maximum tree size do 2: while x sample from ← 3: X samp x nearest node in T to x 4: ← nearest samp x to x in 5: employ a local planner to find a motion from nearest new x the direction of samp 6: the motion is collision-free then if x add x 7: T with an edge from x to to nearest new new 8: if x then is in X goal new return SUCCESS and the motion to 9: x new end if 10: 11: end if end while 12: 13: return FAILURE x is simply q In a typical implementation for a kinematic problem (where ), the sampler in line 3 chooses x randomly from an almost-uniform distribu- samp x , with a slight bias toward states in X tion over . The closest node X nearest goal in the search tree T (line 4) is the node minimizing the Euclidean distance to x from . The state x d (line 5) is chosen as the state a small distance new samp on the straight line to x x . Because d is small, a very simple local nearest samp planner, e.g., one that returns a straight-line motion, will often find a motion connecting x is to x x . If the motion is collision-free, the new state new nearest new added to the search tree T . The net effect is that the nearly uniformly distributed samples “pull” the tree toward them, causing the tree to rapidly explore X . An example of the free effect of this pulling action on exploration is shown in Figure 10.17. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

398 380 10.5. Sampling Methods (Left) A tree generated by applying a uniformly-distributed random Figure 10.17: motion from a randomly chosen tree node does not explore very far. (Right) A tree generated by the RRT algorithm using samples drawn randomly from a uniform dis- tribution. Both trees have 2000 nodes. Figure from [84] used with permission. The basic algorithm leaves the programmer with many choices: how to sam- ple from (line 3), how to define the “nearest” node in T (line 4), and how to X x (line 5). Even a small change plan the motion to make progress toward samp to the sampling method, for example, can yield a dramatic change in the run- ning time of the planner. A wide variety of planners have been proposed in the literature based on these choices and other variations. Some of these variations are described below. 10.5.1.1 Line 3: The Sampler The most obvious sampler is one that samples randomly from a uniform distri- n . This is straightforward for Euclidean C-spaces R bution over , as well as X 1 1 n ( T n ×···× S for S n times), where we can choose = -joint robot C-spaces 2 1 × S R for a a uniform distribution over each joint angle, and for the C-space 2 R mobile robot in the plane, where we can choose a uniform distribution over 1 S and individually. The notion of a uniform distribution on some other curved C-spaces, for example (3), is less straightforward. SO For dynamic systems, a uniform distribution over the state space can be defined as the cross product of a uniform distribution over C-space and a uniform distribution over a bounded velocity set. Although the name “rapidly-exploring random trees” is derived from the idea of a random sampling strategy, in fact the samples need not be gener- ated randomly. For example, a deterministic sampling scheme that generates a progressively finer (multi-resolution) grid on X could be employed instead. To reflect this more general view, the approach has been called rapidly-exploring May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

399 Chapter 10. Motion Planning 381 Which of the three dashed configurations of the car is “closest” to the Figure 10.18: configuration in gray? (RDTs), emphasizing the key point that the samples should even- dense trees tually become dense in the state space (i.e., as the number of samples goes to infinity, the samples become arbitrarily close to every point in X ). 10.5.1.2 Line 4: Defining the Nearest Node X . For an Finding the “nearest” node depends on a definition of distance on n = , a natural choice for the distance C unconstrained kinematic robot on R between two points is simply the Euclidean distance. For other spaces, the choice is less obvious. 1 2 × R , which configura- As an example, for a car-like robot with a C-space S x tion is closest to the configuration : one that is rotated 20 degrees relative samp to x , one that is 2 meters straight behind it, or one that is 1 meter straight samp to the side of it (Figure 10.18)? Since the motion constraints prevent spinning in place or moving directly sideways, the configuration that is 2 meters straight behind is best positioned to make progress toward x . Thus defining a notion samp of distance requires combining components of different units (e.g., degrees, meters, degrees/s, • meters/s) into a single distance measure; and taking into account the motion constraints of the robot. • The closest node should perhaps be defined as the one that can reach x nearest x the fastest, but computing this is as hard as solving the motion planning samp problem. A simple choice of a distance measure from x to x is the weighted sum samp x − x . The weights of the distances along the different components of samp express the relative importance of the different components. If more is known about the set of states that the robot can reach from a state x in limited time, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

400 382 10.5. Sampling Methods this information can be used in determining the nearest node. In any case, the nearest node should be computed quickly. Finding a nearest neighbor is a common problem in computational geometry, and various algorithms, such as d trees and hashing, can be used to solve it efficiently. k 10.5.1.3 Line 5: The Local Planner The job of the local planner is to find a motion from x to some point nearest x x which is closer to . The planner should be simple and it should run new samp quickly. Three examples are as follows. A straight-line planner. The plan is a straight line to x , which may be new x on the straight line to or at a fixed distance chosen at from x d nearest samp x . This is suitable for kinematic systems with no motion constraints. samp Discretized controls planner. For systems with motion constraints, such as wheeled mobile robots or dynamic systems, the controls can be discretized { u , as in the grid methods with motion con- ,u into a discrete set ,... } 1 2 straints (Section 10.4.2 and Figures 10.14 and 10.16). Each control is integrated from x ). Among the for a fixed time ∆ t using ̇ x = f ( x,u nearest new states reached without collision, the state that is closest to is x samp x . chosen as new For a wheeled mobile robot, local plans can be Wheeled robot planners. found using Reeds–Shepp curves, as described in Section 13.3.3. Other robot-specific local planners can be designed. Other RRT Variants 10.5.1.4 The performance of the basic RRT algorithm depends heavily on the choice of sampling method, distance measure, and local planner. Beyond these choices, two other variants of the basic RRT are outlined below. Bidirectional RRT. The bidirectional RRT grows two trees: one “forward” from x and one “backward” from x . The algorithm alternates between goal start growing the forward tree and growing the backward tree, and every so often x from the other tree. it attempts to connect the two trees by choosing samp The advantage of this approach is that a single goal state x can be reached goal exactly, rather than just a goal set X . Another advantage is that, in many goal environments, the two trees are likely to find each other much more quickly than a single “forward” tree will find a goal set. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

401 Chapter 10. Motion Planning 383 The major problem is that the local planner might not be able to connect the two trees exactly. For example, the discretized controls planner of Sec- tion 10.5.1.3 is highly unlikely to create a motion exactly to a node in the other tree. In this case, the two trees may be considered more or less connected when points on each tree are sufficiently close. The “broken” discontinuous trajectory 10.8). can be returned and patched by a smoothing method (Section ∗ The basic RRT algorithm returns SUCCESS once a motion to X RRT is . goal found. An alternative is to continue running the algorithm and to terminate the search only when another termination condition is reached (e.g., a maximum running time or a maximum tree size). Then the motion with the minimum cost can be returned. In this way, the RRT solution may continue to improve as time goes by. Because edges in the tree are never deleted or changed, however, the RRT generally does not converge to an optimal solution. ∗ algorithm is a variation on the single-tree RRT that continually The RRT rewires the search tree to ensure that it always encodes the shortest path from to each node in the tree. The basic approach works for C-space path x start planning with no motion constraints, allowing exact paths from any node to any other node. ∗ To modify the RRT to the RRT 7 of the RRT algorithm, which inserts , line , is replaced by a test of all the nodes T with an edge from x to x x in new new nearest x ∈ X in T that are sufficiently near to x is created . An edge to x new near new from the x ∈X by the local planner that (1) has a collision-free motion and near (2) minimizes the total cost of the path from x to x , not just the cost of new start x ∈X the added edge. The total cost is the cost to reach the candidate plus near the cost of the new edge. The next step is to consider each x ∈X to see whether it could be reached near at lower cost by a motion through x . . If so, the parent of x is changed to x new new In this way, the tree is incrementally rewired to eliminate high-cost motions in favor of the minimum-cost motions available so far. X depends on the number of samples in the tree, details The definition of near of the sampling method, the dimension of the search space, and other factors. ∗ approaches the optimal Unlike the RRT, the solution provided by RRT ∗ solution as the number of sample nodes increases. Like the RRT, the RRT algorithm is probabilistically complete. Figure 10.19 demonstrates the rewiring ∗ 2 R C = behavior of RRT compared to that of RRT for a simple example in . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

402 384 10.5. Sampling Methods 10 10 8 8 6 6 4 4 2 2 0 0 2 2 4 4 6 6 8 8 10 10 8 6 4 2 0 2 4 6 8 10 10 2 10 8 6 4 2 0 10 4 6 8 (Left) The tree generated by an RRT after 5,000 nodes. The goal Figure 10.19: region is the square at the top right corner, and the shortest path is indicated. (Right) ∗ The tree generated by RRT after 5,000 nodes. Figure from [67] used with permission. 10.5.2 The PRM Algorithm C (Section 10.3) The PRM uses sampling to build a roadmap representation of free before answering any specific queries. The roadmap is an undirected graph: the robot can move in either direction along any edge exactly from one node to the next. For this reason, PRMs primarily apply to kinematic problems for which an exact local planner exists that can find a path (ignoring obstacles) from any . The simplest example is a straight-line planner for a robot to any other q q 2 1 with no kinematic constraints. Once the roadmap is built, a particular start node q can be added to start the graph by attempting to connect it to the roadmap, starting with the closest node. The same is done for the goal node q . The graph is then searched for goal ∗ a path, typically using . Thus the query can be answered efficiently once the A roadmap has been built. The use of PRMs allows the possibility of building a roadmap quickly and efficiently relative to constructing a roadmap using a high-resolution grid repre- sentation. The reason is that the volume fraction of the C-space that is “visible” by the local planner from a given configuration does not typically decrease ex- ponentially with increasing dimension of the C-space. The algorithm for constructing a roadmap R with N nodes is outlined in Algorithm 10.4 and illustrated in Figure 10.20. A key choice in the PRM roadmap-construction algorithm is how to sample May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

403 Chapter 10. Motion Planning 385 PRM roadmap construction algorithm (undirected graph). Algorithm 10.4 i = 1 ,...,N do 1: for C sample from q 2: ← i free add to R 3: q i 4: end for for i 5: ,...,N do = 1 6: ( q N ) ← k closest neighbors of q i i for each q ∈N 7: q do ) ( i 8: if there is a collision-free local path from q to q and i to q then there is not already an edge from q i add an edge from q to q to the roadmap 9: R i 10: end if 11: end for end for 12: return R 13: 2 An example PRM roadmap for a point robot in C = R Figure 10.20: . The k = 3 closest neighbors are taken into consideration for connection to a sample node . The q degree of a node can be greater than three since it may be a close neighbor of many nodes. from C . While the default might be sampling randomly from a uniform free distribution on C and eliminating configurations in collision, it has been shown that sampling more densely near obstacles can improve the likelihood of finding May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

404 386 10.6. Virtual Potential Fields narrow passages, thus significantly reducing the number of samples needed to properly represent the connectivity of . Another option is deterministic C free multi-resolution sampling. 10.6 Virtual Potential Fields Virtual potential field methods are inspired by potential energy fields in nature, such as gravitational and magnetic fields. From physics we know that a potential ( q ) defined over C induces a force field = − ∂ P /∂q that drives an object from P F h is the height above the Earth’s surface in high to low potential. For example, if 2 = 9 81 m/s g a uniform gravitational potential field ( ) then the potential energy . = m P ( h ) = mgh and the force acting on it is F = − of a mass P /∂h is − mg . ∂ The force will cause the mass to fall to the Earth’s surface. In robot motion control, the goal configuration q is assigned a low virtual goal potential and obstacles are assigned a high virtual potential. Applying a force to the robot proportional to the negative gradient of the virtual potential naturally pushes the robot toward the goal and away from the obstacles. A virtual potential field is very different from the planners we have seen so far. Typically the gradient of the field can be calculated quickly, so the motion can be calculated in real time (reactive control) instead of planned in advance. With appropriate sensors, the method can even handle obstacles that move or appear unexpectedly. The drawback of the basic method is that the robot can get stuck in local minima of the potential field, away from the goal, even when a feasible motion to the goal exists. In certain cases it is possible to design the potential to guarantee that the only local minimum is at the goal, eliminating this problem. 10.6.1 A Point in C-space q Let’s begin by assuming a point robot in its C-space. A goal configuration goal is typically encoded by a quadratic potential energy “bowl” with zero energy at the goal, 1 T q − q , ( ( q − q K ) ) ( P q ) = goal goal goal 2 is a symmetric positive-definite weighting matrix (for example, the where K identity matrix). The force induced by this potential is ∂ P goal − ) = K ( q q , − ( q ) = F goal goal ∂q an attractive force proportional to the distance from the goal. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

405 Chapter 10. Motion Planning 387 The repulsive potential induced by a C-obstacle B can be calculated from q, ) to the obstacle (Section 10.2.2): the distance ( d B k P ) = ( q (10.3) , B 2 q, B ) ( 2 d 0 is a scaling factor. The potential is only properly defined for points where k > ( B ) > 0. The force induced by the obstacle potential is outside the obstacle, d q, ∂d k ∂ P B = . F q − ( ) = B 3 ( ) B q, ∂q ∂q d The total potential is obtained by summing the attractive goal potential and the repulsive obstacle potentials, ∑ q ) = ) q ( q ) + P , P ( ( P B goal i i yielding a total force ∑ . ) = F q ( q ) + ( F F q ( ) B goal i i Note that the sum of the attractive and repulsive potentials may not give a q . Also, it is common to put a bound minimum (zero force) exactly at goal on the maximum potential and force, as the simple obstacle potential (10.3) would otherwise yield unbounded potentials and forces near the boundaries of obstacles. 2 10.21 shows a potential field for a point in R with three circular Figure obstacles. The contour plot of the potential field clearly shows the global min- imum near the center of the space (near the goal marked with a +), a local minimum near the two obstacles on the left, as well as saddles (critical points that are a maximum in one direction and a minimum in the other direction) near the obstacles. Saddles are generally not a problem, as a small perturbation allows continued progress toward the goal. Local minima away from the goal are a problem, however, as they attract nearby states. F ( To actually control the robot using the calculated ), we have several q options, two of which are: • Apply the calculated force plus damping, u = F ( q ) + B ̇ q. (10.4) If is positive definite then it dissipates energy for all ̇ q 6 = 0, reducing B oscillation and guaranteeing that the robot will come to rest. If B = 0, the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

406 388 10.6. Virtual Potential Fields 2 (Top left) Three obstacles and a goal point, marked with a +, in R . Figure 10.21: (Top right) The potential function summing the bowl-shaped potential pulling the robot to the goal with the repulsive potentials of the three obstacles. The potential function saturates at a specified maximum value. (Bottom left) A contour plot of the potential function, showing the global minimum, a local minimum, and four saddles: between each obstacle and the boundary of the workspace, and between the two small obstacles. (Bottom right) Forces induced by the potential function. robot continues to move while maintaining constant total energy, which 1 T ( ̇ (0) and the initial (0) M q q (0)) ̇ q is the sum of the initial kinetic energy 2 virtual potential energy P ( q (0)). The motion of the robot under the control law (10.4) can be visualized as a ball rolling in gravity on the potential surface of Figure 10.21, where the dissipative force is rolling friction. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

407 Chapter 10. Motion Planning 389 • Treat the calculated force as a commanded velocity instead: (10.5) = ( q q . F ̇ ) This automatically eliminates oscillations. Using the simple obstacle potential (10.3), even distant obstacles have a nonzero effect on the motion of the robot. To speed up evaluation of the repul- sive terms, distant obstacles could be ignored. We can define a range of influence d q, > 0 so that the potential is zero for all d ( of the obstacles B ) ≥ d : range range  ) ( 2  d k B − ) ( q, d range  d if q, ( < d ) B range U ) = q ( 2 q, d B ) d ( B range   otherwise . 0 d ( q, B ) and its gradient are generally difficult to calcu- Another issue is that late. An approach to dealing with this is described in Section 10.6.3. 10.6.2 Navigation Functions A significant problem with the potential field method is local minima. While potential fields may be appropriate for relatively uncluttered spaces or for rapid response to unexpected obstacles, they are likely to get the robot stuck in local minima for many practical applications. 10.11. One method that avoids this issue is the wavefront planner of Figure The wavefront algorithm creates a local-minimum-free potential function by a breadth-first traversal of every cell reachable from the goal cell in a grid representation of the free space. Therefore, if a solution exists to the motion planning problem then simply moving “downhill” at every step is guaranteed to bring the robot to the goal. Another approach to local-minimum-free gradient following is based on re- . A naviga- navigation function placing the virtual potential function with a tion function φ ( q ) is a type of virtual potential function that q ; 1. is smooth (or at least twice differentiable) on 2. has a bounded maximum value (e.g., 1) on the boundaries of all obstacles; 3. has a single minimum at q ; and goal 2 2 4. has a full-rank Hessian φ/∂q ∂ at all critical points q where ∂φ/∂q = 0 (i.e., φ ( q ) is a Morse function). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

408 390 10.6. Virtual Potential Fields 2 2 ∂ exists. Condition 2 puts an upper Condition 1 ensures that the Hessian φ/∂q bound on the virtual potential energy of the robot. The key conditions are 3 φ ) (including minima, ( and 4. Condition 3 ensures that of the critical points of q q maxima, and saddles), there is only one minimum, at . This ensures that goal q is at least locally attractive. There may be saddle points that are minima goal along a subset of directions, but condition 4 ensures that the set of initial states that are attracted to any saddle point has empty interior (zero measure), and therefore almost every initial state converges to the unique minimum . q goal While constructing navigation potential functions with only a single mini- mum is nontrivial, [152] showed how to construct them for the particular case -dimensional C of an consisting of all points inside an n -sphere of radius n free and outside smaller spherical obstacles B , i.e., of radius r R centered at q i i i n for all R q | ‖ q ‖ ≤ R and ‖ q − q . ‖ { sphere world ∈ i } . This is called a > r i i While a real C-space is unlikely to be a sphere world, Rimon and Koditschek showed that the boundaries of the obstacles, and the associated navigation func- tion, can be deformed to a much broader class of star-shaped obstacles. A star-shaped obstacle is one that has a center point from which the line segment to any point on the obstacle boundary is contained completely within the ob- stacle. A star world is a star-shaped C-space which has star-shaped obstacles. Thus finding a navigation function for an arbitrary star world reduces to find- ing a navigation function for a “model” sphere world that has centers at the centers of the star-shaped obstacles, then stretching and deforming that navi- gation function to one that fits the star world. Rimon and Koditschek gave a systematic procedure to accomplish this. Figure 10.22 shows a deformation of a navigation function on a model sphere 2 R world to a star world for the case C ⊂ . 10.6.3 Workspace Potential A difficulty in calculating the repulsive force from an obstacle is obtaining the d ( q, B ). One approach that avoids an exact calculation distance to the obstacle, is to represent the boundary of an obstacle as a set of point obstacles, and to represent the robot by a small set of control points. Let the Cartesian location 3 ∈ f of the ( q of control point on the robot be written R i and boundary point j ) i 3 c c R obstacle be . Then the distance between the two points is ‖ f , ( q ) − ∈ ‖ j i j i due to the obstacle point j is and the potential at the control point k ′ , ( q ) = P ij 2 c 2 ( q ) − ‖ f ‖ j i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

409 Chapter 10. Motion Planning 391 (Left) A model “sphere world” with five circular obstacles. The Figure 10.22: 0). Note that the , contour plot of a navigation function is shown. The goal is at (0 obstacles induce saddle points near the obstacles, but no local minima. (Right) A “star world” obtained by deforming the obstacles and the potential while retaining a navigation function. Figure from [152] used with permission from the American Mathematical Society. yielding the repulsive force at the control point ) ( T ′ ∂ P k ∂f i ij ′ 3 f = F . R ∈ ) c − ) q ( ( ) = q ( − i j ij 4 f ‖ ‖ ∂q c − ) q ∂q ( i j ′ 3 n ( q ) ∈ R F into a generalized force F To turn the linear force ( q ) ∈ R ij ij n × 3 ) R q J ( acting on the robot arm or mobile robot, we first find the Jacobian ∈ i ̇ to the linear velocity of the control point f relating ̇ : q i ∂f i ̇ q q q. ̇ ) ̇ = J ( = f i i ∂q n F ( q ) ∈ R By the principle of virtual work, the generalized force due to the ij ′ 3 repulsive linear force ( q ) ∈ R is simply F ij T ′ q F J ( . ( ) ) F q q ( ) = ij ij i Now the total force F ( q ) acting on the robot is the sum of the easily calculated q attractive force ( q ) and the repulsive forces F . ( j ) for all i and F goal ij 10.6.4 Wheeled Mobile Robots The preceding analysis assumes that a control force u = F ( q ) + B ̇ q (control law (10.4)) or a velocity ̇ q = F ( q ) (control law (10.5)) can be applied in any May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

410 392 10.7. Nonlinear Optimization direction. If the robot is a wheeled mobile robot subject to rolling constraints ) ) ̇ q = 0, however, the calculated F ( q ) must be projected to controls F A ( q q ( proj that move the robot tangentially to the constraints. For a kinematic robot q F employing the control law ̇ ( q ), a suitable projection is = proj ) ( ( ) − 1 T T ) = ( q ) q A ( q ) A − ( q ( I A ) A ( q ) F F ( q ) . proj For a dynamic robot employing the control law = F u ( q )+ B ̇ q , the projection proj was discussed in Section 8.7. 10.6.5 Use of Potential Fields in Planners A potential field can be used in conjunction with a path planner. For example, a ∗ best-first search such as can use the potential as an estimate of the cost-to-go. A Incorporating a search function prevents the planner from getting permanently stuck in local minima. Nonlinear Optimization 10.7 The motion planning problem can be expressed as a general nonlinear optimiza- tion, with equality and inequality constraints, taking advantage of a number of software packages to solve such problems. Nonlinear optimization problems can be solved by gradient-based methods, such as sequential quadratic programming (SQP), or non-gradient methods, such as simulated annealing, Nelder–Mead op- timization, and genetic programming. Like many nonlinear optimization prob- lems, these methods are not generally guaranteed to find a feasible solution when one exists, let alone an optimal one. For methods that use gradients of the objective function and constraints, however, we can expect a locally optimal solution if we start the process with a guess that is “close” to a solution. The general problem can be written as follows: find ( t ) ,q ( t ) ,T (10.6) u ( J u ( t ) ,q minimizing t ) ,T ) (10.7) ( subject to ̇ x ( t ) = f ( x ( t ) ,u ( t )) , ∀ t ∈ [0 ,T ] , (10.8) ∈ u ) ∈U , ∀ t t [0 ,T ] , (10.9) ( q ( t ) ∈C (10.10) , ∀ t ∈ [0 ,T ] , free (0) = x , (10.11) x start x ( T ) = x (10.12) . goal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

411 Chapter 10. Motion Planning 393 To solve this problem approximately by nonlinear optimization, the con- trol t ), trajectory q ( t ), and equality and inequality constraints (10.8)–(10.12) u ( must be discretized. This is typically done by ensuring that the constraints are ,T satisfied at a fixed number of points distributed evenly over the interval [0 ] and choosing a finite-parameter representation of the position and/or control histories. We have at least three choices of how to parametrize the position and controls: q ( t ) . (a) Parametrize the trajectory In this case, we solve for the tra- u ( t ) at any time are calculated jectory parameters directly. The controls using the equations of motion. This approach does not apply to systems with fewer controls than configuration variables, m < n . Parametrize the control u (b) t ) . We solve for u ( t ) directly. Calculating ( the state ( t ) requires integrating the equations of motion. x We have a larger number of variables, Parametrize both ( t ) and u ( (c) ) . q t since we parametrize both q ( t ) and u ( t ). Also, we have a larger number of constraints, as q ( t ) and u ( t ) must satisfy the dynamic equations ̇ x = f ( x,u ) explicitly, typically at a fixed number of points distributed evenly ]. We must be careful to choose the parametrizations ,T over the interval [0 ( t ) and u of t ) to be consistent with each other, so that the dynamic q ( equations can be satisfied at these points. A trajectory or control history can be parametrized in any number of ways. The parameters can be the coefficients of a polynomial in time, the coefficients of a truncated Fourier series, spline coefficients, wavelet coefficients, piecewise constant acceleration or force segments, etc. For example, the control ( t ) u i p + 1 coefficients a could be represented by of a polynomial in time: j p ∑ j t ) = u . ( t a j i j =0 In addition to the parameters for the state or control history, the total time T may be another control parameter. The choice of parametrization has impli- cations for the efficiency of the calculation of q ( t ) and u ( t ) at a given time t . It also determines the sensitivity of the state and control to the parameters and whether each parameter affects the profiles at all times [0 ,T ] or just on a finite- time support base. These are important factors in the stability and efficiency of the numerical optimization. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

412 394 10.8. Smoothing Smoothing 10.8 The axis-aligned motions of a grid planner and the randomized motions of sam- pling planners may lead to jerky motion of a robot. One approach to dealing with this issue is to let the planner handle the work of searching globally for a solution, then post-process the resulting motion to make it smoother. There are many ways to do this; two possibilities are outlined below. Nonlinear Optimization While gradient-based nonlinear optimization may fail to find a solution if initialized with a random initial trajectory, it can make an effective post-processing step, since the plan initializes the optimization with a “reasonable” solution. The initial motion must be converted to a parametrized ( u representation of the controls, and the cost t ) ,q ( t ) ,T ) can be expressed as J ( u ( a function of ) or q ( t ). For example, the cost function t ∫ T 1 T dt ) ̇ u t ( t ) ̇ u ( = J 2 0 penalizes rapidly changing controls. This has an analogy in human motor con- trol, where the smoothness of human arm motions has been attributed to min- imization of the rate of change of torques at the joints [188]. Subdivide and Reconnect A local planner can be used to attempt a con- nection between two distant points on a path. If this new connection is collision- free, it replaces the original path segment. Since the local planner is designed to produce short, smooth, paths, the new path is likely to be shorter and smoother than the original. This test-and-replace procedure can be applied iteratively to randomly chosen points on the path. Another possibility is to use a recursive procedure that subdivides the path first into two pieces and attempts to replace each piece with a shorter path; then, if either portion cannot be replaced by a shorter path, it subdivides again; and so on. 10.9 Summary A fairly general statement of the motion planning problem is as follows. • x (0) = x and a desired final state x , find a Given an initial state start goal time T and a set of controls u : [0 ,T ] → U such that the motion satisfies ∈C x T ) ∈X ]. and q ( x ( t )) ( ,T for all t ∈ [0 free goal • Motion planning problems can be classified in the following categories: path planning versus motion planning; fully actuated versus constrained May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

413 Chapter 10. Motion Planning 395 or underactuated; online versus offline; optimal versus satisficing; exact versus approximate; with or without obstacles. • Motion planners can be characterized by the following properties: multiple- query versus single-query; anytime planning or not; complete, resolution complete, probabilistically complete, or none of the above; and their de- gree of computational complexity. • C , and obstacle space, Obstacles partition the C-space into free C-space, free into separate con- , where C = C ∪C C C . Obstacles may split obs free obs free nected components. There is no feasible path between configurations in different connected components. A conservative check of whether a configuration q is in collision uses a sim- • plified “grown” representation of the robot and obstacles. If there is no collision between the grown bodies, then the configuration is guaranteed collision-free. Checking whether a path is collision-free usually involves sampling the path at finely spaced points and ensuring that if the indi- vidual configurations are collision-free then the swept volume of the robot path is collision-free. • The C-space geometry is often represented by a graph consisting of nodes and edges between the nodes, where edges represent feasible paths. The graph can be undirected (edges flow in both directions) or directed (edges flow in only one direction). Edges can be unweighted or weighted according to their cost of traversal. A tree is a directed graph with no cycles in which each node has at most one parent. A roadmap path planner uses a graph representation of C , and path • free planning problems can be solved using a simple path from q onto the start roadmap, a path along the roadmap, and a simple path from the roadmap to q . goal ∗ • A algorithm is a popular search method that finds minimum-cost The paths on a graph. It operates by always exploring from a node that is (1) unexplored and (2) on a path with minimum estimated total cost. The estimated total cost is the sum of the weights for the edges encountered in reaching the node from the start node plus an estimate of the cost-to-go to the goal. To ensure that the search returns an optimal solution, the cost-to-go estimate should be optimistic. • A grid-based path planner discretizes the C-space into a graph consisting of neighboring points on a regular grid. A multi-resolution grid can be used May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

414 396 10.9. Summary to allow large steps in wide open spaces and smaller steps near obstacle boundaries. Discretizing the control set allows robots with motion constraints to take • advantage of grid-based methods. If integrating a control does not land the robot exactly on a grid point, the new state may still be pruned if a state in the same grid cell has already been achieved with a lower cost. • The basic RRT algorithm grows a single search tree from x to find a start motion to . It relies on a sampler to find a sample x , an in X X samp goal x in the search tree, and a local algorithm to find the closest node nearest planner to find a motion from x to a point closer to x . The samp nearest X quickly. sampling is chosen to cause the tree to explore free The bidirectional RRT grows a search tree from both x and • x and goal start ∗ attempts to join them up. The RRT algorithm returns solutions that tend toward the optimal as the planning time goes to infinity. • The PRM builds a roadmap of C for multiple-query planning. The free roadmap is built by sampling C N times, then using a local planner to free attempt to connect each sample with several of its nearest neighbors. The ∗ . A roadmap is searched using Virtual potential fields are inspired by potential energy fields such as grav- • itational and electromagnetic fields. The goal point creates an attractive potential while obstacles create repulsive potentials. The total potential P ( q ) is the sum of these, and the virtual force applied to the robot is F ( q ) = − ∂ P /∂q . The robot is controlled by applying this force plus damping or by simulating first-order dynamics and driving the robot with ( q ) as a velocity. Potential field methods are conceptually simple but F may get the robot stuck in local minima away from the goal. • A navigation function is a potential function with no local minima. Nav- igation functions result in near-global convergence to q . While they goal are difficult to design in general, they can be designed systematically for certain environments. • Motion planning problems can be converted to general nonlinear optimiza- tion problems with equality and inequality constraints. While optimiza- tion methods can be used to find smooth near-optimal motions, they tend to get stuck in local minima in cluttered C-spaces. Optimization methods typically require a good initial guess at a solution. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

415 Chapter 10. Motion Planning 397 Motions returned by grid-based and sampling-based planners tend to be • jerky. Smoothing the plan using nonlinear optimization or subdivide-and- reconnect can improve the quality of the motion. Notes and References 10.10 Excellent textbooks covering motion planning broadly include the original text by Latombe [80] in 1991 and the more recent texts by Choset et al. [27] and LaValle [83]. Other summaries of the state-of-the-art in motion planning can be found in the Handbook of Robotics [70], and, particularly for robots subject to nonholonomic and actuation constraints, in the Control Handbook [101], the Encyclopedia of Systems and Control [100], and the textbook by Murray et al. [122]. Search algorithms and other algorithms for artificial intelligence are covered in detail by Russell and Norvig [155]. Landmark early work on motion planning for Shakey the Robot at SRI led ∗ to the development of search in 1968 by Hart, Nilsson, and Raphael [53]. A This work built on the newly established approach to dynamic programming for optimal decision-making, as described by Bellman and Dreyfus [10], and it improved on the performance of Dijkstra’s algorithm [37]. A suboptimal ∗ anytime variant of A was proposed in [90]. Early work on multiresolution path planning is described in [65, 96, 45, 54] based on hierarchical decompositions of C-space [156]. One early line of work focused on exact characterization of the free C-space in the presence of obstacles. The visibility graph approach for polygons mov- ing among polygons was developed by Lozano-P ́erez and Wesley in 1979 [97]. In more general settings, researchers used sophisticated algorithms and math- ematical methods to develop cellular decompositions and exact roadmaps of the free C-space. Important examples of this work are a series of papers by 160, Schwartz and Sharir on the piano movers’ problem [159, 161] and Canny’s PhD thesis [23]. As a result of the mathematical sophistication and high computational com- plexity needed to exactly represent the topology of C-spaces, a movement formed in the 1990s to approximately represent C-space using samples, and that move- ment carries on strong today. That line of work has followed two main branches, probabilistic roadmaps (PRMs) [69] and rapidly exploring random trees (RRTs) 86, 85]. Due to their ability to handle complex high-dimensional C-spaces [84, relatively efficiently, research in sampling-based planners has exploded, and some of the subsequent work is summarized in [27, 83]. The bidirectional RRT ∗ and RRT , highlighted in this chapter, are described in [83] and [68], respec- tively. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

416 398 10.11. Exercises The grid-based approach to motion planning for a wheeled mobile robot was introduced by Barraquand and Latombe [8], and the grid-based approach to time-optimal motion planning for a robot arm with dynamic constraints was 39]. introduced in [24, 40, The GJK algorithm for collision detection was derived in [50]. Open-source collision-detection packages are implemented in the Open Motion Planning Li- brary (OMPL) [181] and the Robot Operating System (ROS). An approach to approximating polyhedra with spheres for fast collision detection is described in [61]. The potential field approach to motion planning and real-time obstacle avoidance was first introduced by Khatib and is summarized in [73]. A search- based planner using a potential field to guide the search is described by Bar- raquand et al. [7]. The construction of navigation functions, potential functions with a unique local minimum, is described in a series of papers by Koditschek and Rimon [78, 77, 152, 153]. 76, Nonlinear optimization-based motion planning has been formulated in a number of publications, including the classic computer graphics paper by Witkin and Kass [194] using optimization to generate the motions of an animated jump- ing lamp; work on generating motion plans for dynamic nonprehensile manip- ulation [103]; Newton algorithms for optimal motions of mechanisms [88]; and more recent developments in short-burst sequential action control, which solves both the motion planning and feedback control problems [3, 187]. Path smooth- ing for mobile robot paths by subdivide and reconnect is described by Laumond et al. [82]. 10.11 Exercises One path is to another if it can be continuously Exercise 10.1 homotopic deformed into the other without moving the endpoints. In other words, it can be stretched and pulled like a rubber band, but it cannot be cut and pasted back together. For the C-space in Figure 10.2, draw a path from the start to the goal that is not homotopic to the one shown. Exercise 10.2 Label the connected components in Figure 10.2. For each connected component, draw a picture of the robot for one configuration in the connected component. ◦ ◦ θ ] result in joint angles in the range [175 , Exercise 10.3 185 Assume that 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

417 Chapter 10. Motion Planning 399 reference point Exercise 10.4. Figure 10.23: self-collision for the robot of Figure 10.2. Draw the new joint-limit C-obstacle on top of the existing C-obstacles and label the resulting connected components of C . For each connected component, draw a picture of the robot for one free configuration in the connected component. Draw the C-obstacle corresponding to the obstacle and trans- Exercise 10.4 lating planar robot in Figure 10.23. Exercise 10.5 Write a program that accepts as input the coordinates of a polygonal robot (relative to a reference point on the robot) and the coordinates of a polygonal obstacle and produces as output a drawing of the correspond- ing C-space obstacle. In Mathematica, you may find the function ConvexHull useful. In MATLAB, try convhull . Exercise 10.6 Calculating a square root can be computationally expensive. 10.2.2), For a robot and an obstacle represented as collections of spheres (Section provide a method for calculating the distance between the robot and obstacle that minimizes the use of square roots. Draw the visibility roadmap for the C-obstacles and q Exercise 10.7 and start q in Figure 10.24. Indicate the shortest path. goal Exercise 10.8 Not all edges of the visibility roadmap described in Section 10.3 are needed. Prove that an edge between two vertices of C-obstacles need not be included in the roadmap if either end of the edge does not hit the obstacle tangentially (i.e., it hits at a concave vertex). In other words, if the edge ends May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

418 400 10.11. Exercises goal start Figure 10.24: 10.7. Planning problem for Exercise by “colliding” with an obstacle, it will never be used in a shortest path. ∗ Implement an path planner for a point robot in a plane Exercise 10.9 A with obstacles. The planar region is a 100 × 100 area. The program generates N nodes and E edges, where N and E are chosen by the a graph consisting of N user. After generating randomly chosen nodes, the program should connect randomly chosen nodes by edges until E unique edges have been generated. The cost associated with each edge is the Euclidean distance between the nodes. ∗ A for the Finally, the program should display the graph, search the graph using shortest path between nodes 1 and N , and display the shortest path or indicate FAILURE if no path exists. The heuristic cost-to-go is the Euclidean distance to the goal. ∗ Modify the A Exercise 10.10 planner in Exercise 10.9 to use a heuristic cost- to-go equal to ten times the distance to the goal node. Compare the running ∗ time with the original when they are run on the same graphs. (You may A need to use large graphs to notice any effect.) Are the solutions found with the new heuristic optimal? ∗ A Modify the algorithm from Exercise Exercise 10.11 to use Dijkstra’s 10.9 ∗ algorithm instead. Comment on the relative running times of A and Dijkstra’s algorithm when each is run on the same graphs. Exercise 10.12 Write a program that accepts the vertices of polygonal ob- stacles from a user, as well as the specification of a 2R robot arm, rooted at ( x,y ) = (0 , 0), with link lengths L . Each link is simply a line segment. and L 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

419 Chapter 10. Motion Planning 401 Generate the C-space obstacles for the robot by sampling the two joint angles at k = 5) and checking for intersections between the k -degree intervals (e.g., line segments and the polygon. Plot the obstacles in the workspace, and in the C-space grid use a black square or dot at each configuration colliding with an obstacle. (Hint: At the core of this program is a subroutine to see whether two line segments intersect. If the segments’ corresponding infinite lines intersect, you can check whether this intersection is within the line segments.) ∗ Write an A grid path planner for a 2R robot with obsta- Exercise 10.13 10.12 and cles and display on the C-space the paths you find. (See Exercise Figure 10.10.) Exercise 10.14 Implement the grid-based path planner for a wheeled mo- bile robot (Algorithm 10.2), given the control discretization. Choose a simple method to represent obstacles and check for collisions. Your program should plot the obstacles and show the path that is found from the start to the goal. Exercise 10.15 Write an RRT planner for a point robot moving in a plane with obstacles. Free space and obstacles are represented by a two-dimensional array, where each element corresponds to a grid cell in the two-dimensional space. The occurrence of a 1 in an element of the array means that there is an obstacle there, and a 0 indicates that the cell is in free space. Your program should plot the obstacles, the tree that is formed, and show the path that is found from the start to the goal. Exercise 10.16 Do the same as for the previous exercise, except that obstacles are now represented by line segments. The line segments can be thought of as the boundaries of obstacles. Exercise 10.17 Write a PRM planner to solve the same problem as in Exer- cise 10.15. Exercise 10.18 Write a program to implement a virtual potential field for a 2R robot in an environment with point obstacles. The two links of the robot are line segments, and the user specifies the goal configuration of the robot, the start configuration of the robot, and the location of the point obstacles in the workspace. Put two control points on each link of the robot and transform the workspace potential forces to configuration space potential forces. In one workspace figure, draw an example environment consisting of a few point ob- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

420 402 10.11. Exercises stacles and the robot at its start and goal configurations. In a second C-space figure, plot the potential function as a contour plot over ( θ ,θ ), and overlay a 1 2 planned path from a start configuration to a goal configuration. The robot uses the kinematic control law ̇ q = F ( q ). See whether you can create a planning problem that results in convergence to an undesired local minimum for some initial arm configurations but succeeds in finding a path to the goal for other initial arm configurations. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

421 Chapter 11 Robot Control A robot arm can exhibit a number of different behaviors, depending on the task and its environment. It can act as a source of programmed motions for tasks such as moving an object from one place to another or tracing a trajectory for a spray paint gun. It can act as a source of forces, as when applying a polishing wheel to a workpiece. In tasks such as writing on a chalkboard, it must control forces in some directions (the force must press the chalk against the board) and motions in others (the motion must be in the plane of the board). When the purpose of the robot is to act as a haptic display, rendering a virtual environment, we may want it to act like a spring, damper, or mass, yielding in response to forces applied to it. In each of these cases, it is the job of the robot controller to convert the task specification to forces and torques at the actuators. Control strategies that achieve the behaviors described above are known as motion control , force control , hybrid motion–force control , or impedance control . Which of these behaviors is appropriate depends on both the task and the environment. For example, a force-control goal makes sense when the end-effector is in con- tact with something but not when it is moving in free space. We also have a fundamental constraint imposed by the mechanics, irrespective of the environ- ment: the robot cannot independently control the motion and force in the same direction. If the robot imposes a motion then the environment will determine the force, and if the robot imposes a force then the environment will determine the motion. Once we have chosen a control goal consistent with the task and environ- ment, we can use feedback control to achieve it. Feedback control uses position, velocity, and force sensors to measure the actual behavior of the robot, com- 403

422 404 11.1. Control System Overview pares it with the desired behavior, and modulates the control signals sent to the actuators. Feedback is used in nearly all robot systems. In this chapter we focus on: feedback control for motion control, both in the joint space and in the task space; force control; hybrid motion–force control; and impedance control. 11.1 Control System Overview A typical control block diagram is shown in Figure 11.1(a). The sensors are typically: potentiometers, encoders, or resolvers for joint position and angle sensing; tachometers for joint velocity sensing; joint force–torque sensors; and/or multi-axis force–torque sensors at the “wrist” between the end of the arm and the end-effector. The controller samples the sensors and updates its control signals to the actuators at a rate of hundreds to a few thousands of Hz. In most robotic applications control update rates higher than this are of limited benefit, given the time constants associated with the dynamics of the robot and environment. In our analysis we will ignore the fact that the sampling time is nonzero and treat controllers as if they were implemented in continuous time. While tachometers can be used for direct velocity sensing, a common ap- proach is to use a digital filter to numerically-difference the position signals at successive timesteps. A low-pass filter is often used in combination with the dif- ferencing filter to reduce the high-frequency signal content due to quantization of the differenced position signals. 8.9, there are a number of different technologies for As discussed in Section creating mechanical power, transforming the speeds and forces, and transmitting to the robot joints. In this chapter we lump each joint’s amplifier, actuator, and transmission together and treat them as a transformer from low-power control signals to forces and torques. This assumption, along with the assumption of perfect sensors, allows us to simplify the block diagram of Figure 11.1(a) to the one shown in Figure 11.1(b), where the controller produces the forces and torques directly. The rest of this chapter deals with the control algorithms that go inside the “controller” box in Figure 11.1(b). Real robot systems are subject to flexibility and vibrations in the joints and links, backlash at the gears and transmissions, actuator saturation limits, and limited resolution of the sensors. These raise significant issues in design and control but they are beyond the scope of this chapter. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

423 Chapter 11. Robot Control 405 high forces low power and power desired controls torques controls behavior dynamics of actuators arm and controller and amplifiers environment transmissions local feedback motions and forces sensors (a) forces motions and and desired torques forces behavior dynamics of arm and controller environment (b) Figure 11.1: (a) A typical robot control system. An inner control loop is used to help the amplifier and actuator to achieve the desired force or torque. For example, a DC motor amplifier in torque control mode may sense the current actually flowing through the motor and implement a local controller to better match the desired current, since the current is proportional to the torque produced by the motor. Alternatively the motor controller may directly sense the torque by using a strain gauge on the motor’s output gearing, and close a local torque-control loop using that feedback. (b) A simplified model with ideal sensors and a controller block that directly produces forces and torques. This assumes ideal behavior of the amplifier and actuator blocks in part (a). Not shown are the disturbance forces that can be injected before the dynamics block, or disturbance forces or motions injected after the dynamics block. 11.2 Error Dynamics In this section we focus on the controlled dynamics of a single joint, as the concepts generalize easily to the case of a multi-joint robot. If the desired joint position is θ ) then ( t ) and the actual joint position is θ ( t d we define the joint error to be θ . ( t ) = θ ) ( t ) − θ ( t d e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

424 406 11.2. Error Dynamics θ The differential equation governing the evolution of the joint error ) of the ( t e . The purpose of the feedback error dynamics controlled system is called the ( θ ) tends to zero, or a small t controller is to create an error dynamics such that e increases. value, as t 11.2.1 Error Response A common way to test how well a controller works is to specify a nonzero initial error (0) and see how quickly, and how completely, the controller reduces θ e error response to be the response the initial error. We define the (unit) ( t ) ,t > θ θ 0, of the controlled system for the initial conditions (0) = 1 and e e ̇ ̈ θ (0) = θ = 0. (0) = ··· e e An ideal controller would drive the error to zero instantly and keep the error at zero for all time. In practice it takes time to reduce the error, and the error may never be completely eliminated. As illustrated in Figure 11.2, and ) can be described by a t θ transient response ( a typical error response e steady-state response . The steady-state response is characterized by the a e steady-state error , which is the asymptotic error θ . The ( t ) as t → ∞ e ss transient response is characterized by the overshoot and (2%) settling time . The 2% settling time is the first time T such that | θ ) ( t ) − e e |≤ 0 . 02( θ − (0) e ss e ss t for all T ≥ (see the pair of long-dashed lines). Overshoot occurs if the error response initially overshoots the final steady-state error, and in this case the overshoot is defined as ∣ ∣ ∣ ∣ θ − e min ss e, ∣ ∣ , 100% × overshoot = ∣ ∣ θ − (0) e ss e θ is the least positive value achieved by the error. where min e, A good error response is characterized by • little or no steady-state error, • little or no overshoot, and • a short 2% settling time. 11.2.2 Linear Error Dynamics In this chapter we work primarily with systems with error dynamics linear described by linear ordinary differential equations of the form p ) ( p − 1) ( ̈ ̇ = θ a θ θ ··· a (11.1) + + c. + a θ + θ a + a − 1 2 e p 1 0 e e p e e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

425 Chapter 11. Robot Control 407 1 θ e e ss 0 t 2% settlin gt ime t oo oversh Figure 11.2: An example error response showing steady-state error e , the overshoot, ss and the 2% settling time. th-order differential equation, because p time derivatives of θ This is a are p e homogeneous c is if the constant present. The differential equation (11.1) is if c 6 = 0. zero and nonhomogeneous = 0) linear error dynamics, the p th-order differential c For homogeneous ( equation (11.1) can be rewritten as 1 p ) ( ( p − 1) ̈ ̇ = θ − a θ + θ + a a θ ) + a ( θ + ··· − 1 1 2 e e e p 0 e e a p ′ 1) − ′ ( p ′ ′ ̈ ̇ = θ − −···− a a a θ − − θ θ . a (11.2) e e e 2 − 1 p e 1 0 p th-order differential equation can be expressed as p coupled first-order This ), where x x differential equations by defining the vector ,...,x = ( p 1 x = θ , e 1 ̇ x , = = ̇ θ x 1 2 e . . . . . . ( p − 1) x , = ̇ x = θ 1 p p − e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

426 408 11.2. Error Dynamics and writing Equation (11.2) as ′ ′ ′ a x − a − x x −···− a ̇ = . x 1 p p 2 0 1 1 p − t ) = ( ( t ), where x Then ̇ Ax   0 1 0 ··· 0 0   1 0 ··· 0 0 0     . . . . . . . . . . . .   p × p . . . . . . R . ∈ A =     0 ··· 1 0 0 0     0 ··· 0 0 0 1 ′ ′ ′ ′ ′ a − − − a ··· − a a a − 0 p 2 − 1 p − 1 2 x ( t ) = ax ( t ), which By analogy with the scalar first-order differential equation ̇ at ( t ) = has solution x x (0), the vector differential equation ̇ x ( t ) = Ax ( t ) has so- e At x ( t ) = e x lution (0) using the matrix exponential, as we saw in Section 3.2.3.1. Also analogous to the scalar differential equation, whose solution converges to the equilibrium a is negative, the differential x = 0 from any initial condition if ( equation ̇ ( t ) converges to x = 0 if the matrix A is negative definite, Ax t x ) = (which may be complex) have negative real components. A i.e., all eigenvalues of A are given by the roots of the characteristic polynomial The eigenvalues of s satisfying of A , i.e., the complex values ′ p p − 1 ′ 2 ′ ′ s ) = det( sI + ··· + a + s a + a − s + a A = 0 . (11.3) s p 0 1 − 1 2 p th-order Equation (11.3) is also the characteristic equation associated with the differential equation (11.1). A necessary condition for each root of Equation (11.3) to have a negative real ′ ′ ,...,a must be positive. This condition component is that all coefficients a p − 1 0 ′ ′ ′ is also sufficient for a p = 1 and 2. For a p must also > a = 3, the condition 0 2 1 hold. For higher-order systems, other conditions must hold. If each root of Equation (11.3) has a negative real component, we call the error dynamics stable . If any of the roots has a positive real component, the unstable , and the error ‖ θ can grow without bound as error dynamics are t ) ‖ ( e t →∞ . For second-order error dynamics, a good mechanical analogy to keep in mind is the linear mass–spring–damper (Figure 11.3). The position of the mass m is θ is applied to the mass. The damper applies a force and an external force f e ̇ θ to the mass, where b is the damping constant, and the spring applies a b − e − kθ force to the mass, where k is the spring constant. Therefore the equation e of motion of the mass can be written as ̈ ̇ θ (11.4) + b m θ f. + kθ = e e e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

427 Chapter 11. Robot Control 409 θ e k m f b A linear mass–spring–damper. Figure 11.3: In the limit as the mass approaches zero, the second-order dynamics (11.4) m reduces to the first-order dynamics ̇ b + kθ θ = f. (11.5) e e By the first-order dynamics, an external force generates a velocity rather than an acceleration. In the following subsections we consider the first- and second-order error responses for the homogeneous case ( f = 0) with 0, ensuring that the b,k > error dynamics are stable and that the error converges to zero ( e = 0). ss First-Order Error Dynamics 11.2.2.1 f = 0 can be written in the form The first-order error dynamics (11.5) with k ̇ ( t ) + θ ) = 0 θ t ( e e b or 1 ̇ θ ) + θ (11.6) t ( ( t ) = 0 , e e t where = b/k is called the time constant of the first-order differential equation. t The solution to the differential equation (11.6) is − t/ t θ θ ( t ) = (11.7) . (0) e e e The time constant t is the time at which the first-order exponential decay has decayed to approximately 37% of its initial value. The error response θ ) is ( t e θ (0) = 1. Plots of the error response are shown defined by the initial condition e in Figure 11.4 for different time constants. The steady-state error is zero, there is no overshoot in the decaying exponential error response, and the 2% settling time is determined by solving θ ) ( t e t t/ − = 0 . 02 = e θ (0) e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

428 410 11.2. Error Dynamics 1 decreasing t θ e 0 time t The first-order error response for three different time constants Figure 11.4: . for t . Solving, we get 02 = − t/ t → t = 3 . 91 t , ln 0 . . The response gets faster as the spring a 2% settling time of approximately 4 t k increases or the damping constant b decreases. constant 11.2.2.2 Second-Order Error Dynamics The second-order error dynamics b k ̈ ̇ θ ) = 0 t ( θ ( ( t ) + t θ ) + e e e m m standard second-order form can be written in the 2 ̇ ̈ t ) + 2 ζω , θ θ ) = 0 ( (11.8) ) + ω ( t θ ( t e e n e n where ω . is called the natural frequency and ζ is called the damping ratio n √ √ For the mass–spring–damper, k/ m and ζ = b/ (2 = k m ). The two roots ω n of the characteristic polynomial 2 2 s s + + 2 ζω (11.9) = 0 ω n n are √ √ 2 2 ω − + = s ζω s − 1 and 1 (11.10) = − ζω − ω − . ζ ζ n n 1 n n 2 The second-order error dynamics (11.8) is stable if and only if ζω > 0 and n 2 ω > 0. n If the error dynamics is stable, then there are three types of solutions ( t ) θ e to the differential equation, depending on whether the roots s are real and , 1 2 unequal ( ζ > 1), real and equal ( ζ = 1), or complex conjugates ( ζ < 1). May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

429 Chapter 11. Robot Control 411 : ζ > s Overdamped • 1. The roots are real and distinct, and the 1 , 2 solution to the differential equation (11.8) is t s s t 1 2 e θ + c ( c ) = e t , 2 e 1 can be calculated from the initial conditions. The response and c where c 2 1 is the sum of two decaying exponentials, with time constants = − 1 /s t 1 1 = − 1 /s t . The “slower” time constant in the solution is given by and 2 2 √ 2 1. − ζ + ω = s ζω the less negative root, − n n 1 θ The initial conditions for the (unit) error response are (0) = 1 and e ̇ c (0) = 0, and the constants and c θ can be calculated as e 1 2 1 ζ ζ 1 √ √ . and c = c − = + 2 1 2 2 2 2 2 − ζ ζ − 1 2 1 Critically damped : ζ = 1. The roots s ω are equal and real, • = − 2 , n 1 and the solution is − ω t n + , θ t ) e ( t ) = ( c c 1 2 e i.e., a decaying exponential multiplied by a linear function of time. The time constant of the decaying exponential is /ω t . For the error = 1 n ̇ θ (0) = 1 and θ (0) = 0, response with e e = 1 and c . = c ω n 1 2 : ζ < Underdamped s • are complex conjugates at s = 1. The roots , 1 , 2 2 1 √ 2 ζω ± jω − , where ω = ω damped natural frequency 1 is the − ζ . n n d d The solution is t ζω − n ) = ( c θ cos ω , t + c ( sin ω t t ) e d e 1 2 d = 1 / ( ζω i.e., a decaying exponential (time constant )) multiplied by a t n ̇ sinusoid. For the error response with (0) = 1 and θ θ (0) = 0, e e ζ √ . c c and = = 1 2 1 2 ζ − 1 Example root locations for the overdamped, critically damped, and under- θ 11.5. ( t ), are shown in Figure damped cases, as well as their error responses e This figure also shows the relationship between the root locations and proper- ties of the transient response: roots further to the left in the complex plane May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

430 412 11.2. Error Dynamics s s Im( ) Im( ) s Im( ) ω d ω s s s n 2 1 , 1 2 ) s Re( Re( ) s Re( s ) − ζω n 1 1 / − t / t − − ω 1 2 n 1) overdamped( ζ> 1)criticallydamped( ζ =1)underdamped( ζ< θ e ) s Im( increasing 1 overshoot, overdamped oscillation critically damped Re( s ) 0 t underdamped shorter unstable settling time (Top) Example root locations for overdamped, critically damped, and Figure 11.5: underdamped second-order systems. (Bottom left) Error responses for overdamped, critically damped, and underdamped second-order systems. (Bottom right) Relation- ship of the root locations to properties of the transient response. correspond to shorter settling times, and roots further away from the real axis correspond to greater overshoot and oscillation. These general relationships be- tween root locations and transient response properties also hold for higher-order systems with more than two roots. If the second-order error dynamics (11.8) is stable, the steady-state error e ss is zero regardless of whether the error dynamics is overdamped, underdamped, , where t cor- t or critically damped. The 2% settling time is approximately 4 s responds to the “slower” root if the error dynamics is overdamped. The 1 overshoot is zero for overdamped and critically damped error dynamics and, for underdamped error dynamics, the overshoot can be calculated by finding the ̇ t = 0) where the error response satisfies first time (after θ = 0. This is the e peak of the overshoot, and it occurs at t . = π/ω d p May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

431 Chapter 11. Robot Control 413 Substituting this into the underdamped error response, we get ) ( ( ( ) ) ) ( π π ζ π π/ω − ζω n d √ ω t ( ) = θ θ e sin = + ω cos p d e e d 2 ω ω ω d d d − 1 ζ √ 2 − πζ/ 1 ζ − − e = . √ 2 1 − ζ πζ/ − × 100%. e Therefore, by our definition of overshoot, the overshoot is ζ = 0 . 1 gives an overshoot of 73%, ζ = 0 . 5 gives an overshoot of 16%, and Thus = 0 8 gives an overshoot of 1 . 5%. . ζ 11.3 Motion Control with Velocity Inputs 8, we typically assume that there is direct control of As discussed in Chapter the forces or torques at robot joints, and the robot’s dynamics transforms those controls to joint accelerations. In some cases, however, we can assume that there is direct control of the joint velocities, for example when the actuators are stepper motors. In this case the velocity of a joint is determined directly by 1 the frequency of the pulse train sent to the stepper. Another example occurs when the amplifier for an electric motor is placed in velocity control mode – the amplifier attempts to achieve the joint velocity requested by the user, rather than a joint force or torque. In this section we will assume that the control inputs are joint velocities. 11.4, and indeed in the rest of the chapter, the control inputs are In Section assumed to be joint forces and torques. The motion control task can be expressed in joint space or task space. When the trajectory is expressed in task space, the controller is fed a steady stream X ( t ), and the goal is to command joint velocities of end-effector configurations d that cause the robot to track this trajectory. In joint space, the controller is fed a steady stream of desired joint positions ( t ). θ d The main ideas are well illustrated by a robot with a single joint, so we begin there and then generalize to a multi-joint robot. 1 This assumes that the torque requirements are low enough that the stepper motor can keep up with the pulse train. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

432 414 11.3. Motion Control with Velocity Inputs Motion Control of a Single Joint 11.3.1 11.3.1.1 Feedforward Control Given a desired joint trajectory ), the simplest type of control would be to ( θ t d ̇ ( θ ) as choose the commanded velocity t ̇ ̇ ) = t θ θ ( ( ) , (11.11) t d ̇ or t ) comes from the desired trajectory. This is called a feedforward θ ( where d , since no feedback (sensor data) is needed to implement open-loop controller it. 11.3.1.2 Feedback Control In practice, position errors will accumulate over time under the feedforward control law (11.11). An alternative strategy is to measure the actual position of each joint continually and implement a . feedback controller The simplest feedback con- P Control and First-Order Error Dynamics troller is ̇ ( t ) = K (11.12) ( θ , ( t ) − θ ( t θ K ) θ t ( )) = e d p p K where > 0. This controller is called a proportional controller, or P con- p troller , because it creates a corrective control proportional to the position error θ acts some- ( t ) = θ K ( t ) − θ ( t ). In other words, the constant control gain d e p what like a virtual spring that tries to pull the actual joint position to the desired joint position. The P controller is an example of a , as it creates a con- linear controller ( trol signal that is a linear combination of the error t ) and possibly its time θ e derivatives and time integrals. ̇ setpoint control ( t ) is constant, i.e., The case where θ . ( t ) = 0, is called θ d d In setpoint control, the error dynamics 0 *  ̇ ̇ ̇ θ θ ( t ) = t ( t ) − ) θ ( d e  ̇ is written as follows after substituting in the P controller θ ( t ) = K ): θ t ( e p ̇ ̇ . ( t ) = − K ) = 0 θ t ( t ) → t θ ( ( θ ) + K θ p e e p e e This is a first-order error dynamic equation (11.6) with time constant t = 1 /K . p The decaying exponential error response is illustrated in Figure 11.4. The May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

433 Chapter 11. Robot Control 415 steady-state error is zero, there is no overshoot, and the 2% settling time is 4 . A larger K means a faster response. /K p p ̇ ( t ) is not constant but Now consider the case where θ θ ( t ) is constant, i.e., d d ̇ ( t ) = θ . Then the error dynamics under the P controller can be written c d ̇ ̇ ̇ t ) = θ θ ) ( t ) − ( θ ( t ) = c − K t θ ( , e d e p which we rewrite as ̇ t t K θ θ c. ( ) + ) = ( e e p This is a first-order nonhomogeneous linear differential equation with solution ( ) c c t K − p ) = t ( θ − , θ + (0) e e e K K p p c/K as time goes to infinity. Unlike the which converges to the nonzero value p e case of setpoint control, the steady-state error is nonzero; the joint position ss always lags behind the moving reference. The steady-state error c/K can be p made small by choosing the control gain K large, but there are practical limits p on how large K can be. For one thing, real joints have velocity limits that may p prevent the realization of the large commanded velocities associated with a large K may cause instability when implemented by . For another, large values of K p p a discrete-time digital controller – the large gain may result in a large change in during a single servo cycle, meaning that the control action late in the servo θ e cycle is in response to sensor data that is no longer relevant. An alternative to using PI Control and Second-Order Error Dynamics a large gain is to introduce another term in the control law. A proportional- K p PI controller , adds a term that is proportional to the integral controller, or time-integral of the error: ∫ t ̇ θ K ( θ t ( t ) + K ) = t (11.13) , θ d (t) e p i e 0 where t is the current time and t is the variable of integration. The PI controller block diagram is illustrated in Figure 11.6. ̇ With this controller, the error dynamics for a constant ( t ) becomes θ d ∫ t ̇ ( t ) + K t = θ d ( t ) + K (t) θ c. θ i e p e e 0 Taking the time derivative of this dynamics, we get ̈ ̇ (11.14) ( t ) + K . θ θ ) = 0 ( t ) + K t θ ( e i e p e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

434 416 11.3. Motion Control with Velocity Inputs ̇ + + θ θ θ e d robot K Σ Σ p t d + − θ t d K i The block diagram of a PI controller that produces a commanded Figure 11.6: ̇ velocity θ as input to the robot. We can rewrite this equation in the standard second-order form (11.8), with √ √ K K natural frequency and damping ratio ζ = K ). / (2 = ω i n i p Relating the PI controller of Equation (11.14) to the mass–spring–damper K of Figure 11.3, the gain b/ m for the mass–spring–damper (a plays the role of p K means a larger damping constant larger b ), and the gain K plays the role i p (a larger K m k ). of k/ means a larger spring constant i > The PI-controlled error dynamics equation is stable if 0 and K K > 0, p i and the roots of the characteristic equation are √ 2 K K p p − ± = . K s − 1 i , 2 2 4 K Let’s hold equal to 20 and plot the roots in the complex plane as K grows i p from zero (Figure 11.7). This plot, or any plot of the roots as one parameter is varied, is called a root locus . 2 2 +20) = = 0, the characteristic equation s For + K s s + K ( = s K +20 s = s i p i increases, the roots move toward s s 0 has roots at = − 20. As K = 0 and i 2 1 each other on the real axis of the s -plane as shown in the left-hand panel in Figure 11.7. Because the roots are real and unequal, the error dynamics equation √ = K (2 / is overdamped ( ζ K ) > 1, case I) and the error response is sluggish p i due to the time constant of the exponential corresponding to the = − 1 /s t 1 1 “slow” root. As K increases, the damping ratio decreases, the “slow” root i moves left (while the “fast” root moves right), and the response gets faster. When K 2, and reaches 100, the two roots meet at s / K = = − 10 = − ω n p 1 i , 2 the error dynamics equation is critically damped ( ζ = 1, case II). The error t = 4 / ( ζω response has a short 2% settling time of 4 ) = 0 . 4 s and no overshoot n or oscillation. As K continues to grow, the damping ratio ζ falls below 1 and i the roots move vertically off the real axis, becoming complex conjugates at √ 100 (case III). The error dynamics is underdamped, and − K = − 10 ± s j i 2 , 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

435 Chapter 11. Robot Control 417 III I II I III III III (Left) The complex roots of the characteristic equation of the error Figure 11.7: increases from K dynamics of the PI velocity-controlled joint for a fixed K = 20 as i p zero. This is known as a root locus plot. (Right) The error response to an initial ̇ = 1, θ θ error = 0, is shown for overdamped ( ζ = 1 . 5, K 4, case I), critically = 44 . i e e ζ = 1, K damped ( = 100, case II), and underdamped ( ζ = 0 . 5, K = 400, case III) i i cases. the response begins to exhibit overshoot and oscillation as K increases. The i t = 1 / ( ζω settling time is unaffected as the time constant ) remains constant. n According to our simple model of the PI controller, we could always choose 2 without K and K / 4) and increase K = K K and for critical damping ( K p i i p i p bound to make the error response arbitrarily fast. As described above, however, there are practical limits. Within these practical limits, and K should be K p i chosen to yield critical damping. 11.8 shows for comparison the performances of a P controller and a PI Figure controller attempting to track a constant-velocity trajectory. The proportional gain K is the same in both cases, while K = 0 for the P controller. From the i p shape of the response, it appears that K in the PI controller was chosen to be i a bit too large, making the system underdamped. It is also clear that e = 0 ss for the PI controller but 6 = 0 for the P controller, agreeing with our analysis e ss above. ̇ If the desired velocity ( t ) is anything other than constant, the PI controller θ d cannot be expected to eliminate steady-state error completely. If it changes slowly, however, then a well-designed PI controller can be expected to provide better tracking performance than a P controller. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

436 418 11.3. Motion Control with Velocity Inputs 0.2 1.0 P control PI control 0.5 0 θ θ e P control PI control –0.2 0 1.0 00.5 00.51.0 time(s) time(s) The motion of P-controlled and PI-controlled joints, with initial position Figure 11.8: ̇ error, tracking a reference trajectory (dashed) where θ ( t ) is constant. (Left) The d θ ( t ). (Right) The error responses θ − ( t ) = θ ). ( t ) responses θ ( t e d 11.3.1.3 Feedforward Plus Feedback Control A drawback of feedback control is that an error is required before the joint begins to move. It would be preferable to use our knowledge of the desired trajectory ( t ) to initiate motion before any error accumulates. θ d We can combine the advantages of feedforward control, which commands motion even when there is no error, with the advantages of feedback control, which limits the accumulation of error, as follows: ∫ t ̇ ̇ ) + ) = ( t ) + K t θ ( t θ K ( θ (11.15) θ . (t) d t d e p i e 0 11.9, is our preferred This feedforward–feedback controller, illustrated in Figure control law for producing a commanded velocity to the joint. 11.3.2 Motion Control of a Multi-joint Robot The single-joint PI feedback plus feedforward controller (11.15 ) generalizes im- mediately to robots with joints. The reference position θ ) and actual po- ( t n d sition θ ( t ) are now n -vectors, and the gains K n and K × are diagonal n i p matrices of the form k are positive and I and k k I , where the scalars k and p i p i I is the n × n identity matrix. Each joint is subject to the same stability and performance analysis as the single joint in Section 11.3.1. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

437 Chapter 11. Robot Control 419 ̇ θ d + ̇ + θ θ θ e d robot K Σ Σ p + d t − + θ t d K i The block diagram of feedforward plus PI feedback control that pro- Figure 11.9: ̇ as input to the robot. θ duces a commanded velocity Task-Space Motion Control 11.3.3 We can express the feedforward plus feedback control law in task space. Let t ∈ SE (3) be the configuration of the end-effector as a function of time ( X ) and ( V ) be the end-effector twist expressed in the end-effector frame { b } , i.e., t b 1 − − 1 ̇ ̇ . A X ) and [ ( X . The desired motion is given by V X ] = X t ] = V [ X d d d b d task-space version of the control law (11.15) is ∫ t 1 − ( ) + K ) + t ] V X ( t K V ( . t ) = [Ad d (t) t X (11.16) i p d e b e X X d 0 1 − The term [Ad ] V expresses the feedforward twist V in the actual end- d d X X d effector frame at X X ) rather than the desired (which could also be written sb X (which could also be written X end-effector frame ). When the end-effector sd d = X ), this term reduces to V is at the desired configuration ( . Also, the X d d t X configuration error t ) is not simply X ), since it does not make ( t ) − X ( ( e d SE X sense to subtract elements of (3). Instead, as we saw in Section 6.2, e should refer to the twist which, if followed for unit time, takes to X . The X d se (3) representation of this twist, expressed in the end-effector frame, is [ X ] = e − 1 X ). log( X d 6 × 6 R ,K K take the form ∈ As in Section 11.3.2, the diagonal gain matrices i p I 0. k I , respectively, where k k ,k > and p i i p ̇ from the control law (11.16) realizing V The commanded joint velocities θ b can be calculated using the inverse velocity kinematics from Section 6.3, † ̇ = J θ , ( θ ) V b b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

438 420 11.4. Motion Control with Torque or Force Inputs † ( θ ) is the pseudoinverse of the body Jacobian. J where b Motion control in task space can be defined using other representations of the end-effector configuration and velocity. For example, for a minimal coordinate m R representation of the end-effector configuration , the control law can be x ∈ written ∫ t t ) = ̇ x . ( t ) + K (11.17) ( x ̇ ( t ) − x ( t )) + K t x ( d ( x (t)) (t) − x d i p d d 0 = ( ), with velocities represented X For a hybrid configuration representation R,p ω ̇ p ): by ( , b [ ] ] [ ][ ∫ t T ) ( ) ω ω ( t t t ( ) 0 R ) t ( R d b d X K ( + K ) + t = X (t) d t , i p e e ) ( p ̇ ̇ p ( ) t t I 0 d 0 (11.18) where ] [ T R d log( R t ( ( ) )) d t ( . X ) = e ( ) − p t t ) p ( d Figure 11.10 shows the performance of the control law (11.16), where the end-effector velocity is the body twist V , and the performance of the control b law (11.18), where the end-effector velocity is ( ω ). The control task is to , ̇ p b X at the origin from the initial configuration stabilize d     0 − 1 0 1     1 0 0 1 = , p . R = 0 0 0 0 0 1 The feedforward velocity is zero and K shows the different = 0. Figure 11.10 i paths followed by the end-effector. The decoupling of linear and angular control in the control law (11.18) is visible in the straight-line motion of the origin of the end-effector frame. An application of the control law (11.16) to mobile manipulation can be found in Section 13.5. 11.4 Motion Control with Torque or Force Inputs Stepper-motor-controlled robots are generally limited to applications with low or predictable force–torque requirements. Also, robot-control engineers do not rely on the velocity-control modes of off-the-shelf amplifiers for electric motors, because these velocity-control algorithms do not make use of a dynamic model of the robot. Instead, robot-control engineers use amplifiers in torque-control May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

439 Chapter 11. Robot Control 421 (Left) The end-effector configuration converging to the origin under Figure 11.10: the control law (11.16), where the end-effector velocity is represented as the body twist . (Right) The end-effector configuration converging to the origin under the control V b , law (11.18), where the end-effector velocity is represented as ( ̇ p ). ω b mode: the input to the amplifier is the desired torque (or force). This allows the robot-control engineer to use a dynamic model of the robot in the design of the control law. In this section, the controller generates joint torques and forces to try to track a desired trajectory in joint space or task space. Once again, the main ideas are well illustrated by a robot with a single joint, so we begin there and then generalize to a multi-joint robot. 11.4.1 Motion Control of a Single Joint Consider a single motor attached to a single link, as shown in Figure 11.11. Let be the motor’s torque and θ be the angle of the link. The dynamics can be τ written as ̈ = M τ θ + m gr cos θ, (11.19) where is the scalar inertia of the link about the axis of rotation, m is the M mass of the link, r is the distance from the axis to the center of mass of the link, and g ≥ 0 is the gravitational acceleration. According to the model (11.19) there is no dissipation: if the link were made to move and τ were then set to zero, the link would move forever. This is unrealistic, of course; there is bound to be friction at the various bearings, gears, and transmissions. Friction modeling is an active research area, but in a May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

440 422 11.4. Motion Control with Torque or Force Inputs r θ g m Figure 11.11: A single-joint robot rotating under gravity. The center of mass is indicated by the checkered disk. simple model, rotational friction is due to viscous friction forces, so that ̇ = θ, (11.20) τ b fric where b > 0. Adding the friction torque, our final model is ̇ ̈ M θ, + m gr cos θ + b = τ (11.21) θ which we may write more compactly as ̈ ̇ (11.22) θ + h M θ, τ θ ) , = ( where contains all terms that depend only on the state, not the acceleration. h 2 M = 0 For concreteness in the following simulations, we set 5 kg m , m = . 1 kg, r = 0 . 1 m, and b = 0 . 1 N m s/rad. In some examples the link moves in a horizontal plane, so g = 0. In other examples, the link moves in a vertical 2 g . 81 m/s . plane, so = 9 Feedback Control: PID Control 11.4.1.1 A common feedback controller is linear proportional-integral-derivative con- PID control . The PID controller is simply the PI controller (Equa- trol, or tion (11.13)) with an added term proportional to the time derivative of the error, ∫ ̇ (11.23) t + θ , + K θ τ θ = (t) d K K d i e e p e where the control gains K K , K are positive. The proportional gain , and K d p p i acts as a virtual spring that tries to reduce the position error θ . The = θ θ − d e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

441 Chapter 11. Robot Control 423 + + θ θ τ e d arm K Σ Σ p dynamics + − + θ d t K i d K d dt Figure 11.12: Block diagram of a PID controller. derivative gain acts as a virtual damper that tries to reduce the velocity error K d ̇ ̇ ̇ θ θ − . The integral gain can be used to reduce or eliminate steady-state θ = e d errors. The PID controller block diagram is given in Figure 11.12. PD Control and Second-Order Error Dynamics For now let’s consider K the case where = 0. This is known as PD control. Let’s also assume the i robot moves in a horizontal plane ( = 0). Substituting the PD control law into g the dynamics (11.21), we get ̇ ̇ ̈ ̇ θ θ = K (11.24) ( θ . − θ ) + K ) θ + b θ − M ( d d p d ̇ ̈ with If the control objective is setpoint control at a constant θ = 0, θ θ = d d d ̈ ̈ ̇ ̇ . Equation (11.24) can be rewritten as θ , = θ − = − θ θ , and θ θ θ = − then e d e e ̇ ̈ b + K ) θ θ + ( K M θ = 0 , (11.25) + e p e d e or, in the standard second-order form (11.8), as K b + K d p 2 ̇ ̇ ̈ ̈ θ , ω θ (11.26) = 0 → θ θ + 2 ζω + θ + = 0 θ + e e n e e e e n M M ζ ω are where the damping ratio and the natural frequency n √ b + K K d p √ . ζ = and ω = n M K 2 M p For stability, b + K must be positive. If the error dynamics equation is and K p d stable then the steady-state error is zero. For no overshoot and a fast response, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

442 424 11.4. Motion Control with Torque or Force Inputs the gains should be chosen to satisfy critical damping ( ζ = 1). For a K and K p d should be chosen to be as high as possible, subject to practical K fast response, p issues such as actuator saturation, undesired rapid torque changes (chattering), vibrations of the structure due to unmodeled flexibility in the joints and links, and possibly even instability due to the finite servo rate frequency. Now consider the case PID Control and Third-Order Error Dynamics g > 0). With the of setpoint control where the link moves in a vertical plane ( PD control law above, the error dynamics can now be written ̈ ̇ + ( b θ K (11.27) ) M θ θ. + K cos θ gr = m + e e d e p θ K This implies that the joint comes to rest at a configuration θ = satisfying p e gr m , i.e., the final error θ cos is nonzero when θ θ 6 = ± π/ 2. The reason is that d e 6 = ± π/ θ the robot must provide a nonzero torque to hold the link at rest at 2, θ but the PD control law creates a nonzero torque at rest only if 6 = 0. We can e K but, as discussed make this steady-state error small by increasing the gain p above, there are practical limits. To eliminate the steady-state error, we return to the PID controller by setting K > 0. This allows a nonzero steady-state torque even with zero position error; i integrated error must be nonzero. Figure 11.13 demonstrates the effect only the of adding the integral term to the controller. To see how this works, write down the setpoint error dynamics ∫ ̈ ̇ (11.28) + ( b M K , ) + θ τ + K t = θ d + K (t) θ θ e p e dist d e i e τ is a disturbance torque substituted for the gravity term where m gr cos θ . dist Taking derivatives of both sides, we get the third-order error dynamics (3) ̈ ̇ + ( b + K Mθ ) . θ τ + K = ̇ (11.29) θ θ + K i e e dist d p e e τ If is constant then the right-hand side of Equation (11.29) is zero, and its dist characteristic equation is K K b + K p i d 2 3 + (11.30) . s + s = 0 s + M M M If all roots of Equation (11.30) have a negative real part then the error dynamics is stable, and converges to zero. (While the disturbance torque due to gravity θ e ̇ goes to zero, θ is not constant as the link rotates, it approaches a constant as and therefore similar reasoning holds close to the equilibrium θ = 0.) e May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

443 Chapter 11. Robot Control 425 erm Pt desire = onfig dc fina onfig P lc ID erm Dt 1 θ nt PD rol co e erm Pt g PDfina lc onfig erm It trol on Dc PI 0 lc onfig initia Dt erm 0 5 10 rols cont time(s) K = 2 N m s/rad (Left) The tracking errors for a PD controller with Figure 11.13: d K . = 2 and 205 N m/rad for critical damping, and a PID controller with the same p ̇ π/ = 1 N m/(rad s). The arm starts at θ (0) = − K 2 , PD gains and θ (0) = 0, with i ̇ θ = 0. (Middle) The individual contributions of the terms in = 0 , a goal state θ d d the PD and PID control laws. Note that the nonzero I (integral) term for the PID controller allows the P (proportional) term to drop to zero. (Right) The initial and final configurations, with the center of mass indicated by checkered disks. For all the roots of Equation (11.30) to have a negative real component, the following conditions on the control gains must be satisfied for stability (Sec- tion 11.2.2.2): K > − b d 0 > K p ( + K b ) K d p > K > 0 . i M Thus the new gain K an upper bound (Fig- must satisfy both a lower and i ure K 11.14). A reasonable design strategy is to choose and K for a good d p transient response and then choose K large enough that it is helpful in reducing i or eliminating steady-state errors but small enough that it does not significantly impact stability. In the example of Figure 11.13, the relatively large K wors- i ens the transient response, giving significant overshoot, but steady-state error is eliminated. In practice, K = 0 for many robot controllers, since stability is paramount. i Other techniques can be employed to limit the adverse stability effects of integral control, such as integrator anti-windup , which places a limit on how large the error integral is allowed to grow. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

444 426 11.4. Motion Control with Torque or Force Inputs s ) Im( ) s Re( The movement of the three roots of Equation (11.30) as increases K Figure 11.14: i from zero. First a PD controller is chosen with K K yielding critical damping, and p d giving rise to two collocated roots on the negative real axis. Adding an infinitesimal K > 0 creates a third root at the origin. As we increase K gain , one of the two i i collocated roots moves to the left on the negative real axis while the other two roots move toward each other, meet, break away from the real axis, begin curving to the right, and finally move into the right half-plane when . The = ( b + K /M ) K K p i d system is unstable for larger values of . K i Pseudocode for the PID control algorithm is given in Figure 11.15. While our analysis has focused on setpoint control, the PID controller applies ̇ θ ( t ) 6 = 0. Integral control will not perfectly well to trajectory following, where d eliminate tracking error along arbitrary trajectories, however. 11.4.1.2 Feedforward Control Another strategy for trajectory following is to use a model of the robot’s dy- namics to proactively generate torques instead of waiting for errors. Let the controller’s model of the dynamics be ̇ ̃ ̈ ̃ = ) M θ + ( h ( θ, θ θ ) , (11.31) τ ̃ ̇ ̇ ̃ θ where the model is perfect if M ( θ ) and ) = h ( θ, M θ ) = h ( θ, ( θ ). Note that the ̃ θ M ( θ ) is written as a function of the configuration inertia model . While the inertia of our simple one-joint robot is not a function of configuration, writing May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

445 Chapter 11. Robot Control 427 time = 0 // dt = servo cycle time eint = 0 // error integral qprev = senseAngle // initial joint angle q loop [qd,qdotd] = trajectory(time) // from trajectory generator q = senseAngle // sense actual joint angle qdot = (q - qprev)/dt // simple velocity calculation qprev = q e = qd - q edot = qdotd - qdot eint = eint + e*dt tau = Kp*e + Kd*edot + Ki*eint commandTorque(tau) time = time + dt end loop Figure 11.15: Pseudocode for PID control. the equations in this way allows us to re-use Equation (11.31) for multi-joint 11.4.2. systems in Section ̇ ̈ θ θ θ Given , and , from a trajectory generator, the feedforward torque is d d d calculated as ̈ ̃ ̇ ̃ θ M ( θ (11.32) ( t )) τ ( . ( t ) + t h ( θ )) ( t ) , ) = θ t ( d d d d If the model of the robot dynamics is exact, and there are no initial state errors, then the robot follows the desired trajectory exactly. A pseudocode implementation of feedforward control is given in Figure 11.16. Figure 11.17 shows two examples of feedforward-trajectory following for the link under gravity. Here, the controller’s dynamic model is correct except that it has ̃ r = 0 . 08 m, when actually r = 0 . 1 m. In Task 1 the error stays small, as unmodeled gravity effects provide a spring-like force to = − π/ 2, accelerating θ the robot at the beginning and decelerating it at the end. In Task 2, unmodeled gravity effects act against the desired motion, resulting in a larger tracking error. Because there are always modeling errors, feedforward control is always used in conjunction with feedback, as discussed next. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

446 428 11.4. Motion Control with Torque or Force Inputs time = 0 // dt = servo cycle time loop [qd,qdotd,qdotdotd] = trajectory(time) // trajectory generator tau = Mtilde(qd)*qdotdotd + htilde(qd,qdotd) // calculate dynamics commandTorque(tau) time = time + dt end loop Figure 11.16: Pseudocode for feedforward control. − 4 π/ actual Ta sk1 θ desired 4 − π/ 3 4 π/ 3 desired θ Ta sk2 4 π/ actual g 01234 time(s) Results of feedforward control with an incorrect model: ̃ r = 0 . 08 m, Figure 11.17: but r = 0 . 1 m. The desired trajectory in Task 1 is θ ) for ( t ) = − π/ 2 − ( π/ 4) cos( t d 4) cos( ≤ t ≤ π . The desired trajectory for Task 2 is θ . ( t ) = π/ 2 − ( π/ 0 t ) , 0 ≤ t ≤ π d 11.4.1.3 Feedforward Plus Feedback Linearization All practical controllers use feedback, as no model of robot and environment dynamics will be perfect. Nonetheless, a good model can be used to improve performance and simplify analysis. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

447 Chapter 11. Robot Control 429 ̃ ̃ { h } to Let’s combine PID control with a model of the robot dynamics M, achieve the error dynamics ∫ ̇ ̈ K + θ (11.33) + K K θ c + t = d θ θ (t) e i e e e d p along arbitrary trajectories, not just to a setpoint. The error dynamics (11.33) and a proper choice of PID gains ensure exponential decay of the trajectory error. ̈ ̈ ̈ = θ θ , to achieve the error dynamics (11.33) we choose the robot’s − θ Since d e commanded acceleration to be ̈ ̈ ̈ θ − θ θ = , d e then combine this with Equation (11.33) to get ∫ ̈ ̈ ̇ θ K θ = + θ K + K + θ d (11.34) . θ t (t) d d p i e e e ̈ ̃ ̃ from Equation (11.34) into a model of the robot dynamics { Substituting M, , h } θ we get the feedforward plus feedback linearizing controller , also called inverse dynamics controller computed torque controller : or the the ( ) ∫ ̈ ̇ ̇ ̃ ̃ d ) + K M θ τ + K ( = θ θ (t) θ t + K θ θ, + h ( θ ) . (11.35) e d d p e e i This controller includes a feedforward component due to the use of the planned ̇ ̈ θ θ and acceleration θ and is called feedback linearizing because feedback of d ̃ ̇ ( θ, θ is used to generate the linear error dynamics. The ) term cancels the h ̃ dynamics that depends nonlinearly on the state, and the inertia model M ( θ ) converts the desired joint accelerations into joint torques, realizing the simple linear error dynamics (11.33). A block diagram of the computed torque controller is shown in Figure 11.18. The gains K are chosen to place the roots of the characteristic ,K K , and i p d equation so as to achieve good transient response. In practice K is often chosen i to be zero. Figure 11.19 shows the typical behavior of computed torque control relative to feedforward and feedback only. Pseudocode is given in Figure 11.20. 11.4.2 Motion Control of a Multi-joint Robot The methods applied above for a single-joint robot carry over directly to n -joint robots. The difference is that the dynamics (11.22) now takes the more general, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

448 430 11.4. Motion Control with Torque or Force Inputs ̈ θ d + ̇ ̈ ̈ + θ θ, θ + θ θ e arm τ fb PI D ̇ ̈ ̃ θ , θ Σ Σ Σ θ ) θ ( M d d s d ynamic c r on trolle + + − ̃ ̇ ( θ h θ, ) ̈ Figure 11.18: θ is added to Computed torque control. The feedforward acceleration d ̈ computed by the PID feedback controller to create the commanded θ the acceleration fb ̈ . θ acceleration d t Figure 11.19: Performance of feedforward only (ff), feedback only (fb), and com- puted torque control (ff+fb). The PID gains are taken from Figure 11.13, and the feedforward modeling error is taken from Figure 11.17. The desired motion is Task 2 from Figure 11.17 (left-hand plot). The center plot shows the tracking performance of ∫ 2 τ (t) d t, a standard measure of the the three controllers. The right-hand plot shows control effort, for each of the three controllers. These plots show typical behavior: the computed torque controller yields better tracking than either feedforward or feedback alone, with less control effort than feedback alone. vector-valued, form ̈ ̇ M ( θ ) h θ + τ ( θ, = θ ) , (11.36) where the n n positive-definite mass matrix M is now a function of the con- × θ . In general, the components of the dynamics (11.36) are coupled – figuration the acceleration of a joint is a function of the positions, velocities, and torques at other joints. We distinguish between two types of control for multi-joint robots: decen- tralized control, where each joint is controlled separately with no sharing of information between joints, and centralized control, where full state informa- tion for each of the n joints is available to calculate the controls for each joint. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

449 Chapter 11. Robot Control 431 time = 0 // dt = cycle time eint = 0 // error integral qprev = senseAngle // initial joint angle q loop [qd,qdotd,qdotdotd] = trajectory(time) // from trajectory generator q = senseAngle // sense actual joint angle qdot = (q - qprev)/dt // simple velocity calculation qprev = q e = qd - q edot = qdotd - qdot eint = eint + e*dt tau = Mtilde(q)*(qdotdotd+Kp*e+Kd*edot+Ki*eint) + htilde(q,qdot) commandTorque(tau) time = time + dt end loop Figure 11.20: Pseudocode for the computed torque controller. 11.4.2.1 Decentralized Multi-joint Control The simplest method for controlling a multi-joint robot is to apply at each joint an independent controller, such as the single-joint controllers discussed in Section 11.4.1. Decentralized control is appropriate when the dynamics are decoupled, at least approximately. The dynamics are decoupled when the ac- celeration of each joint depends only on the torque, position, and velocity of that joint. This requires that the mass matrix be diagonal, as in Cartesian or gantry robots, where the first three axes are prismatic and orthogonal. This kind of robot is equivalent to three single-joint systems. Approximate decoupling is also achieved in highly geared robots in the ab- sence of gravity. The mass matrix M ( θ ) is nearly diagonal, as it is dominated by the apparent inertias of the motors themselves (see Section 8.9.2). Signifi- cant friction at the individual joints also contributes to the decoupling of the dynamics. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

450 432 11.4. Motion Control with Torque or Force Inputs 11.4.2.2 Centralized Multi-joint Control When gravity forces and torques are significant and coupled, or when the mass M ) is not well approximated by a diagonal matrix, decentralized con- ( matrix θ trol may not yield acceptable performance. In this case the computed torque can be generalized to a multi-joint robot. The controller (11.35) of Figure 11.18 θ = and the error θ θ and θ -vectors, and the configurations θ are now n − d e d positive scalar gains become positive-definite matrices K ,K ,K : i p d ( ) ∫ ̈ ̃ ̇ ̇ ̃ (11.37) + K ( θ + K ) = M θ τ θ K t + d (t) θ ( θ θ, . + θ h ) i e p d d e e , where I k I , and k k , k I , k , and Typically, we choose the gain matrices as d i i p p are nonnegative scalars. Commonly, k is chosen to be zero. In the case of an k d i ̃ ̃ M , the error dynamics of each joint reduces to exact dynamics model for and h the linear dynamics (11.33). The block diagram and pseudocode for this control and 11.20, respectively. algorithm are found in Figures 11.18 Implementing the control law (11.37) requires calculating potentially com- plex dynamics. We may not have a good model of these dynamics, or the equations may be too computationally expensive to calculate at servo rate. In this case, if the desired velocities and accelerations are small, an approximation to (11.37) can be obtained using only PID control and gravity compensation: ∫ ̇ θ + K τ θ (t) d t + K = K θ (11.38) + ̃ g ( θ ) . e d i e p e = K With zero friction, perfect gravity compensation, and PD setpoint control ( i ̇ ̈ = θ θ 0 and = 0), the controlled dynamics can be written as d d ̈ ̇ ̇ ̇ θ + C ( θ, M θ ) ( θ = K θ ) (11.39) − K θ, θ d p e ̇ ̇ ( θ, where the Coriolis and centripetal terms are written ) θ . We can now C θ define a virtual “error energy,” which is the sum of an “error potential energy” K and an “error kinetic energy”: stored in the virtual spring p 1 1 T T ̇ ̇ ̇ ( θ ) = , V θ M (11.40) + θ K . θ θ θ ( θ ) e p e e e e e 2 2 ̇ Since θ = 0, this reduces to d 1 1 T T ̇ ̇ ̇ θ (11.41) θ ) θ. K θ + , ( θ θ ) = M ( θ V e e p e 2 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

451 Chapter 11. Robot Control 433 Taking the time derivative and substituting (11.39) into it, we get 1 T T T ̇ ̈ ̇ ̇ ̇ ̇ ̇ θ M ( θ ) V θ + K = + θ θ − θ M ( θ ) θ e p 2 ( ) 1 T T T ̇ ̇ ̇ ̇ ̇ ̇ ̇ ̇ θ θ θ ( K M θ. θ − C ( θ, (11.42) θ ) − θ ) + K = θ + θ K θ − p e d p e 2 ̇ M − 2 C Rearranging, and using the fact that is skew symmetric (Proposi- 8.1.2), we get tion 0 :   ( ) ( )  1  T T T  ̇ ̇ ̇ ̇ ̇ ̇ ̇ ̇  θ θ 2 ( M ) θ θ, ( − C ) θ θ K θ − = + θ V θ − K K + θ  e p p e d   2   T ̇ ̇ − θ K = θ ≤ 0 . (11.43) d ̇ ̇ 6 = 0. If This shows that the error energy is decreasing when θ = 0 and θ 6 = θ , θ d ̈ ̇ θ 6 = 0, so the virtual spring ensures that θ will again become nonzero and more e error energy will be dissipated. Thus, by the Krasovskii–LaSalle invariance principle (Exercise 11.12), the total error energy decreases monotonically and θ ( θ the robot converges to rest at = 0) from any initial state. d e 11.4.3 Task-Space Motion Control 11.4.2 we focused on motion control in joint space. On the one hand, In Section this is convenient because joint limits are easily expressed in this space, and the robot should be able to execute any joint-space path respecting these limits. Trajectories are naturally described by the joint variables, and there are no issues of singularities or redundancy. On the other hand, since the robot interacts with the external environment and objects in it, it may be more convenient to express the motion as a trajectory of the end-effector in task space. Let the end-effector trajectory be specified by − 1 ̇ t ) , V is expressed ( t )), where X ∈ SE (3) and [ V V ] = X ( X ( X , i.e., the twist b b b in the end-effector frame { b } . Provided that the corresponding trajectory in joint space is feasible, we now have two options for control: (1) convert to 11.4.2 or (2) a joint-space trajectory and proceed with controls as in Section express the robot dynamics and control law in the task space. The first option is to convert the trajectory to joint space. The forward ̇ X = T ( θ ) and V . Then the joint-space trajectory is = J θ ( θ ) kinematics are b b May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

452 434 11.5. Force Control obtained from the task-space trajectory using inverse kinematics (Chapter 6): − 1 T (inverse kinematics) ( ( θ ( t )) , (11.44) t ) = X † ̇ t J ( t ( θ ( t )) V θ ( ) = ) , (11.45) b b ( ) † ̈ ̇ ̇ ̇ ( θ ( t )) ( t V (11.46) ( t ) − ) = J . ( θ ( t )) J θ ( t ) θ b b b A drawback of this approach is that we must calculate the inverse kinematics, † ̇ J , which may require significant computing power. , and J b b The second option is to express the robot’s dynamics in task-space coordi- 8.6. Recall the task-space dynamics nates, as discussed in Section ̇ θ = Λ( F V . + η ( θ, V ) ) b b b τ are related to the wrenches F The joint forces and torques expressed in the b T J end-effector frame by τ ( θ = F . ) b b We can now write a control law in task space inspired by the computed torque control law in joint coordinates (11.37), = τ ( ( ) ) ∫ d T ̃ 1 − ) Λ( θ θ J ) ( + V ) + K X ([Ad K ] X (t) d t + K V ) + ̃ ( , θ, η V p d e i e d e b X X b d dt (11.47) d ̃ − 1 ) ([Ad V ] where , ̃ { η } represents the controller’s dynamics model and Λ d X X d dt is the feedforward acceleration expressed in the actual end-effector frame at X ̇ at states close to the reference state). The V (this term can be approximated as d − 1 satisfies [ X X ] = log( X configuration error is the twist, expressed in X ): X d e e e the end-effector frame, which, if followed for unit time, would move the current configuration X X . The velocity error is calculated to the desired configuration d as − 1 = [Ad −V ] V V . e d X X d − 1 The transform [Ad ] expresses the reference twist V , which is expressed d X X d in the frame X , in which the actual , as a twist in the end-effector frame at X d V is represented, so the two expressions can be differenced. velocity 11.5 Force Control When the task is not to create motions at the end-effector but to apply forces and torques to the environment, force control is needed. Pure force control is only May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

453 Chapter 11. Robot Control 435 possible if the environment provides resistance forces in every direction (e.g., if the end-effector is embedded in concrete or attached to a spring providing resistance in every motion direction). Pure force control is something of an some direction. abstraction, as robots are usually able to move freely in at least It is a useful abstraction, however, that leads to hybrid motion-force control as discussed in Section 11.6. In ideal force control, the force applied by the end-effector is unaffected by disturbance motions applied to the end-effector. This is dual to the case of ideal motion control, where the motion is unaffected by disturbance forces. be the wrench applied by the manipulator to the environment. The F Let tip manipulator dynamics can be written as T ̈ ̇ ̇ θ + c ( θ, M θ ) + ( ( θ ) + b ( θ θ ) + J ) ( θ ) F g = τ, (11.48) tip θ where and J ( F ) are defined in the same frame (the space frame or the end- tip effector frame). Since the robot typically moves slowly (or not at all) during a force control task, we can ignore the acceleration and velocity terms to get T ( θ ) + J g ( θ ) F (11.49) = τ. tip In the absence of any direct measurements of the force–torque at the robot end-effector, joint-angle feedback alone can be used to implement the force- control law T θ g θ ) + = ̃ τ ( ( ) F (11.50) , J d where ̃ g ( θ ) is a model of the gravitational torques and F is the desired wrench. d This control law requires a good model for gravity compensation as well as precise control of the torques produced at the robot joints. In the case of a DC electric motor without gearing, torque control can be achieved by current control of the motor. In the case of a highly geared actuator, a large friction torque in the gearing can degrade the quality of torque control achieved using only current control. In this case, the output of the gearing can be instrumented with strain gauges to measure the joint torque directly; this information is fed back to a local controller that modulates the motor current to achieve the desired output torque. Another solution is to equip the robot arm with a six-axis force-torque sensor between the arm and the end-effector to directly measure the end-effector wrench 2 F (Figure 11.21). Consider a PI force controller with a feedforward term and tip 2 Derivative control is not typically relevant for two reasons: (1) force measurements are often noisy, so their computed time derivatives are nearly meaningless; and (2) we are assuming the direct control of the joint torques and forces, and our simple rigid-body dynamics models imply direct transmission to the end-effector forces – there is no dynamics that integrates our control commands to produce the desired behavior, unlike with motion control. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

454 436 11.5. Force Control six-axis force-torque sensor A six-axis force–torque sensor mounted between a robot arm and its Figure 11.21: end-effector. gravity compensation, ( ) ∫ T ) + J + ( θ ) τ F (11.51) + K , F = ̃ g ( t θ F d (t) K e e fp d fi and F = F where −F are positive-definite proportional and K K and fp e fi tip d integral gain matrices, respectively. In the case of perfect gravity modeling, plugging the force controller (11.51) into the dynamics (11.49), we get the error dynamics ∫ t = 0 (11.52) + K . d F F (t) K e fp e fi In the case of a nonzero but constant force disturbance on the right-hand side of (11.52), arising from an incorrect model of ̃ g θ ), for example, we take the ( derivative to get ̇ (11.53) F K , + K = 0 F e e fp fi showing that F . converges to zero for positive-definite K K and fp fi e The control law (11.51) is simple and appealing but potentially dangerous if incorrectly applied. If there is nothing for the robot to push against, it will accelerate in a failing attempt to create end-effector forces. Since a typical force-control task requires little motion, we can limit this acceleration by adding May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

455 Chapter 11. Robot Control 437 velocity damping. This gives the modified control law ( ) ∫ T + τ ( θ ) ( F θ + K (11.54) F ) + g K J = ̃ K , (t) d t − F V e fp d fi damp e K where is positive definite. damp 11.6 Hybrid Motion–Force Control Most tasks requiring the application of controlled forces also require the gener- ation of controlled motions. Hybrid motion-force control is used to achieve -dimensional then we are free to specify of n n this. If the the task space is forces and motions at any time the 2 ; the other n are determined by the n t environment. Apart from this constraint, we also should not specify forces and motions in the “same direction,” as then they are not independent. For example, consider a two-dimensional environment modeled by a damper, = v , where f B env [ ] 2 1 B = . env 1 1 Defining the components of v and f as ( v v ,v + ) and ( f v ,f = 2 ), we have f 1 1 2 1 2 2 1 = . We have f + v v n = 2 freedoms to choose among the 2 n = 4 and 2 2 1 f and velocities and forces at any time. As an example, we can specify both 1 are determined independently, because B v is not diagonal. Then v f and 2 1 2 env B . We cannot independently control both , as these are and 2 v by + v f 1 env 2 1 in the “same direction” according to the environment. 11.6.1 Natural and Artificial Constraints A particularly interesting case occurs when the environment is infinitely stiff (rigid constraints) in directions and unconstrained in n − k directions. In k this case, we cannot choose of the 2 n motions and forces to specify – the which contact with the environment chooses the k directions in which the robot can freely apply forces and the n − k directions of free motion. For example, consider a task space with the = 6 dimensions of SE (3). Then a robot firmly grasping n a cabinet door has 6 − k = 1 motion freedom of its end-effector, i.e., rotation about the cabinet hinges, and therefore k = 5 force freedoms; the robot can apply any wrench that has zero moment about the axis of the hinges without moving the door. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

456 438 11.6. Hybrid Motion–Force Control As another example, a robot writing on a chalkboard may freely control the k = 1), but it cannot penetrate the board; it may freely force into the board ( = 5 degrees of freedom (two specifying the motion of the tip of move with 6 − k the chalk in the plane of the board and three describing the orientation of the chalk), but it cannot independently control the forces in these directions. The chalk example comes with two caveats. The first is due to friction – the chalk-wielding robot can actually control forces tangent to the plane of the board provided that the requested motion in the plane of the board is zero and the requested tangential forces do not exceed the static friction limit determined by the friction coefficient and the normal force into the board (see the discussion of friction modeling in Section 12.2). Within this regime, the robot has three motion freedoms (rotations about three axes intersecting at the contact between the chalk and the board) and three linear force freedoms. Second, the robot could decide to pull away from the board. In this regime, the robot would have six motion freedoms and no force freedoms. Thus the configuration of the robot is not the only factor determining the directions of the motion and force freedoms. Nonetheless, in this section we consider the simplified case where the motion and force freedoms are determined solely by the robot’s configuration, and all constraints are equality constraints. For example, the inequality velocity constraint of the board (the chalk cannot penetrate the board) is treated as an equality constraint (the robot also does not pull the chalk away from the board). As a final example, consider a robot erasing a frictionless chalkboard us- ( ing an eraser modeled as a rigid block (Figure 11.22). Let ) ∈ SE (3) X t { b } relative to a space frame { s } . be the configuration of the block’s frame ,v V = ( ω The body-frame twist and wrench are written ,ω ) and ,ω ,v ,v x y b z y x z ), respectively. Maintaining contact with the board F m ,m ,m ,f ,f ,f = ( b x y y x z z puts k = 3 constraints on the twist: , ω = 0 x ω = 0 , y . v = 0 z In the language of Chapter 2, these velocity constraints are holonomic – the differential constraints can be integrated to give configuration constraints. These constraints are called natural constraints , specified by the en- vironment. There are 6 − k = 3 natural constraints on the wrench, too: m f = f = = 0. In light of the natural constraints, we can freely spec- z x y ify any twist of the eraser satisfying the k = 3 velocity constraints and any wrench satisfying the 6 − k = 3 wrench constraints (provided that f 0, to < z maintain contact with the board). These motion and force specifications are May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

457 Chapter 11. Robot Control 439 ˆz b ˆy b ˆz s } b { ˆx b s { } ˆx s ˆy s The fixed space frame { s } is attached to the chalkboard and the body Figure 11.22: { frame } is attached to the center of the eraser. b artificial constraints called . Below is an example set of artificial constraints with the corresponding natural constraints: natural constraint artificial constraint ω = 0 m = 0 x x ω = 0 m = 0 y y m = 0 = 0 ω z z v = k = 0 f x x 1 = 0 = 0 v f y y = 0 f v k < 0 = z z 2 The artificial constraints cause the eraser to move with v while apply- = k 1 x k against the board. ing a constant force 2 11.6.2 A Hybrid Motion–Force Controller We now return to the problem of designing a hybrid motion–force controller. If the environment is rigid, then we can express the k natural constraints on the velocity in task space as the Pfaffian constraints = 0 A θ ) V ( , (11.55) May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

458 440 11.6. Hybrid Motion–Force Control k × 6 6 ( ∈ R for twists V ∈ R A . This formulation includes holonomic and θ where ) nonholonomic constraints. 8.6), in the absence of If the task-space dynamics of the robot (Section constraints, is given by ̇ F V + = Λ( ( θ, V ) , θ ) η T J ( τ θ ) F are the joint torques and forces created by the actuators, where = 8.7, is then the constrained dynamics, following Section T ̇ (11.56) , ( η ( θ V ) + A ) = Λ( θ ) λ F V + θ, ︸ ︷︷ ︸ F tip k ∈ R where are Lagrange multipliers and F λ is the wrench that the robot tip applies against the constraints. The requested wrench F must lie in the column d T θ A space of ( ). Since Equation (11.55) must be satisfied at all times, we can replace it by the time derivative ̇ ̇ V A + ( A ( θ ) V = 0 . (11.57) θ ) To ensure that Equation (11.57) is satisfied when the system state already sat- ̇ ̇ θ ) V = 0, any requested acceleration isfies V A should satisfy A ( θ ) ( V = 0. d d ̇ , substituting the result into (11.57), and Now solving Equation (11.56) for V solving for λ , we get − 1 T − 1 − 1 ̇ ) Λ λ ( A Λ = ( A ( F − η ) − A A V ) , (11.58) ̇ ̇ where we have used = − A V from Equation (11.57). Using Equation (11.58), A V T θ A ( F ) λ that the robot applies against the we can calculate the wrench = tip constraints. Substituting Equation (11.58) into Equation (11.56) and manipulating, the n equations of the constrained dynamics (11.56) can be expressed as the n − k independent motion equations ̇ θ ) F = P ( θ )(Λ( θ ) P V + η ( θ, V )) , (11.59) ( where T − 1 T − 1 − 1 Λ = I A − ) A P A Λ ( A (11.60) ) has rank and n × n matrix P ( θ is the identity matrix. The n − k and I projects an arbitrary manipulator wrench F onto the subspace of wrenches that move the end-effector tangent to the constraints. The rank- k matrix I − P ( θ ) projects an arbitrary wrench onto the subspace of wrenches that act against F the constraints. Thus P partitions the n -dimensional force space into wrenches May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

459 441 Chapter 11. Robot Control that address the motion control task and wrenches that address the force control task. Our hybrid motion–force controller is simply the sum of a task-space motion controller, derived from the computed torque control law (11.47), and a task- space force controller (11.51), each projected to generate forces in its appropriate subspace. Assuming wrenches and twists expressed in the end-effector frame { b } , ( ( ( )) ∫ d T ̃ − 1 ([Ad ] V ) + K X + K K θ = ( V ( t + P Λ( θ ) ) ) θ τ J X d (t) d p e i e d e X X b d dt ︷︷ ︸ ︸ motion control ( ) ∫ P ( θ )) F + ( I + K t F d + K (t) − F e fi fp d e ︷︷ ︸ ︸ force control ) η ( θ, V (11.61) + . ̃ ) b ︸ ︷︷ ︸ Coriolis and gravity Because the dynamics of the two controllers are decoupled by the orthogonal projections P and I − P , the controller inherits the error dynamics and stabil- ity analyses of the individual force and motion controllers on their respective subspaces. A difficulty in implementing the hybrid control law (11.61) in rigid environ- = 0 active at any time. ( θ ) ments is knowing the form of the constraints A V This is necessary to specify the desired motion and force and to calculate the projections, but any model of the environment will have some uncertainty. One approach to dealing with this issue is to use a real-time estimation algorithm to identify the constraint directions on the basis of force feedback. Another is to sacrifice some performance by choosing low feedback gains, which makes the motion controller “soft” and the force controller more tolerant of force error. We can also build passive compliance into the structure of the robot itself to achieve a similar effect. In any case, some passive compliance is unavoidable, owing to flexibility in the joints and links. 11.7 Impedance Control Ideal hybrid motion–force control in rigid environments demands extremes in robot impedance , which characterizes the change in endpoint motion as a func- tion of disturbance forces. Ideal motion control corresponds to high impedance May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

460 442 11.7. Impedance Control k m b x A robot creating a one-dof mass–spring–damper virtual environment. Figure 11.23: A human hand applies a force to the haptic interface. f (little change in motion due to force disturbances) while ideal force control cor- responds to low impedance (little change in force due to motion disturbances). In practice, there are limits to a robot’s achievable impedance range. In this section we consider the problem of impedance control, where the robot end-effector is asked to render particular mass, spring, and damper prop- 3 For example, a robot used as a haptic surgical simulator could be tasked erties. with mimicking the mass, stiffness, and damping properties of a virtual surgical instrument in contact with virtual tissue. The dynamics for a one-dof robot rendering an impedance can be written ̈ x + b ̇ x + kx = f, m (11.62) where is the position, m is the mass, b is the damping, k is the stiffness, x f 11.23). Loosely, we say that the and is the force applied by the user (Figure { m ,b,k } parameters, usually robot renders high impedance if one or more of the including or k , is large. Similarly, we say that the impedance is low if all these b parameters are small. 4 More formally, taking the Laplace transform of Equation (11.62), we get 2 m s ( + bs + k ) X ( s ) = F ( s ) , (11.63) and the impedance is defined by the transfer function from position perturba- tions to forces, ( s ) = F ( s ) /X ( s Z ). Thus impedance is frequency dependent, with a low-frequency response dominated by the spring and a high-frequency admittance , Y response dominated by the mass. The s ), is the inverse of the ( − 1 ( s ) = Z ) = impedance: ( s Y X ( s ) /F ( s ). A good motion controller is characterized by high impedance (low admit- tance), since ∆ = Y ∆ X . If the admittance Y is small then force perturbations F 3 A popular subcategory of impedance control is stiffness control or compliance control , where the robot renders a virtual spring only. 4 If you are unfamiliar with the Laplace transform and transfer functions, do not panic! We do not need the details here. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

461 Chapter 11. Robot Control 443 F ∆ . Similarly, a good force con- produce only small position perturbations ∆ X = X ∆ F troller is characterized by low impedance (high admittance), since ∆ Z implies that motion perturbations produce only small force per- and a small Z turbations. The goal of impedance control is to implement the task-space behavior ̈ x + B ̇ x + Kx = f , M (11.64) ext n x R is the task-space configuration in a minimum set of coordinates, where ∈ 3 ∈ R and ; M,B, e.g., x are the positive-definite virtual mass, damping, and K stiffness matrices to be simulated by the robot, and f is a force applied to the ext robot, perhaps by a user. The values of M,B K may change, depending , and on the location in the virtual environment, in order to represent distinct objects for instance, but we focus on the case of constant values. We could also replace ̈ x , ̇ x , and x with small displacements ∆ ̈ x , ∆ ̇ x , and ∆ x from reference values in a controlled motion of the robot, but we will dispense with any such extra notation here. The behavior (11.64) could be implemented in terms of twists and wrenches x by the (body or spatial) wrench F instead, replacing , ̇ , by the twist V f ext ext ̇ by V ̈ x x by the exponential coordinates S θ . Alternatively, the linear and , and rotational behaviors can be decoupled, as discussed in Section 11.4.3. There are two common ways to achieve the behavior (11.64). • The robot senses the endpoint motion x ( t ) and commands joint torques and forces to create − f , the force to display to the user. Such a robot ext is called , as it implements a transfer function impedance controlled ( s ) from motions to forces. Theoretically, an impedance-controlled robot Z should only be coupled to an admittance-type environment. • The robot senses f using a wrist force–torque sensor and controls its ext motions in response. Such a robot is called admittance controlled , as it implements a transfer function ( s ) from forces to motions. Theoretically, Y an admittance-controlled robot should only be coupled to an impedance- type environment. 11.7.1 Impedance-Control Algorithm In an impedance-control algorithm, encoders, tachometers, and possibly ac- celerometers are used to estimate the joint and endpoint positions, velocities, and possibly accelerations. Often impedance-controlled robots are not equipped with a wrist force–torque sensor and instead rely on their ability to precisely May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

462 444 11.7. Impedance Control − (Equa- f control joint torques to render the appropriate end-effector force ext tion (11.64)). A good control law might be     T ̃ J (11.65) ( θ Λ( θ ) ̈ x + ̃ η ( θ, ̇ x ) , ) τ = − ( ̈ x + B ̇ x + Kx ) M   ︸ ︷︷ ︸ ︸ ︷︷ ︸ f arm dynamics compensation ext ̃ { Λ , ̃ η } is expressed in terms of the co- where the task-space dynamics model x . Addition of an end-effector force–torque sensor allows the use of ordinates − . feedback terms to achieve more closely the desired interaction force f ext x , ̇ x , and x are measured di- In the control law (11.65), it is assumed that ̈ x rectly. Measurement of the acceleration ̈ is likely to be noisy, and there is the problem of attempting to compensate for the robot’s mass after the acceleration has been sensed. Therefore, it is not uncommon to eliminate the mass compen- ̃ x θ ) ̈ sation term and to set Λ( = 0. The mass of the arm will be apparent M to the user, but impedance-controlled manipulators are often designed to be lightweight. It is also not uncommon to assume small velocities and replace the nonlinear dynamics compensation with a simpler gravity-compensation model. Problems can arise when (11.65) is used to simulate stiff environments (the case of large K ). On the one hand, small changes in position, measured by encoders for example, lead to large changes in motor torques. This effective high gain, coupled with delays, sensor quantization, and sensor errors, can lead to oscillatory behavior or instability. On the other hand, the effective gains are low when emulating low-impedance environments. A lightweight backdrivable manipulator can excel at emulating such environments. 11.7.2 Admittance-Control Algorithm In an admittance-control algorithm the force applied by the user is sensed f ext by the wrist load cell, and the robot responds with an end-effector acceleration satisfying Equation (11.64). A simple approach is to calculate the desired end- x according to effector acceleration ̈ d ̈ x + M B ̇ x + Kx = f , ext d where ( x, ̇ x ) is the current state. Solving, we get − 1 (11.66) = M ̇ ̈ ( f . − B x x − Kx ) ext d ̇ ̈ ) defined by ̇ x = J ( θ J ) θ , the desired joint accelerations ( θ θ For the Jacobian d can be solved as † ̈ ̇ ̇ J ( θ )( ̈ x − J = ( θ ) θ θ ) , d d May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

463 Chapter 11. Robot Control 445 and inverse dynamics used to calculate the commanded joint forces and torques τ . Simplified versions of this control law can be obtained when the goal is to simulate only a spring or a damper. To make the response smoother in the face of noisy force measurements, the force readings can be low-pass filtered. Simulating a low-mass environment is challenging for admittance-controlled robots, as small forces produce large accelerations. The effective large gains can produce instability. Admittance control by a highly geared robot can excel at emulating stiff environments, however. Low-Level Joint Force/Torque Control 11.8 Throughout this chapter we have been assuming that each joint produces the torque or force requested of it. In practice this ideal is not exactly achieved, and there are different approaches to approximating it. Some of the most common approaches using electric motors (Section 8.9.1) are listed below, along with their advantages and disadvantages relative to the previously listed approach. Here we assume a revolute joint and a rotary motor. Current Control of a Direct-Drive Motor In this configuration, each joint has a motor amplifier and an electric motor with no gearhead. The torque of the motor approximately obeys the relationship τ = k , i.e., the torque is pro- I t portional to the current through the motor. The amplifier takes the requested k , and generates the motor current I . To torque, divides by the torque constant t create the desired current, a current sensor integrated with the amplifier contin- uously measures the actual current through the motor, and the amplifier uses a local feedback control loop to adjust the time-averaged voltage across the motor to achieve the desired current. This local feedback loop runs at a higher rate than the control loop that generates the requested torques. A typical example is 10 kHz for the local current control loop and 1 kHz for the outer control loop requesting joint torques. An issue with this configuration is that often an ungeared motor must be quite large to create sufficient torque for the application. The configuration can work if the motors are fixed to the ground and connected to the end-effector through cables or a closed-chain linkage. If the motors are moving, as do the motors at the joints of a serial chain, for example, large ungeared motors are often impractical. Current Control of a Geared Motor This configuration is similar to the previous one, except that the motor has a gearhead (Section 8.9.1). A gear ratio May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

464 446 11.8. Low-Level Joint Force/Torque Control 1 increases the torque available to the joint. G > A smaller motor can provide the necessary torques. The motor Advantage: also operates at higher speeds, where it is more efficient at converting electrical power to mechanical power. Disadvantage: The gearhead introduces backlash (the output of the gearhead can move without the input moving, making motion control near zero velocity challenging) and friction. Backlash can be nearly eliminated by using particular types of gearing, such as harmonic drive gears. Friction, however, cannot be Gk I eliminated. The nominal torque at the gearhead output is , but friction in t the gearhead reduces the torque available and creates significant uncertainty in the torque actually produced. Current Control of a Geared Motor with Local Strain Gauge Feedback This configuration is similar to the previous one, except that the harmonic drive gearing is instrumented with strain gauges that sense how much torque is actu- ally being delivered at the output of the gearhead. This torque information is used by the amplifier in a local feedback controller to adjust the current in the motor so as to achieve the requested torque. Advantage: Putting the sensor at the output of the gearing allows compensa- tion of frictional uncertainties. Disadvantage: There is additional complexity of the joint configuration. Also, harmonic drive gearing achieves near-zero backlash by introducing some tor- sional compliance in the gearset, and the added dynamics due to the presence of this torsional spring can complicate high-speed motion control. A series elastic actuator (SEA) consists of an elec- Series Elastic Actuator tric motor with a gearhead (often a harmonic drive gearhead) and a torsional spring attaching the output of the gearhead to the output of the actuator. It is similar to the previous configuration, except that the torsional spring constant of the added spring is much lower than the spring constant of the harmonic drive gearing. The angular deflection ∆ φ of the spring is often measured by optical, magnetic, or capacitive encoders. The torque delivered to the output of the actuator is k ∆ φ , where k is the torsional spring constant. The spring’s deflection is fed to a local feedback controller that controls the current to the motor so as to achieve the desired spring deflection, and therefore the desired torque. Advantage: The addition of the torsional spring makes the joint naturally “soft,” and therefore well suited for human-robot interaction tasks. It also pro- tects the gearing and motor from shocks at the output, such as when the output link hits something hard in the environment. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

465 Chapter 11. Robot Control 447 output cross custom planar torsional spring roller bearing spring output position sensor spring input position sensor frameless brushless harmonic drive DC motor gear set hollow boreincremental optical encoder (Top left) The Robonaut 2 on the International Space Station. (Top Figure 11.24: middle) R2’s hip joint SEA. (Top right) The custom torsional spring. The outer ring of hole mounts connects to the harmonic gearhead output, and the inner ring of hole mounts is the output of the SEA, connecting to the next link. The spring is designed with hard stops after approximately 0.07 rad of deflection. (Bottom) A cross-section of the SEA. The deflection ∆ of the torsional spring is determined by differencing the φ deflection readings at the spring input and the spring output. The optical encoder and spring deflection sensors provide an estimate of the joint angle. The motor controller– amplifier is located at the SEA, and it communicates with the centralized controller using a serial communication protocol. The hollow bore allows cables to pass through the interior of the SEA. All images courtesy of NASA. Disadvantage: There is additional complexity of the joint configuration. Also, the added dynamics due to the softer spring make it more challenging to control high-speed or high-frequency motions at the output. In 2011, NASA’s Robonaut 2 (R2) became the first humanoid robot in space, performing operations on the International Space Station. Robonaut 2 incor- porates a number of SEAs, including the hip actuator shown in Figure 11.24. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

466 448 11.9. Other Topics Other Topics 11.9 While all stable feedback controllers confer some amount of Robust Control robustness of operation to uncertainty, the field of robust control deals with de- signing controllers that explicitly guarantee the performance of a robot subject to bounded parametric uncertainties such as those in its inertial properties. Adaptive Control The adaptive control of robots involves estimating the robot’s inertial or other parameters during execution and updating the control law in real time to incorporate those estimates. Iterative learning control (ILC) generally fo- Iterative Learning Control cuses on repetitive tasks. If a robot performs the same pick-and-place operation over and over again, the trajectory errors from the previous execution can be used to modify the feedforward control for the next execution. In this way, the robot improves its performance over time, driving the execution error to- ward zero. This type of learning control differs from adaptive control in that the “learned” information is generally nonparametric and useful only for a single tra- jectory. However, ILC can account for effects that have not been parametrized in a particular model. Passive Compliance and Flexible Manipulators All robots unavoidably have some passive compliance. Modeling this compliance can be as simple as assuming torsional springs at each revolute joint (e.g., to account for finite stiff- ness in the flexsplines of harmonic drive gearing) or as complicated as treating links as flexible beams. Two significant effects of flexibility are (1) a mismatch between the motor angle reading, the true joint angle, and the endpoint loca- tion of the attached link, and (2) increased order of the dynamics of the robot. These issues raise challenging problems in control, particularly when the vibra- tion modes are at low frequencies. Some robots are specifically designed for passive compliance, particularly those meant for contact interactions with humans or the environment. Such robots may sacrifice motion-control performance in favor of safety. One passively compliant actuator is the series elastic actuator, described above. Variable-Impedance Actuators The impedance of a joint is typically con- trolled using a feedback control law, as described in Section 11.7. There are limits to the bandwidth of this control, however; a joint that is actively con- trolled to behave as a spring will only achieve spring-like behavior in respect of low-frequency perturbations. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

467 Chapter 11. Robot Control 449 A new class of actuators, called or variable-impedance variable-stiffness , is intended to give actuators desired passive mechanical impedance actuators without the bandwidth limitations of an active control law. As an example, a variable-stiffness actuator may consist of two motors, with one motor indepen- dently controlling the mechanical stiffness of the joint (e.g., using the setpoint of an internal nonlinear spring) while the other motor produces a torque. Summary 11.10 • The performance of a feedback controller is often tested by specifying a nonzero initial error θ (0). The error response is typically characterized e by the overshoot, the 2% settling time, and the steady-state error. • The linear error dynamics ) p ( ( p − 1) ̈ ̇ a θ + a θ a + + ··· a θ + + = 0 θ a θ 1 p − 2 p 1 e 0 e e e e is stable, and all initial errors converge to zero, if and only if all the s ,...,s of the characteristic equation complex roots p 1 p − 1 2 p = 0 a + s s s a + ··· + a s + a + a 1 1 − 0 p p 2 s 0 for all ) < have real components less than zero, i.e., Re( i = 1 ,...,p . i • Stable second-order linear error dynamics can be written in the standard form 2 ̈ ̇ θ ζω + 2 θ + θ ω = 0 , e n e e n is the damping ratio and ω where is the natural frequency. The roots ζ n of the characteristic equation are √ 2 . 1 − ζ s ± ω = − ζω n 2 , n 1 The error dynamics are overdamped if ζ > 1, critically damped if ζ = 1, ζ < 1. and underdamped if The feedforward plus PI feedback controller generating joint velocity com- • mands for a multi-joint robot is ∫ t ̇ ̇ ) = t θ , ( θ ) + K t θ d ( t ) + K (t) ( t θ i e p d e 0 where K ) converges to zero = k t I and K ( = k θ I . The joint error i p p i e as t goes to infinity, for setpoint control or constant reference velocities, k 0. > 0 and k > provided that p i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

468 450 11.10. Summary • A task-space version of the feedforward plus PI feedback controller gener- ating twists expressed in the end-effector frame is written ∫ t − 1 (t) ( t V ] V t ( t ) + K X X ) = [Ad ( t ) + K , d i b p e d e X X d 0 1 − X where [ ] = log( X ). X d e • The PID joint-space feedback controller generating joint forces and torques is ∫ ̇ (t) + K θ θ K d t + K τ θ , = d i e e p e θ θ θ where − θ and = is the vector of the desired joint angles. d d e • The joint-space computed torque controller is ( ) ∫ ̇ ̈ ̃ ̇ ̃ τ θ K θ + M = ) θ + (t) d t + K θ θ ( K ( θ, θ h . + ) e d e p d e i This controller cancels nonlinear terms, uses feedforward control to proac- ̈ , and uses linear feedback control θ tively generate the desired acceleration d for stabilization. For robots without joint friction and a perfect model of gravity forces, • joint-space PD setpoint control plus gravity compensation, ̇ τ θ = + K K θ + ̃ g ( θ ) , d p e θ yields global convergence to = 0 by the Krasovskii–LaSalle invariance e principle. Task-space force control can be achieved by the controller • ) ( ∫ T + F + K , F K F ) ( J ) + θ ( g θ = ̃ (t) d t − K τ V fp fi e d damp e consisting of gravity compensation, feedforward force control, PI force feedback, and damping to prevent fast motion. Rigid constraints in the environment specify 6 − k free motion directions • k constraint directions in which forces can be applied. These con- and A ( θ ) V = 0. A wrench F can be partitioned straints can be represented as as F = P ( θ ) F + ( I − P ( θ )) F , where P ( θ ) projects onto wrenches that move the end-effector tangent to the constraints and I − P ( θ ) projects onto May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

469 Chapter 11. Robot Control 451 P ( ) is wrenches that act against the constraints. The projection matrix θ ) and constraints written in terms of the task-space mass matrix Λ( θ ) A ( θ as − T T − 1 − 1 1 I − A ) Λ ( A Λ A P . = A An impedance controller measures end-effector motions and creates end- • point forces to mimic a mass–spring–damper system. An admittance controller measures end-effector forces and creates endpoint motions to achieve the same purpose. Software 11.11 Software functions associated with this chapter are listed below. taulist = ComputedTorque(thetalist,dthetalist,eint,g, Mlist,Glist,Slist,thetalistd,dthetalistd,ddthetalistd,Kp,Ki,Kd) This function computes the joint controls τ for the computed torque control -vectors of joint n law (11.35) at a particular time instant. The inputs are the g ; a list variables, joint velocities, and joint error integrals; the gravity vector of transforms M describing the link center of mass locations; a list of link ,i 1 − i G ; a list of joint screw axes spatial inertia matrices S expressed in the base i i ̇ ̈ n describing the desired motion; and the scalar , θ frame; the -vectors , and θ θ d d d PID gains k , , k I , and k k , where the gain matrices are just K = = k K I , p i d i i p p K k I . and = d d [taumat,thetamat] = SimulateControl(thetalist,dthetalist,g, Ftipmat,Mlist,Glist,Slist,thetamatd,dthetamatd,ddthetamatd, gtilde,Mtildelist,Gtildelist,Kp,Ki,Kd,dt,intRes) This function simulates the computed torque controller (11.35) over a given desired trajectory. The inputs include the initial state of the robot, given by ̇ (0); the gravity vector θ θ g ; an (0) and × 6 matrix of wrenches applied by N the end-effector, where each of the rows corresponds to an instant in time N M in the trajectory; a list of transforms describing the link center-of-mass 1 ,i − i G ; a list of joint screw axes S locations; a list of link spatial-inertia matrices i i expressed in the base frame; the N × n matrices of the desired joint positions, velocities, and accelerations, where each of the N rows corresponds to an instant in time; a (possibly incorrect) model of the gravity vector; a (possibly incorrect) model of the transforms ; a (possibly incorrect) model of the link inertia M i − 1 ,i k matrices; the scalar PID gains , k , where the gain matrices are , and k d i p K rows = k N I , K ; the timestep between each of the = k I I , and K k = d d i i p p May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

470 452 11.12. Notes and References in the matrices defining the desired trajectory; and the number of integration steps to take during each timestep. 11.12 Notes and References 106, The computed torque controller originates from research in the 1970’s [137, 9, 145], and issues with its practical implementation (e.g., its computational complexity and modeling errors) have driven much of the subsequent research in nonlinear control, robust control, iterative learning control, and adaptive control. PD control plus gravity compensation was suggested and analyzed in [184], and subsequent analysis and modification of the basic controller is reviewed in [71]. The task-space approach to motion control, also called operational space control, was originally outlined in [99, 74]. A geometric approach to tracking control for mechanical systems is presented in [22], where the configuration space SO SE (3). for the system can be a generic manifold, including (3) and The notion of natural and artificial constraints in hybrid motion–force con- trol was first described by Mason [107], and an early hybrid motion–force con- troller based on these concepts is reported in [144]. As pointed out by Duffy [42], one must take care in specifying the subspaces in which motions and forces can be controlled. The approach to hybrid motion–force control in this chapter mir- rors the geometric approach of Liu and Li [91]. Impedance control was first described in a series of papers by Hogan [56, 57, 58]. The stiffness matrix for a rigid body whose configuration is represented by X SE (3) is discussed ∈ 93]. in [60, Robot control builds on the well-established field of linear control (e.g., 4]) and the growing field of nonlinear control [63, 64, 72, 126, 158]. Gen- [49, eral references on robot control include the edited volume [33]; the textbooks by Spong et al. [177], Siciliano et al. [ 171], Craig [32], and Murray et al. [122]; chapters in the Handbook of Robotics on Motion Control [29] and Force Con- trol [190]; the Robot Motion Control chapter in the Encyclopedia of Systems and Control [176]; and, for underactuated and nonholonomic robots, the chapters in the Control Handbook [101] and the Encyclopedia of Systems and Control [100]. The principles governing SEAs are laid out in [141], and NASA’s Robonaut 2 and its SEAs are described in [1, 36, 115]. Variable impedance actuators are reviewed in [189]. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

471 Chapter 11. Robot Control 453 11.13 Exercises Exercise 11.1 Classify the following robot tasks as motion control, force control, hybrid motion–force control, impedance control, or some combination. Justify your answer. (a) Tightening a screw with a screwdriver. (b) Pushing a box along the floor. (c) Pouring a glass of water. (d) Shaking hands with a human. (e) Throwing a baseball to hit a target. (f) Shoveling snow. (g) Digging a hole. (h) Giving a back massage. (i) Vacuuming the floor. (j) Carrying a tray of glasses. Exercise 11.2 The 2% settling time of an underdamped second-order system − ζω t n ζω is approximately ), for e t = 4 / ( = 0 . 02. What is the 5% settling time? n Exercise 11.3 Solve for any constants and give the specific equation for an ̇ ω (0) = = 4, ζ = 0 . 2, θ θ (0) = 1, and underdamped second-order system with e e n 0. Calculate the damped natural frequency, approximate overshoot, and 2% settling time. Plot the solution on a computer and measure the exact overshoot and settling time. Exercise 11.4 Solve for any constants and give the specific equation for an ω = 0 = 10, ζ underdamped second-order system with . 1, θ (0) = 0, and e n ̇ θ (0) = 1. Calculate the damped natural frequency. Plot the solution on a e computer. 2 Consider a pendulum in a gravitational field with g = 10 m/s Exercise 11.5 . The pendulum consists of a 2 kg mass at the end of a 1 m massless rod. The b = 0 . 1 N m s/rad. pendulum joint has a viscous-friction coefficient of (a) Write the equation of motion of the pendulum in terms of θ , where θ = 0 corresponds to the “hanging down” configuration. (b) Linearize the equation of motion about the stable “hanging down” equi- librium. To do this, replace any trigonometric terms in θ with the linear term in the Taylor expansion. Give the effective mass and spring con- May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

472 454 11.13. Exercises ̈ ̇ + and θ + b m θ in the linearized dynamics kθ = 0. At the stable k m stants equilibrium, what is the damping ratio? Is the system underdamped, crit- ically damped, or overdamped? If it is underdamped, what is the damped natural frequency? What is the time constant of convergence to the equi- librium and the 2% settling time? (c) Now write the linearized equations of motion for θ = 0 at the balanced k upright configuration. What is the effective spring constant ? (d) You add a motor at the joint of the pendulum to stabilize the upright K K τ position, and you choose a P controller . For what values of = θ p p is the upright position stable? Exercise 11.6 Develop a controller for a one-dof mass–spring–damper system m ̈ x + b ̇ x + kx = f , where f is the control force and m = 4 kg, of the form = 2 Ns/m, and = 0 . 1 N/m. b k (a) What is the damping ratio of the uncontrolled system? Is the uncon- trolled system overdamped, underdamped, or critically damped? If it is underdamped, what is the damped natural frequency? What is the time constant of convergence to the origin? f = K − x is the position error , where x x = x (b) Choose a P controller d p e e x K yields critical damping? and = 0. What value of d p yields = K ̇ x (c) Choose a D controller , where ̇ x = 0. What value of K f d e d d critical damping? (d) Choose a PD controller that yields critical damping and a 2% settling time 01 s. of 0 . = 0, what is the = 1 and ̇ x (e) For the PD controller above, if = ̈ x x d d d steady-state error ( t x t goes to infinity? What is the steady-state ) as e control force? f . Assume x (f) Now insert a PID controller for 6 = 0 and ̇ x = 0. Write = ̈ x d d d ∫ , ̇ x , t on the left-hand the error dynamics in terms of ̈ , and x x (t) d x e e e e side and a constant forcing term on the right-hand side. (Hint: You can write as − k ( x kx − x ) + kx .) Take the time derivative of this equation d d and give the conditions on K for stability. Show that zero , K K , and i p d steady-state error is possible with a PID controller. Exercise 11.7 Simulation of a one-dof robot and robot controller. (a) Write a simulator for a one-joint robot consisting of a motor rotating a link in gravity using the model parameters given in Section 11.4.1. The simulator should consist of: (1) a dynamics function that takes as input the current state of the robot and the torque applied by the motor and gives as output the acceleration of the robot; and (2) a numerical integrator that May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

473 Chapter 11. Robot Control 455 uses the dynamics function to calculate the new state of the system over a series of timesteps ∆ t . A first-order Euler integration method suffices for ̇ ̇ ̇ ̈ ( k ) + ( θ ( k )∆ t , + 1) = θ ( k + 1) = θ θ k k ) + θ θ ( k )∆ t ). this problem (e.g., ( θ = − π/ 2 Test the simulator in two ways: (1) starting the robot at rest at and applying a constant torque of 0 . 5 N m; and (2) starting the robot at rest at = − π/ 4 and applying zero torque. For both examples, plot θ the position as a function of time for sufficient duration to see the basic behavior. Ensure that the behavior makes sense. A reasonable choice of ∆ is 1 ms. t (b) Add two more functions to your simulator: (1) a trajectory generator function that takes the current time and returns the desired state and acceleration of the robot; and (2) a control function that takes the cur- rent state of the robot and information from the trajectory generator and returns a control torque. The simplest trajectory generator would return ̈ ̇ ̈ ̇ and = 0 for all time θ = θ θ = θ , and θ = θ = 0 = 6 = θ θ θ and t < T 1 d 2 d 1 d for all time t ≥ T . This trajectory is a step function in position. Use a PD feedback controller for the control function and set K = 10 N m/rad. For p a well-tuned choice of , give K K (including units) and plot the position d d as a function of time over 2 seconds for an initial state at rest at = − π/ 2 θ θ = 0. The step occurs at π/ = − and a step trajectory with 2 and θ 1 2 d d = 1 s. T K that yield (1) overshoot and (2) (c) Demonstrate two different choices of d sluggish response with no overshoot. Give the gains and the position plots. K (d) Add a nonzero to your original well-tuned PD controller to eliminate i steady-state error. Give the PID gains and plot the results of the step test. Exercise 11.8 Modify the simulation of the one-joint robot in Exercise 11.7 to model a flexible transmission from the motor to the link with a stiffness of 500 Nm/rad. Tune a PID controller to give a good response for a desired trajectory with a step transition from − π/ 2 to θ = θ = 0. Give the gains and plot the response. Exercise 11.9 Simulation of a two-dof robot and robot controller (Fig- 11.25). ure Dynamics. 11.25). Derive the dynamics of a 2R robot under gravity (Figure (a) i from the joint, m The mass of link , the center of mass is a distance r is i i the scalar inertia of link about the joint is I , and the length of link i is i i L . There is no friction at the joints. i (b) Direct drive. Assume each joint is directly driven by a DC motor with May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

474 456 11.13. Exercises stator m no gearing. Each motor comes with specifications of the mass i rotor rotor stator I of the stator and the mass and inertia I m of and inertia i i i the rotor (the spinning portion). For the motor at joint i , the stator is − 1 and the rotor is attached to link attached to link . The links are i i m and length L thin uniform-density rods of mass . i i In terms of the quantities given above, for each link i 1 , 2 } give equa- ∈ { I tions for the total inertia about the joint, the mass m , and the distance i i from the joint to the center of mass. Think about how to assign the r i mass and inertia of the motors to the different links. Geared robot. Assume that motor i G and (c) has gearing with gear ratio i that the gearing itself is massless. As in part (b) above, for each link ∈{ 1 , 2 } i I , about the joint, mass m , give equations for the total inertia i i r from the joint to the center of mass. and distance i Simulation and control. (d) 11.7, write a simulator with (at As in Exercise least) four functions: a dynamics function, a numerical integrator, a tra- jectory generator, and a controller. Assume that there is zero friction at 2 g = 9 . 81 m/s 5 m, in the direction indicated, L the joints, = 1 m, r . = 0 i i 2 2 I = 2 kg, I . Program a = 2 kg m = 3 kg, , and m m = 1 kg m 2 1 2 1 PID controller, find gains that give a good response, and plot the joint angles as a function of time for a reference trajectory which is constant θ ,θ at ( 2) for ) = ( − π/ 2 , 0) for t < 1 s and constant at ( θ π/ ,θ − ) = (0 , 2 1 2 1 t ≥ 1 s. The initial state of the robot is at rest with ( θ 0). ,θ , ) = ( − π/ 2 2 1 Torque limits. Real motors have limits on the available torque. While (e) these limits are generally velocity dependent, here we assume that each max | ≤| τ motor’s torque limit is independent of velocity, τ . Assume that i i max max = 100 N m and τ = 20 N m. The control law may request greater τ 1 2 torque but the actual torque is saturated at these values. Rerun the PID control simulation in (d) and plot the torques as well as the position as a function of time. (f) Add a viscous friction coefficient of b Friction. = 1 N m s/rad to each i joint and rerun the PID control simulation in (e). Exercise 11.10 For the two-joint robot of Exercise 11.9, write a more so- phisticated trajectory generator function. The trajectory generator should take the following as input: • the desired initial position, velocity, and acceleration of each joint; • the desired final position, velocity, and acceleration; and • the total time of motion T . May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

475 Chapter 11. Robot Control 457 ˆy ˆx g θ 1 r 1 r 2 m 2 m 1 θ 2 i Figure 11.25: L A two-link robot arm. The length of link and its inertia about is i 2 . The acceleration due to gravity is g = 9 I 81 m/s the joint is . . i A call of the form [qd,qdotd,qdotdotd] = trajectory(time) returns the desired position, velocity, and acceleration of each joint at time time . The trajectory generator should provide a trajectory that is a smooth function of time. As an example, each joint could follow a fifth-order polynomial trajectory of the form 4 2 3 5 + + t + a t a + a t ) = a a t t ( + a t θ . (11.67) 1 0 5 4 d 3 2 Given the desired positions, velocities, and accelerations of the joints at times t t = T , you can uniquely solve for the six coefficients a = 0 and ,...,a by 5 0 t = 0 and evaluating Equation (11.67) and its first and second derivatives at = T . t Tune a PID controller to track a fifth-order polynomial trajectory moving from rest at ( θ = 2 s. Give ,θ T ) = ( − π/ 2 , 0) to rest at ( θ 2) in ,θ π/ ) = (0 , − 2 2 1 1 the values of your gains and plot the reference positions of both joints and the actual positions of both joints. You are free to ignore torque limits and friction. Exercise 11.11 For the two-joint robot of Exercise 11.9 and fifth-order poly- nomial trajectory of Exercise 11.10, simulate a computed torque controller to stabilize the trajectory. The robot has no joint friction or torque limits. The modeled link masses should be 20% greater than their actual values to create error in the feedforward model. Give the PID gains and plot the reference and May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

476 458 11.13. Exercises actual joint angles for the computed torque controller as well as for PID control only. Exercise 11.12 The Krasovskii–LaSalle invariance principle states the follow- n ( x ) ,x ∈ R ing. Consider a system ̇ = f (0) = 0 and any energy-like x f such that ( x ) such that: V function ( x ) > 0 for all x 6 = 0; • V x ( ) →∞ as x →∞ ; V • ̇ • V V (0) = 0; and (0) = ̇ V ( x ) ≤ 0 along all trajectories of the system. • n ̇ V such that S R ( x ) = 0 and trajectories beginning Let be the largest set of S remain in S for all time. Then, if S in contains only the origin, the origin is globally asymptotically stable – all trajectories converge to the origin. V x ) from Equation (11.40), show how the Krasov- Using the energy function ( skii–LaSalle principle is violated for centralized multi-joint PD setpoint control with gravity compensation if = 0. For a practical robot system, = 0 or K K d p is it possible to use the Krasovskii–LaSalle invariance principle to demonstrate global asymptotic stability even if K = 0? Explain your answer. d Exercise 11.13 can be controlled in task The two-joint robot of Exercise 11.9 = ( X ), as shown in Figure 11.25. space using the endpoint task coordinates x,y ̇ = ( X . Give the Jacobian J The task-space velocity is θ V ) and the task-space ̃ dynamics model Λ( θ ) , ̃ η { θ, V ) } in the computed torque-control law (11.47). ( Exercise 11.14 Choose appropriate space and end-effector reference frames { s } and { b } and express natural and artificial constraints, six each, that achieve the following tasks: (a) opening a cabinet door; (b) turning a screw that ad- vances linearly a distance for every revolution; and (c) drawing a circle on a p chalkboard with a piece of chalk. Exercise 11.15 Assume that the end-effector of the two-joint robot in Fig- ure is constrained to move on the line x − y = 1. The robot’s link lengths 11.25 are L ) and = L x,y = 1. Write the constraint as A ( θ ) V = 0, where X = ( 2 1 ̇ = . X V Exercise 11.16 Derive the constrained motion equations (11.59) and (11.60). Show all the steps. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

477 Chapter 11. Robot Control 459 I measured current sensor k lin _ I τ I com PWM error m co gear PI DC / I τ H-bridge com co m controller motor G + τ measured torque sensor k lin _ τ τ PWM error m co PI DC gear H-bridge controller motor G + strain gauge Two methods for controlling the torque at a joint driven by a geared Figure 11.26: DC motor. (Upper) The current to the motor is measured by measuring the voltage across a small resistance in the current path. A PI controller works to make the actual current match better the requested current I . (Lower) The actual torque delivered com to the link is measured by strain gauges. Exercise 11.17 We have been assuming that each actuator delivers the torque requested by the control law. In fact, there is typically an inner control loop running at each actuator, typically at a higher servo rate than the outer loop, to try to track the torque requested. Figure 11.26 shows two possibilities for a DC electric motor, where the torque τ delivered by the motor is proportional to the current I through the motor, τ = k . The torque from the motor is I t amplified by the gearhead with gear ratio . G In the upper control scheme the motor current is measured by a current sensor and compared with the desired current ; the error is passed through I com a PI controller which sets the duty cycle of a low-power pulse-width-modulation (PWM) digital signal and the PWM signal is sent to an H-bridge that generates the actual motor current. In the lower scheme, a strain gauge torque sensor is inserted between the output of the motor gearing and the link, and the measured torque is compared directly with the requested torque τ . Since a strain gauge com measures deflection, the element on which it is mounted must have a finite torsional stiffness. Series elastic actuators are designed to have particularly May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

478 460 11.13. Exercises flexible torsional elements, so much so that encoders are used to measure the larger deflections. The torque is estimated from the encoder reading and the torsional spring constant. (a) For the current sensing scheme, what multiplicative factor should go in the block labeled /τ ? Even if the PI current controller does its job I com com perfectly ( I is perfectly known, what = 0) and the torque constant k t error effect may contribute to error in the generated torque? (b) For the strain gauge measurement method, explain the drawbacks, if any, of having a flexible element between the gearhead and the link. Exercise 11.18 Modify the SimulateControl function to allow initial state errors. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

479 Chapter 12 Grasping and Manipulation Most of the book so far has been concerned with kinematics, dynamics, motion planning, and control of the robot itself. Only in Chapter 11, on the topics of force control and impedance control, did the robot finally begin interacting with an environment other than free space. Now the robot really becomes valuable – when it can perform useful work on objects in the environment. In this chapter our focus moves outward from the robot itself to the interac- tion between the robot and its environment. The desired behavior of the robot hand or end-effector, whether motion control, force control, hybrid motion– force control, or impedance control, is assumed to be achieved perfectly using the methods discussed so far. Our focus now is on the contact interface between the robot and objects as well as on contacts among objects and between ob- jects and constraints in the environment. In short, our focus is on manipulation rather than the manipulator . Examples of manipulation include grasping, push- ing, rolling, throwing, catching, tapping, etc. To limit our scope, we will assume that the manipulator, objects, and obstacles in the environment are rigid. To simulate, plan, and control robotic manipulation tasks, we need an under- standing of (at least) three elements: contact kinematics; forces applied through contacts; and the dynamics of rigid bodies. In contact kinematics we study how rigid bodies can move relative to each other without penetration and classify these feasible motions according to whether the contacts are rolling, sliding, or separating. Contact force models address the normal and frictional forces that can be transmitted through rolling and sliding contacts. Finally, the ac- tual motions of the bodies are those that simultaneously satisfy the kinematic constraints, contact force model, and rigid-body dynamics. This chapter introduces contact kinematics (Section 12.1) and contact force 461

480 462 (a) (c) (b) (d) 2 Figure 12.1: , drawn as arrows from the origin. (b) The (a) Three vectors in R linear span of the vectors is the entire plane. (c) The positive linear span is the cone shaded gray. (d) The convex span is the polygon and its interior. modeling (Section 12.2) and applies these models to problems in robot grasping and other types of manipulation. The following definitions from linear algebra will be useful in this chapter. n j vectors A = a , we define the ,...,a Definition 12.1. ∈ R Given a set of j 1 linear span , or the set of linear combinations, of the vectors to be { } j ∑ ∣ ∣ ) = A span( a R , k k ∈ i i i =1 i nonnegative linear combinations , sometimes called the positive or the , to be conical span } { j ∑ ∣ ∣ ) = k pos( a 0 A , k ≥ i i i =1 i convex span to be and the { } j ∑ ∑ ∣ ∣ a conv( A ) = k . = 1 k ≥ k 0 and i i i i =1 i i Clearly conv( A ) ⊆ pos( A ) ⊆ span( A ) (see Figure 12.1). The following facts from linear algebra will also be useful. n R can be linearly spanned by n vectors, but no fewer. 1. The space n R 2. The space can be positively spanned by n + 1 vectors, but no fewer. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

481 Chapter 12. Grasping and Manipulation 463 n coordinates to represent -dimensional The first fact is implicit in our use of n vectors Euclidean space. Fact 2 follows from the fact that for any choice of n n T . In i c ≤ 0 for all c { R a A a ∈ ,...,a = } there exists a vector such that n 1 i other words, no nonnegative combination of vectors in A can create a vector in c . However, if we choose a the direction ,...,a to be orthogonal coordinate n 1 ∑ n n n + 1 R = − and then choose a bases of a , we see that this set of +1 n i i =1 n R . vectors positively spans Contact Kinematics 12.1 Contact kinematics is the study of how two or more rigid bodies can move relative to each other while respecting the impenetrability constraint. It also classifies motion at a contact as either rolling or sliding. Let’s start by looking at a single contact between two rigid bodies. 12.1.1 First-Order Analysis of a Single Contact Consider two rigid bodies whose configurations are given by the local coordinate column vectors q , respectively. Writing the composite configuration as and q 2 1 ( = ( ,q q ), we define a distance function d ) between the bodies that is q q 1 2 positive when they are separated, zero when they are touching, and negative d ( q ) when they are in penetration. When 0, there are no constraints on the > motions of the bodies; each is free to move with six degrees of freedom. When ̇ ̈ , ) = 0), we look at the time derivatives d the bodies are in contact ( ( q d , etc., d to determine whether the bodies stay in contact or break apart as they follow a particular trajectory q ( t ). This can be determined by the following table of possibilities: ̈ ̇ d d ··· d 0 > no contact < 0 infeasible (penetration) = 0 > 0 in contact, but breaking free = 0 < 0 infeasible (penetration) = 0 > 0 in contact, but breaking free = 0 = 0 = 0 < 0 infeasible (penetration) etc. The contact is maintained only if all time derivatives are zero. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

482 464 12.1. Contact Kinematics contact tangent plane B A contact ˆ n normal Figure 12.2: A and B in single-point contact define a contact (Left) The bodies perpendicular to the tangent plane. By tangent plane and a contact normal vector ˆ n . Since contact A default, the positive direction of the normal is chosen into body curvature is not addressed in this chapter, the contact places the same restrictions on the motions of the rigid bodies in the middle and right panels. Now let’s assume that the two bodies are initially in contact ( d = 0) at a are written single point. The first two time derivatives of d ∂d ̇ = d q, (12.1) ̇ ∂q 2 ∂d d ∂ T ̈ = ̇ (12.2) q q + d ̈ q. ̇ 2 ∂q ∂q 2 2 and ∂ The terms d/∂q ∂d/∂q carry information about the local contact geom- etry. The gradient vector ∂d/∂q q corresponds to the separation direction in 2 2 contact normal ∂ d/∂q space associated with the (Figure 12.2). The matrix encodes information about the relative curvature of the bodies at the contact point. ∂d/∂q In this chapter we assume that only contact-normal information is available at contacts; other information about the local contact geometry, in- 2 2 cluding the contact curvature ∂ d/∂q and higher derivatives, is unknown. With this assumption, we truncate our analysis at Equation (12.1) and assume that ̇ = 0. Since we are dealing only with the first- d the bodies remain in contact if order contact derivative first-order analysis. , we refer to our analysis as a ∂d/∂q In such a first-order analysis, the contacts in Figure 12.2 are treated identically since they have the same contact normal. second-order analysis incorporating the As indicated in the table above, a 2 2 contact curvature d/∂q ∂ may indicate that the contact is actually breaking or ̇ penetrating even when d = d = 0. We will see examples of this, but a detailed May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

483 Chapter 12. Grasping and Manipulation 465 analysis of second-order contact conditions is beyond the scope of this chapter. 12.1.2 Contact Types: Rolling, Sliding, and Breaking Free Given two bodies in single-point contact, they undergo a roll–slide motion if the contact is maintained. The constraint that contact is maintained is a q d ) = 0. A necessary condition for maintaining contact holonomic constraint, ( ̇ is d = 0. ̇ = 0 in a form, based on the contact d Let’s write the velocity constraint normal, that does not require an explicit distance function (Figure 12.2). Let 3 R ∈ be a unit vector aligned with the contact normal, expressed in a world ˆ n 3 A R be the representation of the contact point on body p in frame. Let ∈ A 3 ∈ R be the representation of the contact point on the world frame, and let p B B . Although the contact-point vectors p are identical initially, and p body A B ̇ d p = 0 can be may be different. Thus the condition p and ̇ the velocities ̇ B A written T (12.3) ( ̇ p ˆ − ̇ p n ) = 0 . B A A , the Since the direction of the contact normal is defined as being into body ̇ ≥ 0 is written as impenetrability constraint d T ( ̇ ˆ p (12.4) n ̇ p . ) ≥ 0 − B A Let us rewrite the constraint (12.4) in terms of the twists V ) and = ( ω ,v A A A 1 B ,v ω ) of bodies A and V in a space frame. = ( Note that B B B ̇ v + ω × p p = v + [ ω ] p , = A A A A A A A = p v + ω × p = v ̇ + [ ω ] p . B B B B B B B We can also define the wrench = ( m,f ) corresponding to a unit force applied F along the contact normal: F = ( p . × ˆ n, ˆ n ) = ([ p ) ]ˆ n, ˆ n A A It is not necessary to appeal to forces in a purely kinematic analysis of rigid bodies, but we will find it convenient to adopt this notation now in anticipation of the discussion of contact forces in Section 12.2. With these expressions, the inequality constraint (12.4) can be written T F ( V (impenetrability constraint) −V ) ≥ 0 (12.5) A B 1 All twists and wrenches are expressed in a space frame in this chapter. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

484 466 12.1. Contact Kinematics (see Exercise 12.1). If T (active constraint) V F −V ( ) = 0 (12.6) B A then, to first order, the constraint is active and the bodies remain in contact. In the case where is a stationary fixture, the impenetrability constraint B (12.5) simplifies to T V ≥ 0 . (12.7) F A T T F 0, V F and V If are said to be repelling . If F > V are = 0, F and V A A A A reciprocal and the constraint is active. said to be V Twists and V first-order roll–slide mo- satisfying (12.6) are called B A Roll–slide contacts may tions – the contact may be either sliding or rolling. rolling contacts sliding contacts . The contact be further separated into and is rolling if the bodies have no motion relative to each other at the contact: ̇ p (12.8) = v . + [ ω p ] (rolling constraint) = ̇ = v p + [ ω ] p B B A A B A B A Note that “rolling” contacts include those where the two bodies remain station- ary relative to each other, i.e., no relative rotation. Thus “sticking” is another term for these contacts. If the twists satisfy Equation (12.6) but not the rolling equations of (12.8) then they are sliding. We assign to a rolling contact the contact label R , to a sliding contact the label , and to a contact that is breaking free (the impenetrability constraint S . (12.5) is satisfied but not the active constraint (12.6)) the label B The distinction between rolling and sliding contacts becomes especially im- portant when we consider friction forces in Section 12.2. Example 12.2. A and B Consider the contact shown in Figure 12.3. Bodies T T = [0 1 0] = = [1 2 0] p with contact normal direction ˆ n . p are in contact at B A The impenetrability constraint (12.5) is T F V ( −V , ) ≥ 0 B A which becomes ] [ − ω ω A B T T ] n ]ˆ n . p ) ˆ 0 ≥ [([ A v − v B A Substituting values, we obtain T ω − − ω ] ω v [0 0 1 0 1 0][ ω − ω v − ω v v − − v v Ay Bx Ax Bz By Az Az By Ay Bz Bx Ax ≥ 0 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

485 Chapter 12. Grasping and Manipulation 467 ˆz ˆy ˆx T p =[120] A A B T =[010] ˆ n roll-slide motions ω ω Az Az breaking free breaking free B B F F penetrating v v Ay Ay v Ax penetrating sliding rolling S R v + ω =0 Ay Az Example 12.2. (Top) Body B Figure 12.3: A at p = = p makes contact with B A T T with normal ˆ n [1 2 0] . (Bottom left) The twists V and their corresponding = [0 1 0] A contact labels for B stationary and A confined to a plane. The contact normal wrench T T is [ m . (Bottom right) Looking down the m m f = [0 0 1 0 1 0] f F f ] z x z y y x -axis. − v Ax or − ω + v 0; − v ≥ ω Az By Bz Ay and therefore roll–slide twists satisfy . (12.9) − ω ω + v = 0 − v By Bz Az Ay Equation (12.9) defines an 11-dimensional hyperplane in the 12-dimensional space of twists ( V , V ). B A May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

486 468 12.1. Contact Kinematics The rolling constraint (12.8) is equivalent to − v p − + ω , p p = v ω ω ω + p By Bz Bx By Az Bz Ay Ay Az Ax + ω , p p − v ω p ω = v − + ω p Bz Ax By Az Bx Ax Bz Az Ay Bx ω ω − + p = v + p p − ω v p ; ω Bx By By Ax Ay Az Bz Bx Ax Ay p substituting values for p and , we get B A − ω v = v 2 − 2 ω (12.10) , Bz Bx Az Ax + ω v = v (12.11) + ω , Bz Az By Ay + 2 + 2 − ω v v ω ω − ω . (12.12) = Ax Bz By Ay Az Bx The constraint equations (12.10)–(12.12) define a nine-dimensional hyperplane subspace of the 11-dimensional hyperplane of roll–slide twists. To visualize the constraints in a low-dimensional space, let’s assume that is stationary ( V is confined to the = 0) and A B z = 0 plane, i.e., V = A B T T is written ω F ω . The wrench v [ v 0] v v ] ω = [0 0 ω v Az Ax Ay Ay Ax Az Ay Ax Az T T m f . The roll–slide constraint (12.9) reduces to ] [ = [1 0 1] f y z x + ω v = 0 , Ay Az while the rolling constraints simplify to ω − 2 v , = 0 Az Ax + ω = 0 . v Az Ay ω ) space, and the ,v The single roll–slide constraint yields a plane in ( ,v Ay Ax Az V = 0, the constraint two rolling constraints yield a line in that plane. Because B surfaces pass through the origin V = 0. If V 6 = 0, this is no longer the case B A in general. Figure 12.3 shows graphically that nonpenetrating twists V must have a A nonnegative dot product with the constraint wrench when V F = 0. B 12.1.3 Multiple Contacts A is subject to n contacts with m other bodies, where Now suppose that a body ≥ m . The contacts are numbered i = 1 ,...,n , and the other bodies are n numbered j = 1 ,...,m . Let j ( i ) ∈ { 1 ,...,m } denote the number of the other to a half-space of body participating in contact constrains V i i . Each contact A its six-dimensional twist space that is bounded by a five-dimensional hyperplane T T V . Taking the union of the set of constraints from = F of the form V F A ( i ) j May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

487 Chapter 12. Grasping and Manipulation 469 2 polyhedral convex set ( for short) V of all the contacts, we get a polytope feasible twists in the V space, written as A T −V |F V = ( V , {V } i 0 for all ≥ ) A A ) i ( j i F ) corresponds to the i th contact normal (pointing into the body where A i V is the twist of the other body at contact i . The constraint at contact and i ) j ( is redundant if the half-space constraint contributed by contact does not i i change the feasible twist polytope V . In general, the feasible twist polytope for a body can consist of a six-dimensional interior (where no contact constraint is active), five-dimensional faces where one constraint is active, four-dimensional faces where two constraints are active, and so on, down to one-dimensional V edges and zero-dimensional points. A twist on a k -dimensional facet of the A polytope indicates that 6 k independent (non-redundant) contact constraints − are active. If all the bodies providing constraints are stationary, i.e., = 0 for all V j j , then each constraint hyperplane defined by (12.5) passes through the origin of V . The feasible twist space. We call such a constraint homogeneous A set becomes a cone rooted at the origin, called a (homogeneous) polyhedral . Let be the constraint wrench of stationary contact i . Then F convex cone i the feasible twist cone V is T } {V |F = . V i ≥ 0 for all V A A i If the F positively span the six-dimensional wrench space or, equivalently, the i convex span of the F contains the origin in the interior then the feasible twist i polytope V reduces to a point at the origin, the stationary contacts completely constrain the motion of the body, and we have form closure , discussed in more 12.1.7. detail in Section 12.1.2, each point contact i can be given a label As mentioned in Section B corresponding to the type of contact: R if the contact if the contact is breaking, is rolling, and S if the contact is sliding, i.e., (12.6) is satisfied but (12.8) is not. The contact mode for the entire system can be written as the concatenation of the contact labels at the contacts. Since we have three distinct contact n n contacts can have a maximum of 3 contact labels, a system of bodies with labels. Some of these contact modes may not be feasible, as their corresponding kinematic constraints may not be compatible. 2 We use the term “polytope” to refer generally to a convex set bounded by hyperplanes in an arbitrary vector space. The set need not be finite; it could be a cone with infinite volume. It could also be a point, or the null set if the constraints are incompatible with the rigid-body assumption. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

488 470 12.1. Contact Kinematics V 3 SBS v v v Ay Ay Ay BBS S SB SBB BB R BBB BSS BS RR v v v Ax Ax Ax B RRB V 3 F 1 BSB (c) (b) (a) Figure 12.4: Motion-controlled fingers contacting a hexagon that is constrained to 12.3). (a) A single stationary finger provides a translate in a plane only (Example V . The feasible-motion half-space single half-space constraint on the hexagon’s twist A B , is shaded gray. The two-dimensional set of twists corresponding to breaking contact S , and the zero-dimensional the one-dimensional set corresponding to sliding contact set corresponding to rolling (fixed) contact R are shown. (b) The union of constraints from two stationary fingers creates a cone of feasible twists. This cone corresponds to four possible contact modes: RR , SB , BS , and BB . The contact label for the finger at upper left is given first. (c) Three fingers, one of which is moving with a linear velocity V , create a closed polygon of feasible twists. There are seven possible contact 3 modes corresponding to the feasible twists: a two-dimensional set where all contacts are breaking, three one-dimensional sets where one contact constraint is active, and three zero-dimensional sets where two contact constraints are active. Note that rolling contact at the moving finger is not feasible, since translation of the hexagon to “follow” the moving finger, as indicated by the ◦ at the lower right of the lower figure, would violate one of the impenetrability constraints. If the third finger were stationary, the RRR only feasible motion of the hexagon would be zero velocity, with contact mode . Example 12.3. Figure 12.4 shows triangular fingers contacting a hexagonal body A . To more easily visualize the contact constraints the hexagon is re- stricted to translational motion in a plane only, so that its twist can be written V 12.4(a) the single stationary finger creates = (0 , 0 , 0 ,v 0). In Figure ,v , Ay A Ax a contact wrench F that can be drawn in V space. All feasible twists have 1 A a nonnegative component in the direction of F . Roll–slide twists satisfying 1 T V = 0 lie on the constraint line. Since no rotations are allowed, the only F A 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

489 Chapter 12. Grasping and Manipulation 471 ω Az t2 ac nt co ˆy co nt ac t1 ac t1 nt co ˆx ac co t3 nt v v Ay nt ac t2 co Ax co ac nt t3 Example 12.4. (Left) Arrows representing the lines of force corre- Figure 12.5: sponding to the contact normals of three stationary contacts on a planar body. If we are concerned only with feasible motions, and do not distinguish between rolling and sliding, contacts anywhere along the lines, with the contact normals shown, are equivalent. (Right) The three constraint half-spaces define a polyhedral convex cone of feasible twists. In the figure the cone is truncated at the plane = 2. The outer v Ay faces of the cone are indicated by hatching on a white background, and the inner faces by hatching on a gray background. Twists in the interior of the cone correspond to all contacts breaking, while twists on the faces of the cone correspond to one active constraint and twists on one of the three edges of the cone correspond to two active constraints. = 0. In Figure 12.4(b) the union of the twist yielding a rolling contact is V A constraints due to two stationary fingers creates a (polyhedral convex) cone 12.4(c) shows three fingers in contact, one of which of feasible twists. Figure V . Because the moving finger has nonzero velocity, its is moving with twist 3 V . The result is a closed constraint half-space is displaced from the origin by 3 polygon of feasible twists. Example 12.4. Figure 12.5 shows the contact normals of three stationary contacts with a planar body A , not shown. The body moves in a plane, so v = Az = ω = 0. In this example we do not distinguish between rolling and ω Ay Ax sliding motions, so the locations of the contacts along the normals are irrelevant. m − ,f = ,f F ), are F , = (0 , 1 , The three contact wrenches, written ( 2) 1 2 x y z − 1 , 0 , 1), yield the motion constraints , and F ( = (1 , 0 , 1) 3 v − 2 ω ≥ 0 , Az Ay − v , + ω 0 ≥ Az Ax v . + ω 0 ≥ Az Ax These constraints describe a polyhedral convex cone of feasible twists rooted at the origin, as illustrated on the right in Figure 12.5. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

490 472 12.1. Contact Kinematics Collections of Bodies 12.1.4 The discussion above can be generalized to find the feasible twists of multiple and j make contact at a point bodies in contact. If bodies , where ˆ n points i p i and F = ([ p ]ˆ n, ˆ n into body V must satisfy and V ) then their spatial twists j i the constraint T F V (12.13) −V ( ) ≥ 0 i j to avoid penetration. This is a homogeneous half-space constraint in the com- posite ( V bodies, each pairwise contact , V m ) twist space. In an assembly of j i m -dimensional composite twist space contributes another constraint in the 6 -dimensional for planar bodies) and the result is a polyhedral convex cone (3 m of kinematically feasible twists rooted at the origin of the composite twist space. The contact mode for the entire assembly is the concatenation of the contact labels at each contact in the assembly. If there are bodies whose motion is controlled, e.g., robot fingers, the con- straints on the motion of the remaining bodies are no longer homogeneous. As a result, the convex polyhedral set of feasible twists of the uncontrolled bodies, in their composite twist space, is no longer a cone rooted at the origin. 12.1.5 Other Types of Contacts 12.6(a), We have been considering point contacts of the type shown in Figure where at least one of the bodies in contact uniquely defines the contact normal. 12.6(b)–(e) show other types of contact. The kinematic constraints pro- Figures vided by the convex–concave vertex, line, and plane contacts of Figures 12.6(b)– (d) are, to first order, identical to those provided by finite collections of single- point contacts. The degenerate case in Figure 12.6(e) is ignored, as there is no unique definition of a contact normal. The impenetrability constraint (12.5) derives from the fact that arbitrarily large contact forces can be applied in the normal direction to prevent pene- tration. In Section 12.2, we will see that tangential forces due to friction may also be applied, and these forces may prevent slipping between two bodies in contact. Normal and tangential contact forces are subject to constraints: the normal force must be pushing into a body, not pulling, and the maximum fric- tion force is proportional to the normal force. If we wish to apply a kinematic analysis that can approximate the effects of friction without explicitly modeling forces, we can define three purely kinematic models of point contacts: a frictionless point contact , a point contact with friction , and a soft contact , also called a soft-finger contact. A frictionless point contact enforces only the roll–slide constraint (12.5). A point contact with May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

491 Chapter 12. Grasping and Manipulation 473 (b) (c) (d) (e) (a) (a) Vertex–face contact. (b) The contact between a convex vertex and Figure 12.6: a concave vertex can be treated as multiple point contacts, one at each face adjacent to the concave vertex. These faces define the contact normals. (c) A line contact can be treated as two point contacts at either end of the line. (d) A plane contact can be treated as point contacts at the corners of the convex hull of the contact area. (e) Convex vertex–vertex contact. This case is degenerate and so is not considered. friction also enforces the rolling constraints (12.8), implicitly modeling friction forces sufficient to prevent slip at the contact. A soft contact enforces the rolling constraints (12.8) as well as one more constraint: the two bodies in contact may not spin relative to each other about the contact normal axis. This models deformation and the resulting friction moment resisting any spin due to the nonzero contact area between the two bodies. For planar problems, a point contact with friction and a soft contact are identical. Planar Graphical Methods 12.1.6 Planar problems allow the possibility of using graphical methods to visualize the feasible motions for a single body, since the space of twists is three dimensional. An example planar twist cone is shown in Figure 12.5. Such a figure would be very difficult to draw for a system with more than three degrees of freedom. A convenient way to represent a planar twist, V = ( ω is as ,v } ,v s ), in { y x z center of rotation ,v − v . /ω ω a (CoR) at ( /ω ) plus the angular velocity z z z y x The CoR is the point in the (projective) plane that remains stationary under 3 the motion, i.e., the point where the screw axis intersects the plane. In the case where the speed of motion is immaterial, we may simply label the CoR with a ‘+’, ‘ − ’, or 0 sign representing the direction of rotation (Figure 12.7). The mapping from planar twists to CoRs is illustrated in Figure 12.8, which shows that the space of CoRs consists of a plane of ‘+’ CoRs (counterclockwise), a plane of ‘ − ’ CoRs (clockwise), and a circle of translation directions. 3 Note that the case ω = 0 must be treated with care, as it corresponds to a CoR at z infinity. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

492 474 12.1. Contact Kinematics +CoR Given the velocity of two points on a planar body, the lines normal to Figure 12.7: the velocities intersect at the CoR. The CoR shown is labeled ‘+’ corresponding to the (counterclockwise) positive angular velocity of the body. V ˆx CCW ω ˆy z rotations v y v x translations ˆy CW ˆx rotations Mapping a planar twist to a CoR. The ray containing the vector V V Figure 12.8: ω intersects the plane of ‘+’ CoRs at − ’ CoRs at ω = − 1, or = 1, or the plane of ‘ z z the circle of translation directions. Given two different twists and V and their corresponding CoRs, the set V 1 2 k V , corresponds k V of linear combinations of these twists, where k ,k ∈ R + 1 1 2 1 2 2 k to the line of CoRs passing through CoR( V ) and CoR( ). Since k and V 1 2 2 1 can have either sign, it follows that if either ω or ω is nonzero then the 2 z 1 z ω CoRs on this line can have either sign. If = ω = 0 then the set of linear z 2 z 1 combinations corresponds to the set of all translation directions. V k , the ,k V ≥ 0. Given two twists A more interesting case is when and 1 2 2 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

493 Chapter 12. Grasping and Manipulation 475 ω z ˆy b + − − c + a ˆx b + a v y c v x The intersection of a twist cone with the unit twist sphere, and the Figure 12.9: representation of the cone as a set of CoRs (the two hatched regions join at infinity to form a single set). nonnegative linear combination of these two velocities is written = pos( {V ) = , V , } V { k } V 0 + k ≥ V ,k | k 1 2 2 1 1 2 1 2 V and which is a planar twist cone rooted at the origin, with V defining 2 1 the edges of the cone. If ω and ω have the same sign then the CoRs of 1 z z 2 their nonnegative linear combinations CoR(pos( {V , V )) all have that sign } 2 1 V ) and CoR( V ) and lie on the line segment between the two CoRs. If CoR( 1 2 − ’ respectively, then CoR(pos( {V are labeled ‘+’ and ‘ , V )) consists of the } 2 1 line containing the two CoRs, minus the segment between the CoRs. This set consists of a ray of CoRs labeled ‘+’ attached to CoR( V ), a ray of CoRs 1 labeled ‘ − ’ attached to CoR( V ), and a point at infinity labeled 0, corresponding 2 to translation. This collection of elements should be considered as a single line segment (though one passing through infinity), just like the first case mentioned above. Figures 12.9 and 12.10 show examples of CoR regions corresponding to positive linear combinations of planar twists. The CoR representation of planar twists is particularly useful for represent- ing the feasible motions of one movable body in contact with stationary bodies. Since the constraints are stationary, as noted in Section 12.1.3, the feasible twists form a polyhedral convex cone rooted at the origin. Such a cone can be represented uniquely by a set of CoRs with ‘+’, ‘ − ’, and 0 labels. A gen- eral twist polytope, as would be generated by moving constraints, cannot be uniquely represented by a set of CoRs with such labels. Given a contact between a stationary body and a movable body, we can plot the CoRs that do not violate the impenetrability constraint. Label all points on the contact normal ‘ ± ’, points to the left of the inward normal ‘+’, and points to the right ‘ − ’. All points labeled ‘+’ can serve as CoRs with positive angular velocity for the movable body, and all points labeled ‘ − ’ can serve as CoRs with May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

494 476 12.1. Contact Kinematics + + + + + + + − + + − − + + + − (c) (b) (d) (a) (a) Positive linear combination of two CoRs labeled ‘+’. (b) Positive Figure 12.10: − ’ CoR. (c) Positive linear combination of linear combination of a ‘+’ CoR and a ‘ − three ‘+’ CoRs. (d) Positive linear combination of two ‘+’ CoRs and a ‘ ’ CoR. +,B + , Sr – , Sl -,Sl ± , R B + , B – , -,Sr Sr – , Sl + , Figure 12.11: The stationary triangle makes contact with a movable body. The − CoRs to the left of the contact normal are labeled ‘+’, to the right are labeled ‘ ’, and on the normal are labeled ‘ ± ’. Also given are the contact labels for the CoRs. Sl For points on the contact normal, the sign assigned to the Sr CoRs switches and at the contact point. Three CORs and their associated labels are illustrated. negative angular velocity, without violating the first-order contact constraint. We can further assign contact labels to each CoR corresponding to the first- order conditions for breaking contact B , sliding contact S , or rolling contact R . For planar sliding, we subdivide the label into two subclasses: Sr , where the S moving body slips to the right relative to the fixed constraint, and Sl , where the moving body slips to the left. Figure 12.11 illustrates the labeling. If there is more than one contact, we simply take the union of the constraints and contact labels from the individual contacts. This unioning of the constraints May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

495 Chapter 12. Grasping and Manipulation 477 SlSlB s s b l l ω z at infinity BSlB b b s l − SrBSr bs s BBSr bbs r r r − BBB F bbb ˆy w 1 F 1 3 s b s F ˆx w l l BSlSl 2 2 s bbfbb s bb BBSr r l BBSl BBR F F 1 2 + + bb s SrBB r w F 3 v y 3 fbb RBB bbb BBB v s bb x SlBB l (b) b s s SlSlB l l SlSlB s b s l l at infinity at infinity BSlB s b b l (b) (c) (a) _ _ Figure 12.12: Example 12.5. (a) A body resting on a table with two contact con- straints provided by the table and a single contact constraint provided by the sta- tionary finger. (b) The feasible twists represented as CoRs, shown in gray. Note that the lines that extend off to the left and to the bottom “wrap around” at infinity and come back in from the right and the top, respectively, so this CoR region should be interpreted as a single connected convex region. (c) The contact modes assigned to RRR . each feasible motion. The zero velocity contact mode is implies that the feasible CoR region is convex, as is the homogeneous polyhedral twist cone. Example 12.5. Figure 12.12(a) shows a planar body standing on a table while being contacted by a stationary robot finger. The finger defines an inequality constraint on the body’s motion and the table defines two more. The cone of twists that do not violate the impenetrability constraints is represented by the 12.12(b)). Each CoRs that are consistently labeled for each contact (Figure feasible CoR is labeled with a contact mode that concatenates the labels for the individual contacts (Figure 12.12(c)). Now look more closely at the CoR indicated by (+, SrBSr ) in Figure 12.12(c). Is this motion really possible? It should be apparent that it is, in fact, not possible: the body would immediately penetrate the stationary finger. Our incorrect conclusion that the motion was possible was due to the fact that our first-order analysis ignored the local contact curvature. A second-order analysis would show that this motion is indeed impossible. However, if the radius of curvature of the body at the contact were sufficiently small then the motion would be possible. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

496 478 12.1. Contact Kinematics _ _ + (Left) The body from Figure 12.12, with three stationary point con- Figure 12.13: tacts and the body’s feasible twist cone represented as a convex CoR region. (Middle) A fourth contact reduces the size of the feasible twist cone. (Right) By changing the angle of the fourth contact normal, no twist is feasible; the body is in form closure. Thus a first-order roll–slide motion might be classified as penetrating or breaking by a second-order analysis. Similarly, if our second-order analysis indicates a roll–slide motion, a third- or higher-order analysis may indicate n penetration or breaking free. In any case, if an th-order analysis indicates that the contact is breaking or penetrating, then no analysis of order greater than n will change the conclusion. 12.1.7 Form Closure Form closure of a body is achieved if a set of stationary constraints prevents all motion of the body. If these constraints are provided by robot fingers, we call this a form-closure grasp . An example is shown in Figure 12.13. 12.1.7.1 Number of Contacts Needed for First-Order Form Closure Each stationary contact provides a half-space twist constraint of the form i T F V ≥ 0 . i V satisfying the constraints is the zero twist. Form closure holds if the only twist j contacts, this condition is equivalent to For 6 {F pos( ,..., F R } ) = j 1 for bodies in three dimensions. Therefore, by fact 2 from the beginning of the chapter, at least 6 + 1 = 7 contacts are needed for the first-order form closure May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

497 Chapter 12. Grasping and Manipulation 479 ± ± ± ± ± (a) (b) (c) (d) (e) (f) Figure 12.14: (a) Four fingers yielding planar form closure. The first-order analysis treats (b) and (c) identically, saying that the triangle can rotate about its center in each case. A second-order analysis shows this is not possible for (b). The grasps in (d), (e), and (f) are identical by a first-order analysis, which says that rotation about any center on the vertical line is possible. This is true for (d), while for (e), rotation if possible about only some of these centers. No motion is possible in (f). of spatial bodies. For planar bodies, the condition is 3 pos( ,..., F , } ) = R {F j 1 and 3 + 1 = 4 contacts are needed for first-order form closure. These results are summarized in the following theorem. Theorem 12.6. For a planar body, at least four point contacts are needed for first-order form closure. For a spatial body, at least seven point contacts are needed. Now consider the problem of grasping a circular disk in the plane. It should be clear that kinematically preventing motion of the disk is impossible regardless of the number of contacts; it will always be able to spin about its center. Such exceptional objects are called – the positive span of the contact normal forces n n , where at all points on the object is not equal to R = 3 in the planar case and n = 6 in the spatial case. Examples of such objects in three dimensions include surfaces of revolution, such as spheres and ellipsoids. Figure 12.14 shows examples of planar grasps. The graphical methods of Section 12.1.6 indicate that the four contacts in Figure 12.14(a) immobilize the body. Our first-order analysis indicates that the bodies in Figures 12.14(b) and 12.14(c) can each rotate about their centers in the three-finger grasps, but in fact this is not possible for the body in Figure 12.14(b) – a second-order analysis would tell us that this body is actually immobilized. Finally, the first- order analysis tells us that the two-fingered grasps in Figures 12.14(d)–(f) are identical, but in fact the body in Figure 12.14(f) is immobilized by only two fingers owing to curvature effects. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

498 480 12.1. Contact Kinematics To summarize, our first-order analysis always correctly labels breaking and penetrating motions but second- and higher-order effects may change first-order roll–slide motions to breaking or penetrating. If a body is in form closure by first-order analysis, it is in form closure for any analysis. If only roll-slide motions are feasible by first-order analysis, the body could be in form closure by a higher-order analysis; otherwise, the body is not in form closure by any analysis. 12.1.7.2 A Linear Programming Test for First-Order Form Closure n × j F Let ··· F be a matrix whose columns are formed by the F ∈ R = [ F ] j 2 1 j contact wrenches. For spatial bodies, n = 6 and for planar bodies, n = 3 with T = [ m F f . The contacts yield form closure if there exists a vector of f ] iy iz i ix j n R weights , k ≥ 0, such that Fk + F k = 0 for all F ∈ ∈ R . ext ext F F ) < Clearly the body is not in form closure if the rank of is not full (rank( n ). If F is full rank, the form-closure condition is equivalent to the existence of strictly positive coefficients 0 such that Fk = 0. We can formulate this test k > as the following set of conditions, which is an example of a linear program : find k T 1 k minimizing (12.14) such that = 0 Fk ≥ , i = 1 ,...,j, k 1 i 1 is a j -vector of ones. If F is full rank and there exists a solution k where to (12.14), the body is in first-order form closure. Otherwise it is not. Note T that the objective function k is not necessary to answer the binary question, 1 depending on the LP solver, but it is included to make sure the problem is well posed. Example 12.7. The planar body in Figure 12.15 has a hole in the center. Two fingers each touch two different edges of the hole, creating four contact normals. The matrix F = [ F ] is given by F F F 3 2 1 4   0 0 − 1 2   0 − 1 0 1 F = . − 1 0 1 0 The matrix F is clearly rank 3. The linear program of (12.14) returns a solution k with = 1, so with this grasp the body is in form closure. = k k = 2, k = 2 4 1 3 You could test this in MATLAB, for example, using the linprog function, which takes as arguments: the objective function, expressed as a vector of weights f May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

499 Chapter 12. Grasping and Manipulation 481 F 4 F ˆy 3 F 1 ˆx F 2 Figure 12.15: Two fingers grasping the interior of an object. k ; a set of inequality constraints on k of the form Ak ≤ on the elements of b (used to encode k ≥ 1); and a set of equality constraints of the form A = k b eq i eq (used to encode Fk = 0); f = [1,1,1,1]; A = [[-1,0,0,0]; [0,-1,0,0]; [0,0,-1,0]; [0,0,0,-1]]; b = [-1,-1,-1,-1]; F = [[0,0,-1,2]; [-1,0,1,0]; [0,-1,0,1]]; % the F matrix Aeq = F; beq = [0,0,0]; k = linprog(f,A,b,Aeq,beq); which yields the result k = 2.0000 1.0000 2.0000 1.0000 If the right-hand finger were moved to the bottom right corner of the hole, the new F matrix   − 2 0 0 0   − 1 0 1 0 = F 1 0 − 1 0 − would still be full rank, but there would be no solution to the linear program. This grasp does not yield form closure: the body can slide downward on the page. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

500 482 12.1. Contact Kinematics Both grasps yield form closure, but which is better? Figure 12.16: 12.1.7.3 Measuring the Quality of a Form-Closure Grasp 12.16. Which is a better Consider the two form-closure grasps shown in Figure grasp? Answering this question requires a metric measuring the quality of a grasp. takes the set of contacts A grasp metric } and returns a single value {F i {F 0 indicates that the grasp does not yield form } ), where Qual( {F Qual( } ) < i i closure, and larger positive values indicate better grasps. There are many reasonable choices of grasp metric. As an example, suppose that, to avoid damaging the body, we require that the magnitude of the force at contact i be less than or equal to f > 0. Then the total set of contact i, max wrenches that can be applied by the j contacts is given by } { j ∑ ∣ ∣ f CF F ] = (12.15) f . ∈ [0 ,f i, max i i i =1 i See Figure 12.17 for an example in two dimensions. This shows the convex sets of wrenches that the contacts can apply to resist disturbance wrenches applied to the body. If the grasp yields form closure, the set includes the origin of the wrench space in its interior. Now the problem is to turn this polytope into a single number representing the quality of the grasp. Ideally this process would use some idea of the distur- bance wrenches that the body can be expected to experience. A simpler choice is to set Qual( {F ) to be the radius of the largest ball of wrenches, centered at } i the origin of the wrench space, that fits inside the convex polytope. In evaluat- ing this radius, two caveats should be considered: (1) moments and forces have different units, so there is no obvious way to equate force and moment mag- nitudes, and (2) the moments due to contact forces depend on the location of the space-frame origin. To address (1), it is common to choose a characteristic May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

501 Chapter 12. Grasping and Manipulation 483 F 2 F 1 d (a ) F 3 F 2 F 1 d ) (b F 3 (a) A set of three contact wrenches in a two-dimensional wrench space, Figure 12.17: d and the radius of the largest ball of wrenches centered at the origin that fits inside the wrench polygon. (b) A different set of three wrenches yielding a larger inscribed ball. r of the grasped body and convert contact moments m length m/r . to forces To address (2), the origin can be chosen somewhere near the geometric center of the body or at its center of mass. Given the choice of the space frame and the characteristic length r , we simply calculate the signed distance from the origin of the wrench space to each hyperplane on the boundary of . The minimum of these distances is CF {F 12.17). } ) (Figure Qual( i Returning to our original example in Figure 12.16, we can see that if each finger is allowed to apply the same force then the grasp on the left may be considered better, as the contacts can resist greater moments about the center of the object. 12.1.7.4 Choosing Contacts for Form Closure Many methods have been suggested for choosing form-closure contacts for fix- turing or grasping. One approach is to sample candidate grasp points on the surface of the body (four for planar bodies or seven for spatial) until a set is found yielding form closure. From there, the candidate grasp points may be incrementally repositioned according to gradient ascent, using the grasp metric, i.e., ∂ Qual( p ) /∂p , where p is the vector of all the coordinates of the contact May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

502 484 12.2. Contact Forces and Friction 4 locations. 12.2 Contact Forces and Friction 12.2.1 Friction A commonly used model of friction in robotic manipulation is Coulomb fric- tion . This experimental law states that the tangential friction force magnitude is related to the normal force magnitude f f by f is called ≤ μf μ , where n t t n friction coefficient . If the contact is sliding, or currently rolling but with the f incipient slip (i.e., at the next instant the contacts are sliding), then = μf n t and the direction of the friction force is opposite to that of the sliding direction, i.e., friction dissipates energy. The friction force is independent of the speed of sliding. Often two friction coefficients are defined, a static friction coefficient μ s μ μ , where and a kinetic (or sliding) friction coefficient . This implies ≥ μ k s k that a larger friction force is available to resist the initial motion but, once motion has begun, the resisting force is smaller. Many other friction models have been developed with different functional dependencies on factors such as the speed of sliding and the duration of static contact before sliding. All these are aggregate models of complex microscopic behavior. For simplicity, we will use the simplest Coulomb friction model with a single friction coefficient μ . This model is reasonable for hard, dry, materials. The friction coefficient depends on the two materials in contact and typically ranges from 0.1 to 1. For a contact normal in the +ˆz-direction, the set of forces that can be trans- mitted through the contact satisfies √ 2 2 + f f μf ≤ (12.16) . , f 0 ≥ z z x y Figure 12.18(a) shows that this set of forces forms a friction cone . The set of forces that the finger can apply to the plane lies inside the cone shown. Figure 12.18(b) shows the same cone from a side view, illustrating the friction − 1 α = tan angle μ , which is the half-angle of the cone. If the contact is not sliding, the force may be anywhere inside the cone. If the finger slides to the right, the force it applies lies on the right-hand edge of the friction cone, with magnitude determined by the normal force. Correspondingly, the plane applies the opposing force to the finger, and the direction of the tangential (frictional) portion of this force opposes the sliding direction. 4 The gradient vector ∂ Qual( p ) /∂p must be projected onto the tangent planes at the points of contact to keep the contact locations on the surface of the object. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

503 Chapter 12. Grasping and Manipulation 485 μf z ˆz f z μ α 1 ˆy ˆx (c) (b) ) (a Figure 12.18: (a) A friction cone illustrating all possible forces that can be trans- mitted through the contact. (b) A side view of the same friction cone showing the − 1 α = tan μ friction coefficient μ . (c) An inscribed polyhedral and the friction angle convex cone approximation to the circular friction cone. To allow linear formulations of contact mechanics problems, it is often con- venient to represent the convex circular cone by a polyhedral convex cone. Fig- ure 12.18(c) shows an inscribed four-sided pyramidal approximation of the fric- f 1), ,f tion cone, defined by the positive span of the ( ,f , ) cone edges ( μ, 0 z x y ( − μ, 0 , 1), (0 ,μ, 1), and (0 , − μ, 1). We can obtain a tighter approximation to the circular cone by using more edges. An inscribed cone underestimates the friction forces available, while a circumscribed cone overestimates the friction forces. The choice of which to use depends on the application. For example, if we want to ensure that a robot hand can grasp an object, it is a good idea to underestimate the friction forces available. For planar problems, no approximation is necessary – a friction cone is ex- actly represented by the positive span of the two edges of the cone, similarly to the side view illustrated in Figure 12.18(b). Once we choose a coordinate frame, any contact force can be expressed as a F = ([ wrench ] f,f ), where p is the contact location. This turns a friction cone p into a wrench cone. A planar example is shown in Figure 12.19. The two edges of the planar friction cone give two rays in the wrench space, and the wrenches that can be transmitted to the body through the contact give the positive span of basis vectors along these edges. If F are basis vectors for these and F 2 1 wrench cone edges, we write the wrench cone as W C = pos( {F ). , F } 2 1 If multiple contacts act on a body, then the total set of wrenches that can be transmitted to the body through the contacts is the positive span of all the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

504 486 12.2. Contact Forces and Friction m z ˆy F 2 ˆx μ f f 1 2 α 1 f f y x F 1 (a ) (b) m z f f 3 ˆy 4 f f 1 2 composite wrench cone F 3 ˆx f F y 4 f x F 1 (d) F (c) 2 (a) A planar friction cone with friction coefficient μ and corresponding Figure 12.19: − 1 = tan friction angle α μ . (b) The corresponding wrench cone. (c) Two friction cones. (d) The corresponding composite wrench cone. individual wrench cones C W , i } { ∑ ∣ ∣ . } ) = C W C k 0 F ≥ = pos( {W F ,k ∈W C i i i i i i i This composite wrench cone is a convex cone rooted at the origin. An example of such a composite wrench cone is shown in Figure 12.19(d) for a planar body with the two friction cones shown in Figure 12.19(c). For planar problems, the composite wrench cone in the three-dimensional wrench space is polyhedral. For spatial problems, wrench cones in the six-dimensional wrench space are not polyhedral unless the individual friction cones are approximated by polyhedral cones, as in Figure 12.18(c). If a contact or set of contacts acting on a body is ideally force-controlled, the wrench F specified by the controller must lie within the composite wrench cont May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

505 Chapter 12. Grasping and Manipulation 487 F + F 1 2 ˆy ˆy F 2 ˆx ˆx F 1 (Left) The planar wrench F = ( m , ,f 2) represented ,f , ) = (2 . 5 Figure 12.20: − 1 y z x as an arrow in the ˆx–ˆy-plane. (Middle) The same wrench can be represented by an arrow anywhere along the line of action. (Right) Two wrenches are summed by sliding their arrows along their lines of action until the bases of the arrows are coincident, then doing a vector sum by the parallelogram construction. cone corresponding to those contacts. If there are other non-force-controlled contacts acting on the body, then the cone of possible wrenches on the body is equivalent to the wrench cone from the non-force-controlled contacts but F . translated to be rooted at cont 12.2.2 Planar Graphical Methods 12.2.2.1 Representing Wrenches F ) with a nonzero linear component can be m Any planar wrench ,f ,f = ( z y x represented as an arrow drawn in the plane, where the base of the arrow is at the point 1 x,y ) = ( f ) m − , ( m f x z z y 2 2 f + f y x and the head of the arrow is at ( x + f ). The moment is unchanged if we ,y + f y x slide the arrow anywhere along its line, so any arrow of the same direction and length along this line represents the same wrench (Figure 12.20). If f = 0 = f y x May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

506 488 12.2. Contact Forces and Friction m and = 0, the wrench is a pure moment, and we do not try to represent it 6 z graphically. Two wrenches, represented as arrows, can be summed graphically by sliding the arrows along their lines until the bases of the arrows are coincident. The arrow corresponding to the sum of the two wrenches is obtained as shown in Fig- ure 12.20. The approach can be applied sequentially to sum multiple wrenches represented as arrows. 12.2.2.2 Representing Wrench Cones In the previous section each wrench had a specified magnitude. However, a rigid- body contact implies that the contact normal force can be arbitrarily large; the normal force achieves the magnitude needed to prevent two bodies from penetrating. Therefore it is useful to have a representation of all wrenches of 3 F is a basis vector. k ≥ 0 and F ∈ R k the form , where moment labeling One such representation is . The arrow for the basis wrench F is drawn as described in Section 12.2.2.1. Then all points in the plane to the left of the line of the arrow are labeled ‘+’, indicating that any positive F creates a positive moment m scaling of about those points, and all points in z the plane to the right of the arrow are labeled ‘ ’, indicating that any positive − F creates a negative moment about those points. Points on the line scaling of ± ’. are labeled ‘ Generalizing, moment labels can represent any homogeneous convex planar wrench cone, much as a homogeneous convex planar twist cone can be rep- resented as a convex CoR region. Given a collection of directed force lines ) can corresponding to wrenches F } for all k {F ≥ 0, the wrench cone pos( k i i i i F be represented by labeling each point in the plane with a ‘+’ if each makes a i nonnegative moment about that point, with a ‘ − ’ if each F makes a nonpositive i moment about that point, with a ‘ ± ’ if each F makes zero moment about that i point, or with a blank label if at least one wrench makes a positive moment and at least one wrench makes a negative moment about that point. The idea is best illustrated by an example. In Figure 12.21(a), the basis wrench F is represented by labeling the points to the left of the force line with 1 a ‘+’ and points to the right of the line with a ‘ − ’. Points on the line are labeled ‘ ± ’. In Figure 12.21(b), another basis wrench is added, which could represent the other edge of a planar friction cone. Only the points in the plane that are consistently labeled for both lines of force retain their labels; inconsis- tently labeled points lose their labels. Finally, a third basis wrench is added in Figure 12.21(c). The result is a single region labeled ‘+’. A nonnegative linear combination of the three basis wrenches can create any line of force in the plane May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

507 Chapter 12. Grasping and Manipulation 489 F F F F 1 F 1 2 2 1 − + − + + F 3 ± ± (b) ) (a (c) (a) Representing a line of force by moment labels. (b) Representing Figure 12.21: the positive span of two lines of force by moment labels. (c) The positive span of three lines of force. that passes around this region in a counterclockwise sense. No other wrench can be created. If an additional basis wrench were added passing clockwise around the region labeled ‘+’ in Figure 12.21(c), then there would be no consistently labeled point in the plane; the positive linear span of the four wrenches would be the entire 3 R . wrench space The moment-labeling representation is equivalent to a homogeneous convex wrench cone representation. The moment-labeling regions in each part, (a), (b) and (c), of Figure 12.21 are properly interpreted as a single convex region, much like the CoR regions of Section 12.1.6. 12.2.3 Force Closure Consider a single movable body and a number of frictional contacts. We say the contacts result in if the composite wrench cone contains force closure the entire wrench space, so that any external wrench F on the body can be ext balanced by contact forces. We can derive a simple linear test for force closure which is exact for planar F , i = 1 ,...,j , be the wrenches cases and approximate for spatial cases. Let i corresponding to the edges of the friction cones for all the contacts. For planar problems, each friction cone contributes two edges and, for spatial problems, each friction cone contributes three or more edges, depending on the polyhedral approximation chosen (see Figure 12.18(c)). The columns of an n × j matrix F are the F = 6 for spatial problems. , where n = 3 for planar problems and n i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

508 490 12.2. Contact Forces and Friction _ + (b) (a) Figure 12.22: An equilateral triangle can be force-closure-grasped by two fingers on ◦ ≥ tan 30 0 ≈ the edges of the triangle if μ 577. (a) The grasp shown with μ = 0 . 25 . would not be in force closure, as indicated by the consistently labeled moment-labeling region. (b) The grasp shown is in force closure with μ = 1; the dashed line indicates that the two contacts can “see” each other, i.e., their line of sight is inside both friction cones. Now, the test for force closure is identical to that for form closure. The contacts yield force closure if • rank F = n , and • there exists a solution to the linear programming problem (12.14). μ In the case of = 0, each contact can provide forces only along the normal direction, and force closure is equivalent to first-order form closure. Number of Contacts Needed for Force Closure 12.2.3.1 For planar problems, four contact wrenches are sufficient to positively span the three-dimensional wrench space, which means that as few as two frictional contacts (with two friction cone edges each) are sufficient for force closure. Using moment labeling, we see that force closure is equivalent to having no consistent moment labels. For example, if the two contacts can “see” each other by a line of sight inside both friction cones, we have force closure (Figure 12.22(b)). It is important to note that force closure simply means that the contact friction cones can generate any wrench. It does not necessarily mean that the body will not move in the presence of an external wrench. For the example of Figure 12.22(b), whether the triangle falls under gravity depends on the internal forces between the fingers. If the motors powering the fingers cannot provide sufficient forces, or if they are restricted to generate forces only in certain directions, the triangle may fall despite force closure. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

509 Chapter 12. Grasping and Manipulation 491 t3 co nt ac e S plan ˆz r 3 ˆx ˆy r 2 r 1 t1 ac nt co ac co nt t2 Figure 12.23: A spatial rigid body restrained by three point contacts with friction. Two frictional point contacts are insufficient to yield force closure for spatial bodies, as there is no way to generate a moment about the axis joining the two contacts. A force-closure grasp can be obtained with as few as three frictional contacts, however. A particularly simple and appealing result due to [89] reduces the force-closure analysis of spatial frictional grasps to a planar force-closure problem. Referring to Figure 12.23, suppose that a rigid body is constrained by three frictional point contacts. If the three contact points happen to be collinear then obviously any moment applied about this line cannot be resisted by the three contacts. We can therefore exclude this case and assume that the three contact points are not collinear. The three contacts then define a unique plane S and, at each contact point, three possibilities arise (see Figure 12.24): • the friction cone intersects in a planar cone; S the friction cone intersects in a line; • S the friction cone intersects S at a point. • S in a The body is in force closure if and only if each friction cone intersects planar cone and S is also in planar force closure. Theorem 12.8. Given a spatial rigid body restrained by three point contacts with friction, the body is in force closure if and only if the friction cone at each S of the contacts in a cone and the plane S is in contact intersects the plane planar force closure. Proof. First, the necessity condition – if the spatial rigid body is in force closure then each friction cone intersects S in a planar cone and S is also in planar force May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

510 492 12.2. Contact Forces and Friction Figure 12.24: Three possibilities for the intersection between a friction cone and a plane. S closure – is easily verified: if the body is in spatial force closure then (which is a part of the body) must also be in planar force closure. Moreover, if even one S in a line or point then there will be external moments friction cone intersects (e.g., about the line between the remaining two contact points) that cannot be resisted by the grasp. To prove the sufficiency condition – if each friction cone intersects S in a is also in planar force closure then the spatial rigid body is in S planar cone and force closure – choose a fixed reference frame such that S lies in the ˆx–ˆy-plane 3 and let R r denote the vector from the fixed-frame origin to contact point ∈ i 3 by f i ∈ R (see Figure 12.23). Denoting the contact force at i , the contact i 6 wrench F ∈ R is then of the form i [ ] m i = F , (12.17) i f i m f = r 3. Denote the arbitrary external wrench × where each , , i = 1 , 2 i i i 6 ∈ F R by ext [ ] m ext 6 = F (12.18) ∈ R . ext f ext Force closure then requires that there exist contact wrenches , i = 1 F 2 , 3, each , i lying inside its respective friction cone, such that, for any external disturbance F , the following equality is satisfied: wrench ext F + F (12.19) + F = 0 + F ext 1 2 3 or, equivalently, f (12.20) + f , + f = 0 + f ext 2 1 3 ( r (12.21) × f . ) + ( r = 0 × f m ) + ( r ) + × f 3 2 ext 2 1 1 3 If each contact force and moment, as well as the external force and moment, is orthogonally decomposed into components lying on the plane spanned by S (corresponding to the ˆx–ˆy-plane in our chosen reference frame) and its normal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

511 Chapter 12. Grasping and Manipulation 493 N (corresponding to the ˆz-axis in our chosen reference frame) then the subspace previous force-closure equalities can be written as = + f + f f , (12.22) f − 2 ext ,S 3 S S 1 S ) + ( × f (12.23) , ) + ( r m × f − ) = ( r f × r 3 S S 2 2 ext ,S S 1 1 3 = f + f + − f , (12.24) f N 1 ext ,N 3 N N 2 r (12.25) × f . m ) + ( r − × f ) = f ) + ( r ( × N 3 N 2 2 N ext ,N 1 1 3 S to refer both to the slice of the rigid body cor- In what follows we shall use responding to the ˆx–ˆy-plane and to the ˆx–ˆy-plane itself; we will always identify with the ˆz-axis. N S is in pla- Proceeding with the proof of sufficiency, we now show that if nar force closure then the body is in spatial force closure. In terms of Equa- tions (12.24) and (12.25) we wish to show that, for any arbitrary forces f ∈ ,S ext f S N and arbitrary moments m ∈ S , m , ∈ N , there exist ∈ ext ,S ext ,N ,N ext contact forces f ∈ S , f 3, that satisfy (12.24) and(12.25) such ∈ N , i = 1 , 2 , iN iS i = 1 , 2 , 3, the contact force f . that, for each f i + f lies in friction cone = iN iS i First consider the force-closure equations (12.24) and (12.25) in the nor- mal direction N . Given an arbitrary external force f and external N ∈ ,N ext moment m ∈ S , Equations (12.24) and (12.25) constitute a set of three lin- ,S ext ear equations in three unknowns. From our assumption that the three contact points are never collinear, these equations will always have a unique solution ∗ ∗ ∗ . N } in ,f ,f set f { 3 N 2 N N 1 Since S is assumed to be in planar force closure, for any arbitrary f ∈ S ,S ext m , 3, that ∈ N there will exist planar contact forces f , ∈ S and i = 1 , 2 iS ,N ext lie inside their respective planar friction cones and also satisfy Equations (12.22) and (12.23). This solution set is not unique: one can always find a set of internal = 1 ∈ S forces i η , 2 , 3, each lying inside its respective friction cone, satisfying , i + + η (12.26) η η , = 0 3 1 2 r × η ) + ( r (12.27) ( η ) + ( r × η ) = 0 . × 1 2 3 1 2 3 (To see why such η exist, recall that since S is assumed to be in planar force i closure, solutions to (12.22) and (12.23) must exist for = μ = 0; these f ext ,S ext ,N η solutions are precisely the internal forces ). Note that these two equations i constitute three linear equality constraints involving six variables, so that there exists a three-dimensional linear subspace of solutions for η . ,η { ,η } 3 1 2 Now, if { f + ,f ,f η + ,f f { } satisfy (12.22) and (12.23) then so will 1 S S S 2 1 S 2 S 1 3 η can, in turn, be chosen to have ,f } ,η + η ,η } . The internal forces { η 1 3 2 S 3 3 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

512 494 12.3. Manipulation sufficiently large magnitudes that the contact forces ∗ + f (12.28) f f = + η , 1 1 1 S N 1 ∗ = f f , η + f (12.29) + S 2 2 2 2 N ∗ = f f η (12.30) + f + S 3 3 3 3 N all lie inside their respective friction cones. This completes the proof of the sufficiency condition. 12.2.3.2 Measuring the Quality of a Force-Closure Grasp Friction forces are not always repeatable. For example, try putting a coin on a book and tilting the book. The coin should begin to slide when the book is at 1 − α μ with respect to the horizontal. If you do the experiment = tan an angle , owing to effects μ several times then you may find a range of measured values of that are difficult to model. For that reason, when choosing between grasps it is reasonable to choose finger locations that minimize the friction coefficient needed to achieve force closure. 12.2.4 Duality of Force and Motion Freedoms Our discussion of kinematic constraints and friction should have made it ap- parent that, for any point contact and contact label, the number of equality constraints on a body’s motion caused by that contact is equal to the number of wrench freedoms it provides. For example, a breaking contact B provides zero equality constraints on the body’s motion and also allows no contact force. A fixed contact R provides three motion constraints (the motion of a point on the body is specified) and three freedoms in the contact force: any wrench in the interior of the contact wrench cone is consistent with the contact mode. Finally, a slipping contact S provides one equality motion constraint (one equation on the body’s motion must be satisfied to maintain the contact) and, for a given motion satisfying the constraint, the contact wrench has only one freedom: the magnitude of the contact wrench on the edge of the friction cone and opposing the slipping direction. In the planar case, the motion constraints and wrench freedoms for B , S , and R contacts are 0, 1, and 2, respectively. 12.3 Manipulation So far we have studied the feasible twists and contact forces due to a set of contacts. We have also considered two types of manipulation: form-closure and force-closure grasping. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

513 Chapter 12. Grasping and Manipulation 495 Manipulation consists of much more than just grasping, however. It in- cludes almost anything where manipulators impose motions or forces with the purpose of achieving the motion or restraint of objects. Examples include car- rying glasses on a tray without toppling them, pivoting a refrigerator about one of its feet, pushing a sofa along the floor, throwing and catching a ball, trans- porting parts on a vibratory conveyor, etc. Endowing a robot with methods of manipulation beyond grasp-and-carry allows it to manipulate several parts simultaneously, manipulate parts that are too large to be grasped or too heavy to be lifted, or even to send parts outside the workspace of the end-effector by throwing them. To plan such manipulation tasks, we use the contact kinematic constraints of Section 12.1, the Coulomb friction law of Section 12.2, and the dynamics of rigid bodies. Restricting ourselves to a single rigid body and using the notation 8, the body’s dynamics are written as of Chapter ∑ T ̇ k , F (12.31) = G + V − [ad ∈WC ] F GV , k F ≥ 0 , i i V i i ext i where V is the body’s twist, G is its spatial inertia matrix, F is the external ext wrench acting on the body due to gravity, etc., WC is the set of possible i ∑ wrenches acting on the body due to contact is the wrench due to k i F , and i i the contacts. All wrenches are written in the body’s center-of-mass frame. Now, given a set of motion- or force-controlled contacts acting on the body, and the initial state of the system, one method for solving for the motion of the body is the following. (a) Enumerate the set of possible contact modes considering the current state of the system (e.g., a contact that is currently sticking can transition to sliding or breaking). The contact modes consist of the contact labels R , S , and B at each contact. (b) For each contact mode, determine whether there exists a contact wrench ∑ that is consistent with the contact mode and Coulomb’s law, and k F i i ̇ V consistent with the kinematic constraints of the contact an acceleration mode, such that Equation (12.31) is satisfied. If so, this contact mode, contact wrench, and body acceleration comprises a consistent solution to the rigid-body dynamics. This kind of “case analysis” may sound unusual; we are not simply solving a set of equations. It also leaves open the possibility that we could find more than one consistent solution, or perhaps no consistent solution. This is, in fact, the case: we can define problems with multiple solutions ( ambiguous problems) and problems with no solutions ( inconsistent problems). This state of affairs May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

514 496 12.3. Manipulation is a bit unsettling; surely there is exactly one solution to any real mechanics problem! But this is the price we pay for using the assumptions of perfectly rigid bodies and Coulomb friction. Despite the possibility of zero or multiple solutions, for many problems the method described above will yield a unique contact mode and motion. Some of the manipulation tasks below are , where the velocities quasistatic and accelerations of the bodies are small enough that inertial forces may be ignored. Contact wrenches and external wrenches are always in force balance, and Equation (12.31) reduces to ∑ F k (12.32) F . = 0 , k ∈WC ≥ 0 + F , i i i i ext i Below we illustrate the methods of this chapter with four examples. (A block carried by two fingers) Example 12.9 Consider a planar block in . 12.25(a). The friction coeffi- gravity supported by two fingers, as in Figure μ = 1, and the other contact is cient between one finger and the block is frictionless. Thus the cone of wrenches that can be applied by the fingers is {F 12.25(b). , F pos( , F ), as shown using moment labeling in Figure } 3 2 1 Our first question is whether the stationary fingers can keep the block at ) = (0 m rest. To do so, the fingers must provide a wrench ,f ) ,f g = ( , 0 , m F y x z F = (0 to balance the wrench , 0 , − m g ) due to gravity, where g > 0. As shown ext in Figure 12.25(b), this wrench is not in the composite cone of possible contact wrenches. Therefore the contact mode RR is not feasible, and the block will move relative to the fingers. Now consider the case where the fingers each accelerate to the left at 2 g . In this case the contact mode RR requires that the block also accelerate to the left at 2 g , − 2 m g, 0). Therefore the . The wrench needed to cause this acceleration is (0 , 2 m g, 0) −F = total wrench that the fingers must apply to the block is (0 − ext , − 2 m g, m g ). As shown in Figures (0 12.25(c), (d), this wrench lies inside the composite wrench cone. Thus (the block stays stationary relative to the RR g fingers) is a solution as the fingers accelerate to the left at 2 . dynamic grasp – inertial forces are used to keep the block This is called a pressed against the fingers while the fingers move. If we plan to manipulate the block using a dynamic grasp then we should make certain that no contact modes other than RR are feasible, for completeness. Moment labels are convenient for understanding this problem graphically, but we can also solve it algebraically. The lower finger contacts the block at ( x,y ) = ( − 3 , − 1) and the upper finger contacts the block at (1 , 1). This gives the basis contact wrenches 1 1 √ √ − ( 4 , − 1 , 1) , F 0) = , 1 − ( − 2 , 1 , 1) , F , = (1 . F = 3 2 1 2 2 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

515 Chapter 12. Grasping and Manipulation 497 − ˆy F 3 m g F F 2 1 + ˆx m − g (a) (b) k + k F F 1 2 1 2 − − m g k F 3 3 acceleration + + force (d) (c) (a) A planar block in gravity supported by two robot fingers, the lower Figure 12.25: μ = 1 and the upper with μ = 0. (b) The composite wrench with a friction cone with cone that can be applied by the fingers represented using moment labels. To balance the block against gravity, the fingers must apply the line of force shown. This line − ’, and therefore it cannot be has a positive moment with respect to points labeled ‘ generated by the two fingers. (c) For the block to match the fingers’ acceleration to the left, the contacts must apply the vector sum of the wrench to balance gravity and the wrench needed to accelerate the block to the left. This total wrench lies inside the composite wrench cone, as the line of force has a positive moment with respect to points labeled ‘+’ and a negative moment with respect to points labeled ‘ − ’. (d) The total wrench applied by the fingers in (c) can be translated along the line of action without changing the wrench. This allows us to visualize easily the components F + k F and k k F provided by the fingers. 2 1 2 3 1 3 Let the fingers’ acceleration in the ˆx-direction be written a . Then, under the x assumption that the block stays fixed with respect to the fingers ( RR contact mode), Equation (12.31) can be written k (12.33) F . + k 0) F , + k a F m + (0 , 0 , − m g ) = (0 , 3 3 x 2 2 1 1 May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

516 498 12.3. Manipulation k ,k ,k . Solving, we get This yields three equations in the three unknowns, 2 3 1 1 1 1 √ √ a ( a + g ) m , k = = − − ( k = + 5 g ) m , k ) m . g ( a 3 − x 3 x 1 2 x 2 2 2 2 2 For the k . For ˆx-direction finger to be nonnegative, we need − 5 g ≤ a g ≤ − x i accelerations in this range, a dynamic grasp is a consistent solution. Example 12.10 Try this experiment. Get a meter (The meter-stick trick) . stick (or any similar long smooth stick) and balance it horizontally on your two index fingers. Place your left finger near the 10 cm mark and your right finger near the 60 cm mark. The center of mass is closer to your right finger but still between your fingers, so that the stick is supported. Now, keeping your left finger stationary, slowly move your right finger towards your left until they touch. What happens to the stick? If you didn’t try the experiment, you might guess that your right finger passes under the center of mass of the stick, at which point the stick falls. If you did try the experiment, you saw something different. Let’s see why. Figure 12.26 shows the stick supported by two frictional fingers. Since all motions are slow, we use the quasistatic approximation that the stick’s accel- eration is zero and so the net contact wrench must balance the gravitational wrench. As the two fingers move together, the stick must slip on one or both fingers to accommodate the fact that the fingers are getting closer to each other. Figure 12.26 shows the moment-labeling representation of the composite wrench cone for three different contact modes: RSr , where the stick remains stationary relative to the left finger and slips to the right relative to the right finger; SlR , where the stick slips to the left relative to the left finger and remains stationary relative to the right finger; and , where the stick slips on both fingers. It SlSr SlR contact mode can provide a wrench is clear from the figure that only the that balances the gravitational wrench. In other words, the right finger, which supports more of the stick’s weight, remains fixed relative to the stick while the left finger slides under the stick. Since the right finger is moving to the left in the world frame, this means that the center of mass is moving to the left at the same speed. This continues until the center of mass is halfway between the fingers, at which point the stick transitions to the SlSr contact mode, and the center of mass stays centered between the fingers until they meet. The stick never falls. Note that this analysis relies on the quasistatic assumption. It is easy to make the stick fall if you move your right finger quickly; the friction force at the right finger is not large enough to create the large stick acceleration needed to maintain a sticking contact. Also, in your experiment, you might notice that, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

517 Chapter 12. Grasping and Manipulation 499 m g + − Sr R g m RSr ± + g m − − + R Sl g m SlR Sr Sl SlSr Figure 12.26: Top left: Two frictional fingers supporting a meter stick in gravity. The other three panels show the moment labels for the RSr , SlR , and SlSr contact modes. Only the SlR contact mode yields force balance. when the center of mass is nearly centered, the stick does not actually achieve the idealized SlSr SlR contact mode, but instead switches rapidly between the and RSr contact modes. This occurs because the static friction coefficient is larger than the kinetic friction coefficient. Example 12.11 (Stability of an assembly) . Consider the arch in Figure 12.27. Is it stable under gravity? For a problem like this, graphical planar methods are difficult to use, since May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

518 500 12.3. Manipulation F 9 F 8 3 F 10 F 7 1 2 F 11 F 6 F 5 F 12 F F 1 F 2 F F F 14 15 3 16 F 4 F 13 (Left) An arch under gravity. (Right) The friction cones at the contacts Figure 12.27: of stone 1 and the contacts of stone 2. there are potentially multiple moving bodies. Instead we test algebraically for R . The friction cones consistency of the contact mode with all contacts labeled 12.27. With these labelings of the friction cone edges, the are shown in Figure k arch can remain standing if there is a consistent solution if there exist ≥ 0 i for = 1 ,..., 16 satisfying the following nine wrench-balance equations, three i for each body: 8 ∑ , = 0 k F F + i ext1 i i =1 16 ∑ k = 0 F , + F ext2 i i =9 i 12 ∑ − k F + F , = 0 ext3 i i i =5 where F is the gravitational wrench on body i . The last set of equations i ext comes from the fact that the wrenches that body 1 applies to body 3 are equal and opposite those that body 3 applies to body 1, and similarly for bodies 2 and 3. This linear constraint satisfaction problem can be solved by a variety of methods, including linear programming. (Peg insertion) . Example 12.12 12.28 shows a force-controlled planar Figure peg in two-point contact with a hole during insertion. Also shown are the contact friction cones acting on the peg and the corresponding composite wrench cone, illustrated using moment labels. If the force controller applies the wrench F 1 F . to the peg, it may jam – the hole may generate contact forces that balance 1 Therefore the peg may get stuck in this position. If the force controller applies the wrench F , however, the contacts cannot balance the wrench and insertion 2 proceeds. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

519 Chapter 12. Grasping and Manipulation 501 F 2 F 1 − (Left) A peg in two-point contact with a hole. (Right) The wrench Figure 12.28: F F continues to push the peg into the may cause the peg to jam, while the wrench 1 2 hole. If the friction coefficients at the two contacts are large enough that the 12.22(b)), the peg is in force two friction cones “see” each other’s base (Figure closure and the contacts may be able to resist any wrench (depending on the internal force between the two contacts). The peg is said to be wedged. 12.4 Summary Three ingredients are needed to solve rigid-body contact problems with • friction: (1) the contact kinematics, which describes the feasible motions of rigid bodies in contact; (2) a contact force model, which describes the forces that can be transmitted through frictional contacts; and (3) rigid- 8. body dynamics, as described in Chapter A and B , be in point contact at Let two rigid bodies, in a space frame. • p A 3 ∈ R Let ˆ be the unit contact normal, pointing into body A . Then the n F associated with a unit force along the contact spatial contact wrench T T T p . The impenetrability constraint is ]ˆ n ) normal is ˆ n F ] = [([ A T ( V , −V F ) ≥ 0 A B where V . and V B are the spatial twists of A and B A • A contact that is sticking or rolling is assigned the contact label R , a con- tact that is sliding is assigned the contact label S , and a contact that is breaking free is assigned the contact label B . For a body with multiple con- tacts, the contact mode is the concatenation of the labels of the individual contacts. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

520 502 12.4. Summary A single rigid body subjected to multiple stationary point contacts has a • homogeneous (rooted-at-the-origin) polyhedral convex cone of twists that satisfy all the impenetrability constraints. 3 A homogeneous polyhedral convex cone of planar twists in R • can be equi- valently represented by a convex region of signed rotation centers in the plane. • If a set of stationary contacts prevents a body from moving, purely by a kinematic analysis considering only the contact normals, the body is said F for contacts to be in first-order form closure. The contact wrenches i n ,...,j positively span R i , where n = 3 for the planar case and n = 6 = 1 for the spatial case. • At least four point contacts are required for first-order form closure of a planar body, and at least seven point contacts are required for first-order form closure of a spatial body. • The Coulomb friction law states that the tangential frictional force mag- nitude f is the friction coefficient at a contact satisfies f μ ≤ μf , where n t t f is the normal force. When the contact is sticking, the frictional and n force can be anything satisfying this constraint. When the contact is slid- f and the direction of the friction force opposes the direction = μf ing, t n of sliding. • Given a set of frictional contacts acting on a body, the wrenches that can be transmitted through these contacts is the positive span of the wrenches that can be transmitted through the individual contacts. These wrenches form a homogeneous convex cone. If the body is planar, or if the body is spatial but the contact friction cones are approximated by polyhedral cones, the wrench cone is also polyhedral. 3 A homogeneous convex cone of planar wrenches in R can be represented • as a convex region of moment labels in the plane. • A body is in force closure if the homogeneous convex cone of contact 3 R wrenches from the stationary contacts is the entire wrench space ( or 6 R ). If the contacts are frictionless, force closure is equivalent to first-order form closure. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

521 Chapter 12. Grasping and Manipulation 503 Notes and References 12.5 The kinematics of contact draw heavily from concepts in linear algebra (see, for 127, 2, example, the texts [179, 18, 118]) and, more specifically, screw theory [6, 113]. Graphical methods for analysis of planar constraints were introduced by Reuleaux [147], and Mason introduced graphical construction of contact labels for planar kinematics and moment labels for representation of homogeneous 109]. Polyhedral convex cones, and their application in wrench cones [108, representing feasible twist cones and contact wrench cones, are discussed in [109, 44, 66, 55]. The formalization of the friction law used in this chapter was given by Coulomb in 1781 [31]. Surprising consequences of Coulomb friction are problems 109, of ambiguity and inconsistency [94, 112] and that infinite friction does not necessarily prevent slipping at an active contact [102]. Form closure and force closure are discussed in detail in the Handbook of Robotics [142]. In particular, that reference uses the term “frictional form closure” to mean the same thing that “force closure” means in this chapter. According to [142], force closure additionally requires that the hand doing the grasping be sufficiently capable of controlling the internal “squeezing” forces. Similar distinctions are made in [11] and the reviews [13, 12]. In this chapter we do not consider the details of the robot hand and adopt a definition of force closure based solely on the geometry and friction of the contacts. The numbers of contacts needed for planar and spatial form closure were established by Reuleaux [147] and Somoff [174], respectively. Other foundational 120, results in form and force closure are developed in [79, 105] and are reviewed in [12, 142]. An overview of grasp quality metrics is given in [142]. The result that two friction cones that can “see” each other’s base are sufficient for planar force closure was first reported in [125], and the result reviewed in this chapter on three-finger force-closure grasps in 3D appeared in [89]. Salisbury applied Gr ̈ubler’s formula to calculate the mobility of a grasped object using kinematic models of contact [111]. Second-order models of contact constraints were introduced by Rimon and Burdick [149, 148, 150, 151] and used to show that curvature effects allow form closure by fewer contacts. 124, 192], Jamming and wedging in robotic insertion were described in [172, and the notion of a dynamic grasp was first introduced in [110]. An important class of methods for simulating systems of rigid bodies in frictional contact, not covered in this chapter, are based on solving linear and nonlinear complementarity problems [178, 130, 185]. These complementarity formulations directly encode the fact that if a contact is breaking, then no force is applied; if a contact is sticking, then the force can be anywhere inside the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

522 504 12.6. Exercises friction cone; and if a contact is sliding, the force is on the edge of the friction cone. General references on contact modeling and manipulation include Handbook 142] and the texts by Mason [109] and Murray et of Robotics chapters [66, al. [122]. 12.6 Exercises Exercise 12.1 Prove that the impenetrability constraint (12.4) is equivalent to the constraint (12.7). Exercise 12.2 Representing planar twists as centers of rotation. V = = ( ω V , ,v (a) Consider the two planar twists 2 ,v , ) = (1 0) and 1 1 x 1 z 2 1 y , ω 1). Draw the corresponding CoRs in a planar ,v − 0 ,v ( , ) = (1 2 y z x 2 2 {V coordinate frame, and illustrate pos( , V } ) as CoRs. 2 1 (b) Draw the positive span of V = = ( ω V 0) and ,v , 2 ,v , ) = (1 1 y x 1 z 2 1 1 0 ω 1) as CoRs. ,v − , ,v , 1 ( − ) = ( 2 z 2 x 2 y Exercise 12.3 p = (1 , 2 , 3) with a contact normal A rigid body is contacted at into the body ˆ n = (0 , 1 , 0). Write the constraint on the body’s twist V due to this contact. Exercise 12.4 { s } is defined at a contact between a stationary A space frame constraint and a body. The contact normal, into the body, is along the ˆz-axis of the s } frame. { (a) Write down the constraint on the body’s twist V if the contact is a fric- tionless point contact. (b) Write down the constraints on V if the contact is a point contact with friction. V if the contact is a soft contact. (c) Write down the constraints on Exercise 12.5 Figure 12.29 shows five stationary “fingers” contacting an ob- ject. The object is in first-order form closure and therefore force closure. If we take away one finger, the object may still be in form closure. For which subsets of four fingers is the object still in form closure? Prove your answers using graphical methods. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

523 Chapter 12. Grasping and Manipulation 505 1 3 4 5 2 Figure 12.29: A triangle in contact with five stationary fingers, yielding first-order form closure and therefore force closure. Analyze the contact when one or more fingers ◦ are removed. The hypotenuse of the triangle is 45 from the vertical on the page, and ◦ contact normal 5 is 22 . 5 from the vertical. Exercise 12.6 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by finger 1. Label the feasible CoRs with their contact labels. Exercise 12.7 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1 and 2. Label the feasible CoRs with their contact labels. Exercise 12.8 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 2 and 3. Label the feasible CoRs with their contact labels. Exercise 12.9 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1 and 5. Label the feasible CoRs with their contact labels. Exercise 12.10 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1, 2, and 3. Exercise 12.11 Draw the set of feasible twists as CoRs when the triangle of May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

524 506 12.6. Exercises A 4 4 planar square restrained by five frictionless point contacts. Figure 12.30: × Figure 12.29 is contacted only by fingers 1, 2, and 4. Exercise 12.12 Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1, 3, and 5. 12.29. Exercise 12.13 Refer again to the triangle of Figure ◦ = 22 . 5 (a) Draw the wrench cone from contact 5, assuming a friction angle α (a friction coefficient μ = 0 . 41), using moment labeling. (b) Add contact 2 to the moment-labeling drawing. The friction coefficient at contact 2 is μ = 1. Exercise 12.14 Refer again to the triangle of Figure 12.29. Draw the moment- μ = 1 and contact 4 with μ labeling region corresponding to contact 1 with = 0. Exercise 12.15 The planar grasp of Figure 12.30 consists of five frictionless point contacts. The square’s size is 4 × 4. (a) Show that this grasp does not yield force closure. (b) The grasp of part (a) can be modified to yield force closure by adding one frictionless point contact. Draw all the possible locations for this contact. Exercise 12.16 Assume the contacts shown in Figure 12.31 are frictionless point contacts. Determine whether the grasp yields force closure. If it does not, May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

525 Chapter 12. Grasping and Manipulation 507 ◦ 45 ◦ 45 Figure 12.31: A planar disk restrained by three frictionless point contacts. L L 2 1 L x L Figure 12.32: An L-shaped planar object restrained by two point contacts with friction. how many additional frictionless point contacts are needed to construct a force closure grasp? Exercise 12.17 Consider the L-shaped planar object of Figure 12.32. (a) Suppose that both contacts are point contacts with friction coefficient = 1. Determine whether this grasp yields force closure. μ (b) Now suppose that point contact 1 has friction coefficient μ = 1, while point contact 2 is frictionless. Determine whether this grasp yields force closure. (c) The vertical position of contact 1 is allowed to vary; denote its height by x . Find all positions x such that the grasp is force closure with μ = 1 for May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

526 508 12.6. Exercises μ = 0 for contact 2. contact 1 and 0.5 c f 2 f 3 1 h ˆy α ˆx 0.5 f 1 Figure 12.33: A square restrained by three point contacts. Exercise 12.18 A square is restrained by three point contacts as shown in Figure 12.33: f f is a point contact with friction coefficient μ , while f and 2 1 3 1 1 = c are frictionless point contacts. If h = , find the range of values of and μ 4 2 such that grasp yields force closure. B A A B 1 1 β 1 C 1 C (a) Grasp for Exercise 12.19(a). (b) Grasp for Exercise 12.19(b). Figure 12.34: Planar grasps. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

527 Chapter 12. Grasping and Manipulation 509 g m Figure 12.35: A zero-thickness rod supported by a single contact. Exercise 12.19 C (a) For the planar grasp of Figure 12.34(a), assume contact is frictionless, and while the friction coefficient at contacts is μ = 1. Determine A B whether this grasp is force closure. 12.34(b), assume contacts (b) For the planar grasp of Figure and B are A C frictionless, while contact β . Find the has a friction cone of half-angle range of values of β that makes this grasp force closure. Exercise 12.20 Find a formula for the minimum friction coefficient, as a function of n n -sided , needed for a two-fingered planar force-closure grasp of an n is odd. Assume that the fingers can make contact only regular polygon, where with the edges, not the vertices. If the fingers could also contact the vertices, how does your answer change? You can assume that the fingers are circular. Exercise 12.21 Consider a table at rest, supported by four legs in frictional contact with the floor. The normal forces provided by each leg are not unique; there is an infinite set of solutions to the normal forces yielding a force balance with gravity. What is the dimension of the space of normal-force solutions? (Since there are four legs, the space of normal forces is four dimensional, and the space of solutions must be a subspace of this four-dimensional space.) What is the dimension of the space of contact-force solutions if we include tangential frictional forces? Exercise 12.22 A thin rod in gravity is supported from below by a single stationary contact with friction, shown in Figure 12.35. One more frictionless contact can be placed anywhere else on the top or the bottom of the rod. Indi- cate all the places where this contact can be put so that the gravitational force is balanced. Use moment labeling to justify your answer. Prove the same us- ing algebraic force balance, and comment on how the magnitude of the normal May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

528 510 12.6. Exercises A frictionless finger pushes a box to the right. Gravity acts downward. Figure 12.36: Does the box slide flat against the table, does it tip over the lower right corner, or does it slide and tip over that corner? forces depends on the location of the second contact. Exercise 12.23 A frictionless finger begins pushing a box over a table (Fig- ure 12.36). There is friction between the box and the table, as indicated in the figure. There are three possible contact modes between the box and the table: either the box slides to the right flat against the table, or it tips over at the right lower corner, or it tips over that corner while the corner also slides to the right. Which actually occurs? Assume a quasistatic force balance and answer the following questions. (a) For each of the three contact modes, draw the moment-labeling regions corresponding to the table’s friction cone edges active in that contact mode. (b) For each moment-labeling drawing, determine whether the pushing force plus the gravitational force can be quasistatically balanced by the support forces. From this, determine which contact mode actually occurs. (c) Graphically show a different support-friction cone for which the contact mode is different from your solution above. In Figure 12.37 body 1, of mass m with center of mass at Exercise 12.24 1 ). Both ,y ( ), leans on body 2, of mass m with center of mass at ( x ,y x 1 2 2 2 1 are supported by a horizontal plane, and gravity acts downward. The friction x , 0), at ( x 0)) is ,y ), at ( x , , 0), and at ( coefficient at all four contacts (at (0 R L L μ > 0. We want to know whether it is possible for the assembly to stay standing by some choice of contact forces within the friction cones. Write down the six equations of force balance for the two bodies in terms of the gravitational forces and the contact forces, and express the conditions that must be satisfied for this May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

529 Chapter 12. Grasping and Manipulation 511 x ( ) ,y L body 2 body 1 ,y ) x ( 1 1 ( x ) ,y 2 2 g ˆy ˆx x ( , 0) x ( 0) , R L Figure 12.37: One body leaning on another (Exercise 12.24). assembly to stay standing. How many equations and unknowns are there? Exercise 12.25 Write a program that accepts a set of contacts acting on a planar body and determines whether the body is in first-order form closure. Exercise 12.26 Write a program that accepts a set of contacts acting on a spatial body and determines whether the body is in first-order form closure. Exercise 12.27 Write a program that accepts a friction coefficient and a set of contacts acting on a planar body and determines whether the body is in force closure. Exercise 12.28 Write a program that accepts a friction coefficient and a set of contacts acting on a spatial body and determines whether the body is in force closure. Use a polyhedral approximation to the friction cone at each contact point that underestimates the friction cone and that has four facets. Exercise 12.29 Write a program to simulate the quasistatic meter-stick trick of Example 12.10. The program takes as input: the initial -position of the x left finger, the right finger, and the stick’s center of mass; the constant speed ̇ x of the right finger (toward the left finger); and the static and kinetic friction coefficients, where μ . The program should continue the simulation until ≥ μ k s May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

530 512 12.6. Exercises the two fingers touch or until the stick falls. It should plot the position of the left finger (which is constant), the right finger, and the center of mass as a function of time. Include an example where = μ μ , an example where μ is s k s is much larger than , and an example where μ μ only slightly larger than μ . k s k Exercise 12.30 Write a program that determines whether a given assembly g acts in the of planar bodies can remain standing in gravity. Gravity ˆy- − direction. The assembly is described by bodies, n contacts, and the friction m coefficient μ bodies is described by its , all entered by the user. Each of the m m and the ( x mass ,y ) location of its center of mass. Each contact is described i i i by the index i of each of the two bodies involved in the contact and the unit normal direction (defined as into the first body). If the contact has only one body involved, the second body is assumed to be stationary (e.g., ground). The program should look for a set of coefficients k 0 multiplying the friction-cone ≥ j edges at the contacts (if there are contacts then there are 2 n friction-cone edges n m and coefficients) such that each of the bodies is in force balance, considering gravity. Except in degenerate cases, if there are more force-balance equations m ) than unknowns (2 n ) then there is no solution. In the usual case, where (3 n > 3 m , there is a family of solutions, meaning that the force at each contact 2 cannot be known with certainty. One approach is to have your program generate an appropriate linear pro- gram and use the programming language’s built-in linear-programming solver. Exercise 12.31 This is a generalization of the previous exercise. Now, instead of simply deciding whether the assembly stays standing for a stationary base, the base moves according to a trajectory specified by the user, and the program determines whether the assembly can stay together during the trajectory (i.e., whether sticking contact at all contacts allows each body to follow the specified trajectory). The three-dimensional trajectory of the base can be specified as a polynomial in ( x ( t ) ,y ( t ) ,θ ( t )), for a base reference frame defined at a particu- lar position. For this problem, you also need to specify the scalar moment of inertia about the center of mass for each body in the assembly. You may find it convenient to express the motion and forces (gravitational, contact, inertial) in the frame of each body and solve for the dynamics in the body frames. Your program should check for stability (all contact normal forces are nonnegative while satisfying the dynamics) at finely spaced discrete points along the trajec- tory. It should return a binary result: the assembly can be maintained at all points along the trajectory, or not. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

531 Chapter 13 Wheeled Mobile Robots A kinematic model of a mobile robot governs how wheel speeds map to robot velocities, while a dynamic model governs how wheel torques map to robot accel- erations. In this chapter, we ignore the dynamics and focus on the kinematics. We also assume that the robots roll on hard, flat, horizontal ground without skidding (i.e., tanks and skid-steered vehicles are excluded). The mobile robot is assumed to have a single rigid-body chassis (not articulated like a tractor- } ∈ SE (2) representing a chassis-fixed frame { b trailer) with a configuration T sb relative to a fixed space frame } in the horizontal plane. We represent T { s sb q = ( φ,x,y ). We also usually represent the velocity of by the three coordinates ̇ = ( φ, ̇ x, ̇ y ). Occasionally the chassis as the time derivative of the coordinates, ̇ q V ,v = ( ω ) ,v it will be convenient to refer to the chassis’ planar twist bx bz by b expressed in { b } , where       ̇ ω 1 0 0 φ bz       φ sin v φ 0 cos V = , (13.1) = x ̇ bx b φ cos φ sin − 0 v ̇ y by       ̇ 0 0 1 ω φ bz       0 cos φ − φ sin v q = ̇ = (13.2) . ̇ x bx φ φ 0 sin cos v ̇ y by This chapter covers kinematic modeling, motion planning, and feedback con- trol for wheeled mobile robots, and concludes with a brief introduction to mobile manipulation, which is the problem of controlling the end-effector motion of a robot arm mounted on a mobile platform. 513

532 514 13.1. Types of Wheeled Mobile Robots (Left) A typical wheel that rolls without sideways slip – here a unicycle Figure 13.1: wheel. (Middle) An omniwheel. (Right) A mecanum wheel. Omniwheel and mecanum wheel images from VEX Robotics, Inc., used with permission. 13.1 Types of Wheeled Mobile Robots Wheeled mobile robots may be classified in two major categories, omnidirec- and nonholonomic . Omnidirectional mobile robots have no equality tional ̇ q = ( φ, ̇ x, ̇ y ), while nonholonomic robots are constraints on the chassis velocity ̇ subject to a single Pfaffian velocity constraint A ( q ) ̇ q = 0 (see Section 2.4 for an introduction to Pfaffian constraints). For a car-like robot, this constraint pre- vents the car from moving directly sideways. Despite this velocity constraint, φ,x,y the car can reach any ( ) configuration in an obstacle-free plane. In other words, the velocity constraint cannot be integrated to an equivalent configura- tion constraint, and therefore it is a nonholonomic constraint. Whether a wheeled mobile robot is omnidirectional or nonholonomic depends 13.1). Nonholonomic mobile in part on the type of wheels it employs (Figure robots employ conventional wheels, such as you might find on your car: the wheel rotates about an axle perpendicular to the plane of the wheel at the wheel’s center, and optionally it can be steered by spinning the wheel about an axis perpendicular to the ground at the contact point. The wheel rolls without sideways slip, which is the source of the nonholonomic constraint on the robot’s chassis. Omnidirectional wheeled mobile robots typically employ either omniwheels 1 or . An omniwheel is a typical wheel augmented with rollers mecanum wheels on its outer circumference. These rollers spin freely about axes in the plane of 1 These types of wheels are often called “Swedish wheels,” as they were invented by Bengt Ilon working at the Swedish company Mecanum AB. The usage of, and the differentiation between, the terms “omniwheel,” “mecanum wheel,” and “Swedish wheel” is not completely standard, but here we use one popular choice. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

533 Chapter 13. Wheeled Mobile Robots 515 the wheel and tangential to the wheel’s outer circumference, and they allow sideways sliding while the wheel drives forward or backward without slip in that direction. Mecanum wheels are similar, except that the spin axes of the circumferential rollers are not in the plane of the wheel (see Figure 13.1). The sideways sliding allowed by omniwheels and mecanum wheels ensures that there are no velocity constraints on the robot’s chassis. Omniwheels and mecanum wheels are not steered, only driven forward or backward. Because of their small diameter rollers, omniwheels and mecanum wheels work best on hard, flat ground. The issues in the modeling, motion planning, and control of wheeled mobile robots depend intimately on whether the robot is omnidirectional or nonholo- nomic, so we treat these two cases separately in the following sections. Omnidirectional Wheeled Mobile Robots 13.2 13.2.1 Modeling An omnidirectional mobile robot must have at least three wheels to achieve an ̇ = ( φ, arbitrary three-dimensional chassis velocity ̇ q x, ̇ y ), since each wheel has ̇ only one motor (controlling its forward–backward velocity). Figure shows 13.2 two omnidirectional mobile robots, one with three omniwheels and one with four mecanum wheels. Also shown are the wheel motions obtained by driving the wheel motors as well as the free sliding motions allowed by the rollers. Two important questions in kinematic modeling are the following. q , at what speeds must the wheels be (a) Given a desired chassis velocity ̇ driven? (b) Given limits on the individual wheel driving speeds, what are the limits on the chassis velocity ̇ q ? To answer these questions, we need to understand the wheel kinematics illustrated in Figure 13.3. In a frame ˆx at the center of the wheel, the –ˆy w w = ( v ,v ), which satisfies v linear velocity of the center of the wheel is written x y [ ] ] [ ] [ v − 1 sin γ x v + (13.3) , v = slide drive v cos γ 0 y where γ denotes the angle at which free “sliding” occurs (allowed by the passive rollers on the circumference of the wheel), v is is the driving speed, and v slide drive May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

534 516 13.2. Omnidirectional Wheeled Mobile Robots free “sliding” driving driving free “sliding” (Left) A mobile robot with three omniwheels. Also shown for one Figure 13.2: omniwheel is the direction in which the wheel can freely slide due to the rollers, as well as the direction in which the wheel rolls without slipping when driven by the wheel motor. (The upper image is from www.superdroidrobots.com, used with permission.) (Right) The KUKA youBot mobile manipulator system, which uses four mecanum wheels for its mobile base. (The upper image is from KUKA Roboter GmbH, used with permission.) the sliding speed. For an omniwheel γ = 0 and, for a mecanum wheel, typically ◦ ± 45 γ . Solving Equation (13.3), we get = v + v v tan γ, = drive y x = v / cos γ. v slide y Letting r be the radius of the wheel and u be the driving angular speed of the wheel, v 1 drive = u = (13.4) . ( v ) + v γ tan y x r r ̇ q = ( ̇ φ, ̇ x, To derive the full transformation from the chassis velocity ̇ y ) to the driving angular speed u for wheel i , refer to the notation illustrated in i May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

535 Chapter 13. Wheeled Mobile Robots 517 driven component = + v ta n γ v x y free “sliding” direction ˆy v ,v w =( ) v x y free component = γ v cos / γ y xˆ w driving direction Figure 13.3: (Left) The driving direction and the direction in which the rollers = 0 and, for a mecanum wheel, allow the wheel to slide freely. For an omniwheel γ ◦ = 45 . (Right) The driven and free sliding speeds for the wheel velocity γ typically ± v v ,v -axis is aligned with ) expressed in the wheel frame ˆx = ( –ˆy , where the ˆx w y x w w the forward driving direction. { b Figure 13.4. The chassis frame is at q = ( φ,x,y ) in the fixed space frame } s } . The center of the wheel and its driving direction are given by ( β ,y ,x ) { i i i b , the wheel’s radius is r , and the wheel’s sliding direction is { expressed in } i is related to ̇ . Then u by given by γ q i i = h u φ ) ̇ q = ( i i     ̇ [ ][ ][ ] 0 0 1 φ 1 tan γ sin β cos β − y 1 0 i i i i     sin φ 0 cos φ . x ̇ − sin β cos β 0 1 x r r i i i i i cos φ − 0 sin φ y ̇ (13.5) Reading from right to left: the first transformation expresses ̇ q as V ; the second b { b } ; the third trans- transformation produces the linear velocity at the wheel in formation expresses this linear velocity in the wheel frame ˆx ; and the final –ˆy w w transformation calculates the driving angular velocity using Equation (13.4). Evaluating Equation (13.5) for ( φ ), we get h i   T γ + β cos( y − ) γ ) + x β sin( i i i i i i 1   ) φ γ + + β cos( (13.6) . φ ) = ( h i i i γ cos r i i φ ) + γ sin( β + i i × m 3 3 wheels, the matrix H ( φ ) ∈ R For an omnidirectional robot with m ≥ 3 mapping a desired chassis velocity ̇ q ∈ R to the vector of wheel driving speeds May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

536 518 13.2. Omnidirectional Wheeled Mobile Robots driving direction β i wheel i x ) ( ,y γ i i i free “sliding” direction ˆy b ˆx b ˆy b }{ φ ) y x, ( ˆx } s { The fixed space frame { s } , a chassis frame { b } at ( φ,x,y ) in { s } , Figure 13.4: b i and wheel ,y . The sliding ) with driving direction β } , both expressed in { x at ( i i i direction of wheel i is defined by γ . i m ( R m rows h ∈ is constructed by stacking the φ ): u i   ) ( h φ   1 ̇ φ   φ ( h ) 2     . (13.7) = = H u ) ̇ φ q ( x ̇   . .   . ̇ y ( ) h φ m u and the body twist V . This We can also express the relationship between b φ : mapping does not depend on the chassis orientation   (0) h   1 ω   bz h (0) 2     v u (0) = = V H (13.8) .   . bx b .   . v by h (0) m The wheel positions and headings ( β , and their free slid- ,x } ,y b ) in { i i i ing directions γ (0) is rank 3. For example, if we , must be chosen so that H i constructed a mobile robot of omniwheels whose driving directions and sliding directions were all aligned, the rank of H (0) would be 2, and there would be no way to controllably generate translational motion in the sliding direction. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

537 Chapter 13. Wheeled Mobile Robots 519 wheel 1 γ =0 ,β =0 1 1 wheel 1 wheel 4 γ = 4 π/ − γ = 4 π/ 1 4 ˆy w b ˆy b ˆx b ˆx b d γ = π/ 4 γ − = 4 π/ 2 3 wheel 3 wheel 2 wheel 3 wheel 2 = ,β 2 =0 − 3 π/ γ =2 ,β π/ 3 =0 γ 2 2 3 3 Kinematic models for mobile robots with three omniwheels and four Figure 13.5: r and the driving direction for each of mecanum wheels. The radius of all wheels is the mecanum wheels is β = 0. i m > 3, as for the four-wheeled youBot of Figure 13.2, choosing In the case 3 u R such that Equation (13.8) is not satisfied for any implies that the V ∈ b wheels must skid in their driving directions. Using the notation in Figure 13.5, the kinematic model of the mobile robot with three omniwheels is       ω u 0 1 − d bz 1 1       v u sin( 3) π/ − d − 1 / 2 − = = (13.9) u = H (0) V bx 2 b r v u / 3) sin( 2 π/ d − 1 − by 3 and the kinematic model of the mobile robot with four mecanum wheels is       − 1 w u 1 ` − − 1 ω bz     1 ` + 1 1 u w 2       v u = (13.10) . (0) = = H V bx b     ` − u 1 + 1 w r 3 v by 1 1 − ` − u w 4 , all wheels drive forward For the mecanum robot, to move in the direction +ˆx b at the same speed; to move in the direction +ˆy , wheels 1 and 3 drive backward b and wheels 2 and 4 drive forward at the same speed; and to rotate in the counterclockwise direction, wheels 1 and 4 drive backward and wheels 2 and 3 drive forward at the same speed. Note that the robot chassis is capable of the same speeds in the forward and sideways directions. May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

538 520 13.3. Nonholonomic Wheeled Mobile Robots i If the driving angular velocity of wheel u is subject to the bound |≤ u | , max i i, i.e., , − ≤ u u = h u (0) V ≤ b i, max i max i, i u and then two parallel constraint planes defined by = h = (0) V u − b i, max i max i, (0) are generated in the three-dimensional space of body twists. Any V h V i b b between these two planes does not violate the maximum driving speed of wheel , while any V . The normal direction to outside this slice is too fast for wheel i i b T (0), and the points on the planes closest to the origin h the constraint planes is i 2 T 2 T (0) u ‖ (0) / ‖ h − (0) ‖ . and u are ‖ h h / (0) h i, i i max i, max i i m V is bounded If the robot has wheels then the region of feasible body twists m pairs of parallel constraint planes. The region V is therefore a convex by the m three-dimensional polyhedron. The polyhedron has 2 faces and the origin (corresponding to zero twist) is in the center. Visualizations of the six-sided V for the three-wheeled and four-wheeled models in and eight-sided regions Figure are shown in Figure 13.6. 13.5 13.2.2 Motion Planning Since omnidirectional mobile robots are free to move in any direction, any of the trajectory planning methods for kinematic systems in Chapter 9, and most of the motion planning methods of Chapter 10, can be adapted. Feedback Control 13.2.3 Given a desired trajectory ( t q ), we can adopt the feedforward plus PI feedback d controller (11.15) to track the trajectory: ∫ t t ) = ̇ q ( ( t ) + K ̇ ( q K q ( ) − q ( t )) + t (t) (13.11) , t ( q d (t)) − q i d p d d 0 3 × 3 3 × 3 where I have positive values along k = and K R = k K I ∈ R ∈ p i p i q t ) is an estimate of the actual configuration derived from the diagonal and ( q ( sensors. Then ̇ ) can be converted to the commanded wheel driving velocities t u ( t ) using Equation (13.7). 13.3 Nonholonomic Wheeled Mobile Robots In Section k Pfaffian velocity constraints acting on a system with con- 2.4, the n k × n A were written as A ( q ) ̇ q = 0, where figuration ( q ) ∈ R q ∈ R . Instead of specifying the k directions in which velocities are not allowed, we can write the May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

539 Chapter 13. Wheeled Mobile Robots 521 ω bz ω bz v bx v bx v by v by v v by by v v bx bx (Top row) Regions of feasible body twists V for the three-wheeled (left) Figure 13.6: and four-wheeled (right) robots of Figure 13.5. Also shown for the three-wheeled robot is the intersection with the ω = 0 = 0 plane. (Bottom row) The bounds in the ω bz bz plane (translational motions only). allowable velocities of a kinematic system as a linear combination of n − k veloc- ity directions. This representation is equivalent, and it has the advantage that the coefficients of the linear combinations are precisely the controls available to us. We will see this representation in the kinematic models below. The title of this section implies that the velocity constraints are not inte- grable to equivalent configuration constraints. We will establish this formally in Section 13.3.2. Modeling 13.3.1 13.3.1.1 The Unicycle The simplest wheeled mobile robot is a single upright rolling wheel, or unicycle. Let r be the radius of the wheel. We write the configuration of the wheel as May 2017 preprint of Modern Robotics , Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org

540 522 13.3. Nonholonomic Wheeled Mobile Robots θ ˆz ˆy φ ( ) y x, ˆx