Table Of Content

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! Contents Foreword by Roger Brockett ix Foreword by Matthew Mason xi Preface xiii 1 Preview 1 2 Configuration Space 11 2.1 Degrees of Freedom of a Rigid Body . . . . . . . . . . . . . . . . 12 2.2 Degrees of Freedom of a Robot . . . . . . . . . . . . . . . . . . . 15 2.2.1 Robot Joints . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.2.2 Gru¨bler’s Formula . . . . . . . . . . . . . . . . . . . . . . 17 2.3 Configuration Space: Topology and Representation . . . . . . . . 23 2.3.1 Configuration Space Topology . . . . . . . . . . . . . . . . 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 Rotation Matrices . . . . . . . . . . . . . . . . . . . . . . 68 3.2.2 Angular Velocities . . . . . . . . . . . . . . . . . . . . . . 76 3.2.3 Exponential Coordinate Representation of Rotation . . . 79 3.3 Rigid-Body Motions and Twists . . . . . . . . . . . . . . . . . . . 89 i ii Contents 3.3.1 Homogeneous Transformation Matrices . . . . . . . . . . 89 3.3.2 Twists . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 3.3.3 ExponentialCoordinateRepresentationofRigid-BodyMo- tions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 3.4 Wrenches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 3.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 3.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 115 3.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 4 Forward Kinematics 137 4.1 Product of Exponentials Formula . . . . . . . . . . . . . . . . . . 140 4.1.1 First Formulation: Screw Axes in the Base Frame . . . . 141 4.1.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 143 4.1.3 Second Formulation: Screw Axes in the End-E↵ector Frame148 4.2 The Universal Robot Description Format . . . . . . . . . . . . . 152 4.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 4.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159 4.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 160 4.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160 5 Velocity Kinematics and Statics 171 5.1 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 178 5.1.1 Space Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 178 5.1.2 Body Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 183 5.1.3 Visualizing the Space and Body Jacobian . . . . . . . . . 185 5.1.4 Relationship between the Space and Body Jacobian . . . 187 5.1.5 Alternative Notions of the Jacobian . . . . . . . . . . . . 187 5.1.6 Looking Ahead to Inverse Velocity Kinematics . . . . . . 189 5.2 Statics of Open Chains . . . . . . . . . . . . . . . . . . . . . . . . 190 5.3 Singularity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . 191 5.4 Manipulability . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196 5.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 5.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202 6 Inverse Kinematics 219 6.1 Analytic Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . 221 6.1.1 6R PUMA-Type Arm . . . . . . . . . . . . . . . . . . . . 221 6.1.2 Stanford-Type Arms . . . . . . . . . . . . . . . . . . . . . 225 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org Contents iii 6.2 Numerical Inverse Kinematics . . . . . . . . . . . . . . . . . . . . 226 6.2.1 Newton–Raphson Method . . . . . . . . . . . . . . . . . . 227 6.2.2 Numerical Inverse Kinematics Algorithm . . . . . . . . . 227 6.3 Inverse Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . 232 6.4 A Note on Closed Loops . . . . . . . . . . . . . . . . . . . . . . . 234 6.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235 6.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235 6.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 236 6.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236 7 Kinematics of Closed Chains 245 7.1 Inverse and Forward Kinematics . . . . . . . . . . . . . . . . . . 247 7.1.1 3 RPR Planar Parallel Mechanism . . . . . . . . . . . . . 247 ⇥ 7.1.2 Stewart–Gough Platform . . . . . . . . . . . . . . . . . . 249 7.1.3 General Parallel Mechanisms . . . . . . . . . . . . . . . . 251 7.2 Di↵erential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 252 7.2.1 Stewart–Gough Platform . . . . . . . . . . . . . . . . . . 252 7.2.2 General Parallel Mechanisms . . . . . . . . . . . . . . . . 254 7.3 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256 7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 261 7.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 262 7.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263 8 Dynamics of Open Chains 271 8.1 Lagrangian Formulation . . . . . . . . . . . . . . . . . . . . . . . 272 8.1.1 Basic Concepts and Motivating Examples . . . . . . . . . 272 8.1.2 General Formulation . . . . . . . . . . . . . . . . . . . . . 277 8.1.3 Understanding the Mass Matrix . . . . . . . . . . . . . . 279 8.1.4 Lagrangian Dynamics vs. Newton–Euler Dynamics . . . . 281 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 8.3.1 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org iv Contents 8.8 Robot Dynamics in the URDF . . . . . . . . . . . . . . . . . . . 303 8.9 Actuation, Gearing, and Friction . . . . . . . . . . . . . . . . . . 303 8.9.1 DC Motors and Gearing . . . . . . . . . . . . . . . . . . . 305 8.9.2 Apparent Inertia . . . . . . . . . . . . . . . . . . . . . . . 310 8.9.3 Newton–Euler Inverse Dynamics Algorithm Accounting for Motor Inertias and Gearing . . . . . . . . . . . . . . . 312 8.9.4 Friction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314 8.9.5 Joint and Link Flexibility . . . . . . . . . . . . . . . . . . 314 8.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315 8.11 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 319 8.12 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 320 8.13 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 321 9 Trajectory Generation 325 9.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325 9.2 Point-to-Point Trajectories . . . . . . . . . . . . . . . . . . . . . 326 9.2.1 Straight-Line Paths . . . . . . . . . . . . . . . . . . . . . 326 9.2.2 Time Scaling a Straight-Line Path . . . . . . . . . . . . . 328 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 9.4.3 A Variation on the Time-Scaling Algorithm . . . . . . . . 342 9.4.4 Assumptions and Caveats . . . . . . . . . . . . . . . . . . 344 9.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345 9.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 346 9.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 347 9.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348 10 Motion Planning 353 10.1 Overview of Motion Planning . . . . . . . . . . . . . . . . . . . . 353 10.1.1 Types of Motion Planning Problems . . . . . . . . . . . . 354 10.1.2 Properties of Motion Planners . . . . . . . . . . . . . . . 355 10.1.3 Motion Planning Methods . . . . . . . . . . . . . . . . . . 356 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 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org Contents v 10.4 Grid Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369 10.4.1 Multi-Resolution Grid Representation . . . . . . . . . . . 372 10.4.2 Grid Methods with Motion Constraints . . . . . . . . . . 373 10.5 Sampling Methods . . . . . . . . . . . . . . . . . . . . . . . . . . 378 10.5.1 The RRT Algorithm . . . . . . . . . . . . . . . . . . . . . 379 10.5.2 The PRM Algorithm . . . . . . . . . . . . . . . . . . . . . 384 10.6 Virtual Potential Fields . . . . . . . . . . . . . . . . . . . . . . . 386 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 10.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394 10.10Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 397 10.11Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398 11 Robot Control 403 11.1 Control System Overview . . . . . . . . . . . . . . . . . . . . . . 404 11.2 Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405 11.2.1 Error Response . . . . . . . . . . . . . . . . . . . . . . . . 406 11.2.2 Linear Error Dynamics . . . . . . . . . . . . . . . . . . . 406 11.3 Motion Control with Velocity Inputs . . . . . . . . . . . . . . . . 413 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 11.4.1 Motion Control of a Single Joint . . . . . . . . . . . . . . 421 11.4.2 Motion Control of a Multi-joint Robot . . . . . . . . . . . 429 11.4.3 Task-Space Motion Control . . . . . . . . . . . . . . . . . 433 11.5 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434 11.6 Hybrid Motion–Force Control . . . . . . . . . . . . . . . . . . . . 437 11.6.1 Natural and Artificial Constraints . . . . . . . . . . . . . 437 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 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org vi Contents 11.10Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 449 11.11Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451 11.12Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 452 11.13Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 453 12 Grasping and Manipulation 461 12.1 Contact Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 463 12.1.1 First-Order Analysis of a Single Contact . . . . . . . . . . 463 12.1.2 Contact Types: Rolling, Sliding, and Breaking Free . . . . 465 12.1.3 Multiple Contacts . . . . . . . . . . . . . . . . . . . . . . 468 12.1.4 Collections of Bodies . . . . . . . . . . . . . . . . . . . . . 472 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 12.2.1 Friction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 484 12.2.2 Planar Graphical Methods . . . . . . . . . . . . . . . . . . 487 12.2.3 Force Closure . . . . . . . . . . . . . . . . . . . . . . . . . 489 12.2.4 Duality of Force and Motion Freedoms . . . . . . . . . . . 494 12.3 Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494 12.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 501 12.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 503 12.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 504 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 13.3.3 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . 537 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 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org Contents vii A Summary of Useful Formulas 565 B Other Representations of Rotations 575 B.1 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 575 B.1.1 Algorithm for Computing the ZYX Euler Angles . . . . . 577 B.1.2 Other Euler Angle Representations . . . . . . . . . . . . . 577 B.2 Roll–Pitch–Yaw Angles . . . . . . . . . . . . . . . . . . . . . . . 580 B.3 Unit Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . 581 B.4 Cayley–Rodrigues Parameters . . . . . . . . . . . . . . . . . . . . 582 C Denavit–Hartenberg Parameters 585 C.1 Assigning Link Frames . . . . . . . . . . . . . . . . . . . . . . . . 585 C.2 Why Four Parameters are Su�cient . . . . . . . . . . . . . . . . 589 C.3 Manipulator Forward Kinematics . . . . . . . . . . . . . . . . . . 590 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 May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org viii Contents May2017preprintofModernRobotics,LynchandPark,CambridgeU.Press,2017. http://modernrobotics.org