cover next page > Start of Citation[PU]MIT Press{/PU][DP]1990[/DP]End of Citation Foundations of Robotics: Analysis and Control Yoshikawa, Tsuneo. ‘tith 0262240289, 9780262240284 9780585322827 cover next page >page ii < previous page next page > Foundations of Robotics Analysis and Control ‘Tsuneo Yoshikawa ‘The MIT Press Cambridge, Massachusetts London, England Start of Citation[PU]MIT Press{/PUJ[DP]1990[/DP]End of Citation < previous page next page >see iv < previous page page_iv next page > Page iv © 1990 Massachusetts Institute of Technology. Based on Foundations of Robot Control, published in Fapan by Corona Publishing Co. Ltd, (1988). All rights reserved. No part of this book may be reproduced in any form by any electronic or mschanical means including photocopying, recording, or information storage and retrieval) without permission in writing from the publisher. This book was set in Times Roman by Asco Trade Typesetting Ltd. in Hong Kong and was prirted and bound in the United States of America, Library of Congress Cataloging-in-Publication Data Yoshikawa, Tsuneo, 1941 [Robotto seigyo kisoron. English] Foundations of robotics: analysis and control/Tsuneo Yoshikawa p.cm, Revised translation of: Robotto seigyo kisoron Includes bibliographical references, ISBN 0-262-24028-9 1. Robotics. I. Title. TI211.Y6713 1990 629,8'92de20 89-29363 cP ‘Start of Citation[PU]MIT Press[/PU][DP]1990[/DP]End of Citation < previous page page_iv next page >pege_y < previous page Contents Preface Overview of Robotic Mechanisms and Controller 2 1d Mechanisms 12 Controller Exercises References Kinematics 21 Position and Orientation of Objects 244 Object Coordinate Frame 21.2 Rotation Matrix. 213 Euler Angles 214 Roll, Pitch, and Yaw Angles 22 Page_v 12 1B 1B 13 4 next page > Page vCoordinate Transformation 22.1 Homogeneous Transform 222 Product and Inverse of Homogeneous Transform 23 Joint Variables and Position of End Effector 23.1 General Relation 232 Link Parameters 233 Link Frames 23.4 Solution to Direct Kinematics Problem 24 Inverse Kinematics Problem 25 Jacobian Matrix 251 ‘Translational and Rotational Velocity of Objects 252 Definition of the Jacobian Matrix 253 ink Velocities of a Manipulator 31 39 4s 53 37 63page_y 254 General Expression of the Jacobian Matrix J 255 Joint Velocity for Achieving Desired End-Effector Velocity 25.6 Singular Configurations 2.6 Statics and Jacobian Matrices 261 Equivalent Forces Represented in Different Frames 262 Joint Driving Force Equivalent to Force Applied to Tip of Manipulator Exercises References Start of Citation[PUJMIT Press{/PU][DP]1990[/DP]End of Citation < previous page page_v 67 n 1 74 16 80 next page >page vi < previous page page_vi next page > Page vi 3 Dynamics 81 81 31 Lagrangian and Newton-Euler Formulations 82 32 Some Basics of Kinematics 82 3.21 Newton's Equation and Euler's Equation 86 3.22 Lagrange's Equation 88 33 Derivation of Dynamics Equations Based on Lagrangian Formulation 88 33.1 ‘Two-Link Manipulator 91 33.2 n-Link Manipulator 98 333 Parallel-Drive Two-Link Manipulator 100 34 Derivation of Dynamic Equations Based on Newton-Euler Formulation 100 3.4.1 Basic Procedure of Newton-Euler Formulation 102 342 Link Accelerations of a Manipulatorpage vi 343 ink Manipulator 3.5 Use of Dynamics Equations and Computational Load 3.8.1 Real-Time Controllnverse Dynamics Problem 3.5.2 SimulationDirect Dynamics Problem 3.6 Identification of Manipulator Dynamics 3.6.1 Identification Problem of Manipulators 3.62 Identification Scheme Based on Lagrangian Formulation 3.63 Identification of Load Exercises References 4 Manipulability 41 Manipulability Ellipsoid and M: lability Measure 42 Best Configurations of Robotic Mechanisms from Manipulability Viewpoint 421 103 108 108 110 112 112, 113 122, 123 125 127 127 133 133page vi ‘Two-Link Mechanism 422 SCARA-Type Robot Manipulator 423 PUMA-Type Robot Manipulator 424 Orthogonal-, Cylindrical-, and Polar-Coordinate Manipulators 425 Four-Joint Robotic Finger 43 Various Indices of Manipulability Start of Citation[PUJMIT Press{/PU][DP]1990[/DP]End of Citation < previous page page_vi 135 137 139 139 142, next page >page sii < previous page Ppage_vii next page > Page vii 143 44 Dynamic Manipulability 143 441 Dynamic-Manipulability Ellipsoid and Dynamic-Manipulability Measure 148 442 ‘Two-Link Mechanism 153 Exercises 153 References 5 Position Control 155 155 SA Generating a Desired Trajectory 155 S11 Joint-Variable Scheme 161 5.12 Scheme for Position Variables of End Effector 166 52 Linear Feedback Control 166 5.2.1 Effectiveness of Linear Feedback Control 168 5.22 Stability of Proportional and Differential Feedback Control 170page sii 53 Two-Stage Control by Linearization and Servo Compensation 170 5.3.1 Basic Concept of Two-Stage Control 175 53.2 Structure of Control System 176 533 Parallel Processing Scheme 182 54 Design and Evaluation of Servo Compensation 182 541 Linear Servosystem Theory 187 5.4.2 Stability Margin and Sensitivity 192 5.5 Decoupling Control 192 55.1 ‘Theory of Decoupling Control for Nonlinear Systems 196 5.5.2 Application to Manipulators 197 5.53 Consideration of Actuator Dynamics 199 5.6 Adaptive Control 207 Exercises 209page sii References 6 Force Control 61 Impedance Control 64d Passive-Impedance Method 612 Active-Impedance MethodOne-Degree-of-Freedom Case 613 Active-Impedance MethodGeneral Case Start of Citation[PUJMIT Press[/PU][DP]1990[/DP]End of Citation < previous page 2u1 2u1 2i1 214 218 next page >page viii < previous page page_vii next page > Page viii 222 62 Hybrid Control 222 621 Hybrid Control via Feedback Compensation 227 622 Dynamic Hybrid Control 240 Exercises 242 References 7 Control of Redundant Manipulators 244 244 TW Redundant Manipulators 245 72 Task-Decomposition Approach 245 721 Decomposing a Task into Subtasks with Priority Order 245 722 Basic Equations 247 723 Second Subtask Given by Desired Trajectory 248, 724 Second Subtask Given by Criterion Funetion 249page viii 725 Formulation as Instantaneous Optimization Problem 73 Application to Avoiding Obstacles and Singularities 73.1 Avoiding Obstacles 73.2 Avoiding Singularities 74 Computational Method for Desired Joint Velocity Exercises References Appendix 1 Function atan2 Appendix 2 Pseudo-Inverses Appendix 3 Singular-Value Decomposition Appendix 4 Lyapunov Stability Theory Solutions to Selected Exercises Index Start of Citation[PUJMIT Press{/PU][DP]1990[/DP]End of Citation < previous page page_vii 249 249 283 256 257 258 259 263 268 272 277 283 next page >page. ix < previous page page_ix next page > Preface Robots are now widely used in factories, and applications of robots in space, the oceans, nuclear industries, and other fields are being actively developed. Also, for the future, the use of robots in every facet of society, including the home, is being seriously considered. To support the development of these broad applications, robotics has evolved into a systematic approach to the engineering of robots This book has been written with the objective of presenting some fundamental concepts and mehodologies for the analysis, design, and control of robot manipulators in an easily understandable way. 1 believe that this knowledge is useful not only in robotics but also in the analysis and control of other types of mechanical systems. The book is based on class notes used at Kyoto University. It is intended as a text or reference took on robotics, mainly for graduate students in engineering. Junior and senior undergraduate students, however, should also be able to understand the material without much difficulty. The physical meanings of the concepts and equations used in the book are explained as fully as possible, and the required background in kinetics, linear algebra, and control theory is presented in an intuitively clear way to spare the reader from having to refer to texts ir those other fields. The book is organized as follows. After an overview in chapter 1, chapters 2 through 4 cover the analysis of robot ‘manipulator mechanisms. Based on this analysis, chapters 5 through 7 discuss the control of rotot manipulators. Chapter 1 introduces several typical robot manipulator mechanisms and their controllers, in order to acquaint the reader with the kind of hardware that will be dealt with. Chapter 2 covers the kinematics of robot manipulators, studying geometrically the motion of manipulator links and objects related to the manipulation iask in terms of position, velocity, and acceleration. Chapter 3 deals with the dynamics of robot manipulators, looking at how the manipulator’s motion is affected by its mass distribution and applied forces. This chapter includes the derivation of the dynamic equations of motion, their use for control and simulation, and the identification of nertial parameters. Chapter 4 develops the concept of manipulability to analyze and evaluate quantitatively the maripulation ability of. manipulators. This concept is examined first from the viewpoint of kinematics and then from th: viewpoint of dynamics. Chapter 5 covers various position-control algorithms that make the end effector of a manipulater follow a desired position trajectory. Chapter 6 Start of Citation[PU]MIT Press{/PU][DP]1990[/DP]End of Citation < previous page page_ix next page >Poge.s < previous page page_x next page > Page x discusses two typical force-control methods. These make the contact force between the end effector and its environment follow a desired force trajectory. Chapter 7 discusses, for manipulators with redundant degrees of freedom, a way to develop control algorithms for active utilization of the redundancy. The appendixes give ‘compact reviews of the function atan2, pseudo-i theory. When writing a textbook in a changing field like robotics in which many new achievements are being unveiled day by day, one usually has a difficult time deciding what to include. I have attempted to include ax many important results as possible. At the same time, I have tried to present them plainly with the help of many illustrative examples, It is my pleasure to thank several people who have contributed in various ways to the completicn of this book. The content and the form of this book are much influenced by Richard P. Paul's pioneering book Robot Manipulators (MIT Press, 1981), which I translated into Japanese in 1984. I wish to thank Yoshihiko Nakamura and Toshiharu Sugie, with whom I have worked on research projects that some parts of this book are based on. Yasuyoshi Yokokohji read the manuscript and provided many useful comments. Discussions with Junichi Imura and Kiyoshi Mackawa proved valuable in the writing of the section on adaptive control (section 5.6). Mike Lipsett, who happened to be with me at Kyoto University at a critical stage in the writing of the English verson, contributed many improvements. Takashi Hosoda, Jun Koreishi, Junichi Imura, and Osamu Suzuki helped ne with revisions and corrections, Masako Awakura performed the magic of turning all my vague scribbling into finely typed ‘manuscript. This book is mostly a translation of my Japanese book Robotto Seigyo Kisoron (Foundations of Robot Control), published in Tokyo by Corona Publishing Co. Ltd. in 1988. [ am grateful to Corona for its cooperation and support in publishing this English version. I also wish to thank Frank P. Satlow and Teresa Ehling of The MIT Press for patiently awaiting my manuscript. My wife, Sanae, supported and encouraged me at home during the writing of this book, Start of Citation[PUJMIT Press[/PU][DP]1990[/DP]End of Citation < previous page page_x next page >1 Overview of Robotic Mechanisms and Controller A robot system generally consists of three subsystems: a motion subsystem, a recognition subsystem, and a control subsystem (figure 1.1). The motion subsystem is the physical structure that carries out desired motions, corre- sponding to human arms or legs. The recognition subsystem uses various sensors to gather information about any objects being acted upon, about the robot itself, and about the environment; it recognizes the robot's state, the objects, and the environment from the gathered information. The control subsystem influences the motion subsystem to achieve a given task using the information from the recognition subsystem. This book covers mainly the theoretical fundamentals of mechanism analysis and control of robot manipulators which are necessary for design- ing their mechanisms and their controller. For the motion subsystem described above, manipulator mechanisms that function like human arms will be the subject of this book. Their kinematics, dynamics, and perfor- mance evaluation are discussed. For the control subsystem, position con- trol of an end effector, control of the force applied by the end effector on an object, and the control of redundant manipulators are discussed. The recognition subsystem plays a key role in using robots outside of factories and in making robots intelligent. However, this subsystem will not be examined closely, since in this book a recognition subsystem need only provide information about position, velocity, and force. This chapter discusses several typical mechanisms and controllers of robot manipulators in order to acquaint the reader with the hardware that will be dealt with in the following chapters. 1.1 Mechanisms Most robot manipulators can be regarded as open-loop link mechanisms consisting of several links connected together by joints. Typical joints are revolute joints and prismatic joints, which are represented by the symbols shown in figure 1.2. Joint c in this figure is sometimes called the pivot joint to distinguish it from joint b. The endpoint of the mechanism is moved by driving these joints with appropriate actuators. A manipulator can usually be divided into an arm portion, a wrist portion, and a hand portion. Several typical mechanisms of the arm and wrist portions will be shown. The hand is not addressed here, since its mechanism depends on the task to be performed.2 Chapter 1 Motion subsystem Control subsystem Figure 1.1 Robot system. ; @ © wh «e) © Figure 1.2 ‘Symbols of joints (arrows show direction of motion). (a) Prismatic joint. (b) Revolute joint 1 (¢} Revolute joint 2. (cl) Up-and-down rotation. (¢2) Back-and-forth rotation. Figure 1.3 shows several types of arm mechanisms: (a) the orthogonal- coordinate type, (b) the cylindrical-coordinate type, (c) the polar-coordinate type, (d) the vertical multi-joint type, and (e) the horizontal multi-joint type. Type a is structurally simple and rigid, and so its positioning accuracy is high. Types b-e are inferior to type a in positioning accuracy; however, they need less floor area for a base, and they have broader reach. Every mechanism in figure 1.3 has three degrees of freedom, which is the minimum number of degrees of freedom needed for placing the endpoint of the arm at an arbitrary point in three-dimensional space. Here the degree of freedom is defined as the minimal number of position variables necessary for com- pletely specifying the configuration of a mechanism. The wrist is connected to the end of the arm portion. The main role of the wrist is to change the orientation of the hand. Examples of wrist mechanisms are shown in figure 1.4. Type a is similar to the human wrist;Overview Figure 1.3, ‘Arm mechanisms. (a) Orthogonal-coordinate type. (b) Cylindrical-coordinate type. (c) Polar-coordinate type. (4) Vertical multijoint type. (e) Horizontal multijoint type. To arm yoy oy £ Ot soy or End effector @) wb) © Figure 1.4 Wrist mechanisms.4 Chapter | ‘Maximum atts rable reyion J2 Figure 1.5 Three Roll Wrist, (a) Drive mechanism. (b) Profile. type b is often used in industrial manipulators. Type c is equivalent to the Three Roll Wrist.' This can also be regarded as a variation of type b in which the axis of the middle joint is tilted a little. The Three Roll Wrist elegantly uses bevel gears as shown in figure 1.5a to make the attainable region of orientations as large as in figure 1.5b. These are all three-degrees- of-freedom mechanisms; thus, they have enough degrees of freedom to let the hand portion pose in an arbitrary orientation. However, each of these wrists has some configurations in which it can no longer change the hand orientation in a certain direction. Such a configuration is called a singular configuration. For instance, the configurations shown in figures 1.4b and 1 4c are singular ones because it is impossible to rotate the hand from side to side on the page. The wrist mechanisms shown in figures 1.6 and 1.7 have been developed with the purpose of overcoming the degeneracy problem of singular con- figurations. Figure 1.6 shows the ET (Elephant Trunk) wrist? a three- degrees-of-freedom wrist with a special mechanism that relocates the singu- lar configurations out of the main working domain. More specifically, as figure 1.6a shows, it consists of five revolute joints, J1, J2, J3, J3’, and J2, with each element of the pairs {J2,J2'} and {J3,3’} rotating through the same angles. For instance, joints J2 and J2’ rotate as in figure 1.6b. Figure 1.6 shows a hardware implementation of the mechanism using two uni- versal joints connected by gears. The only singular configurations of the ET wrist are those in which the approach direction Zy of the hand portion and the outward direction Zw of the wrist base are in completely opposite directions (for example, 0, = 90° in figure 1.6b). Figure 1.7 shows a four- degrees-of-freedom wrist, which can get away from the singular con- figurations by actively using the mechanical redundancy.Overview ‘Mechanism for rotating through 51S} the same angle J2 J3 ay se - Zi (@ ) Connected to hhand portion Figure 1.6 ET wrist. (a) Basic structure. (b) Example of motion. (c) Mechanism. n Je 33 J4 Figure 1.7 Four-degrees-of-freedom wrista Chapter 1 Figure 18 Combination of arm portion and wrist portion Figure 1.9 PUMA 260 (courtesy of Westinghouse Automation Division/Unimation Inc). Combining an arm and a wrist mechanism generally gives a manipulator with six degrees of freedom, excluding the degree of freedom in the hand, For example, figure 1.8 is the combination of figures 1.3d and 1.4b, and the design of the robot in figure 1.9 is based on this combination. Figure 1.10 is the combination of figures 1.3d and 1.4c, and figure 1.11 shows an example of this combination. Figure 1.12 is the combination of figures 1.3¢ and 1.4b; figure 1.13 shows an example of this kind of robot. Figure 1.14 is the combination of figures 1.3a and 1.4b, and figure 1.15 shows an example of this combination. Depending on the purpose, there are often cases where six degrees of freedom are not required, and there are many industrial manipulators on the market that have only five degrees of freedom or fewer. One represen- tative example is shown in figure 1.16, which is the combination of figureOverview Figure 1.10 Combination of arm portion and wrist portion. Figure 1.11 ‘T3-786 Robot (courtesy of Cincinnati Milacron, Japan Branch). Figure 1.12 ‘Combination of arm portion and wrist portion,8 Chapter 1 Figure 1.13 Kawasaki Unimate (courtesy of Kawasaki Heavy Industries, Ltd.) Wj A, 77 Figure 1.14 Combination of arm portion and wrist portion. Figure 1.15 IBM 7565 Robotic System (1984) (courtesy of IBM Corp).Overview 9 a Figure 1.16 SCARA-type robot 1.3e and a wrist portion with just one joint rotating about a vertical axis. This four-degrees-of-freedom manipulator is called a SCARA (Selective Compliance Assembly Robot Arm).* Manipulators with seven degrees of freedom or more have also been developed. 1.2 Controller ‘The fundamental elements of tasks performed by robot manipulators are (1) to move the end effector, with or without a load, along a desired trajectory and (2) to exert a desired force on an object when the end effector is in contact with it. The former is called position control (or trajectory control) and the latter force control. Figure 1.17 is a rough sketch of a typical controller for position control. Joint positions and velocities are generally measured by joint sensors, such as potentiometers, tachometer-generators, and/or encoders. Using these data, the controller determines the inputs to the joint actuators so that the end effector follows the desired trajectory as closely as possible. The detailed structure of a position controller is shown in figure 1.18. The outputs on the right of the figure are the joint positions of the manipu- lator. The joint-trajectory generator determines the desired joint trajec- tories from the desired trajectory for the end effector. The desired joint trajectories are then given to the joint controllers. Each joint controller is a servomechanism for a single joint position. Figure 1.19 shows a simple servomechanism that uses the position and velocity feedback with constant feedback gains. This kind of controller has often been used for general industrial manipulators. Generally, changes in dynamics are due to changes in the manipulator configuration; there is also interaction among the joints. The above controller design implicitly assumes the possibility of coping with changes in manipulator dynamics and joint interactions by regarding10 Chapter 1 Desired trajectory Driving input for end effector Le Enaettector position Joint displacement ‘velocity? Figure 1.17 Rough sketch of position-control system. Desired trajectory Driving input sil efector position forint Rese RHP Baer ae, Position (velocity) af Desired trajectory}. joint ‘joint 1 for end effector [Desired . , vat Sa conraer ronot Position (eat? of [trajectory] Joint 2 joint 2 contr tr] » \ joint joint or = Tor jon so for joint Sensor for joint 1 Figure 1.18 Example of controller. Contellee for joint Driving input a Vor joint locity | 1 control gainf-T——~|_Robot Desired trajectory 7 ~ for joint « ‘ Position ‘Sensor for joint Figure 1.19 Position-velocity feedback servosystem.Overview uM Figure 1.20 Four-joint wrist with distance between axes of joints J2 and J3 set to zero, (J2 and J3 are in the horizontal plane and orthogonal to each other.) them as disturbances. However, when there are severe demands for fast and accurate positioning, the tracking performance of this type of controller is no longer adequate. Eventually it would be necessary to return to figure 1.17 and to design the controller taking into consideration the interaction among joints and the change of dynamics. In force control, it is generally necessary to measure the forces driving the joints or the contact force between the end effector and the object by force sensors, and to feed these signals back to the controller. In the following chapters, we will study the fundamentals of the analysis and control of robot systems such as those described above. Exercises 1.1 How many degrees of freedom do you think the human arm has (except for the hand portion)? Draw a structural model of a manipulator equivalent to the human arm using the symbols in figure 1.2. 1.2 Figure 1.20 shows a wrist mechanism obtained from the four-degrees- of-freedom wrist shown in figure 1.7 by setting the distance between the axes of joints J2 and J3 to zero. Find the singular configurations of this wrist. Is it possible to avoid these singular configurations without changing the orientation of the end effector? 1.3 Consider the task of inserting a peg with a circular cross-section into a cylindrical hole in a plate with arbitrary position and inclination. How12 Chapter 1 many degrees of freedom should a manipulator have in order to perform this task? References 1. T. Stackhouse, “A New Concept in Robot Wrist Flexibility,” in Proceedings of the Ninth International Symposium on Industrial Robots (1979), pp. 589-599, J. P. Trevelyanet al, "ET—A Wrist Mechanism without Singular Positions,” International Journal of Robotics Research 4, no. 4 (1986): 71-85. 3. T, Yoshikawa and S. Kiriyama, “Four-Joint Redundant Wrist Mechanism and Its Con- trol” ASME Journal of Dynamic Systems, Measurement, and Control 111, no. 2 (1989) 200-204 4. H. Makino and N. Furuya, “SCARA Robot and Its Family.” in Proceedings of the International Conference on Assembly Automation (1982), pp. 433-444, The following are well-written textbooks on robotics. They affected the style of this book in many ways. R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control (MIT Press, 1981). H_ Asada and J.-J. E. Slotine, Robot Analysis and Control (Wiley, 1986). J.J. Craig, Introduction 10 Robotics (Addison-Wesley, 1986).2 Kinematics This chapter will be devoted to the kinematics of robot manipulators, which means studying geometrically the motion of the manipulator links and/or objects related to the manipulation task in terms of position, velocity, and acceleration. First, to express the position and orientation of a rigid object, the method of assigning a coordinate frame to the object will be explained. Homogene- ous transforms, which are convenient for describing the relations among many objects, will be introduced. Then an expression for finding the end effector’s position in space from the description of the manipulator mecha- nism and its joint displacements will be derived. Methods for obtaining the joint displacements that realize a given end-effector position will also be discussed. Next, the Jacobian matrix describing the relation between the joint’s velocity and the end effector’s velocity will be introduced. We will consider the problem of finding a joint velocity that achieves a given end effector velocity and discuss singular configurations and an application of the Jacobian matrix to some problems of statics. 2.1 Position and Orientation of Objects 2.1.1 Object Coordinate Frame The first thing we have to do to analyze a manipulator is represent mathe- matically the position and orientation of the manipulator itself, the tool it holds, and the objects on which the robot works in three-dimensional space. For this purpose we adopt a method generally used in mechanics!: we attach an orthogonal coordinate frame to each object and express its position and orientation by the position of the origin and the directions of the three axes of the attached frame relative to a given reference orthogonal coordinate frame. The former is called the object frame and the latter the reference frame. Consider the object shown in figure 2.1. The reference frame is denoted by E,, its origin by O,, and the three coordinate axes by X,, Y,, and Z, Similarly, the object frame is denoted by Ep, its origin by Og, and the three axes by Xg, Ys, and Zy. The vector from OQ, to Og (ie., the position vector of Og relative to O,), expressed in £4, is denoted as “p,.* The unit vectors * Vectors and matrices are denoted by boldface italic letters, the only exception being identity matrices and zero matrices; those will be denoted by I and O, respectively.14 Chapter 2 Figure 2.1 Reference frame and object frame. in the directions of Xp, Yq, and Z», expressed in E,, are denoted “xp, “ys, and 4zy. Then the position of the object is represented by “pg, and its orientation is represented by {4xg,4yg,4zg}. The superscript 4 on the left means that the vector is expressed in the frame £,. Hereafter, all frames denoted by E are right-hand orthogonal coordinate frames. 2.1.2 Rotation Matrix It was established in subsection 2.1.1 that the orientation of an object can be specified by the three vectors {4x,, “yg, “zg}. It is often convenient, however, to use a matrix defined by Ra = (4x0 “Yn “Za] (2.1) instead of the three vectors. This matrix “Rp, which can be regarded as describing the rotational part of the relative displacement of frame ¥, from frame &,, is called the rotation matrix. Example 2.1 Consider the problem of expressing the position and orien- tation of a two-fingered hand. The hand frame Zy is attached to the hand as shown in figure 2.2a. In words, the origin Oy is taken to be at the midpoint of the two fingers. The Zy axis is in the approach direction of the hand. The ¥, axis is in one of the two directions in the plane including the two fingers and normal to Zy. The Xy axis is assigned so as to be normal to Zand Yq, making Ey, a right-hand coordinate system. We assume that, starting from the state (shown in figure 2.2b) where the hand frame Ey agrees with the reference frame Z,, the hand is rotated 90° around the axis that goes through [0,0,2]" of Z, and parallel to X,. The hand's positionKinematics 15 Zn Ow Yu @ (by Figure 22 Representation of position and orientation of a hand. (The black dot on one finger distinguishes it from the other finger.) (a) Hand frame Zy. (b) Relation between E, and Ey. and orientation after rotation are given by “Px = (0,2,21" and 10 0 4Ry=|0 0 —1].0° ol 0 Some properties of the rotation matrix will now be given. Since 4x,, 4yg, and “z, are unit vectors orthogonal to one another, they satisfy (Arg) 4x5 = 1, (99)? yn = 1 (45) “zy = 1, (4x9)" “yn = 0, e) ya)" “za = 0, (425) 4xp = 0. Hence Ry satisfies (Ag) (ARs) = Is, 23) * An open square (a) denotes the end of an example,16 Chapter 2 Figure 2.3 Interpretation of 4r = 4Ry¥. where I, is the 3 x 3 identity matrix. From equation 2.3, we have ARy* = (ARs) (2.4) where R™ denotes the inverse of a matrix R. Therefore, the rotation matrix 4R, is an orthogonal matrix. Consider the case where the origins of ©, and E, are at the same point, as shown in figure 2.3, and assume that a vector r is described by {p= [ert ey when expressed in £,, and by Dirge" 4, when expressed in Ey. Then we have tra Ante, + Ayah + Anh or “rn AR". (25) Replacing E, and Z, yields = "Ry *r. (2.6) From equations 2.5 and 2.6 we have 4y = ARPR, Ar. (2.7) Since equation 2.7 holds for any 4r, we obtainKinematics 17 ARR, = Uy, (2.8) or PR, = (4Rp)* = (4Rp)”. (2.9) Further, consider the case where the origin of a third frame, Zc, coincides with the common origin of £, and Ey, and assume that r is described by “r when expressed by Ec. Then Re, (2.10) ARC. (2.11) Hence, from equations 2.5, 2.10, and 2.11, we have ARC = ARg "Ro. (2.12) Since equation 2.12 holds for any ‘, we obtain the relation ARe = 4Ry "Re. (2.13) An expression of 4R, different from equation 2.1 can be obtained using equation 2.13. First, from equation 2.13 we have ARy = (Ry) Rp. (2.14) Let “x,, °y,, and °z, denote the unit vectors in the directions X4, Y,, and Z,, and let “xp, “yg, and “Zp denote those in the directions Xp, Yg, and Z,, both expressed in £_. Then 4R, can also be written as (xen (Cea Ve (Cx) Se ARs =| (Va) Xp (Va Ye Va)" Ee |. (2.15) (a) x_ (Za Ye (a)? Se Note that, for example, the (1,1) element, (°x,)” xp, is the cosine of the angle between the two vectors ©x, and xg. The same holds for all other elements. For this reason, the rotation matrix 4R, is also called the direc- tional cosine matrix. The value of Ry is, of course, independent of the choice of Ec. Although 4R, has nine variables, they always satisfy equation 2.2. Hence, the representation by “Rs is redundant. When any two of the three vectors xg, “yg, and 4zy are given, we can obtain the last vector using equation18 Chapter 2 Figure 2.4 Euler angles (9, 0,). 2.2.* For instance, the pair {4zy,4x,), which has six variables, can represent the orientation. So long as the elements of the rotation matrix themselves are used, there seems to be no way of representing the orientation by five or fewer variables. On the other hand, intuitively speaking, since equation 2.2 provides six relations to nine variables, the orientation should be describable by three variables, just as the position is. In fact, Euler angles and roll-pitch-yaw angles are convenient established representations. Both of them regard the object frame E, as a result of three sequential rotations about some fixed axes from the reference frame E,, and represent the orientation of Eg by the set of three rotational angles. 2.1.3 Euler Angles There are several definitions of Euler angles. A rather common definition, illustrated in figure 2.4, is adopted here: (i) The coordinate frame obtained from a rotation about the Z, axis by an angle ¢ of a frame initially coincident with Z, is named £4(O, ~ XpVqZ4-) (ii) The frame obtained from E,. by a rotation about Y,. by an angle 0 is named Ey(Og — X ge Yao Zar): ii) The frame obtained from Z,.. by a rotation about Z,.. by an angle y is named Zp. Thus, the orientation of Ey with respect to Z, can be represented by the * Strictly speaking, determining the last vector requires, besides equation 2.2, the condition that “xg, “Yan and 4zq form a right-hand system. See exercise 2.17Kinematics set of three angles (¢,0,y)), which are called te Euler angles, The relation between the Euler angles and the rotation matrix “R, is as follows. First, from figure 2.4a, the relation between EZ, and Z,. is given by cosé —sing 0 AR, =| sing cosé 0]. o o 1 (2.16) Similarly, the rotation matrix “‘R,. between Z,. and ,.., and the rotation matrix 4"Ry between E,.. and Lp, are given by cos? 0 sin® “Ry =| 0 1 0 —sin® 0 cosé and cosy —siny 0 “R,=|sinp cosy 0]. 0 o 1 (2.17) (2.18) From equation 2.13, the final rotation matrix “R, resulting from the three rotations is obtained from ARs = 4*Ry A Ry “Rp, which is (2.19) cos$ cosd cosy — singsiny —cospcos@siny — singcosy cos¢sind AR, =| sing cosOcosy + cospsinyy —sing cos siny + cos cosy —sin cosy sind siny sing sind cost (2.20) Let R(W,a) denote the rotation matrix between a coordinate frame and the frame obtained by its rotation about an axis W by an angle x. Then equation 2.20 can also be written as ARg = RZ 4,9) R(Yu 9) RZ). (2.21) Hence we have shown that the rotation matrix equivalent to any Euler angles (4,0,#) is given uniquely by equation 2.20. Now consider the problem of obtaining the Euler angles for a given “Ry. Assume that20 Chapter 2 Riu Riz Ris ARe=| Ro, Rar Raa (2.22) Ry R32 Rags is given. Equating each element of equation 2.20 and equation 2.22, we have cosdcos0 cosy ~ sing siny = Ry, (2.23) —cosd cos@ sin — sing cos = Ry 2, (2.236) cosdsin® = Ry, (2.230) sing cosO cosy + cosdsiny = R21, (2.234) ~sing cosdsinyy + cosp cos = R22, (2.23e) sing sind = Ry, (2.23f) —sinO cosy = R31. (2.23g) sind siny = Ry, (2.23h) cost = Rys (2.231) From equations 2.23c and 2.23f, sind = +\/R,3? + Ra5 (2.24) Hence, from equations 2.24 and 2.23i we obtain @ = atan2(+\/Ryy? + Ry57, R33) (2.25) Either the top or the bottom symbol of + or + should be used consistently throughout this section. In equation 2.25, atan2 is a scalar function defined by atan2(a,b) = arg(b + ja), (2.26) where j is the imaginary unit and arg(-) is the argument of a complex number (see figure 2.5). The function atan2 is a kind of arc tangent function that satisfies 0 = atan2(sind,cos0) (227) and 0 = atan2(k sind, k cos8) (2.28)Kinematics 21 (ba atan 2a, b) Re Figure 2.5 Function atan2. for any positive real number k. Several properties of the function atan2 are also summarized in appendix 1. Angles ¢ and y/ will be determined next. If sin@ # 0, we have @ = atan2(+Rz3,+Rj3) (2.29) from equations 2.23c and 2.23f, and W = atan2(+ R52, F R51) (2.30) from equations 2.23g and 2.23h. Hence, if R,;? + R23? #0, the Euler angles are obtained from equations 2.25, 2.29, and 2.30: @ = atan2(+ Ro3,4R13), (2.31a) 0 = atan2( + /R,3? + Ra3’sRa3)s (2.316) y = atan2(+R32,FRs,). (2316) It is also straightforward to show that these solutions satisfy the other equations in 2.23. Although there are two sets of Euler angles for a given 4Rg, if we put the constraint 0 <0 < x on 0 we have the following unique Euler angles: ¢ = atan2(R23,R;3) (2.32a) 6 = atan2(,/R,3? + Rz37,R33) (2.32b) = atan2(R32,—R3,). (2.320) On the other hand, if R, 5? + R,3? = 0, we have @ = arbitrary, (2.33a) 0 = 90° — Rys x 90°, (2.33b)n Chapter 2 WY = atan2(R,,,R32) — Ras6. (2.330) In other words, in the case of 0 = 0 or 0 = x, there is an infinite number of combinations of ¢ and y for just one orientation. So special care should be taken in treating this case. If we wish to keep uniqueness even in this case, one way is to set $ = Oand y = atan2(R,,,R22). We can aiso find a general expression of the Euler angles for a given rotation matrix which needs no distinction between the cases sin? = 0 and sinO 4 0; see exercise 2.2. Example 2.2 The representation by Euler angles of the orientation in example 2.1 is (¢.6.) = (—90°,90°,90°), or (9,8) = (90°, — 90°, — 90°). a 2.1.4 Roll, Pitch, and Yaw Angles The concept of roll, pitch, and yaw angles is basically the same as that of Euler angles except for a difference in the way we select the third rotational axis, In the case of roll, pitch, and yaw angles (figure 2.6), (i) the frame obtained from Z, by a rotation about Z, by ¢ is named Ey. as with Euler angles, Figure 2.6 Roll, pitch, and yaw angles (9,0,Kinematics 23 Gi) the frame obtained from E,. by a rotation about ¥, by 0 is named E,., again as with Euler angles, and Gii) the frame obtained from Z,. by a rotation about X,- by is named Ep. Then 4, @ and yp are called the roll, pitch, and yaw angles, respectively. The triple (,0,Y) is said to be the representation by the roll, pitch, and yaw angles of the orientation of Eg with respect to E, Example 2.3. The representation by roll, pitch, and yaw angles of the orientation in example 2.1 is given by (4,8,) = (0°,0°,90°), or (¢,0,W) = (180°, 180°, — 90°). o 2.2 Coordinate Transformation 2.2.1 Homogeneous Transform In figure 2.7 we assume that the relation between two coordinate frames £4 and Ey is given by the position vector “pg and the rotation matrix 4Ry of E, with respect to E,. Then the relation between the expressions 4r and *y of a point in space by (respectively) E, and Ep is Ay =4Rg™r + App. (2.34) Xe Figure 2.7 Frames E, and Ep.pr) Chapter 2 This equation can also be expressed as * AR, “pa |{ * =|------ ~-}24T,|--|. (2.35) 1 0 0 0 1 1 1 In this new expression, the three-dimensional vectors 4r and " must be represented by the four-dimensional vectors obtained by adding the ele- ment | at the bottom of the original vectors. In return for this, the expression obtained is simpler in the sense that the transformation from the expression of a vector with respect to EZ, to that with respect to Zp is done by the multiplication of just one matrix: 47. The transformation represented by the 4x4 matrix “Tp is called the homogencous transform. The vector (4r7,1]? may also be written as “r when there is no confusion; for example, equation 2.35 may also be written as 4p = “Ty *r. (2.36) Homogeneous transforms can be used for the following purposes: (i) Changing the coordinate frame with respect to which a point in space is expressed. This is what we did in equations 2.35 and 2.36 when we wished to obtain the expression “r with respect to Z, of a point expressed as *r with respect to Zp. (ii) Describing the relation between two coordinate frames. Since “Tg con- tains both of the parameters (4pg and 4R,) that represent the relation of Ep to Ly, “Ty can be regarded as describing this relation. Although 47, has no merit over the pair {4pp,4Rp} in real numerical calculation, it allows for greater simplicity of expression. Example 2.4 As is shown in figure 2.8, when E, is obtained from E, by a rotation about Z, for a, their relation is described by cosa —sina 0 0 sina cose 0 0 “T, = *=|o 0 1 0f° 0 0 01 Example 2.5 When E, is obtained from £, by a translation of 2 units in Y, and then by a translation of 1 unit in Z,, their relation is given byKinematics 25 Zam te Figure 28 Frames £, and Ey Figure 29 Transfer of a point. 1000 0102 “7, = T=)o 011° 0001 (iii) Description of transfer of a point in space. We assume that a point in space with a fixed coordinate frame is transferred in a given way. The homogeneous transform can be used to describe the relation between the original point location and the location after the transfer. Example 2.6 As shown in figure 2.9, we consider a point 4r, expressed in £,. Let 4r, denote the new location of this point after rotating 30° about Z, and then translating 2 units along Y, and —1 units along Z,. The relation between 4r, and 4r, is described by cos30° —sin30° 0 0 sin30°_cos30° 0. 2 n=! 9 0 1 1/8 0 0 0 126 Chapter 2 Figure 2.10 Frames £, and Z, These three uses of homogeneous transformation are of course closely related, as example 2.7 will show. Example 2.7 When the transform given in example 2.6 is regarded as °T, describing the relation between two frames E, and Zp, we have the change in frame shown in figure 2.10, Note that Z, is obtained from a frame originally at the same place as £,, first by translating it —2 units along Y, and I unit along Z, and then by rotating it — 30° about Z,. Also note that the point whose representation in Z, is *r, is represented as +r, in Ey. 0 2.2.2 Product and Inverse of Homogeneous Transform Consider three frames, £4, Eg, and Ze, and assume that the relations between Z, and Ey and between Ey and Ee are given by 47, and *T respectively. Then the relation “7, between E, and Zc is given by the product of 47 and °Ze: AT. = “Tp "Te. (2.37) This is an extension of equation 2.13. The following two interpretations in terms of coordinate-frame trans- formations are possible for equation 2.37: (i) The frame Ep is obtained from E, by translating Z4 by 4pg with respect to E, and then rotating the translated frame (using “R,) about the origin of the translated frame itself. The frame L, is obtained from L, by trans- lating by “p- and then rotating the translated frame (using "R_) about the origin of the translated frame.Kinematics 27 (ii) We denote as Zp. the frame obtained by first rotating a frame initially coincident with E, using ®R, and then translating it by "pc, both with respect to Z,. Further, we rotate Dy using “Ry and translate it by 4p,, both with respect to £,. Thus we obtain the frame L¢. In interpretation i, the product of two transforms is interpreted from the left one (47), and the second transformation (87) is done with respect to the new frame obtained from the left transformation. In contrast, in inter- pretation ii the product is interpreted from the right transform (87), and both transformations are done with respect to the original frame. The difference of order between the translation and the rotation in interpreta- tions i and ii could be understood by substituting into equation 2.37 the relation AR, AT, = |- s (2.38) ' 1 1 t o | and a similar relation for ®7,, where 0 denotes a vector or a matrix whose elements are all zeros. Example 2.8 Assume that 47, and "7; are given by J3/2 -1/2 0 2 47, =| 2 J32 01 0 0 10 0 0 01 and W/2 1/2 01 ee a 0 0 10 0 0 o1 The two interpretations of 47, are illustrated schematically in figure 2.11. In this example the Z axis is always normal to the page and thus it is omitted from the figure. The final location of ¢ is, of course, the same in both cases. a The inverse of “Ty is, from equation 2.4,28 Chapter 2 @) Figure 211 Interpretation of 47-. (a) Interpretation by (i) (b) Interpretation by (i). ‘7, =4T,) = 2.39) 2.3. Joint Variables and Position of End Effector 2.3.1. General Relation In this subsection an overview is given of the relation between joint dis- placements (rotational displacements for revolute joints and linear dis- placements for prismatic joints) and end-effector position for a manipulator with n degrees of freedom. In figure 2.12 the joints are numbered 1, 2, ..., n, starting from the base of the manipulator. The displacement of joint i is denoted q, and is called the joint variable. The collection of joint variables (2.40) is called the joint vector. The position of the end effector is denoted by the m-dimensional vector r= [titan tn] 41) where m < n, For a general case where the end effector can take an arbitrary position and orientation in three-dimensional Euclidean space, we haveKinematics 29 r=(rio, rel? Figure 2.12 link manipulator. m = 6. However, when the manipulator moves in a two-dimensional plane and we are concerned only with its endpoint position in the plane, we can set m = 2. If we are further concerned with the orientation of the endpoint in the plane, we have to set m = 3. The relation between r and g, determined by the manipulator mechanism, generally is nonlinear. We assume that this relation is given by r=f(9). (2.42) This equation is called the kinematic equation of the manipulator. When the joint vector q is given, the corresponding r is determined uniquely and the calculation is rather simple. However, when some task is assigned to the manipulator, what is given first is usually its end-effector position r or a trajectory ofr. Thus we have to calculate a joint vector g which will realize the required end-effector position r—that is, we have to obtain q satisfying equation 2.42. This solution can be written formally as . (2.43) q= Note, however, that g does not necessarily exist, and even when it does exist it may not be unique. The problem of obtaining r for a given q is called the direct kinematics problem, and that of obtaining g corresponding to a given r is called the inverse kinematics problem. As we can see from the above argument, the inverse kinematics problem is usually the more difficult of the two, Example 2.9 The relations 2.42 and 2.43 will be obtained for the two- degrees-of-freedom manipulator moving in the X-Y plane as shown in figure 2.13. The joint vector in this case is g = [6,,0,]", the end-effector position is given by r = [x,y]’, and the lengths of the links are /, and I. The relation 2.42 is easily obtained:30 Chapter 2 Figure 2.13 Two-link manipulator. x = 1,cos0, + I,cos(8, + 03), y= hsind, + l,sin(@, + 4). As for the relation 2.43, if r satisfies (-bPse+y sh thy? then there exists at least one g corresponding to r. Further, if x? + y? 40, using the function atan2, from equations A1.11, A1.4, and A1.6 of appendix 1. we obtain tan2(y,x) F atan2(x,x? + y? + 1)? — 1,7) where wa obey F122 OE EF LP a LT. ‘The two solutions given above correspond to the two arm configurations in figure 2.14. If, on the other hand, x? + y* = (I, ~ [,)? = 0, then 0, = arbitrary and 0, = +180°; that is, there are infinitely many solutions. 0Kinematics 31 ay atan2ixx! yb) Figure 214 Solutions of inverse kinematics problem for two-link manipulator. Soin ¢ Link /-1©)_ ink) dont 1 Joint i~1 Joint 2 a Link 1 Link Joint 1 Link 0 base) Figure 2.15 Link and joint numbers for n-link manipulator. As example 2.9 shows, it is rather easy to solve the direct kinematics problem and the inverse kinematics problem for manipulators with around two degrees of freedom. However, both problems become increasingly difficult as the number of degrees of freedom increases. One way to cope with this difficulty is to assign an appropriate coordinate frame to each link and to describe the relation among the links by the relation among these frames. In this way, deriving the function f(g) becomes systematic and obtaining f,-"(r) analytically or numerically becomes much easier. This approach is developed in the following subsections. 2.3.2 Link Parameters Consider a manipulator consisting of n links connected serially by n joints, with one degree of freedom each, which may be either revolute or prismatic. As is shown in figure 2.15, the links and the joints are numbered 1, ...,32 Chapter 2 Joint i cb + Joint i+ 1 7 Joint i= 1 Link i = 1 Join ie - At ‘ f~ Figure 2.16 Joint axes, joint variables, and link parameters: (a) when joint i is revolute; (b) when joint i is prismatic.Kinematics 33 starting from the base side. The base itself is called link 0; hence, link 0 is connected to link 1 by joint 1. For each joint i, the joint axis i is defined as. the rotational axis in the case of a revolute joint or as an arbitrary straight line parallel to the direction of translation in the case of a prismatic joint. In figure 2.16, dashed lines show the joint axes in these two cases. The common normal between joint axes i and i + 1 is considered to be the mathematical model of link i. The common normals are shown as solid lines in the figure. When the joint axes i and i + 1 are parallel, the common normal is not unique, so we select one common normal arbitrarily as the mathematical model of link i. With these preparations, we can now describe the size and shape of link i by two variables: the length a, of the common normal, and the angle a, between the orthogonal projections of joint axes i and i + 1 onto a plane normal to the common normal. The variable a; is called the link length, and a, is the twist angle. The relative positional relation between links i — 1 and i at joint i can be described by the distance d, between the feet of two common normals on the joint axis i, and the angle 6, between the orthogonal projections of these common normals to a plane normal to the joint axis i. The variable d, is called the joint length, and 6, is called the joint angle. If joint iis revolute, 4d, is constant and @, expresses the rotational angle of the joint; if joint i is prismatic, 6, is constant and d, expresses the translational distance of the joint. Hence, when joint i is revolute we adopt 6, as the joint variable q,, and when joint i is prismatic we adopt d,. The other three variables are constant and are called link parameters. This way of describing link mecha- nisms using a,, 4%, d,, and , is usually called the Denavit-Hartenberg notation.” 2.3.3 Link Frames Now we will define coordinate frames, one fixed to each link. As is shown in figure 2.17, the origin of coordinate frame £, of link iis set at the endpoint of the mathematical model of link i on joint axis i. The Z axis of £,, denoted Z,, is selected in such a way that it aligns with joint axis i in the direction pointing toward the distal end of the manipulator. When the direction toward the distal end is not clear, the direction of Z, is arbitrary. The X axis of £,, X;, is selected so that it is on the common normal and points from joint i to joint i + 1. The Y axis, ¥,, is selected in such a way that 2, is a right-hand coordinate frame.34 Chapter 2 Joint ¢ ' Joint i+ 1 Join Joint 1 Link o 23 Figure 2.17 Link frame ,. (29 and E, are shown for the case when the joint variables are zero.) By the above procedure, the link frames for links 1 through n — 1 are determined. For link 0, the link frame £, is defined to be equal to £, for an arbitrarily selected reference configuration of joint 1. For link n, the joint nis fixed at an arbitrarily selected configuration and the origin of £, is set to be at the endpoint of the mathematical model of link n — 1 on joint axis n. The axis Z, is aligned with the joint axis n with its direction pointed toward the distal end of the manipulator when the direction toward the distal end is clear. The axis X, is aligned with X,-,, and Y, is determined in such a way that &, is a right-hand frame. Now that we have defined the frame ,, provided that the positive sense of each variable is determined so that it harmonizes with ¥,, the four variables introduced in the previous subsection can be expressed as follows: a, = the distance measured along the X;, axis from Z, to Z,.,, x, = the angle measured clockwise about the X; axis from Z, to Z,.1. d, = the distance measured along the Z, axis from X,_; to X,, and 6, = the angle measured clockwise about the Z, axis from X,_, to X,. In other words, the frame E, can be obtained from ¥,_, by the following four transformations: (i translation along X;- for a distance 4;-, (ii) rotation about X,_, by an angle a,_,,Kinematics 35 Joint iy Joint i +1 Joint, ~2 Joint i+ 2 Figure 2.18 ‘An assignment of link frame i when joint i is prismatic (a, = 0, d,., = 0). ii) translation along Z;_, after the rotation ii (which is Z,) for a distance d,,and (iv) rotation about Z,-, after the rotation ii (which is Z;) by an angle 6, Note that the above relation holds also between Ey and Z,, and between E,-; and E,, with Zy and E, defined earlier by selecting the link parameters as follows: dy = % = 0, d, =0 when joint 1 is revolute, 6, =0 when joint 1 is prismatic, d,=0 when joint nis revolute, 6, = 0 when joint n is prismatic. It is desirable to make as many link parameters zero as we can, since this will make the later analysis easier and will decrease the amount of computa- tion necessary to solve the direct and inverse kinematics problems. Re- viewing the procedure of determining the joint axes and the common normals from this viewpoint makes it clear that when they are not unique the following scheme is a desirable one. When determining the joint axis i for a prismatic joint, we use the fact that its location is arbitrary while its direction is fixed. We can set a,_, or a, to zero, and d,_, or d,s, can also be set to zero. Hereafter we set a, = 0 and d;,, = 0 by having the joint axis i pass through the endpoint of the mathematical model of link i + 1 on the joint axis i + 1, as is shown in figure 2.18. When the joint axes i and i + 1 are parallel, d; or d,,, can be36 Chapter 2 Joint) Joint +1 | | Joint i+ 2 Joint j= 1 \ Figure 219 ‘An assignment of link frame i when joint axes i and i + 1 are parallel (d,, = 0) set to zero. Hereafter, as is shown in figure 2.19, we set d;,, = 0 by having the common normal pass through the endpoint of the mathematical model of link i + 1 on the joint axis i + 1. In these two cases, if there exists any arbitrariness in the location of the mathematical model of link i + 1, we can further determine the link frames so as to set a,-, dy, etc. to zero. Also note that in figures 2.18 and 2.19 the ¥ axes of the link frames are not shown because they are uniquely determined by the X and Z axes. This convention will be used hereafter. In summary, the link frames are determined as follows: (i) Determine the joint axes. (ii) Determine the common normals. (iii) Determine the link frames. (iv) For joint axes and common normals that have arbitrariness in their location, return to step i or ii and modify their location to set as many parameters to zero as possible, There are still cases where the above procedure does not yield a unique set of link frames. In those cases, we can just select one set of link frames arbitrarily. Example 2.10 The Stanford manipulator, developed mainly for research purposes, has the mechanism shown in figure 2.20. The link frames deter- mined by the above procedure for this manipulator are shown in figure 2.21; the link parameters are given in table 2.1. Note that figure 2.21 shows a reference configuration for which 8, = 0(i = 1,2, 4, 5, 6) and all the X, axes are in the same direction. Also note that d, is not taken to be zero in the figure, because a configuration with d, = 0 is unattainable. 0Kinematics 37 Link 6 Link 5S) int 5 Link 4 Link 3 Link 2 seine 2-4) Joint 6 Link 1 Link 0 @ Figure 220 Stanford manipulator. (a) Link structure. (b) Appearance. IL. Xo X Zu Zs z Figure 221 Link frames for Stanford manipulator.38 Chapter 2 Table 2.1 Link parameters for Stanford manipulator. (Parentheses indicate joint variables.) i a, oy 4, 9 1 oO o 0 (8) 2 oO —90° d, (03) 3 ° 90° as) ° 4 ° ° ° Wo s 0 —90° 0 (05) 6 0 90° 0 (0,) Joint 1 " Joint 4 Joint i= 1 / Link Ke ~ Soin Joint 2 Link 1 Joint Link 0 Figure 2.22 Another assignment of link frames. The merit of the assignment scheme of link frames given above is that the frame of each link has its origin on the joint axis closer to the base of the link. Also, the Z axis of the frame agrees with the joint axis whose displacement directly causes the motion of that link. This assignment scheme appeared in Craig's Introduction to Robotics.* Another scheme of assigning link frames is shown in figure 2.22, where the origin of the frame is on the distal joint axis of the corresponding link. Its Z axis is the joint axis for the motion of the next link at the distal end. A more detailed description on this scheme can be found in Paul’s Robot Manipulators.® In principle, any set of n coordinate frames are feasible as link frames as long as each link has a frame fixed to it, Any assignment that sets the origin of E, on joint axis i (when joint i is revolute) and aligns the Z, axis with joint axis i has most of the merits of the Denavit-Hartenberg notation. The assignment scheme described in this subsection has been selected simply39 Kinematics because it is intuitively easier to understand and because it gives a unique set of link frames for most manipulators. 2.3.4 Solution to Direct Kinematics Problem The homogeneous transform that describes the relation between Z, and Zi is PNT, = Ty X51) TeX 10-1) Tr( Zid) Te(Zi9)), (2.44) where 7;(X,a) denotes a translation along the X axis for a distance a, and T,(X,a) denotes a rotation about the X axis by an angle a. From equation 2.44 we obtain 100 a,]f1 0 o 0 0 1.0 0 |{0 cosy, sina, 0 001 0 JO sing, cosa, 0 000 1 {lo o o 1 10 0 0]fcosé, —sind, 0 0 fo 100 snd vost 00 ood 10 0001 o1 0s, —sind, 4,4 cosa,-,sin®, cosa,,cos#, —sina,, —sina,,d, =| (2.45) sina,,sin®, sina,-ycos®, cosa, cosa yd; 0 0 0 1 Let the homogeneous transform relating ©, to £q be °7,. Then we have 7, = TT, OT, (2.46) When the values of all link parameters are given, ‘~*7; is a function of q, only. Hence, °7, is a function of the joint vector g. Let Ly be the coordinate frame fixed to the end effector that is attached to link n, and let "7, be the homogeneous transform describing the relation between Zp and E,. Let Eq be the reference frame, and let ®7) describe the relation between Ex and the base frame Lo. The transforms "7; and ®T, are constant ones determined by the way the end effector is attached to link n and by the location of the base with respect to the reference frame. The relation between the end effector and the reference frame is then40 Chapter 2 Figure 2.23 PUMA-type manipulator. (a) PUMA robot (courtesy of Westinghouse Automation Division/Unimation Inc.) (b) Link structure. ®T, = "87, °T, "Te. (2.47) Since the position vector r of the end effector is determined uniquely from RT, and since T, is a function of g, we can obtain the function f(g) of equation 2.42 from equation 2.47. Thus, if each element of °7, is obtained as a function of g, the direct kinematics problem is basically solved. Example 2.11 Let us assign the link frames to the PUMA-type manipu- lator shown in figure 2.23 and obtain °7,. Following the scheme of the previous subsection, we obtain the link frames shown in figure 2.24 and the link parameters in table 2.2. Hence, from equation 2.45, cos, —sin®, 0 0 cosé, —sin®, 0 op, —| Sin cos 0 0) yp _ | 0 o 1 ‘ 0 0 10; ©? | —sine@, —cos#, 0 P 0 0 o1 0 o o 1 cosé, —sind, 0 |. cosé, ~sin®, O |, ap, | Sindy cos, 0 0} 4, _] 0 o 14k > 0 o 1 0f “*” | —sina, ~cos& 0 0 |’ 0 0 Ot 0 o otKinematics 41 zs x Figure 224 Link frames for PUMA robot. Table 2.2 Link parameters. i cd Lie 4 4 1 0 o 0 (0) 2 0 ~90" hod (61) 3 h o 0 (0) 4 4 - 90° y @) 5 0 90° 0 (@s) 6 0 -#" 0 A)a Chapter 2 [cos#, -sin®fe 0 0 cosh, —sind, 0 0 oa) 0 -1 0} ._} 0 o 10 S| sin@, cos 0 of “S| -sin@ -cosé, 0 0} 0 0 o1 0 0 01 - (248) Now we calculate °7,. We first find °7, for the arm portion and °7, for the wrist portion; then we multiply them. Finding these two transforms is just ‘a convenience for later use. The transform °T, is given by* FOiCr — C823 Sy CC = he = LS, SiCz3 —SySp3 Cy US Cy Hy = LIC) “Si, -C 0 =I.S; 0 0 0 1 a The transform *7, is given by CsCsCg — SaS6 —CaCsSo~SsCg —CaSs |e S.C, -S.S, C. | ne Co 65 sty 3.49 OP] HSiCsCo— CaSe SsCsSo—CaCe SUS 0 (249) 0 0 0 1 Thus, we finally obtain Ry Rizr Ris Ps Ray Rar Ray Py o7, = a Pr) (2.505 Ra, Raz Ras Pe [0 0 0 1 where Ruy = Ci[Cas(CaCsCo — Su8o) — S2y5sCo] + S\(SuCsCo + CaSoh Ruz = CiL—Ca(CaCsSo + Sug) + S235sSo] ~ S\(SuCsSo — Calo * Here we use the following notations: C, = c058,, $, = sin. C33 = c0s(6y + @s).and S. sint(, + 05). This kind of notation will be used throughout the book. For example. 5, sin((, + 0; + ®)and Cys4 = COs(@, + 03 +O)Kinematics Ry3 = —C,(Cz3CgSs + S23Cs) — S, S455, Rar = SiLCrs(CaCsCo — SaSe) — S238sCo] — Cy (Sas Co + CaSe), Raz = SiL—Cas(CaCsSo + Seg) + S2355S6] + Cy(SuCsSo — Ca Co), Ry = —S,(Cr3CaSs + S23Cs) + CiS4Ss, Ry1 = —S23(CaCs Ce — SaSe) — Ca3SsCo, R32 = Sz3(CaCsSo + S4Co) + Cr35sSo5 R33 = S3CaSs ~ Cass, Px = Ci(leCz + beCos — IyS2) — (ly ~ lS Py = Sy(IeCz + leCos — S23) + (ly — Cr, Pz = IS — leSr3 — 1pCr3-0 4a (2.51) (2.52a) (2.52b) (2.520) Example 2.12 For the PUMA-type manipulator in example 2.11, assume that the reference frame Eq and the end-effector frame Ey are as illustrated in figure 2.25. Assume also that the end-effector position vector r is PCat tel Figure 2.25 Reference frame Eq and end-effector frame Ey44 Chapter 2 where ry, 12, and rs are the X, Y, and Z coordinates of the origin of £, and where rg, rs, and rg are the Euler angles of £, with respect to Lp. We first solve the direct kinematics problem; then we find the value r for the case where the joint vector is q* = [0°, — 45°,0°,0°, —45°,90°]7. First, 1000 ® 0100 mlo otk 0001 and 1000 . o100 Te=loo 14, 0001 Hence, from equations 2.47 and 2.50, Riu Ria Ris + Rial Ray Rar Ras Py + Rasly Rs Rar Ray Pz tla + Rasly 0 0 oO 1 ®7, = From equations 2.32 and 2.33, the corresponding position vector ris given by r= Pet Rishy 12 = Py + Raslos n= 2 +l + Raslys ry = atan2(R23,R13), r, = atan2(/Rj,? + Ry37,Ra9)s atan2(R32,—R31) if R,3? + Ry? #0 atan2(Rz1.R22) — Ryra_ if Ris? + Raz” = 0. Second, when ¢ = q* we have