Theory of Applied Robotics: Kinematics, Dynamics, and Control [3rd ed. 2022] 3030932192, 9783030932190

Citation preview

Reza N. Jazar

Theory of Applied Robotics Kinematics, Dynamics, and Control Third Edition

Theory of Applied Robotics

Reza N. Jazar

Theory of Applied Robotics Kinematics, Dynamics, and Control Third edition

123

Reza N. Jazar School of Engineering RMIT University Melbourne, VIC, Australia

ISBN 978-3-030-93219-0 ISBN 978-3-030-93220-6 (eBook) https://doi.org/10.1007/978-3-030-93220-6 © The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Switzerland AG 2007, 2010, 2022 This work is subject to copyright. All rights are solely and exclusively licensed by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Switzerland AG The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

v

I am Cyrus, king of the world, great king, mighty king, king of Babylon, king of Sumer and Akkad, king of the four quarters. I ordered to write books, many books, books to teach my people, I ordered to make schools, many schools, to educate my people. Marduk, the lord of the gods, said burning books is the greatest sin. I, Cyrus, and my people, and my army will protect books and schools. They will fight whoever burns books and burns schools, the great sin. Cyrus the great

to: Vazan Kavosh Mojgan

Preface to the Third Edition

Ten years have passed since the second edition of Theory of Applied Robotics has been published. The second edition of the book has been strongly accepted in academia for research and teaching. Many universities and colleges adopted this book as a teaching text or as a reference. The book also had great success in Chinese universities, especially after its Chinese translation was officially published by Springer in 2018. During the past 10 years, I have received many constructive comments from all around the world, from colleagues, friends, instructors, researchers, and students. In this third edition, I used those comments to improve the text as well as remove typos and numerical errors. Further, I tried to make the third edition more educational and clearer to make the book more suitable for self-training. The key concept of robotics’ kinematics and dynamics is the understanding of the coordinate frame transformation, a topic that Theory of Applied Robotics is covering in the best possible way. We begin this topic by analyzing rotation about a principal axis of global and body coordinate frames. Rotation about an arbitrary axis is the next step. Understanding of rotation about an arbitrary axis is the best way to study kinematics of robots. We will show how to break such a complicated rotation into a series of principal rotations to develop the required mathematical relations by a series of simple steps. To make it possible, we will show how we may introduce extra dummy coordinate frames to simplify moving from initial to final coordinate frame by a modular and structural process. Auxiliary coordinate frames is a unique concept that has been added to the kinematic knowledge body of robot kinematics by this book. Readers of this book will become masters in using this scientific trick in kinematic analysis. The robot kinematics will be continued by covering the first and second time derivatives of rotation transformation mathematics. The first derivative introduces the complicated and strange concept of angular velocity. Angular velocity is a complicated concept because we do not have any unique mathematical quantity whose time derivative will be angular velocity. Angular velocity is a vectorial quantity, but it is not time derivative of another scalar or vectorial physical quantity. This fact makes angular velocity appear as a new individual concept. It, however, will be connected to a combination of time derivative of rotation transformation matrices. The appearance of rotation transformation matrices in the definition of angular velocity gives us the ability to define time derivative operations in different coordinate frames. Therefore, we will see that there are several different velocities all mathematically correct but not all sensible. The second derivative and introduction of acceleration kinematics make this concept more complicated such that we will be able to define and calculate greater accelerations than those we work with. The freedom of taking a vectorial physical quantity from one coordinate frame and taking derivative in another coordinate and still expressing in a third coordinate frame helped me discover a new acceleration called Razi acceleration (Jazar 2011; Harithuddin et al. 2015). My intention in this book is to explain robotics in a manner I would have liked to be explained to me as a student. This book can now help students by being a great reference that covers all aspects of robotics and that provides students with detailed explanations and information.

ix

x

Organization of the Book The text is organized in such a manner that it can be used for teaching or self-study. Chapter 1 “Introduction,” contains general preliminaries with a brief review of the historical development and classification of robots. Part I, Kinematics, presents the forward and inverse kinematics of robots. Kinematics analysis refers to position, velocity, and acceleration analysis of robots in both joint and base coordinate spaces. It establishes kinematic relations among the end-effecter and the joint variables. The method of Denavit-Hartenberg for representing body coordinate frames is introduced and utilized for forward kinematics analysis. The concept of modular treatment of robots is well covered to show how we may combine simple links to make the forward kinematics of a complex robot. For inverse kinematics analysis, the idea of decoupling, the inverse matrix method, and the iterative technique are introduced. It is shown that the presence of a spherical wrist is what we need to apply analytic methods in inverse kinematics. Part II, Derivative Kinematics, explains how the derivatives of vectors are calculated and how they are related to each other. It covers angular velocity, velocity, and acceleration kinematics. Definitions of derivatives and coordinate frames are covered in this part. It is fascinating to understand that derivative is a frame-dependent operation. Part III, Dynamics, presents a detailed discussion of robot dynamics. An attempt is made to review the basic approaches and demonstrate how these can be adapted for the active displacement framework utilized for robot kinematics in the earlier chapters. The concepts of recursive Newton-Euler dynamics, Lagrangian function, manipulator inertia matrix, and generalized forces are introduced and applied for derivation of dynamic equations of motion. Part IV, Control, presents the floating time technique for time-optimal control of robots. The outcome of the technique is applied to an open-loop control algorithm. Then, a computedtorque method is introduced, in which a combination of feedforward and feedback signals are utilized to render the system error dynamics.

Method of Presentation The structure of presentation is in a “fact-reason-application” fashion. The “fact” is the main subject we introduce in each section. Then the reason is given as a “proof.” Finally, the application of the fact is examined in some “examples.” The “examples” are a very important part of the book because they show how to implement the knowledge introduced in “facts.” They also cover some other facts that are needed to expand the subject.

Level of the Book This book has evolved from nearly a decade of research in nonlinear dynamic systems and teaching undergraduate- and graduate-level courses in robotics. It is addressed primarily to the last year of undergraduate study and the first-year graduate student in engineering. Hence, it is an intermediate textbook. This book can even be the first exposure to topics in spatial kinematics and dynamics of mechanical systems. Therefore, it provides both fundamental and advanced topics on the kinematics and dynamics of robots. The whole book can be covered in two successive courses; however, it is possible to jump over some sections and cover the book in one course. The students are required to know the fundamentals of kinematics and dynamics, as well as a basic knowledge of numerical methods. The contents of the book have been kept at a fairly theoretical-practical level. Many concepts are deeply explained and their use emphasized, and most of the related theory and formal proofs have been explained. Throughout the book, a strong emphasis is put on the physical meaning of the concepts introduced. Topics that have been selected are of high interest in the field. An attempt has been made to expose the students to a broad range of topics and approaches.

Preface to the Third Edition

Preface to the Third Edition

xi

Prerequisites Since the book is written for senior undergraduate and first-year graduate level students of engineering, the assumption is that users are familiar with matrix algebra as well as basic feedback control. Prerequisites for readers of this book consist of the fundamentals of kinematics, dynamics, vector analysis, and matrix theory. These basics are usually taught in the first three undergraduate years.

Unit System The system of units adopted in this book is, unless otherwise stated, the international system of units (SI ). The units of degree (deg) or radian (rad) are utilized for variables representing angular quantities.

Symbols • Lowercase bold letters indicate a vector. Vectors may be expressed in an n dimensional Euclidian space. Example: r , s ,d, a ,b, c p,q,v,w,y, z ω,α,, θ ,δ,φ • Uppercase bold letters indicate a dynamic vector or a dynamic matrix. Example: F , M , I , L • Lowercase letters with a hat indicate a unit vector. Unit vectors are not bolded. Example: ıˆ , jˆ , kˆ , eˆ , uˆ , nˆ Iˆ , Jˆ , Kˆ , eˆθ , eˆϕ , eˆψ • Lowercase letters with a tilde indicate a 3 × 3 skew symmetric matrix associated to a vector. Example: ⎡ ⎤ ⎡ ⎤ 0 −a3 a2 a1 ⎣ ⎦ ⎣ a˜ = a3 0 −a1 , a = a2 ⎦ −a2 a1 0 a3 • An arrow above two uppercase letters indicates the start and end points of a position vector. Example: −−→ ON = a position vector from point O to point N • A double arrow above a lowercase letter indicates a 4 × 4 matrix associated to a quaternion. Example: ⎡

q0 ⎢ q 1 ← → q =⎢ ⎣ q2 q3

−q1 q0 q3 −q2

−q2 −q3 q0 q1

⎤ −q3 q2 ⎥ ⎥ −q1 ⎦ q0

q = q0 + q1 i + q2 j + q3 k

xii

Preface to the Third Edition

• The length of a vector is indicated by a non-bold lowercase letter. Example: r = |r| , a = |a| , b = |b| , s = |s| • Capital letters A, Q, R, and T indicate rotation or transformation matrices. Example: ⎡ ⎤ ⎡ ⎤ cα 0 −sα −1 cos α − sin α 0 ⎢ 0 1 0 0.5 ⎥ G ⎥ TB = ⎢ QZ,α = ⎣ sin α cos α 0 ⎦ ⎣ sα 0 cα 0.2 ⎦ 0 0 1 0 0 0 1 • Capital letter B is utilized to denote a body coordinate frame. Example: B(oxyz)

B(Oxyz)

B1 (o1 x1 y1 z1 )

• Capital letter G is utilized to denote a global, inertial, or fixed coordinate frame. Example: G G(XY Z) G(OXY Z) • Right subscript on a transformation matrix indicates the departure frames. Example: TB = transformation matrix from frame B(oxyz) • Left superscript on a transformation matrix indicates the destination frame. Example: G

TB = transformation matrix from frame B(oxyz) to frame G(OXY Z)

• Whenever there is no sub or superscript, the matrices are shown in a bracket. Example: ⎡

cos α ⎢ 0 [T ] = ⎢ ⎣ sin α 0

0 − sin α 1 0 0 cos α 0 0

⎤ −1 0.5 ⎥ ⎥ 0.2 ⎦ 1

• Left superscript on a vector denotes the frame in which the vector is expressed. That superscript indicates the frame that the vector belongs to, and so the vector is expressed using the unit vectors of that frame. Example: G

r = position vector expressed in frame G(OXY Z)

• Right subscript on a vector denotes the tip point that the vector is referred to. Example: G

rP = position vector of point P expressed in coordinate frame G(OXY Z)

• Left subscript on a vector indicates the frame that the angular vector is measured with respect to. Example: G B vP

= velocity vector of point P in coordinate frame B(oxyz) expressed in the global coordinate frame G(OXY Z)

We drop the left subscript if it is the same as the left superscript. Example: B B vP

≡

B

vP

Preface to the Third Edition

xiii

• Unit vectors must always be expressed in their own coordinate frame to be meaningful. Example: 0 ıˆ1 and 0 ıˆ2 must be transformed into 0-frame and be expressed by unit vectors of the 0-frame. 0 ıˆ1 = 0 R1 1 ıˆ1 0

ıˆ2 = 0 R2 2 ıˆ2

• Right subscript on an angular velocity vector indicates the frame that the angular vector is referred to. Example: ωB = angular velocity of the body coordinate frame B(oxyz) • Left subscript on an angular velocity vector indicates the frame that the angular vector is measured with respect to. Example: G ωB

= angular velocity of the body coordinate frame B(oxyz) with respect to the global coordinate frame G(OXY Z)

• Left superscript on an angular velocity vector denotes the frame in which the angular velocity is expressed. Example: B2 G ω B1

= angular velocity of the body coordinate frame B1 with respect to the global coordinate frame G and expressed in body coordinate frame B2

Whenever the left subscript and superscript of an angular velocity are the same, we usually drop the left superscript. Example: G ωB

≡

G G ωB

• If the right subscript on a force vector is a number, it indicates the number of coordinate frame in a serial robot. Coordinate frame Bi is set up at joint i + 1. Example: Fi = force vector at joint i + 1 measured at the origin of Bi (oxyz) At joint i there is always an action force Fi , that link (i) applies on link (i + 1), and a reaction force −Fi , that link (i + 1) applies on link (i). On link (i) there is always an action force Fi−1 coming from link (i − 1), and a reaction force −Fi coming from link (i + 1). Action force is called driving force, and reaction force is called driven force. • If the right subscript on a moment vector is a number, it indicates the number of coordinate frames in a serial robot. Coordinate frame Bi is set up at joint i +1. Example: Mi = moment vector at joint i + 1 measured at the origin of Bi (oxyz) At joint i there is always an action moment Mi , that link (i) applies on link (i + 1), and a reaction moment −Mi , that link (i + 1) applies on link (i). On link (i) there is always an action moment Mi−1 coming from link (i − 1), and a reaction moment −Mi coming from link (i + 1). Action moment is called driving moment, and reaction moment is called driven moment.

xiv

Preface to the Third Edition

• Left superscript on derivative operators indicates the frame in which the derivative of a variable is taken. Example: G

G

d x dt

dB rP dt

B

dG rP dt B

If the variable is a vector function, and also the frame in which the vector is defined is the same as the frame in which a time derivative is taken, we use the following short notation, G B dG dB rP = G r˙ P rP = Bo r˙ P dt dt o and write equations simpler. Example: G

v=

G

d dt

G

r(t) =

G

r˙

If the vector is in different frame than the derivative frame, we have a mixed derivative. The result of a mixed derivative would be a vector indicated by two left indices. Its left subscript indicates the frame in which the derivative is taken, and its left superscript indicates the frame it is expressed in. Derivative operation will not change the expression frame. Example: G dB r = BG r˙ = BG v dt B

dG ˙= G r= G Br Bv dt • If followed by angles, lowercase c and s denote cos and sin functions in mathematical equations. Example: cα = cos α sϕ = sin ϕ • Capital bold letter I indicates a unit matrix, which, depending on the dimension of the matrix equation, could be a 3 × 3 or a 4 × 4 unit matrix. I3 or I4 are also being used to clarify the dimension of I. Example: ⎡

⎤ 100 I = I3 = ⎣ 0 1 0 ⎦ 001 • An asterisk  indicates a more advanced subject or example that is not designed for undergraduate teaching and can be dropped in the first reading. • Two parallel joint axes are indicated by a parallel sign, (). • Two orthogonal joint axes are indicated by an orthogonal sign, (). Two orthogonal joint axes are intersecting at a right angle. • Two perpendicular joint axes are indicated by a perpendicular sign, (⊥). Two perpendicular joint axes are at a right angle with respect to their common normal.

Preface to the Second Edition

Second edition of this novel would not be possible without comments and contribution of all my students, especially those at Columbia University in New York. New topics introduced in this edition result from students’ feedback which assisted me in clarifying and better presenting certain aspects of this book. The intent of this book is to explain robotics in a manner I would have liked explained to me as a student. This book can now help students by being a great reference that covers all aspects of robotics and providing students with detailed explanations and information. The first edition of this book was published in 2006 by Springer. Soon after its publication, the book become very popular in the field of robotics. It was appreciated by many students and instructors in addition to my own students and colleagues. Their questions, comments, and suggestion have helped me in creating the second edition.

xv

Preface to the First Edition

This book is designed to serve as a text for engineering students. It introduces the fundamental knowledge used in robotics. This knowledge can be utilized to develop computer programs for analyzing the kinematics, dynamics, and control of robotic systems. The subject of robotics may appear overdosed by the number of available texts because the field has been growing rapidly since 1970. However, the topic remains alive with modern developments, which are closely related to the classical material. It is evident that no single text can cover the vast scope of classical and modern materials in robotics. Thus the demand for new books arises because the field continues to progress. Another factor is the trend toward analytical unification of kinematics, dynamics, and control. Classical kinematics and dynamics of robots has its roots in the work of great scientists of the past four centuries who established the methodology and understanding of the behavior of dynamic systems. The development of dynamic science, since the beginning of the twentieth century, has moved toward analysis of controllable man-made systems. Therefore, merging the kinematics and dynamics with control theory is the expected development for robotic analysis. The other important development is the fast growing capability of accurate and rapid numerical calculations, along with intelligent computer programming.

Level of the Book This book has evolved from nearly a decade of research in nonlinear dynamic systems, and teaching undergraduate-graduate level courses in robotics. It is addressed primarily to the last year of undergraduate study and the first year graduate student in engineering. Hence, it is an intermediate textbook. This book can even be the first exposure to topics in spatial kinematics and dynamics of mechanical systems. Therefore, it provides both fundamental and advanced topics on the kinematics and dynamics of robots. The whole book can be covered in two successive courses however, it is possible to jump over some sections and cover the book in one course. The students are required to know the fundamentals of kinematics and dynamics, as well as a basic knowledge of numerical methods. The contents of the book have been kept at a fairly theoretical-practical level. Many concepts are deeply explained and their use emphasized, and most of the related theory and formal proofs have been explained. Throughout the book, a strong emphasis is put on the physical meaning of the concepts introduced. Topics that have been selected are of high interest in the field. An attempt has been made to expose the students to a broad range of topics and approaches.

Organization of the Book The text is organized so it can be used for teaching or for self-study. Chapter 1 “Introduction,” contains general preliminaries with a brief review of the historical development and classification of robots.

xvii

xviii

Preface to the First Edition

Part I “Kinematics,” presents the forward and inverse kinematics of robots. Kinematics analysis refers to position, velocity, and acceleration analysis of robots in both joint and base coordinate spaces. It establishes kinematic relations among the end-effecter and the joint variables. The method of Denavit-Hartenberg for representing body coordinate frames is introduced and utilized for forward kinematics analysis. The concept of modular treatment of robots is well covered to show how we may combine simple links to make the forward kinematics of a complex robot. For inverse kinematics analysis, the idea of decoupling, the inverse matrix method, and the iterative technique are introduced. It is shown that the presence of a spherical wrist is what we need to apply analytic methods in inverse kinematics. Part II “Dynamics,” presents a detailed discussion of robot dynamics. An attempt is made to review the basic approaches and demonstrate how these can be adapted for the active displacement framework utilized for robot kinematics in the earlier chapters. The concepts of the recursive Newton-Euler dynamics, Lagrangian function, manipulator inertia matrix, and generalized forces are introduced and applied for derivation of dynamic equations of motion. Part III “Control,” presents the floating time technique for time-optimal control of robots. The outcome of the technique is applied for an open-loop control algorithm. Then, a computedtorque method is introduced, in which a combination of feedforward and feedback signals are utilized to render the system error dynamics.

Method of Presentation The structure of presentation is in a “fact-reason-application” fashion. The “fact” is the main subject we introduce in each section. Then the reason is given as a “proof.” Finally the application of the fact is examined in some “examples.” The “examples” are a very important part of the book because they show how to implement the knowledge introduced in “facts.” They also cover some other facts that are needed to expand the subject. Melbourne, VIC, Australia

Reza N. Jazar

How to Use This Book

This book is suitable for the first course in robotics and is written for a full semester of 16 weeks at graduate level. If the level of the course is undergraduate or mixed, and if the length of semester is shorter than 16 weeks, then the following pattern can be suggested to fit the contents to different classes. Teaching Chapter 10, is optional in all cases depending on the background of students. The main topic of this chapter is to review dynamics and remind students the Lagrangean method of deriving equations of motion. Chapter 10 may be left for students to read by themselves. Undergraduate level, 10 or 12 weeks semester: Skip all asterisk sections and teach the following chapters: 1 2 3 4 5 6 8 11 12

Introduction Rotation Kinematics Orientation Kinematics Motion Kinematics Forward Kinematics Inverse Kinematics Velocity Kinematics Robot Dynamics Path Planning

Undergraduate level, 16 weeks semester: Skip all asterisk sections and teach the following chapters: 1 2 3 4 5 6 7 8 9 11 12

Introduction Rotation Kinematics Orientation Kinematics Motion Kinematics Forward Kinematics Inverse Kinematics Angular Velocity Velocity Kinematics Acceleration Kinematics Robot Dynamics Path Planning

Graduate level, 10 or 12 weeks semester: Skip asterisk sections of Chaps. 3, 4, 5, 7, 9, and 11 and teach the following chapters: 1 2 3 4

Introduction Rotation Kinematics Orientation Kinematics Motion Kinematics xix

xx

How to Use This Book

5 6 7 8 9 11 12

Forward Kinematics Inverse Kinematics Angular Velocity Velocity Kinematics Acceleration Kinematics Robot Dynamics Path Planning

Undergraduate level, 16 weeks semester: Skip asterisk sections based on your preference and teach the following chapters: 1 2 3 4 5 6 7 8 9 11 12 14

Introduction Rotation Kinematics Orientation Kinematics Motion Kinematics Forward Kinematics Inverse Kinematics Angular Velocity Velocity Kinematics Acceleration Kinematics Robot Dynamics Path Planning Control Techniques

Contents

1

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Historical Development . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Robot Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.1 Link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.2 Joint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.3 Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.4 Wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.5 End-Effector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.6 Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.7 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.8 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Robot Classifications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.1 Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.3 Actuation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.4 Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.5 Application . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 Robot’s Kinematics, Dynamics, and Control . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5 Principle of Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.1  Triad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.2 Unit Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.3 Orthogonality Condition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.4 Coordinate Frame and Transformation . . . . . . . . . . . . . . . . . . . . . . . 1.5.5  Vector Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5.6 Vector Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.7 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1 2 2 2 3 5 6 7 7 8 8 8 9 11 12 12 13 13 14 14 15 20 20 22 25 28 29 31

Part I Kinematics 2

Rotation Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Rotation About Global Cartesian Axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Successive Rotation About Global Cartesian Axes . . . . . . . . . . . . . . . . . . . . . 2.3 Rotation About Local Cartesian Axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Successive Rotation About Local Cartesian Axes . . . . . . . . . . . . . . . . . . . . . . 2.5 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.6 Local Axes Versus Global Axes Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.7 General Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.8  Active and Passive Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

37 37 43 52 55 59 69 71 79

xxi

xxii

Contents

2.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.10 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

80 82 83

3

Orientation Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Axis–Angle Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2  Order-Free Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3  Euler Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4  Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.5  Spinors and Rotators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6  Problems in Representing Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6.1  Rotation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6.2  Axis–Angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6.3  Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6.4  Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6.5  Euler Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.7  Composition and Decomposition of Rotations . . . . . . . . . . . . . . . . . . . . . . 3.7.1  Composition of Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.7.2  Decomposition of Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.9 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

91 91 102 109 117 124 126 127 127 128 129 130 132 132 134 137 139 140

4

Motion Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Rigid Body Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Homogenous Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Inverse and Reverse Homogenous Transformation . . . . . . . . . . . . . . . . . . . . . 4.4 Combined Homogenous Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5  Order-Free Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.6  Screw Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.7  Inverse Screw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.8  Combined Screw Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.9  The Plücker Line Coordinate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.10  The Geometry of Plane and Line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.10.1  Moment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.10.2  Angle and Distance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.10.3  Plane and Line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.11  Screw and Plücker Coordinate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.12 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.13 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

149 149 153 162 167 176 182 197 200 202 207 208 208 209 212 213 215 216

5

Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Denavit–Hartenberg Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Transformation Between Adjacent Coordinate Frames . . . . . . . . . . . . . . . . . . 5.3 Forward Position Kinematics of Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Spherical Wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 Assembling Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.6  Coordinate Transformation Using Screws . . . . . . . . . . . . . . . . . . . . . . . . . . 5.7  Non-Denavit–Hartenberg Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.9 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

225 225 232 247 269 276 289 293 299 300 302

Contents

xxiii

6

Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 Decoupling Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Inverse Transformation Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3  Iterative Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4  Comparison of the Inverse Kinematics Techniques . . . . . . . . . . . . . . . . . . 6.4.1  Existence and Uniqueness of Solution . . . . . . . . . . . . . . . . . . . . . 6.4.2  Inverse Kinematics Techniques . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5  Singular Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.7 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

313 313 329 343 347 347 347 347 349 350 352

Part II Derivative Kinematics 7

Angular Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 Angular Velocity Vector and Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2  Time Derivative and Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3 Rigid Body Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4  Velocity Transformation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.5 Derivative of a Homogenous Transformation Matrix . . . . . . . . . . . . . . . . . . . 7.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.7 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

361 361 376 388 392 400 405 407 409

8

Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1  Rigid Link Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 Forward Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3 Jacobian Generating Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.4 Inverse Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.5  Linear Algebraic Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.6 Matrix Inversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.7 Nonlinear Algebraic Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.8  Jacobian Matrix From Link Transformation Matrices . . . . . . . . . . . . . . . . 8.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.10 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

415 415 419 429 442 448 458 463 469 476 477 479

9

Acceleration Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.1 Angular Acceleration Vector and Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.2 Rigid Body Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3  Acceleration Transformation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.4 Forward Acceleration Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.5 Inverse Acceleration Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.6  Rigid Link Recursive Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.7  Second Derivative and Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . 9.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.9 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

489 489 508 510 518 520 526 534 543 545 547

xxiv

Contents

Part III Dynamics 10

Applied Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1 Force and Moment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1.1 Force and Moment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1.2 Momentum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1.3 Equation of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1.4 Work and Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.2 Rigid Body Translational Kinetics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.3 Rigid Body Rotational Kinetics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.4 Mass Moment Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.5 Lagrange’s Form of Newton’s Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.6 Lagrangian Mechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.8 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

557 557 557 558 558 559 565 567 576 584 591 597 600 602

11

Robot Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Rigid Link Newton–Euler Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2  Recursive Newton–Euler Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Robot Lagrange Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.4  Lagrange Equations and Link Transformation Matrices . . . . . . . . . . . . . . 11.5 Robot Statics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.7 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

609 609 626 632 659 667 673 676 678

Part IV Control 12

Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.1 Cubic Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.2 Polynomial Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.3  Non-polynomial Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.4  Spatial Path Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.5 Forward Path Robot Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.6 Inverse Path Robot Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.7  Rotational Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.9 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

687 687 692 702 705 708 712 720 724 725 726

13

 Time Optimal Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.1  Minimum Time and Bang-Bang Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2  Floating Time Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.3  Time Optimal Control for Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.5 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

731 731 738 746 752 753 754

Contents

xxv

14 Control Techniques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.1 Open- and Closed-Loop Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.2 Computed Torque Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.3 Linear Control Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.3.1 Proportional Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.3.2 Integral Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.3.3 Derivative Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4 Sensing and Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4.1 Position Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4.2 Speed Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4.3 Acceleration Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.6 Key Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

759 759 764 767 768 768 768 770 771 771 771 772 773 774

A

Global Frame Triple Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

777

B

Local Frame Triple Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

779

C

Principal Central Screws Triple Combination . . . . . . . . . . . . . . . . . . . . . . . . . . . .

781

D

Industrial Link DH Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

783

E

Matrix Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

791

F

Trigonometric Formula . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

797

G

Algebraic Formula . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

805

H

Unit Conversions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

807

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

809

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

815

About the Author

Reza N. Jazar is Professor of Mechanical Engineering. Reza received his PhD degree from Sharif University of Technology, and MSc and BSc from Tehran Polytechnic. His areas of expertise include nonlinear dynamic systems and applied mathematics. He obtained original results in non-smooth dynamic systems, applied nonlinear vibrating problems, time optimal control, and mathematical modeling of vehicle dynamics and stability. He has authored several monographs in vehicle dynamics, robotics, dynamics, vibrations, and mathematics and published numerous professional articles, as well as book chapters, in research volumes. Most of his textbooks have been adopted by many universities for teaching and research, and by many research agencies as standard model for research results. Dr. Jazar has the pleasure to work in several Canadian, American, Asian, Middle Eastern, and Australian universities, as well as several years in automotive industries all around the world. Working in different engineering firms and educational systems provided him a vast experience and knowledge to publish his researches on important topics in engineering and science. His unique style of writing helps readers to learn the topics deeply in an easy way.

xxvii

1

Introduction

Isaac Asimov’s Laws of Robotics: Law Zero: A robot may not harm humanity, or, by inaction, allow humanity to come to harm. Law One: A robot may not injure a human being, or, through inaction, allow a human being to come to harm, unless this would violate a higher order law. Law Two: A robot must obey orders given by human beings, except where such orders would conflict with a higher order law. Law Three: A robot must protect its own existence as long as such protection does not conflict with a higher order law.

Fig. 1.1 A high performance robot hand

ˇ The great Czech writer Karel Capek (1890–1938) is best known for his 1920 play Rossumovi univerzální roboti, RU R, (RU R: Rossum’s Universal Robots), in which he introduced the word “Robot” and “Robotess” to the world for the first time ˇ (Capek, 1994; Husbands et al., 2008). The word “robot” is of Slavic origin close to Russian, in which the word “pa6oTa” (rabota) means labor. Later, Isaac Asimov (1920–1992) adopted the term “Robot” and proposed Laws 1–3 in his short story “Runaround,” which was first published in the March 1942 issue of Astounding Science Fiction magazine (Asimov, 1942, 1950a). In 1985 Asimov added a Zeroth Law later, in “Robots and Empire,” (Asimov, 1950b). Isaac Asimov proposed these four refined laws of “Robotics” to protect us from intelligent generations of robots. These laws, although look logical, have been challenged by others in late 20th and early 21st centuries. In science and engineering, the term Robotics refers to the study and use of Robots. Based on definition of the Robotics Institute of America (RI A): “A robot is a reprogrammable multifunctional manipulator designed to move material, parts, tools, or specialized devices through variable programmed motions for the performance of a variety of tasks.” It means, Robots are similar to mechanisms but also capable to change the task they will be doing. Hence, Robot is a mechanism that we can change its operation task, usually by only a computer program. From an engineering point of view, Robots are complex, versatile devices that contain a mechanical structure, a sensory system, and an automatic control system. Theoretical fundamentals of Robotics rely on the results of research in mechanics, electric, electronics, automatic control, mathematics, and computer sciences. © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_1

1

2

1.1

1 Introduction

Historical Development

The first position controlling apparatus was invented around 1938 for spray painting. However, the first modern industrial Robot were the Unimates, made by Joseph Engelberger (1925–2015) in the early 1960s. Unimation was the first to market robots. Therefore, Engelberger has been called the father of Robotics. In the 1980s the robot industry grew very fast primarily because of the huge investments by the automotive industry (Rosheim, 1994). In research communities the first automata were probably William Grey Walter’s (1910–1977) machina in 1940s and the Johns Hopkins beast built in the 1960s. The first programmable Robot was designed by George Devol (1912–2011) in 1954. Devol funded Unimation. In 1959 the first commercially available Robot appeared on the market. Robotic manipulators were used in industries after 1960 and faced sky rocketing growth in the 1980s. Robots appeared as a result of combination of two technologies: teleoperators and computer numerical control (CNC) of milling machines. Teleoperators were developed during World War II to handle radioactive materials, and CNC was developed to increase the precision required in machining of new technologic parts. Therefore, the first Robots were numerical controlled mechanical linkages that were designed to transfer material from point A to B (Niku, 2020). Today, more complicated applications, such as welding, painting, and assembling, require much more motion capability and sensing. Hence, a Robot is a multi-disciplinary engineering device. Mechanical engineering deals with the design of mechanical components, arms, end-effectors, and also is responsible for kinematics, dynamics, and control analyses of Robots. Electrical engineering works on Robot actuators, sensors, power, and control systems. System design engineering deals with perception, sensing, and control methods of Robots. Programming, or software engineering, is responsible for logic, intelligence, communication, and networking. Today we have more than 1000 robotics-related organizations, associations, and clubs; more than 500 robotics-related magazines, journals, and newsletters; more than 100 robotics-related conferences and competitions every year; and more than 50 robotics-related courses in colleges. Robots find a vast amount of industrial applications and are used for various technological operations. Robots enhance labor productivity in industry and deliver relief from tiresome, monotonous, or hazardous works. Moreover, Robots perform many operations better than human do, and they provide higher accuracy and repeatability. In many engineering fields, high technological standards are hardly attainable without Robots. Apart from industry, Robots are used in extreme environments. They can work at low and high temperatures; they usually do not need lights, rest, fresh air, and more importantly they do not need salary, or promotions, and do not get sad. Robots are prospective machines whose application area is widening and their structures getting more complex. Figure 1.1 illustrates a high performance robot hand. It is claimed that robots appeared to perform in 4A for 4D, or 3D3H environments. 4A performances are Automation, Augmentation, Assistance, Autonomous; and 4D environments are Dangerous, Dirty, Dull, Difficult. 3D3H means Dull, Dirty, Dangerous, Hot, Heavy, Hazardous (Tsai, 1999; Veit, 1992).

1.2

Robot Components

We kinematically model a Robotic manipulator as a multibody system made of connected rigid bodies by revolute or prismatic joints that allow the bodies to relatively move. We apply rigid body kinematics to the connected bodies and show how to determine their relative motions. However, a Robot as a system also consists of a manipulator or rover, wrist, end-effector, actuators, sensors, controllers, processors, and software. Figure 1.2 illustrates the Shuttle arm, which is a robotic manipulator as a multibody system.

1.2.1

Link

Every individual rigid member of a robot that can move relative to all other members is called a link. In robotics we may use bar and arm and object equal to link. A robot arm or a robot link is a rigid member that is able to have relative motion with respect to all other links. Any two or more connected links, such that no relative motion can occur among them, are considered a single compound link.

1.2 Robot Components

3

Fig. 1.2 Shuttle arm is a robotic manipulator as a multibody system

Fig. 1.3 Illustration of revolute and prismatic joints

Fig. 1.4 Symbolic illustration of revolute joints in robotic models

1.2.2

Joint

Two rigid bodies that are permanently in contact with a possible relative motion is called a kinematic pair. Two links are connected by contact at a joint where their relative motion can be expressed by a single joint coordinate. Joints are typically revolute (rotary) or prismatic (translatory). Figure 1.3 depicts the geometric form of a revolute and a prismatic joint. A revolute joint (R) is like a hinge and allows relative rotation between two links. A prismatic joint (P ) allows a translation of relative motion between two links. Relative rotation of connected links by a revolute joint occurs about a line called axis of joint. Also, translation of two connected links by a prismatic joint occurs along a line also called axis of joint. The value of the coordinate describing the relative position of two connected links at a joint is called joint coordinate or joint variable. It is an angle for a revolute joint, and a distance for a prismatic joint. A symbolic illustration of revolute and prismatic joints is shown in Figs. 1.4a–c and 1.5a–c, respectively.

4

1 Introduction

Fig. 1.5 Symbolic illustration of prismatic joints in robotic models

Fig. 1.6 Joints of class 1 with one DOF : Revolute, R, Prismatic, P , and Screw, S

The coordinate of an active joint is controlled by an actuator. A passive joint does not have any actuator. The coordinate of a passive joint is a function of the coordinates of active joints and the geometry of the robot arms. Passive joints are also called inactive or free joints. Active joints are usually prismatic or revolute; however, passive joints may be any of the lower pair joints that provide surface contact. Revolute and prismatic joints are the most common joints that are utilized in serial robotic manipulators. Prismatic and revolute joints provide one degree of freedom. Therefore, the number of joints of a manipulator is the degrees-of-freedom (DOF ) of the manipulator. Typically a spacial manipulator possesses at least six DOF : three for positioning and three for orientation. A manipulator having more than six DOF is referred to as a kinematically redundant manipulator. Although prismatic and revolute joints are the most applied connections, there are other types of joints that are classified according to the number of degrees of freedom (DOF ) they eliminate or they allow. A joint can provide a maximum of five DOF and a minimum of one DOF . Proof When there is no contact between two bodies A and B, then B has six DOF with respect to A. Every permanent contact eliminates some rotational or translational DOF . All possible kinematic pairs can be classified by the number of rotational, R, or translational, T , degrees of freedom the contact provides. Class 1 are joints that provide one DOF between links A and B. To get one DOF we kinematically need to have five contact points between the two links. The class 1 joints and their kinematic models are shown in Fig. 1.6. There are three types of joints in class 1: Revolute, R, with a rotational freedom R1; Prismatic, P , with a translational freedom T 1; and Helical or Screw, S, with a proportional rotational-translational freedom R(T ). Class 2 are joints that provide two DOF between links A and B. To get two DOF we kinematically need to have four contact points between the two links. The class 2 joints and their kinematic models are shown in Fig. 1.7. There are three types of joints in class 2: Sphere in slot, with two rotational freedom R2; Cylindrical, C, with a coaxial rotational and a translational freedom R1T 1; and disc in slot, with a perpendicular rotational and translational freedom R1T 1. Class 3 are joints that provide three DOF between links A and B. To get three DOF we kinematically need to have three contact points between the two links. The class 3 joints and their kinematic models are shown in Fig. 1.8. There are three types of joints in class 3: Spherical, S, with three rotational freedom R3; Sphere in slot, with two rotational and a translational freedom R2T 1; and Disc in slot, with a rotational and two translational freedom R1T 2.

1.2 Robot Components

5

Fig. 1.7 Joints of class 2 with two DOF : Sphere in slot R2; Cylindrical, C, and Disc in slot R1T

Fig. 1.8 Joints of class 3 with three DOF : Spherical, S, Sphere in slot, R2T 1; and Disc in slot, R1T 2

Class 4 are joints that provide four DOF between links A and B. To get two DOF we kinematically need to have two contact points between the two links. The class 4 joints and their kinematic models are shown in Fig. 1.9. There are two types of joints in class 4: Sphere in slot, with three rotational and one translational freedom R3T 1, and Cylinder in slot, with two rotational and two translational freedom R2T 2. These joints provide four DOF between two links A and B. Class 5 are joints that provide five DOF between links A and B. To get one DOF we kinematically need to have one contact point between the two links. The kinematic model of class 5 joint is shown in Fig. 1.10. There is only one type of joints in class 5: Sphere on plane, with three rotational and two translational freedom R3T 2. The DOF of a mechanism f is f = 6n − j5 − 2j4 − 3j3 − 4j2 − 5j1 (1.1) where, n is the number of links and jk , k = 1, 2, 3, 4, 5, indicates the number of joints of class jk .

1.2.3



Manipulator

The main body of a robot consisting of the links, joints, and other structural elements is called the manipulator. A manipulator becomes a robot when we attach wrist and gripper, and install its control system. However, in literature robots and manipulators are utilized equivalently and both refer to robots. Figure 1.11 shows kinematic model of a 3R manipulator, and Fig. 1.16 illustrates a 3R manipulator.

6

1 Introduction

Fig. 1.9 Joints of class 4 with four DOF : sphere in slot R3T 1 and cylinder in slot R2T 2

Fig. 1.10 Joint of class 5 with five DOF : Sphere on plane R3T 2

Fig. 1.11 Model of a 3R manipulator

1.2.4

Wrist

The joints in the kinematic chain of a robot between the forearm and end-effector are referred to as the wrist. It is common to design manipulators with spherical wrists by which it means three revolute joint axes intersecting at a common point called the wrist point. Figure 1.12 shows kinematic model of a spherical wrist, and Fig. 1.13 illustrates a spherical wrist at rest position. It is made of three revolute joints with orthogonal axes of rotations and shown by RRR mechanism. The spherical wrist greatly simplifies the kinematic analysis by allowing us to decouple the positioning and orienting of the end-effector. Therefore, the manipulator will possess three degrees-of-freedom for position of the wrist point. Positioning is set by controlling three joints of three arms. The number of DOF for orientation will then depend on the wrist. We may design a wrist having one, two, or three DOF depending on the application.

1.2 Robot Components

7

Fig. 1.12 Model of a spherical wrist kinematics

Fig. 1.13 A spherical wrist kinematics

1.2.5

End-Effector

The end-effector is the part mounted on the last link to do the required job of the robot. The simplest end-effector is a gripper, which is usually capable of two actions: opening and closing. The arm and wrist assemblies of a robot are used primarily for positioning the end-effector and any tool it may carry. It is the end-effector that performs the work. A great deal of research is devoted to the design of special purpose end-effectors and tools. There is also extensive research on the development of anthropomorphic hands. Such hands have been developed for prosthetic use in manufacturing. Hence, a robot is composed of a manipulator or mainframe and a wrist plus an end-effector. The wrist and end-effector assembly is also called a hand. Figure 1.14 illustrates an end-effector.

1.2.6

Actuators

Actuators are drivers that act as muscles of robots to change their configuration. The actuators provide power to act on the mechanical structure against gravity, inertia, and other external forces to modify the geometric location and orientation of the robot’s hand. The actuators can be of electric, hydraulic, or pneumatic, and have to be controllable.

8

1 Introduction

Fig. 1.14 An end-effector

1.2.7

Sensors

The elements that are utilized to detect and collect information about internal and environmental states are sensors. According to the scope of this book, joints’ positions, velocities, accelerations, and forces are the most important information to be sensed and measured. Sensors, integrated into the robot, send information about each link and joint to the control unit, and the control unit determines the configuration of the robot.

1.2.8

Controller

The controller or control unit of a robot has three roles: 1. 1-Information role, which consists of collecting and processing the information provided by the robot’s sensors. 2. 2-Decision role, which consists of planning the geometric motion of the robot structure. 3. 3-Communication role, which consists of organizing the information between the robot and its environment. The control unit includes the processor and software.

1.3

Robot Classifications

The Robotics Institute of America (RIA) considers classes 3-6 of the following classification to be Robots, and the Association Francaise de Robotique (AFR) combines classes 2-4, as the same type and divides robots in 4 classes. However, the Japanese Industrial Robot Association divides robots in 6 different classes: Class 1: Manual handling devices: A device with multi degrees of freedom that is actuated by an operator. Class 2: Fixed sequence robot: A device that performs successive stages of a task according to a predetermined and fixed program. Class 3: Variable sequence robot: A device that performs successive stages of a task according to a predetermined but programmable method. Class 4: Playback robot: A human operator performs the task manually by leading the robot, which records the motions for later playback. The robot repeats the same motions according to the recorded information. Class 5: Numerical control robot: The operator supplies the robot with a motion program rather than teaching it the task manually. Class 6: Intelligent robot: A robot with the ability to understand its environment and the ability to successfully complete a task despite changes in the surrounding conditions under which it is to be performed. Other than these official classifications, robots can be classified by other criteria such as geometry, workspace, actuation, control, and application.

1.3 Robot Classifications

1.3.1

9

Geometry

A robot is called a serial or open-loop manipulator if its kinematic structure does not make a loop chain. It is called a parallel or closed-loop manipulator if its structure makes a loop chain. A robot is a hybrid manipulator if its structure consists of both open and closed-loop chains. As a mechanical system, we may think of a robot as a set of rigid bodies connected together at some joints. The joints can be either revolute (R) or prismatic (P ), because all other kinds of joint can be modeled as a combination of these two simple joints. Most industrial manipulators have six DOF . The open-loop manipulators can be classified based on their first three joints starting from the grounded joint. Using the two types of joints, there are mathematically 72 different industrial manipulator configurations, because each joint can be P or R, and the axes of two adjacent joints can be parallel (), orthogonal (), or perpendicular (⊥). Two orthogonal joint axes intersect at a right angle; however, two perpendicular joint axes are in rightangle with respect to their common normal. Two perpendicular joint axes become parallel if one axis turns 90 deg about the common normal. Two perpendicular joint axes become orthogonal if the length of their common normal tends to zero. Out of the 72 possible manipulators, the important ones are RRP (SCARA), RRR (articulated), RR⊥P (spherical), RPP (cylindrical), and PPP (Cartesian). Proof Most of the industrial robots are made of a wrist with tree rotational degrees-of-freedom to be attached to the tip point of a manipulator with three degrees-of-freedom. The duty of the manipulator is to move the tip point to a given coordinate in the 3D space. Starting from the ground, the first link of the manipulator is connected to the ground by the first joint, which may be a P or an R joint. The second link of the manipulator will be connected to the first link by the second joint P or R whose axis may be parallel (), orthogonal (), or perpendicular (⊥) to the axis of the first joint. The third link will be attached to the second link by a joint R or P whose axis may also be parallel (), orthogonal (), or perpendicular (⊥) to the axis of the second joint. Hence, a 3D industrial manipulator will be made of three links and two joints. Considering every joint with two options, P or R, every two axes with three options, parallel (), orthogonal (), or perpendicular (⊥), we will have a total of 72 configurations. 2 × 3 × 2 × 3 × 2 = 72 (1.2) Therefore, we show a manipulator by the type of the first joint, axis type of first and second joint, the type of second joint, axis type of second and third joint, followed by the type of the third joint. As an example, an articulated manipulator will be shown by RRR, because it is made by three links. The first link will be connected to the ground by a revolute joint R. The second link is connected to the first one whose axis is orthogonal to the axis of the first joint, R  R. The third link will be connected to the second link via another revolute joint whose axis is parallel to the axis of the second joint, R  R  R.  1. 2RP manipulator, RRP The SCARA arm (Selective Compliant Articulated Robot for Assembly) shown in Fig. 1.15 is a popular manipulator, which, as its name suggests, is made for assembly operations. 2. Articulated manipulator, RRR The RRR configuration, illustrated in Figs. 1.11 and 1.16, is called elbow, revolute, articulated, or anthropomorphic. It is a suitable configuration for industrial robots. Almost 25% of industrial robots, P U MA for instance, are made of this kind. 3. Spherical manipulator, RR⊥P The spherical manipulator is a suitable configuration for small robots. Almost 15% of industrial robots, Stanford arm for instance, are made of this configuration. The RR⊥P configuration is illustrated in Fig. 1.17. By replacing the third joint of an articulate manipulator with a prismatic joint, we obtain a spherical manipulator. The term spherical manipulator derives from the fact that the spherical coordinates define the position of the end-effector with respect to the base frame. Figure 1.18 schematically illustrates the Stanford arm, one of the most well-known spherical robots. 4. Cylindrical manipulator, RPP The cylindrical manipulator is a suitable configuration for medium load capacity robots. Almost 45% of industrial robots are made of this kind. The RPP configuration is illustrated in Fig. 1.19. The first joint of a cylindrical manipulator is revolute and produces a rotation about the base, while the second and third joints are prismatic. As the name suggests, the joint variables are the cylindrical coordinates of the end-effector with respect to the base.

10

1 Introduction

Fig. 1.15 An RRP manipulator

Fig. 1.16 Structure and terminology of an RRR elbow manipulator

Fig. 1.17 The RR⊥P spherical configuration of robotic manipulators

1.3 Robot Classifications

11

Fig. 1.18 Illustration of Stanford arm; an RR⊥P spherical manipulator

Fig. 1.19 The RP⊥P configuration of robotic manipulators

5. Cartesian manipulator, PPP The Cartesian configuration is a suitable configuration for heavy load capacity and large robots. Almost 15% of industrial robots are made of this configuration. The PPP configuration is illustrated in Fig. 1.20. For a Cartesian manipulator, the joint variables are the Cartesian coordinates of the end-effector with respect to the base. The kinematic description of this manipulator is the simplest of all manipulators. Cartesian manipulators are useful for table-top assembly applications and, as gantry robots, for transfer of cargo.

1.3.2

Workspace

The workspace of a manipulator is the total volume of space the end-effector can reach. The workspace is constrained by the geometry of the manipulator as well as the mechanical constraints on the joints. The workspace is broken into a reachable

12

1 Introduction

Fig. 1.20 The PPP Cartesian configuration of robotic manipulators

workspace and a dexterous workspace. The reachable workspace is the volume of space within which every point is reachable by the end-effector in at least one orientation. The dexterous workspace is the volume of space within which every point can be reached by the end-effector in all possible orientations. The dexterous workspace is a subset of the reachable workspace. Most of the open-loop chain manipulators are designed with a wrist subassembly attached to the main three links assembly. Therefore, the first three links are long and are utilized for positioning while the wrist is utilized for control and orientation of the end-effector. This is why the subassembly made by the first three links is the arm, and the subassembly made by the other links is the wrist (Shahinpoor, 1987).

1.3.3

Actuation

Actuators translate power into motion. Robots are usually actuated electrically, hydraulically, or pneumatically. Other types of actuation might be considered as piezoelectric, magnetostriction, shape memory alloy, and polymeric. Electrically actuated robots are powered by AC or DC motors and are considered the most acceptable actuators. They are cleaner, quieter, and more precise compared to the hydraulic and pneumatic actuators. Electric motors are efficient at high speeds so a high ratio gearbox is needed to reduce the high speed. Non-backdrivability and self-braking is an advantage of high ratio gearboxes in case of power loss. However, when high speed or high load-carrying capabilities are needed, electric drivers are unable to compete with hydraulic drivers. Hydraulic actuators are satisfactory because of high speed and high torque/mass or power/mass ratios. Therefore, hydraulic driven robots are used primarily for lifting heavy loads. Negative aspects of hydraulics, besides their noisiness and tendency to leak, include a necessary pump and other hardware. Pneumatic actuated robots are inexpensive and simple but cannot be controlled precisely. Besides the lower precise motion, they have almost the same advantages and disadvantages as hydraulic actuated robots.

1.3.4

Control

Robots can be classified by control method into servo (closed-loop control) and non-servo (open-loop control) robots. Servo robots use closed-loop computer control to determine their motion, and hence, they are capable of being truly multifunctional reprogrammable devices. Servo controlled robots are further classified according to the method that the controller uses to guide the end-effector. The simplest type of a servo robots is the point-to-point robot. A point-to-point robot can be taught a discrete set of points, called control points, but there is no control on the path of the end-effector in between the points. On the other hand, in continuous path robots, the entire path of the end-effector can be controlled. For example, the robot end-effector can be taught to follow a straight line between two points or even to follow a contour such as a welding seam. In addition, the velocity and/or acceleration of the end-effector can often be controlled. These are the most advanced robots and require the most sophisticated computer controllers and software development. Non-servo robots are essentially open-loop devices whose movement is limited to predetermined mechanical stops, and they are primarily used for material transfer.

1.4 Robot’s Kinematics, Dynamics, and Control

1.3.5

13

Application

Regardless of size, robots can mainly be classified according to their application into assembly and non-assembly robots. However, in the industry they are classified by the category of application such as machine loading, pick and place, welding, painting, assembling, inspecting, sampling, manufacturing, biomedical, assisting, remote controlled mobile, and telerobot. According to design characteristics, most industrial robot arms are anthropomorphic, in the sense that they have a “shoulder,” (first two joints) an “elbow,” (third joint), and a “wrist” (last three joints). Therefore, in total, they usually have six degrees of freedom needed to put an object in any position and orientation. Most manufacturers of commercial serial manipulators try to use revolute joints more than prismatic. Compared to prismatic joints, revolute joints cost less and provide a larger dextrous workspace for the same robot volume. Serial robots are very heavy; compared to the maximum load they can move without losing their accuracy. Their useful load-to-weight ratio is less than 1/10. The robots are so heavy because the links must be stiff in order to work rigidly. Simplicity of the forward and inverse position and velocity kinematics has always been one of the major design criteria for industrial manipulators. Hence, almost all of them have a special kinematic structure.

1.4

Robot’s Kinematics, Dynamics, and Control

The forward kinematics problem is when the kinematical data are known for the joint coordinates and are utilized to find the data in the base Cartesian coordinate frame. The inverse kinematics problem is when the kinematics data are known for the end-effecter in Cartesian space and the kinematic data are needed in joint space. Inverse kinematics is highly nonlinear and usually a much more difficult problem than the forward kinematics problem. The inverse velocity and acceleration problems are linear, and much simpler, once the inverse position problem has been solved (Chernousko et al., 1994). Kinematics, which is the English version of the French word cinématique from the Greek κ ı´υημα (movement), is a branch of science that analyzes motion with no attention to what causes the motion. By motion we mean any type of displacement, which includes changes in position and orientation. Therefore, displacement, and its successive derivatives with respect to time, velocity, acceleration, and jerk, all are included in kinematics (Erdman, 1993; Dugas, 1995). Positioning is to bring the end-effector to an arbitrary point within dextrose, while orientation is to move the end-effector to the required orientation at the position. The positioning is the job of the arm, and orientation is the job of the wrist. To simplify the kinematic analysis, we may decouple the positioning and orientation of the end-effector. In terms of the kinematic formation, a 6 DOF robot comprises six sequential moveable links and six joints. Generally speaking, almost all problems of kinematics can be interpreted as a vector addition. However, every vector in a vectorial equation must be transformed and expressed in a common reference frame. Therefore, reference and coordinate frames transformation is the main part of kinematic analysis of robots. Dynamics is the study of systems that undergo changes of state as time evolves. In mechanical systems such as robots, the change of states involves motion. Derivation of the equations of motion for the system is the main step in dynamic analysis of the system, as the equations of motion are essential in the design, analysis, and control of the system. The dynamic equations of motion describe dynamic behavior of a robot. They can be used for computer simulation of the robot’s motion, design of suitable control equations, and evaluation of the dynamic performance of the design. Similar to kinematics, the problem of robot dynamics may be considered as direct and inverse dynamics problems. In direct dynamics, we should predict the motion of the robot for a given set of initial conditions and torques at active joints. In the inverse dynamics problem, we should compute the forces and torques necessary to generate the prescribed trajectory for a given set of positions, velocities, and accelerations. The robot control problem may be characterized as the desired motion of the end-effector. Such a desired motion is specified as a trajectory in Cartesian coordinates while the control system requires input in joint coordinates (Fahimi, 2009). Sensors generate data to find the actual state of the robot at joint space. This implies a requirement for expressing the kinematic variables in Cartesian space to be transformed into their equivalent joint coordinate space. These transformations are highly dependent on the kinematic geometry of the manipulator. Hence, the robot control comprises three computational problems (Aspragathos and Dimitros, 1998): 1. Determination of the trajectory in Cartesian coordinate space. 2. Transformation of the Cartesian trajectory into equivalent joint coordinate space. 3. Generation of the motor torque commands to realize the trajectory.

14

1.5

1 Introduction

Principle of Kinematics

Kinematics is the study of positions, velocities, and accelerations, regardless of the forces that cause these motions. Vectors and reference frames are essential tools for analyzing motions of complex systems, especially when the motion is three dimensional and involves many parts. Kinematics is the language with which scalars, vectors, and tensors are expressed, transformed, and communicated. To make the science of kinematics, we need to define space, axes, scales, coordinate frames, and unit vectors. The communication between coordinate frames will be covered in next part of the book.

1.5.1

 Triad

To indicate the position of a point P relative to another point O in a three dimensional (3D) space, we need to establish a coordinate frame and provide three relative coordinates. The three coordinates are scalar functions and can be used to define a position vector and derive other kinematic characteristics. Take any four non-coplanar points O, A, B, C. The triad OABC is defined by three lines OA, OB, OC forming a rigid body. Rotate OB about O in the plane OAB such that the angle AOB becomes 90 deg, the direction of rotation of OB being such that OB moves through an angle less than 90 deg. Next, rotate OC about a perpendicular line in AOB until OC becomes perpendicular to the plane AOB, in such a way that OC moves through an angle less than 90 deg. Calling now the new position of OABC an orthogonal triad. Any other orthogonal triad can be superposed on the OABC. Given an orthogonal triad OABC, another triad OA BC may be derived by moving A to the other side of O to make the opposite triad OA BC. All orthogonal triads can be superposed either on a given orthogonal triad OABC or on its opposite OA BC. One of the two triads OABC and OA BC is defined as the positive triad and used as standard triad. The other is then defined as negative triad. It is immaterial which one is chosen as positive; however, usually the right-handed convention is chosen as positive, the one for which the direction of rotation from OA to OB propels a right-handed screw in the direction OC. A right-handed (positive) orthogonal triad cannot be superposed to a left-handed (negative) triad. Thus there are just two essentially distinct types of triad. This is an essential property of three dimensional space. We use an orthogonal triad OABC with scaled lines OA, OB, OC to locate a point in a 3D space. When the three lines OA, OB, OC have scales, then such a triad is called a coordinate frame. Every moving link of a robot caries a moving frame that is rigidly attached to the link and moves with the link. A moving frame may also be called a body frame or local frame. The body frame accepts every motion of the link. The position and orientation of a link with respect to other frames are expressed by the position and orientation of its local coordinate frame. When there are several relatively moving coordinate frames, we choose one of them as a reference frame in which we express motions of other local frames and measure kinematic information. The motion of a body may be observed and measured in different reference frames; however, we usually compare the motion of different links in the global reference frame. A global reference frame is assumed to be motionless and attached to the ground (Jazar, 2011). Example 1  Cyclic interchange of letters. Symmetric equations. Cyclic interchanging of the letters ABC in any orthogonal triad OABC produces another orthogonal triad superposable on the original triad. Cyclic interchanging means relabeling A as B, B as C, and C as A or selecting any three consecutive letters from ABCABCABC · · · . Instead of letters, A, B, C, we may use numbers 1, 2, 3 to indicated axes of triads. Then, assigning the axes of an orthogonal trial by any three consecutive numbers from 123123123 · · · , makes a positive orthogonal trial. When an equation is invariant by cyclic interchanging of the letters or numbers, it is called a symmetric equation. Cyclic interchanging simplifies expression of equations. Example 2 Right-hand rule. Examples and origin. The right-hand rule states: If we indicate the OC axis of an orthogonal triad by the thumb of the right hand, the other fingers should turn from OA to OB to close our fist. A right-handed triad is identified by the right-hand rule. The right-hand rule also shows the rotation of Earth when the thumb of the right hand indicates the north pole. Point your right thumb to the center of a clock, then the other fingers simulate the rotation of the clock’s hands. Point your index finger of the right hand in the direction of an electric current. Then point your middle finger in the direction of the magnetic field. Your thumb now points the direction of the magnetic force.

1.5 Principle of Kinematics

15

Fig. 1.21 A mobius strip

If the thumb, index finger, and middle finger of the right hand are held so that they form three right angles, then the thumb indicates the Z-axis when the index finger indicates the X-axis and the middle finger the Y -axis. The right-hand rule was introduced by English electrical engineer and physicist Sir John Ambrose Fleming (1849–1945) in the late nineteenth century for use in electromagnetism.

Example 3  Independent orthogonal coordinate frames. Having only two types of orthogonal triads in 3D space is associated with the fact that a plane has only two sides. In other words, there are two opposite normal directions to a plane. This may also be interpreted as: We can arrange the letters A, B, and C in only two orders when cyclic interchange is allowed: ABC

ACB

(1.3)

In a 4D space, there are six cyclic orders for four letters A, B, C, and D: ABCD, ABDC, ACBD, ACDB, ADBC, ADCB

(1.4)

So, there are six different tetrads in a 4D space. In an nD space there are (n − 1)! cyclic orders for n letters, so there are (n − 1)! different coordinate frames in an nD space. These theories are only valid in Cartesian n dimensional space, which is considered as linear Euclidean space. NonNewtonian spaces are nonlinear and the number of possible independent coordinate frames are different (Jazar, 2011). As an example, there is only one possible coordinate frame on a Mobius strip or Mobius surface, which has only one side. Figure 1.21 illustrates a Mobius strip plotted by the following parametric equations (Fosdick and Fried, 2016):  α β x = 1 + cos cos β 2 2  α β y = 1 + cos sin β 2 2 z=

1.5.2

α β sin 2 2

(1.5) (1.6) (1.7)

Unit Vectors

Figure 1.22 illustrates a positive orthogonal triad OABC. We select a unit length and define a directed line ıˆ on OA with the −−→ unit length. A point P1 on OA is at a distance x from O such that the directed line OP1 from O to P1 is x times ıˆ, indicated −−→ by OP1 = xˆı . The directed line ıˆ is called a unit vector on OA, the unit length is called the scale, point O is called the origin, −−→ and the real number x is called the ıˆ-coordinate of P1 . The distance x may also be called the ıˆ measure number of OP1 .

16

1 Introduction

ˆ and a position vector r with components x, y, z Fig. 1.22 A positive orthogonal triad OABC, unit vectors ıˆ, jˆ, k,

Similarly, we define the unit vectors jˆ on OB and kˆ on OC and use y and z as their coordinates, respectively. Although it is not necessary, we usually use the same scale for ıˆ, jˆ, kˆ and refer to OA, OB, OC by ıˆ, jˆ, kˆ as well as x, y, z. The scalar coordinates x, y, z are respectively the length of projections of a point P on OA, OB, OC and may be called the components −→ of OP . The components x, y, z are independent and we may vary any of them while keeping the others unchanged. Hence, a scaled positive orthogonal triad with unit vectors ıˆ, jˆ, kˆ is called an orthogonal coordinate frame. The position of a point P with respect to O is defined by three coordinates x, y, z and is shown by a position vector r = rP : r = rP = xˆı + y jˆ + zkˆ

(1.8)

We indicate coordinate frames by a capital letter, such as G and B, to clarify the coordinate frame in which the vector r is expressed. We show the name of the frame as a left superscript to the vector name. B

r = xˆı + y jˆ + zkˆ

(1.9)

A vector r is expressed in a coordinate frame B only if its unit vectors ıˆ, jˆ, kˆ belong to the axes of B-frame. If necessary, we also use a left superscript B and show the unit vectors as B ıˆ, B jˆ, B kˆ to indicate that ıˆ, jˆ, kˆ belong to B-frame: B

r = x B ıˆ + y B jˆ + z B kˆ

(1.10)

However, we may drop the frame indicator superscript as long as we have only one coordinate frame. The distance between O and P is a scalar number r that is called the length, magnitude, modulus, norm, or absolute value of the vector r:

r = |r| = x 2 + y 2 + z2 (1.11) We may also define a new unit vector uˆ r on r and show r by r = r uˆ r

(1.12)

The equation r = r uˆ r is called the natural expression of vector r, while the equation r = xˆı + y jˆ + zkˆ is called the ˆ From Eqs. (1.11) and (1.12) we have decomposition or decomposed expression of vector r over the axes ıˆ, jˆ, k. uˆ r =

xˆı + y jˆ + zkˆ xˆı + y jˆ + zkˆ =

r x 2 + y 2 + z2

=

x x 2 + y 2 + z2

ıˆ +

y x 2 + y 2 + z2

jˆ +

z x 2 + y 2 + z2

kˆ

(1.13)

1.5 Principle of Kinematics

17

ˆ Because the length of uˆ r is unity, the components of uˆ r are the cosines of the angles α 1 , α 2 , α 3 between uˆ r and ıˆ, jˆ, k, respectively: cos α 1 =

x x =

2 r x + y 2 + z2

(1.14)

cos α 2 =

y y =

2 r x + y 2 + z2

(1.15)

cos α 3 =

z z =

2 r x + y 2 + z2

(1.16)

The cosines of the angles α 1 , α 2 , α 3 are called the directional cosines of uˆ r , which, as is shown in Fig. 1.22, are the same as the directional cosines of any other vector on the same axis as uˆ r , including r. Equations (1.14)–(1.16) indicate that the three directional cosines are related by the equation cos2 α 1 + cos2 α 2 + cos3 α 3 = 1

(1.17)

ˆ we have From definition of the unit vectors ıˆ, jˆ, k, ıˆ2 = 1

jˆ2 = 1

kˆ 2 = 1

(1.18)

Moreover, based on the right-hand rule and definition of vector inner and outer product we have jˆ × kˆ = ıˆ

kˆ × ıˆ = jˆ

ıˆ × jˆ = kˆ

(1.19)

ıˆ · jˆ = 0

jˆ · kˆ = 0

kˆ · ıˆ = 0

(1.20)

Employing the inner product relationship,  r · ıˆ = xˆı + y jˆ + zkˆ · ıˆ = x

(1.21)

 r · jˆ = xˆı + y jˆ + zkˆ · jˆ = y

(1.22)

 r · kˆ = xˆı + y jˆ + zkˆ · kˆ = z

(1.23)

we may show any vector r in the following form: ˆ kˆ r = (r · ıˆ)ˆı + (r · jˆ)jˆ + (r · k)

(1.24)

Vector addition is the key operation in kinematics. Vectors can be added only when they are expressed in the same frame. Thus, a vector equation such as a=b+c (1.25) is meaningless without indicating the frame they are expressed in. B

a=

B

b + Bc

(1.26)

Example 4 Position vector of a point P . Length, directional cosines. Assume a point P with coordinates x = 1, y = 2, z = 3. The position vector of P is r = ıˆ + 2jˆ + 3kˆ

(1.27)

18

1 Introduction

The distance between O and P is r = |r| =

√ 12 + 22 + 32 = 14 3.7416

(1.28)

and the unit vector uˆ r on r is x y z 1 2 2 ˆ ıˆ + jˆ + kˆ = ıˆ + jˆ + k r r r 3.7416 3.7416 3.7416 = 0.26726ˆı + 0.53452jˆ + 0.80178kˆ

uˆ r =

(1.29)

The directional cosines of uˆ r are x = 0.26726 r y cos α 2 = = 0.53452 r z cos α 3 = = 0.80178 r cos α 1 =

(1.30)

and therefore, the angles between r and the x, y, z axes are: x = cos−1 0.26726 = 1.30025 rad ≈ 74.498 deg r y α 2 = cos−1 = cos−1 0.53452 = 1.00685 rad ≈ 57.688 deg r z −1 α 3 = cos = cos−1 0.80178 = 0.64052 rad ≈ 36.699 deg r α 1 = cos−1

(1.31)

Example 5  Inner and outer products of two vectors. Scalar product, vector product, right-hand rule. We may prove the result of the inner and outer products of two vectors by using decomposed expression and expansion:   r1 · r2 = x1 ıˆ + y1 jˆ + z1 kˆ · x2 ıˆ + y2 jˆ + z2 kˆ = x1 x2 ıˆ · ıˆ + x1 y2 ıˆ · jˆ + x1 z2 ıˆ · kˆ +y1 x2 jˆ · ıˆ + y1 y2 jˆ · jˆ + y1 z2 jˆ · kˆ +z1 x2 kˆ · ıˆ + z1 y2 kˆ · jˆ + z1 z2 kˆ · kˆ = x 1 x 2 + y 1 y 2 + z1 z2 r1 · r2 = |r1 | |r2 | cos θ

(1.32) (1.33)

  r1 × r2 = x1 ıˆ + y1 jˆ + z1 kˆ × x2 ıˆ + y2 jˆ + z2 kˆ = x1 x2 ıˆ × ıˆ + x1 y2 ıˆ × jˆ + x1 z2 ıˆ × kˆ +y1 x2 jˆ × ıˆ + y1 y2 jˆ × jˆ + y1 z2 jˆ × kˆ +z1 x2 kˆ × ıˆ + z1 y2 kˆ × jˆ + z1 z2 kˆ × kˆ = (y1 z2 − y2 z1 ) ıˆ + (x2 z1 − x1 z2 ) jˆ + (x1 y2 − x2 y1 ) kˆ r1 × r2 = nˆ |r1 | |r2 | sin θ

(1.34) (1.35)

1.5 Principle of Kinematics

19

where, θ is the angle between r1 and r2 , and nˆ is a unit vector, perpendicular to the plane of (r1 , r2 ). We may also find the outer product of two vectors by expanding a determinant and derive the same result as Eq. (1.34):

ıˆ jˆ kˆ

r1 × r2 =

x1 y1 z1

x 2 y 2 z2

(1.36)

The outer vector product has the following three laws: 1. Antisymmetric law: r1 × r2 = −r2 × r1

(1.37)

2. Associative law for scalar multiplication: (cr1 × r2 ) = c (r1 × r2 )

(1.38)

r1 × (r2 + r3 ) = r1 × r2 + r1 × r3

(1.39)

3. Distributive law:

Example 6  bac-cab rule. Expansion of vector triple product, a × (b × c). Lagrange formula. If a, b, c are three vectors, we may expand their triple cross product and show that a × (b × c) = b (a · c) − c (a · b)

(1.40)



ıˆ jˆ kˆ

a

a a a × (b × c) =



1



2



3



b2 b3 b3 b1 b1 b2

c c c c c c

2 3 3 1 1 2

(1.41)

because ⎡

⎤ ⎛⎡ ⎤ ⎡ ⎤⎞ a1 b1 c1 ⎣ a2 ⎦ × ⎝⎣ b2 ⎦ × ⎣ c2 ⎦⎠ a3 b3 c3 ⎤ ⎡ a2 (b1 c2 − b2 c1 ) + a3 (b1 c3 − b3 c1 ) = ⎣ a3 (b2 c3 − b3 c2 ) − a1 (b1 c2 − b2 c1 ) ⎦ −a1 (b1 c3 − b3 c1 ) − a2 (b2 c3 − b3 c2 )  =

b1 (a1 c1 + a2 c2 + a3 c3 ) − c1 (a1 b1 + a2 b2 + a3 b3 ) b2 (a1 c1 + a2 c2 + a3 c3 ) − c2 (a1 b1 + a2 b2 + a3 b3 ) b3 (a1 c1 + a2 c2 + a3 c3 ) − c3 (a1 b1 + a2 b2 + a3 b3 )



(1.42) Equation (1.40) may be referred to as the bac-cab rule, which makes it easy to remember. It is also called Lagrange formula for vector triple product. The bac-cab rule is the most important equation in 3D vector algebra. It is the key to prove a great number of other theorems. This identity can be generalized to n dimensions space. a2 × · · · × an−1 × (b1 × · · · × bn−1 )

b1 ··· ··· bn−1

a · b · · · · · · a2 · bn−1

= (−1)n−1

2 1

···

··· ··· ···

an−1 · b1 · · · · · · an−1 · bn−1

(1.43)

The vector triple product also makes Jacobi symmetric identity by cyclic interchanging. a × (b × c) = b × (c × a) = c × (a × b)

(1.44)

20

1 Introduction

1.5.3

Orthogonality Condition

Orthogonal coordinate frames are the most important type of coordinates and usually the only type we use in Robotics. It is compatible to our everyday life and our sense of dimensions. There is an orthogonality condition that is the principal equation to express any vector in an orthogonal coordinate frame. Consider a coordinate system (Ouvw) with unit vectors uˆ u , uˆ v , uˆ w . The condition for the coordinate system (Ouvw) to be orthogonal is that uˆ u , uˆ v , uˆ w are mutually perpendicular and hence, uˆ u · uˆ v = 0 uˆ v · uˆ w = 0

(1.45)

uˆ w · uˆ u = 0 In an orthogonal coordinate system, every vector r can be shown in its decomposed expression as r = (r · uˆ u )uˆ u + (r · uˆ v )uˆ v + (r · uˆ w )uˆ w

(1.46)

We call Eq. (1.46) the orthogonality condition of the coordinate system (Ouvw). The orthogonality condition for a Cartesian coordinate system reduces to ˆ kˆ r = (r · ıˆ)ˆı + (r · jˆ)jˆ + (r · k) (1.47) Proof Assume that the coordinate system (Ouvw) is an orthogonal frame. Using the unit vectors uˆ u , uˆ v , uˆ w and the components u, v, w, we can show any vector r in the coordinate system (Ouvw) as r = u uˆ u + v uˆ v + w uˆ w

(1.48)

Because of orthogonality, we have uˆ u · uˆ v = 0

uˆ v · uˆ w = 0

uˆ w · uˆ u = 0

(1.49)

Therefore, the inner product of r by uˆ u , uˆ v , uˆ w would be equal to     r · uˆ u = u uˆ u + v uˆ v + w uˆ w · 1uˆ u + 0uˆ v + 0uˆ w = u     r · uˆ v = u uˆ u + v uˆ v + w uˆ w · 0uˆ u + 1uˆ v + 0uˆ w = v     r · uˆ v = u uˆ u + v uˆ v + w uˆ w · 0uˆ u + 0uˆ v + 1uˆ w = w

(1.50)

Substituting for the components u, v, and w in Eq. (1.48), we may show the vector r as r = (r · uˆ u )uˆ u + (r · uˆ v )uˆ v + (r · uˆ w )uˆ w

(1.51)

ˆ and therefore, If vector r is expressed in a Cartesian coordinate system, then uˆ u = ıˆ, uˆ v = jˆ, uˆ w = k, ˆ kˆ r = (r · ıˆ)ˆı + (r · jˆ)jˆ + (r · k) The orthogonality condition is the most important reason for employing orthogonal coordinate systems.

1.5.4

(1.52) 

Coordinate Frame and Transformation

In robotics, we attach at least one coordinate frame to every link of a robot and a coordinate frame to every object of the robot’s environment. The relative positions and motions of the links are solely calculated by position and orientation of

1.5 Principle of Kinematics

21

Fig. 1.23 Position vector of a point P may be decomposed in body frame B, or global frame G

their coordinate frames. Communication among the coordinate frames, which is called transformation of frames, is the fundamental concept in the analysis of kinematics, dynamics, modeling, and programming of a robot. A fixed frame is a motionless reference frame that is attached to the ground. The motion of a robot takes place in the fixed frame called the global reference frame. A moving frame is a coordinate frame that moves with a link. Every moving link has an attached coordinate frame that sticks to the link and accepts every motion of the link. The moving coordinate frame is called the local coordinate frame. The position and orientation of a link with respect to the ground is expressed by the position and orientation of its local coordinate frame in the global coordinate frame. We use “reference frame,” “coordinate frame,” and “coordinate system” equivalently, because a Cartesian system is the only system we will use. A coordinate system is different from coordinate frames. A coordinate system determines the way we describe the motion in each coordinate frame. A Cartesian system is the most popular coordinate system used in robotics, but cylindrical, spherical, and other systems may also be used. The position of a point P of a rigid body B is indicated by a vector r. As shown in Fig. 1.23, the position vector of P can be decomposed either in global coordinate frame G

r = XIˆ + Y Jˆ + Z Kˆ

(1.53)

r = xˆı + y jˆ + zkˆ

(1.54)

or in body coordinate frame. B

The coefficients (X, Y, Z) and (x, y, z) are called coordinates or components of the point P in global and local coordinate frames, respectively. It is efficient for mathematical calculations to show vectors G r and B r by vertical arrays made by its components. ⎡ ⎤ ⎡ ⎤ X x G B r = ⎣Y ⎦ r= ⎣ y ⎦ (1.55) Z z A coordinate frame is defined by a set of basis vectors, such as unit vectors along the three coordinate axes. Coordinate transformation is mathematically equivalent to multiplication of the coordinates of a vector in a coordinate frame, say B, by a transformation matrix R, to derive the coordinates of the same vector in another coordinate frame, say G. G

r = [R] B r

(1.56)

So, a transformation matrix can also be viewed as defining a change of basis from one coordinate frame to another (Denavit and Hartenberg, 1955; Hunt, 1978).

22

1 Introduction

The angular motion of a rigid link with respect to another link can be expressed in several ways. The most popular methods are: 1. A set of rotations about a right-handed globally fixed Cartesian axes. 2. A set of rotations about a right-handed moving Cartesian axes. 3. Angular rotation about a fixed axis in space. A transformation matrix can be interpreted in three ways: 1. Mapping. It represents a coordinate transformation, mapping and relating the coordinates of a point P in two different frames. 2. Description of a frame. It gives the orientation of a transformed coordinate frame with respect to a fixed coordinate frame. 3. Operator. It is an operator taking a vector and rotating it to a new vector. Rotation of a rigid link can be expressed by several methods such as rotation matrix, Euler angles, angle-axis convention, Euler parameters, quaternion, each with advantages and disadvantages. The advantage of rotation matrix method is direct interpretation in change of basis while its disadvantage is that nine dependent parameters must be stored for only three independent parameters. The physical role of individual parameters is lost, and only the matrix as a whole has meaning. Euler angles are defined by three successive rotations about three axes of local coordinate frames. The advantage of using Euler angles is that the rotation is described by three independent parameters with clear physical interpretations. Their disadvantage is that their representation is not unique and leads to a problem with singularities. There is also no simple way to compute multiple rotations unless they are converted into matrix representation. Angle-axis convention is the most intuitive representation of rotations. However, it requires four parameters to store for a single rotation, computation of combined rotations is not simple, and it is ill-conditioned for small rotations. Euler parameters and quaternions are good in preserving most of the intuition of the angle-axis representation while overcoming the ill conditioning for small rotations and admitting a group structure that allows computation of combined rotations. The disadvantage of Euler parameters and quaternion is that four parameters are needed to express a rotation. The parameterization is more complicated than angle-axis and sometimes loses physical meaning. Euler parameters and Quaternion multiplication is not as clear as matrix multiplication.

1.5.5

 Vector Definition

By a vector we mean any physical quantity that can be represented by a directed section of a line with a start point, such as −→ O, and an end point, such as P . We may show a vector by an ordered pair of points with an arrow, such as OP . Hence,the −→ sign P P indicates a zero vector at point P (Jazar, 2011; Hunt, 1978). Length and direction are necessary to have a vector; however, a vector may have up to five characteristics: 1. Length. The length of section OP corresponds to the magnitude of the physical quantity that the vector is representing. 2. Axis. A straight line that indicates the line on which the vector is sitting. The vector axis is also called the line of action. 3. End point. A start or an end point indicates the point at which the vector is applied. Such a point is called the affecting point. 4. Direction. The direction indicates at what direction on the axis the vector is pointing. 5. Physical quantity. Any vector represents a physical quantity. If a physical quantity can be represented by a vector, it is called a vectorial physical quantity. The value of the quantity is proportional to the length of the vector. Having a vector that represents no physical quantity is meaningless, although a vector may be dimensionless. Vectors serve as the basis of our study of kinematics and dynamics. Positions, velocities, accelerations, momenta, forces, and moments all are vectors. Position vectors locate a point according to a given reference. Only vectors representing same physical quantity can be added with each other. Depending on the physical quantity and application, there are seven types of vectors:

1.5 Principle of Kinematics

23

Fig. 1.24 Seven types of vectors

1. Vecpoint. When all of the five characteristics of length, axis, end point, direction, physical quantity, are specified, the vector is called a bounded vector, point vector, or vecpoint. Such a vector is fixed at a point with no movability. 2. Vecline. If the start and end points of a vector are not fixed on the vector axis, the vector is called a sliding vector, line vector, or vecline. A sliding vector is free to slide on its axis. 3. Vecface. When the affecting point of a vector can move on a surface while the vector displaces parallel to itself, the vector is called a surface vector or vecface. If the surface is a plane, then the vector is a plane vector or veclane. 4. Vecfree. If the axis of a vector is not fixed, the vector is called a free vector, direction vector, or vecfree. Such a vector can move to any point of a specified space while it remains parallel to itself and keeps its direction. 5. Vecpoline. If the start point of a vector is fixed while the end point can slide on a line, the vector is a point-line vector or vecpoline. Such a vector has a constraint variable length and orientation. However, if the start and end points of a vecpoline are on the sliding line, its orientation is fixed. 6. Vecpoface. If the start point of a vector is fixed, while the end point can slide on a surface, the vector is a point-surface vector or vecpoface. Such a vector has a constraint variable length and orientation. The start and end points of a vecpoface may both be on the sliding surface. If the surface is a plane, the vector is called a point-plane vector or vecpolane. 7. Vecporee. When the start point of a vector is fixed and the end point can move anywhere in a specified space, the vector is called a point-free vector or vecporee. Such a vector has a variable length and orientation. Figure 1.24 illustrates a vecpoint, a vecline, a vecface, a vecfree, a vecpoline, a vecpoface, and a vecporee. We may compare two vectors only if they represent the same physical quantity and are expressed in the same coordinate frame. Two vectors are equal if they are comparable and are the same type and have the same characteristics. Two vectors are equivalent if they are comparable and the same type and can be substituted with each other. Example 7 Physical examples of vector types. Moving from a point A to a point B is called the displacement. Displacement is equal to the difference of two position vectors. Displacement is a vecpoint. A position vector rP of a point P starts from the origin of a coordinate frame G and ends at the point in the frame G. If point A is at rA and point B at rB , then displacement from A to B is rA/B =

B rA

= rA − rB

(1.57)

Assume a point P moves from the origin of a global coordinate frame G to a point at (2, 2, 0) and then moves to (4, 3, 0). If we express the first displacement by a vector r1 and its final position by r3 , the second displacement is r2 .

24

1 Introduction

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 4 2 2 r2 = r3 − r1 = ⎣ 3 ⎦ − ⎣ 2 ⎦ = ⎣ 1 ⎦ 0 0 0

(1.58)

In Newtonian mechanics, a force can be applied on a body at any point of its axis of action and provides the same reaction. Force is a vecline. In Newtonian mechanics, a moment can be applied on a body at any point parallel to itself and provides the same reaction. Torque is an example of vecfree. A space curve is expressed by a vecpoline, a surface is expressed by a vecpoface, and a field is expressed by a vecporee. Example 8 Scalars. Any physical quantity that can be expressed by only a number is called scalar. If a physical quantity can be represented by a scalar, it is called a scalaric physical quantity. We may compare two scalars only if they represent the same physical quantity. Temperature, density, and work are examples of scalaric physical quantities. Two scalars are equal if they represent the same scalaric physical quantity and they have the same number in the same system of units. Two scalars are equivalent if we can substitute one with the other. Scalars must be equal to be equivalent. Example 9  Vector addition, inner multiplication, and linear space. Vectors and adding operation make a linear space. For any two vectors r1 , r2 we have the following properties: 1. Commutative: r1 + r2 = r2 + r1

(1.59)

2. Associative: r1 + (r2 + r3 ) = (r1 + r2 ) + r3

(1.60)

0+r=r

(1.61)

r + (−r) = 0

(1.62)

3. Null element: 4. Inverse element: Vector addition and scalar multiplication make a linear space. k1 (k2 r) = (k1 k2 ) r

(1.63)

(k1 + k2 ) r = k1 r + k2 r

(1.64)

k (r1 + r2 ) = kr1 + kr2

(1.65)

1·r = r (−1) · r = −r

(1.66) (1.67)

0·r = 0

(1.68)

k·0 = 0

(1.69)

Example 10  Norm of a vector and vector space. Assume r, r1 , r2 , r3 are four arbitrary vectors and c, c1 , c3 are three arbitrary scalars. The norm of a vector r is defined as a real-valued function on a vector space V such that for all {r1 , r2 } ∈ V and all c ∈ R we have 1. Positive definition: r > 0 if r = 0 andr = 0 if r = 0. 2. Homogeneity: cr = c r. 3. Triangle inequality: r1 + r2  ≤ r1  + r2 . The most common definition of the norm of a vector is the length. The definition of norm is up to the investigator and may vary depending on the application.  r = |r| =

r12 + r22 + r32

(1.70)

1.5 Principle of Kinematics

25

Fig. 1.25 A planar 3R manipulator and position vector of the tip point P in global coordinate G(X, Y )

The set V with vector elements is called a vector space if the following conditions are fulfilled: 1. 2. 3. 4. 5.

Addition: If {r1 , r2 } ∈ V and r1 + r2 = r, then r ∈ V . Commutativity: r1 + r2 = r2 + r1 . Associativity: r1 + (r2 + r3 ) = (r1 + r2 ) + r3 and c1 (c2 r) = (c1 c2 ) r. Distributivity: c (r1 + r2 ) = cr1 + cr2 and (c1 + c2 ) r = c1 r + c2 r. Identity element: r + 0 = r, 1r = r, and r − r = r + (−1) r = 0.

1.5.6

Vector Function

If either the magnitude of a vector r and/or the direction of r in a coordinate frame B depends on a scalar variable, say θ, then r is called a vector function of θ in B. A vector r may be a function of a variable in one coordinate frame, but be independent of the variable in another frame. Example 11 Coordinate frame and vector functions. In Fig. 1.25, P represents the tip point of a planar 3R robot made by three links, joined by revolute joint. The point P is ideally free to move on and in a circle with radius l1 + l2 + l3 . The variables θ, ϕ, and ψ are the angles of links with respect to horizon. Then the position vector r of the point P is a function of θ , ϕ, and ψ in the coordinate frame G(X, Y ). The length and direction of r depend on θ, ϕ, and ψ. If G(X, Y ), and B2 (x, y) designate coordinate frames attached to the ground and link (2), and P the tip point of link (3) as shown in Fig. 1.26, then the position vector r of point P in coordinate frame B2 is only a function of ϕ and ψ, and is independent of θ.

Example 12 Variable vectors. There are two ways that a vector can vary: 1−length and 2−direction. A variable-length vector is a vector in the natural expression where its magnitude is variable, say of time t. r = r(t) uˆ r

(1.71)

The axis of a variable-length vector is fixed. A variable-direction vector is a vector in its natural expression where the axis of its unit vector varies. Let us use the decomposed expression of a vector r and show that its directional cosines are variable.  r = r uˆ r (t) = r u1 (t)ˆı + u2 (t)jˆ + u3 (t)kˆ  u21 + u22 + u23 = 1

(1.72) (1.73)

26

1 Introduction

Fig. 1.26 A planar 3R manipulator and position vector of the tip point P in second link local coordinate B2 (x, y)

The axis and direction characteristics are not fixed for a variable-direction vector, while its magnitude remains constant. The end point of a variable-direction vector slides on a sphere with a center at the starting point. A variable vector may have both the length and direction variable. Such a vector is shown in its decomposed expression with variable components: r = x(t)ˆı + y(t)jˆ + z(t)kˆ (1.74) It can also be shown in its natural expression with variable length and direction: r = r(t) uˆ r (t)

(1.75)

Example 13 Vector function and derivative. If a vector is expressed in a Cartesian coordinate frame, its derivative can be found by taking derivative of its components. The Cartesian unit vectors are invariant and have zero derivative with respect to any parameter. A vector r = r (t) is a vector function of the scalar variable t if there is a definite vector for every value of t. In a Cartesian coordinate frame G, the specification of the vector function r (t) is equivalent to the specification of three scalar functions x (t), y (t), z (t) (Milne, 1948; Jazar, 2012): G r (t) = x (t) ıˆ + y (t) jˆ + z (t) kˆ (1.76) If the vector r is expressed in Cartesian decomposition form, then the derivative dr/dt in the coordinate frame G is G

dG dx (t) dy (t) dz (t) ˆ r= ıˆ + jˆ + k dt dt dt dt

(1.77)

and if r is expressed in its natural form, G

 r = r uˆ r = r (t) u1 (t) ıˆ + u2 (t) jˆ + u3 (t) kˆ

(1.78)

then, using the chain rule, the derivative dr/dt is G

dG dr d r= uˆ r + r uˆ r dt dt dt  dr  du1 du2 du3 ˆ ˆ ıˆ + jˆ + k = u1 ıˆ + u2 jˆ + u3 k + r dt dt dt dt   dr du1 dr du2 = u1 + r ıˆ + u2 + r jˆ dt dt dt dt  dr du3 ˆ + u3 + r k dt dt

(1.79)

1.5 Principle of Kinematics

27

If the independent variable t is time, an overdot r˙ (t) is used as a shorthand notation to indicate the time derivative (Harithuddin et al., 2015). G dG G v= r (t) = x˙ (t) ıˆ + y˙ (t) jˆ + z˙ (t) kˆ (1.80) dt If a variable vector G r is expressed in a natural form G

r = r (t) uˆ r (t)

(1.81)

we express the unit vector uˆ r (t) in its decomposed form G

r = r (t) uˆ r (t)  = r (t) u1 (t) ıˆ + u2 (t) jˆ + u3 (t) kˆ

(1.82)

and take the derivative using the chain rule and variable-length vector derivative. G

G

G dG d uˆ r r = r˙ uˆ r + r dt dt   = r˙ u1 ıˆ + u2 jˆ + u3 kˆ + r u˙ 1 ıˆ + u˙ 2 jˆ + u˙ 3 kˆ

v=

= (˙r u1 + r u˙ 1 ) ıˆ + (˙r u2 + r u˙ 2 ) jˆ + (˙r u3 + r u˙ 3 ) kˆ

(1.83)

Consider a moving point P with a continuously varying position vector r = r (t). When the starting point of r is fixed at the origin of G, its end point traces a continuous curve C. The curve C is called a configuration path that describes the motion of P , and the vector function r (t) is its vector representation. At each point of the continuously smooth curve C = {r (t) , t ∈ [τ 1 , τ 2 ]} there exists a tangent line and a derivative vector dr (t) /dt that is directed along the tangent line and directed toward increasing the parameter t. If the parameter is the arc length s of the curve that is measured from a convenient point on the curve, the derivative of G r with respect to s is the tangential unit vector uˆ t to the curve at G r: G

dG r = uˆ t ds

(1.84)

Example 14 Velocity, acceleration, jerk, and other derivatives. If the vector r = G r (t) is a position vector in a coordinate frame G, then its time derivative is a velocity vector G v. It shows the speed and the direction of motion of the tip point of G r: G

v=

G

dG r (t) = x˙ (t) ıˆ + y˙ (t) jˆ + z˙ (t) kˆ dt

(1.85)

A time derivative of a velocity vector G v is called the acceleration G a, G

a=

G

dG v (t) = x¨ (t) ıˆ + y¨ (t) jˆ + z¨ (t) kˆ dt

(1.86)

and the time derivative of an acceleration vector G a is called the jerk G j, G

j=

G

... dG ... ... a (t) = x (t) ıˆ + y (t) jˆ + z (t) kˆ dt

(1.87)

As an example, consider a moving point P on a helix with position vector r in a coordinate frame G. G

r (t) = cos (ωt) ıˆ + sin (ωt) jˆ + 2t kˆ

(1.88)

28

1 Introduction

The helix is uniformly turning on a circle in the (x, y)-plane while the circle is moving with a constant speed in the z-direction. Taking time derivatives show that the velocity, acceleration, and jerk of the point P are: G

v (t) = −ω sin (ωt) ıˆ + ω cos (ωt) jˆ + 2 kˆ

(1.89)

G

a (t) = −ω cos (ωt) ıˆ − ω sin (ωt) jˆ

(1.90)

j (t) = ω3 sin (ωt) ıˆ − ω3 cos (ωt) jˆ

(1.91)

G

2

2

The derivative of acceleration or the third time derivative of the position vector r is called the jerk j; in England the word jolt is used instead of jerk. The third derivative may also wrongly be called pulse, impulse, bounce, surge, shock, or superacceleration. In engineering, acceleration is important because of equations of motion, and jerk is important for evaluating the destructive effects of motion of a moving object. High jerk is a reason for discomfort of passengers in a vehicle. Jerk is the reason for liquid splashing from an open container. The movement of fragile objects, such as eggs, needs to be kept within specified limits of jerk to avoid damage. It is required that engineers keep the jerk of public transportation vehicles less than 2 m/s3 for passenger comfort (Jazar, 2017). Although there are no universally accepted names for the fourth and higher derivatives of a position vector r, the terms snap s and jounce s have been used for derivatives of jerk. The fifth derivative of r is crackle c, the sixth derivative is pop þ, the seventh derivative is larz z, the eight derivative is bong b, the ninth derivative is jeeq q, and the tenth derivative is sooz u (Jazar, 2011). Not much application for fourth and higher derivatives of position vector have been reported.

1.6

Summary

There are two kinds of robots: serial and parallel. A serial robot is made from a series of rigid links, where each pair of links is connected by a revolute (R) or prismatic (P ) joint. An R or P joint provides only one degree of freedom, which is rotational or translational, respectively. The final link of a robot, also called the end-effector, is the operating member of a robot that interacts with the environment. To reach any point in a desired orientation, within a robot’s workspace, a robot needs at least 6 DOF . Hence, it must have at least 6 links and 6 joints. Most robots use 3 DOF to position the wrist point, and use the other 3 DOF to orient the end-effector about the wrist point. We attach a Cartesian coordinate frame to each link of a robot and determine the position and orientation of each frame with respect to the others. Therefore, to determine the position and orientation of the end-effector, we need to find the end-effector frame in the base frame. Any physical quantity that can be represented by a directed section of a line with a start and an end point is a vector quantity. A vector may have five characteristics: length, axis, end point, direction, and physical quantity. The length and direction are necessary. There are seven types of vectors: vecpoint, vecline, vecface, vecfree, vecpoline, vecpoface, and pecporee. Vectors can be added when they are coaxial. In case the vectors are not coaxial, the decomposed expression of vectors must be used to add the vectors.

1.7 Key Symbols

1.7

29

Key Symbols

0 a, x, ¨ a, v˙ aij k a, b, c, p, q A, B A, B, C A, B, C, D b B(oxyz), B1 , B2 c C dr ds G do D g G G(OXY Z) ıˆ, jˆ, kˆ Iˆ, Jˆ, Kˆ ... j, a, ˙ v, ¨ r k l n nˆ O OABC (Ouvw) (Oq1 q2 q3 ) P q, p q = b˙ r = |r| r B rA rp , rq R s s = dj/dt S t T u, v, w u1 , u2 , u3 uˆ T uˆ l uˆ r uˆ 1 , uˆ 2 , uˆ 3 uˆ u , uˆ v , uˆ w u v

Zero vector Acceleration Inner product constant of xi Vectors, constant vectors Points Axes of triad, constant parameters Axes of tetrad, constant parameters Bong Body coordinate frames Crackle Space curve Infinitesimal displacement Arc length element Position vector of the origin of B in G Dimension Gravitational acceleration Gravitational constant Global coordinate frame Unit vectors of a Cartesian coordinate frame Unit vectors of a global Cartesian system G Jerk Scalar coefficient Length, a line Number of dimensions of an nD space Perpendicular unit vector Origin of a triad, origin of a coordinate frame A triad with axes A, B, C An orthogonal coordinate frame An orthogonal coordinate system Prismatic joint, point, particle Parameters, variables Jeeq Length of r Position vector Position vector of point A relative to B Partial derivatives of G r Revolute joint, transformation matrix Arc length parameter Snap, jounce Screw joint, surface Time Translatory Components of a vector r in (Ouvw) Components of uˆ r Transpose of uˆ Unit vector on a line l A unit vector on r Unit vectors along the axes q1 , q2 , q3 Unit vectors of (Ouvw) Sooz Speed

30

1 Introduction

v, x, ˙ v V x, y, z x X, Y, Z z = þ˙ Z

Velocity Vector space Axes of orthogonal Cartesian coordinate frames Vector functions, unknown vector Global coordinate axes Larz Short notation symbol

Greek α α, β, γ α1, α2, α3 θ, ϕ, ψ

Angle between two vectors Directional cosines of a line, angles Directional cosines of r and uˆ r Angle, angular coordinate, angular parameter, joint variable

Symbol · ×   ⊥ → þ

Inner product of two vectors Outer product of two vectors Orthogonal Parallel sign Perpendicular Vector sign Pop

Exercises

31

Exercises 1. Meaning of indexes for a position vector. G Explain the meaning of G rP , G B rP , and G rP , if r is the position vector of a point P . 2. Meaning of indexes for a velocity vector. B Explain the meaning of G vP , G B vP , and B vP , if v is a velocity vector. 3. Meaning of indexes for an angular velocity vector. B2 B Explain the meaning of B2 ωB1 , G B2 ωB1 , G ωB , and B3 ωB1 , if ω is an angular velocity vector. 4. Meaning of indexes for a transformation matrix. Explain the meaning of B2 TB1 , and G TB , if T is a transformation matrix. 5. Laws of robotics. What is the difference between law zero and law one of robotics? 6. New law of robotics. What do you think about adding a fourth law to robotics, such as: A robot must protect the other robots as long as such protection does not conflict with a higher order law. 7. Robot classification. Explain why a crane is not considered as a robot. 8. Robot market. Most small robot manufacturers went out of the market around 1990, and only a few large companies remained. Why do you think this happened? 9. Humanoid robots. The mobile robot industry is trying to make robots as similar to humans as possible. What do you think is the reason for this. Does humans have the best structural design? Does humans have the simplest design? Do these kinds of robots can be sold in the market better? 10. Robotic person. Why do you think we call somebody who works or behaves mechanically, showing no emotion and often responding to orders without question, a robotic person? 11. Robotic journals. Find the name of 10 technical journals related to robotics. 12. Number of robots in industrial poles. Search the robotic literatures and find out, approximately, how many industrial robots are currently in operation in the USA, Europe, and Japan. 13. Robotic countries. Search the robotic literatures and find out what countries are ranked first, second, and third according to the number of industrial robots in use? 14. Advantages and disadvantages of robots. Search the robotic literatures and name 10 advantages and 2 disadvantages of robots compared to mechanisms. 15. Mechanisms and robots. Why do we not replace every mechanism, in an assembly line for example, with robots? Are robots substituting mechanisms? 16.  Higher pairs and lower pairs. Joints can be classified as lower pairs and higher pairs. Find the meaning of “lower pairs” and “higher pairs” in mechanics of machinery. 17. Human wrist DOF . Examine and count the number of DOF of your wrist. 18. Human hand is a redundant manipulator. An arm (including shoulder, elbow, and wrist) has 7 DOF . What is the advantage of having one extra DOF (with respect to 6 DOF ) in our hands? 19. Multiple DOF robot. Sometimes we do not need many DOF robots to do a specific job that can be done by a low DOF robot. What do you think about having a robot with variable DOF ? 20. Usefulness of redundant manipulators. Discuss possible applications in which the redundant manipulators would be useful.

32

1 Introduction

21.  Disadvantages of a non-orthogonal triad. Why do we use an orthogonal triad to define a Cartesian space? Can we define a 3D space with non-orthogonal triads? 22.  Usefulness of an orthogonal triad. Orthogonality is the common property of all useful coordinate systems such as Cartesian, cylindrical, spherical, parabolical, and ellipsoidal coordinate systems. Why do we only define and use orthogonal coordinate systems? Do you think ability to define a vector, based on inner product and unit vectors of the coordinate system, such as ˆ kˆ r = (r · ıˆ)ˆı + (r · jˆ)jˆ + (r · k)

(1.92)

is the main reason for defining the orthogonal coordinate systems? 23.  Three coplanar vectors. Show that if a × b · c = 0, then a, b, c are coplanar. 24.  Vector function, vector variable. A vector function is defined as a dependent vectorial variable that relates to a scalar independent variable. r = r(t)

(1.93)

Describe the meaning and define an example for a vector function of a vector variable, a = a(b)

(1.94)

f = f (b)

(1.95)

and a scalar function of a vector variable. 25.  Frame dependent and frame independent. A vector function of scalar variables is a frame-dependent quantity. Is a vector function of vector variables frame dependent? What about a scalar function of vector variables? 26.  Coordinate frame and vector function. Explain the meaning of B vP (G rP ), if r is a position vector, v is a velocity vector, and v(r) means v is a function of r. 27. Vector multiplication. Prove that: r2 = r · r = r 2

(1.96)

(r1 + r2 )2 = r21 + 2r1 · r2 + r22

(1.97)

(r1 − r2 ) · (r1 + r2 ) = r21 − r22

(1.98)

28. Vector equation. Assume x is an unknown vector, k is a scalar, and a, b, and c are three constant vectors in the following vector equation: kx + (b · x) a = c

(1.99)

ax + x × b = c

(1.100)

(a) Solve the equation for x. (b) Solve for x. 29. Vector outer product. Consider a point P at x = 10, y = 15, z = 5. (a) Write the position vector of P in natural expression, rP = r uˆ r and determine uˆ r and r.

(1.101)

Exercises

33

(b) Determine the directional cosines of rP . (c) Determine a vector rQ , perpendicular to rP such that their outer product is a vector in z-direction with a length of 10π. 30. Vectors and geometry. (a) Assume rQ and rP to be position vectors of points Q and P . Employing vector arithmetic, determine an equation to calculate the area A of the triangle OP D. (b) Determine a vector from O and perpendicular to P Q. The length of that vector would be the height of the triangle OP D. (c) Assume rM = rP × rQ . Determine area of triangle MP Q. (d) Assume rN = rQ × rP . Determine area of triangle NP Q. 31. Cosine law. Consider a triangle ABC where its sides are expressed by vectors as −→ AB = c

−→ AC = b

−→ CB = a

c=a+b

(1.102)

Use vector algebra and prove the cosine law, c2 = a 2 + b2 − 2ab cos α

(1.103)

α = ACB

(1.104)

where 32. A trigonometric equation. Assume two planar vectors a and b that respectively make angles α and β with the x-axis. Prove the following trigonometric identity: cos (α − β) = cos α cos β + sin α sin β (1.105) 33.  Spherical trigonometric equations. Prove the following spherical trigonometric equations in a spherical triangle ABC with sides a, b, c and angle α, β, γ : cos a = cos b cos c + sin b sin c cos α

(1.106)

cos b = cos c cos a + sin c sin a cos β

(1.107)

cos c = cos a cos b + sin a sin b cos γ

(1.108)

34. Three colinear points. Consider three points A, B, and C indicated by vectors a, b, and c. If the points are colinear, then cy − ay cx − ax cz − az = = bx − ax by − ay bz − az

(1.109)

Show that this condition can be expressed by (a × b) + (b × c) + (c × a) = 0

(1.110)

35. A derivative identity. Assume a variable vector a = a (t) and a constant vector b to prove d (a · (˙a × b)) = a · (a¨ × b) dt

(1.111)

36.  Lagrange and Jacobi identities. (a) Show that for any four vectors a, b, c, d, the Lagrange identity is correct: (a × b) · (c × d) = (a · c) (b · d) − (a · d) (b · c)

(1.112)

34

1 Introduction

(b) Show that for any four vectors a, b, c, d the following identities are correct: (a × b) × (c × d) = [abd] c − [abc] d

(1.113)

[abc] d = [dbc] a + [dca] b + [dab] c

(1.114)

where, [abc] is the scalar triple product. This product for the vectors r1 , r2 , r3 is shown by the [r1 r2 r3 ] and is equal to (1.115) [r1 r2 r3 ] = r1 · r2 × r3 (c) Show that for any four vectors a, b, c, d, the Jacobi identity is correct: a × (b × c) + c × (a × b) + b × (c × a) = 0

(1.116)

37.  Moving a point on a given curve. Assume a particle is moving on a planar curve y = f (x) such that the x-component of its velocity vector v, remains constant. Determine the acceleration and jerk of the particle if: (a) y = x 2 (b) y = x 3 (c) y = ex (d) Determine the angle between velocity vectors of curves (a) and (b) at their intersection.

Part I Kinematics

Kinematics is the science of geometry in motion. It is restricted to a pure geometrical description of motion by means of position, orientation, and their time derivatives. In robotics, the kinematic descriptions of manipulators and their assigned tasks are utilized to set up the fundamental equations for dynamics and control. Because the links and arms in a robotic system are modeled as rigid bodies, the properties of rigid body displacement take a central place in robotics. Vector and matrix algebra are utilized to develop a systematic and generalized approach to describe and present the location of the arms of a robot with respect to a global fixed reference frame G. Because the arms of a robot may rotate or translate with respect to each other, body-attached coordinate frames A, B, C, · · · or B1 , B2 , B3 , · · · will be established for each link to find their relative configurations, and in the reference frame G. The position of a link B relative to another link A is defined kinematically by a coordinate transformation A TB between coordinate frames attached to the links. The direct kinematics problem is reduced to finding a transformation matrix G TB that relates the body local coordinate frame B to the global reference coordinate frame G. A 3 × 3 rotation matrix is utilized to describe the rotational operations of the local frame with respect to the global frame. The homogenous coordinates are then introduced to represent position vectors and directional vectors in a three dimensional space. The rotation matrices are expanded to 4 × 4 homogenous transformation matrices to include both, rotational and translational motions.Homogenous matrices that express the relative rigid links of a robot are made by a special set of rules and are called Denavit–Hartenberg matrices after Denavit and Hartenberg (1955). The advantage of using the Denavit–Hartenberg method is its algorithmic and universality in deriving the kinematic equation of robot links. The analytical description of displacement of a rigid body is based on the notion that all points in a rigid body must retain their original relative positions regardless of the new position and orientation of the body. The total rigid body displacement can always be reduced to the sum of its two basic components: the translation displacement of an arbitrary reference point fixed in the rigid body plus the unique rotation of the body about a line through that point. Study of motion of rigid bodies leads to the relation between the time rate of change of a vector in a global frame and the time rate of change of the same vector in a local frame. Transformation from a local coordinate frame B to a global coordinate frame G is expressed by G

r=

G

RB B r + G d B

where B r is the position vector of a point in B, G r is the position vector of the same point expressed in G, and d is the position vector of the origin o of the body coordinate frame B(oxyz) with respect to the origin O of the global coordinate frame G(OXY Z). Therefore, a transformation has two parts: a translation d that brings the origin o on the origin O, plus a rotation, G RB that brings the axes of oxyz on the corresponding axes of OXY Z.

36

I Kinematics

The transformation formula G r = G RB B r + G dB can be expanded to connect more than two coordinate frames. The combination formula for a transformation from a local coordinate frame B1 to another coordinate frame B2 followed by a transformation from B2 to the global coordinate G is G

r=

G

=

G

=

G

   RB2 B2 r + G d B2 + B2 RB1 B1 r + B2 d B1

RB2 B2 RB1 B1 r + RB1

B1

r+

G

G

d B2 +

B2

d B1

d B1

A robot consists of n rigid links with relative motions. The link attached to the ground is link (0) and the link attached to the final moving link, the end-effector, is link (n). There are two important problems in kinematic analysis of robots: the forward kinematics problem and the inverse kinematics problem. In forward kinematics, the problem is that the position vector of a point P is in the coordinate frame Bn attached to the end-effector at n rp , and we are looking for the position of P in the base frame B0 shown by 0 rp . The forward kinematics problem is equivalent to having the values of joint variables and asking for the position of the end-effector. In inverse kinematics, the problem is that we have the position vector of a point P in the base coordinate frame B0 as 0 rp , and we are looking for n rp , the position of P in the end-effector frame Bn . This problem is equivalent to having the position of the end-effector and asking for a set of joint variables that make the robot reach the point P . In this part, we develop the transformation formula to move the kinematic information back and forth from a coordinate frame to another coordinate frame.

2

Rotation Kinematics

Consider a rigid body B with a fixed point O. Rotation about the fixed point O is the only possible motion of the body. We represent the rigid body by a body coordinate frame B (O, x, y, z), which rotates in another coordinate frame G (O, X, Y, Z), as is shown in Fig. 2.1. In this chapter, we develop the rotation calculus based on transformation matrices to determine the orientation of B in G and relate the coordinates of a body point P in both frames.

Fig. 2.1 A rotated body frame B in a fixed global frame G, about a fixed point at O

2.1

Rotation About Global Cartesian Axes

Consider a rigid body B with a local coordinate frame Oxyz that is originally coincident with a global coordinate frame OXY Z. Point O is the origin of both coordinate frames and is the only point of the body B to be fixed in the global frame G. If the rigid body B rotates α radians about the Z-axis of the global coordinate frame, then coordinates of any point P of the rigid body in the local and global coordinate frames are related by G

r = QZ,α B r

(2.1)

where B r is the B-expression of the position vector of the point P in B, and G r is the G-expression of the same vector, ⎡

⎤ X G r = ⎣Y ⎦ Z

⎡ ⎤ x B r= ⎣ y ⎦ z

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_2

(2.2)

37

38

2 Rotation Kinematics

Fig. 2.2 Position vector of point P when local and global frames are coincident

and QZ,α is the Z-rotation transformation matrix. ⎡

QZ,α

cos α − sin α = ⎣ sin α cos α 0 0

⎤ 0 0⎦ 1

(2.3)

Similarly, rotation β about the Y -axis and rotation γ about the X-axis of the global frame relate the local and global coordinates of point P by the following equations: G

r = QY,β B r

(2.4)

G

r = QX,γ B r

(2.5)

where ⎡

QY,β

QX,γ

⎤ 0 sin β 1 0 ⎦ 0 cos β ⎤ 0 ⎥ − sin γ ⎦ 0 sin γ cos γ

cos β =⎣ 0 − sin β ⎡ 1 0 ⎢ = ⎣ 0 cos γ

(2.6)

(2.7)

Equations (2.1), (2.4), and (2.5) show how to go from B coordinate frame to the G coordinate frame. In other words, we have coordinates of a point of the B-frame as B r and we are to determine its coordinates in the G-frame as G r. ˆ and (Iˆ, Jˆ, K) ˆ be the unit vectors along the coordinate axes of Oxyz and OXY Z, respectively. The rigid Proof Let (ˆı , jˆ, k) body has a globally fixed point O, which is the common origin of Oxyz and OXY Z. Figure 2.2 illustrates the top view of the system when the coordinate frames B and G are initially coincident. The initial position of a point P is indicated by P1 . The position vector r1 of P1 can be expressed in body and global coordinate frames by B

r1 = x1 ıˆ + y1 jˆ + z1 kˆ

(2.8)

G

r1 = X1 Iˆ + Y1 Jˆ + Z1 Kˆ

(2.9)

where x1 = X1 B

y 1 = Y1

z1 = Z1

(2.10) G

The vector r1 refers to the position vector r1 expressed in the body coordinate frame B, and r1 refers to the position vector r1 expressed in the global coordinate frame G.

2.1 Rotation About Global Cartesian Axes

39

Fig. 2.3 Position vector of point P when the body and its frame B are rotated α about the Z-axis

If the rigid body undergoes a rotation of α radians about the Z-axis, then the local frame Oxyz, point P , and the position vector r will be seen in a second position, as shown in Fig. 2.3. Now the position vector r2 of P2 is expressed in both coordinate frames by B

r2 = x2 ıˆ + y2 jˆ + z2 kˆ

(2.11)

G

r2 = X2 Iˆ + Y2 Jˆ + Z2 Kˆ

(2.12)

Using the definition of the inner product and Eq. (2.11), we may write

or equivalently,

X2 = Iˆ · r2 = Iˆ · x2 ıˆ + Iˆ · y2 jˆ + Iˆ · z2 kˆ

(2.13)

Y2 = Jˆ · r2 = Jˆ · x2 ıˆ + Jˆ · y2 jˆ + Jˆ · z2 kˆ

(2.14)

Z2 = Kˆ · r2 = Kˆ · x2 ıˆ + Kˆ · y2 jˆ + Kˆ · z2 kˆ

(2.15)

⎤ ⎡ Iˆ · ıˆ X2 ⎢ ⎣ Y2 ⎦ = ⎣ Jˆ · ıˆ Z2 Kˆ · ıˆ ⎡

Iˆ · jˆ Jˆ · jˆ Kˆ · jˆ

⎤⎡ ⎤ Iˆ · kˆ x2 ⎥ Jˆ · kˆ ⎦ ⎣ y2 ⎦ z2 Kˆ · kˆ

(2.16)

The elements of the Z-rotation matrix, QZ,α , are direction cosines of B r2 with respect to OXY Z. Figure 2.4 shows the top view of the initial and final configurations of r in both coordinate systems B (Oxyz) and G (OXY Z). Figure 2.4 indicates that Iˆ · ıˆ = cos α

Iˆ · jˆ = cos (π + α) Iˆ · kˆ = cos π = 0 = − sin α ˆ ˆ J · ıˆ = cos (π − α) J · jˆ = cos α Jˆ · kˆ = cos π = 0 = sin α Kˆ · ıˆ = cos π = 0 Kˆ · jˆ = cos π = 0 Kˆ · kˆ = cos 0 = 1

(2.17)

Combining Eqs. (2.16) and (2.17) shows that we can find the components of G r2 by multiplying the Z-rotation matrix, QZ,α , and the vector B r2 . ⎡ ⎤ ⎡ ⎤⎡ ⎤ x2 X2 cos α − sin α 0 ⎣ Y2 ⎦ = ⎣ sin α cos α 0 ⎦ ⎣ y2 ⎦ (2.18) Z2 0 0 1 z2

40

2 Rotation Kinematics

Fig. 2.4 Position vectors of point P before and after the rotation α of the local frame B about the Z-axis of the global frame G

It can also be shown in a short notation. G

r2 = QZ,α B r2

(2.19)

Equation (2.19) says that the vector r at the second position in the global coordinate frame G r is equal to QZ times the position vector in the local coordinate frame B r. Hence, we are able to find the global coordinates of a point of a rigid body after rotation about the Z-axis, if we have its local coordinates. Similarly, rotation β about the Y -axis and rotation γ about the X-axis are expressed by the Y -rotation matrix QY,β and the X-rotation matrix QX,γ . The rotation matrices QZ,α , QY,β , and QX,γ ⎡

QZ,α

cos α − sin α = ⎣ sin α cos α 0 0 ⎡

QY,β

QX,γ

cos β ⎣ = 0 − sin β ⎡ 1 0 = ⎣ 0 cos γ 0 sin γ

⎤ 0 0⎦ 1

⎤ 0 sin β 1 0 ⎦ 0 cos β ⎤ 0 − sin γ ⎦ cos γ

(2.20)

(2.21)

(2.22)

are called basic global rotation matrices. We usually refer to the first, second, and third rotations about the axes of the global coordinate frame by α, β, and γ , respectively. All of the basic global rotation matrices QZ,α , QY,β , and QX,γ transform a B-expression vector to its G-expression.  Example 15 Successive rotation about global axes. Intermediate and final position of the corner of a brick. The final position of the corner P (5, 30, 10) of the slab shown in Fig. 2.5 after 30 deg rotation about the Z-axis, followed by 30 deg about the X-axis, and then 90 deg about the Y -axis can be found by first multiplying QZ,π /6 by [5, 30, 10]T to get the new global position after the first rotation π π ⎤⎡ ⎤ ⎡ ⎤ − sin 0 X2 5 −10.67 6 6 ⎢ ⎥ ⎣ Y2 ⎦ = ⎢ sin π cos π 0 ⎥ ⎣ 30 ⎦ = ⎣ 28.48 ⎦ ⎣ ⎦ 6 6 Z2 10 10.0 0 0 1 ⎡

⎤

⎡

cos

(2.23)

2.1 Rotation About Global Cartesian Axes

41

Fig. 2.5 Corner P of the slab at first position

Fig. 2.6 Corner P and the slab at first, second, third, and final positions

and then multiplying QX,π /6 and [−10.67, 28.48, 10.0]T to get the position of P after the second rotation ⎡

⎤

⎡

⎤

⎡ ⎤ ⎡ ⎤ X3 −10.67 π ⎥ −10.67 π ⎢ ⎣ Y3 ⎦ = ⎢ 0 cos − sin ⎥ ⎣ 28.48 ⎦ = ⎣ 19.66 ⎦ ⎣ π6 π6 ⎦ Z3 10.0 22.9 0 sin cos 6 6 1

0

0

(2.24)

and finally multiplying QY,π /2 and [−10.67, 19.66, 22.9]T to get the final position of P after the third rotation. The slab and the point P in first, second, third, and fourth positions are shown in Fig. 2.6. ⎡

π π 0 sin X4 2 2 ⎢ ⎣ Y4 ⎦ = ⎢ 0 1 0 ⎣ π π Z4 − sin 0 cos 2 2 ⎡

⎤

cos

⎤

⎡ ⎤ ⎡ ⎤ 22.90 ⎥ −10.68 ⎥ ⎣ 19.66 ⎦ = ⎣ 19.66 ⎦ ⎦ 22.9 10.67

(2.25)

Example 16 Time dependent global rotation. Consider a rigid body B that is continuously turning about the Y -axis of the G-frame at a rate of 0.3 rad/s. The rotation transformation matrix of the body is ⎡ ⎤ cos 0.3t 0 sin 0.3t G QB = ⎣ 0 1 0 ⎦ (2.26) − sin 0.3t 0 cos 0.3t

42

2 Rotation Kinematics

Any point of B will move on a circle with radius R =

√

X2 + Z 2 parallel to (X, Z)-plane.

⎡

⎤ ⎡ ⎤⎡ ⎤ X cos 0.3t 0 sin 0.3t x ⎣Y ⎦ = ⎣ 0 1 0 ⎦⎣y ⎦ Z − sin 0.3t 0 cos 0.3t z ⎡ ⎤ x cos 0.3t + z sin 0.3t ⎦ =⎣ y z cos 0.3t − x sin 0.3t

(2.27)

X2 + Z 2 = (x cos 0.3t + z sin 0.3t)2 + (z cos 0.3t − x sin 0.3t)2 = x 2 + z2 = R 2

(2.28)

 T Consider a point P at B r = 1 0 0 . After t = 1 s, the point will be seen at ⎡

⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ X cos 0.3 0 sin 0.3 1 0.955 ⎣Y ⎦ = ⎣ 0 1 0 ⎦⎣0⎦ = ⎣ 0 ⎦ Z − sin 0.3 0 cos 0.3 0 −0.295 and after t = 2 s, at

⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ X cos 0.6 0 sin 0.6 1 0.825 ⎣Y ⎦ = ⎣ 0 1 0 ⎦⎣0⎦ = ⎣ 0 ⎦ Z − sin 0.6 0 cos 0.6 0 −0.565

(2.29)

⎡

(2.30)

We can find the global velocity of the body point P by taking a time derivative of rP = QY,β B rP ⎡ ⎤ cos 0.3t 0 sin 0.3t QY,β = ⎣ 0 1 0 ⎦ − sin 0.3t 0 cos 0.3t G

(2.31) (2.32)

Therefore, the global expression of its velocity vector is ⎡ ⎤ −0.3 sin 0.3t d G G ˙ Y,β B rP = 0.3 ⎣ ⎦ vP = rP = Q 0 dt −0.3 cos 0.3t ⎤ − sin 0.3t 0 cos 0.3t ˙ Y,β = 0.3 ⎣ ⎦ Q 0 0 0 − cos 0.3t 0 − sin 0.3t   T The velocity of a general point at B r = x y z will be where

(2.33)

⎡

⎡ ⎤ z cos 0.3t − x sin 0.3t d G G ˙ Y,β B r = 0.3 ⎣ ⎦ v= r=Q 0 dt −x cos 0.3t − z sin 0.3t

(2.34)

(2.35)

Example 17 Global rotation is given, and local position is requested. If a point P is moved to G r2 = [4, 3, 2]T after a 60 deg rotation about the Z-axis, its position in the local coordinate is

2.2 Successive Rotation About Global Cartesian Axes

43

Fig. 2.7 Positions of point P in Example 17 before and after rotation

G r2 = Q−1 Z,π /3 r2 ⎡ π ⎤−1 ⎡ ⎤ ⎡ π ⎡ ⎤ ⎤ − sin 0 cos x2 4.60 4 3 3 ⎢ ⎥ ⎣ y2 ⎦ = ⎢ sin π cos π 0 ⎥ ⎣ 3 ⎦ = ⎣ −1.95 ⎦ ⎣ ⎦ 3 3 2 2.0 z2 0 0 1 B

(2.36)

The local coordinate frame was coincident with the global coordinate frame before rotation, and thus the global coordinates of P before rotation was also G r1 = [4.60, −1.95, 2.0]T . Positions of P before and after rotation are shown in Fig. 2.7.

2.2

Successive Rotation About Global Cartesian Axes

The final global position of a point P of a rigid body B with position vector r, after a series of rotations Q1 , Q2 , Q3 , ..., Qn about the global axes can be found by one transformation: G

r=

G

QB B r

(2.37)

where G G

QB = Qn · · · Q3 Q2 Q1

B

(2.38) G

The vectors r and r indicate the position vector r in the global and local coordinate frames. The matrix QB , which transforms the local coordinates to their corresponding global coordinates, is called the global rotation matrix. Because matrix multiplications do not commute, the order of performing rotations is important. Proof Assume a body frame B that undergoes two sequential rotations Q1 and Q2 about the global axes. The body coordinate frame B is initially coincident with the global coordinate frame G. The rigid body rotates about a global axis, and the global rotation matrix Q1 gives us the new global coordinate G r1 of the body point at B r. G

r1 = Q1 B r

(2.39)

Before the second rotation, we put the B-frame aside and assume a new body coordinate frame B1 to be coincident with the global frame. Therefore, the second rotation is treated similar to the first rotation. The new body coordinate would be B1 r ≡ G r1 . The second global rotation matrix Q2 provides the new global position G r2 of the body points B1 r: r = Q2 B1 r

(2.40)

r = Q2 Q1 B r

(2.41)

B1

Substituting (2.39) into (2.40) shows that G

44

2 Rotation Kinematics

Following the same procedure, we can determine the final global position of a body point after a series of sequential rotations Q1 , Q2 , Q3 , ..., Qn as (2.38). Rotation about global coordinate axes is conceptually simple because the axes of rotations are fixed in space. Assume we have the coordinates of every point of a rigid body in the global frame that is equal to the local coordinates initially. The rigid body rotates about a global axis, and then the proper global rotation matrix gives us the new global coordinate of the points. Then, our situation before the second rotation is similar to what we had before the first rotation. This process may be continued until all rotations are performed. The resultant transformation matrix would be equal to matrix multiplication of all individual transformation matrices in order. The multiplication of rotation transformation matrices is associative. Q3 Q2 Q1 = Q3 [Q2 Q1 ] = [Q3 Q2 ] Q1

(2.42) 

Example 18 Successive global rotation matrix. Several rotations about the global principal axes and an equivalent transformation matrix. The global rotation matrix G QB after a rotation QZ,α followed by QY,β and then QX,γ is QB = QX,γ QY,β QZ,α ⎡ ⎤⎡ ⎤⎡ ⎤ 1 0 0 cos β 0 sin β cos α − sin α 0 ⎢ ⎥⎢ ⎥⎢ ⎥ = ⎣ 0 cos γ − sin γ ⎦ ⎣ 0 1 0 ⎦ ⎣ sin α cos α 0 ⎦ 0 sin γ cos γ − sin β 0 cos β 0 0 1 ⎡ ⎤ cα cβ −cβ sα sβ ⎢ ⎥ = ⎣ cγ sα + cα sβ sγ cαcγ − sα sβ sγ −cβ sγ ⎦ sα sγ − cα cγ sβ cαsγ + cγ sαsβ cβ cγ

G

(2.43)

where c stands for cos and s stands for sin. c ≡ cos

s ≡ sin

(2.44)

G

If we change the order of rotation, then the global rotation matrix QB after a rotation QX,γ followed by QY,β and thenQZ,α is G

QB = QZ,α QY,β QX,γ ⎡ ⎤ cα cβ cα sβ sγ − cγ sα sα sγ + cα cγ sβ = ⎣ cβ sα cα cγ + sα sβ sγ cγ sα sβ − cα sγ ⎦ −sβ cβ sγ cβ cγ

(2.45)

A negative rotation cancels a positive rotation and the resultant rotation matrix will be a unity matrix. The global rotation matrix G QB after a rotation QX,γ followed by QY,β and QZ,α and then a rotation QZ,−α followed by QY,−β and then QX,−γ is ⎡

G

QB = QX,−γ QY,−β QZ,−α QZ,α QY,β QX,γ

⎤ 100 = ⎣0 1 0⎦ 001

(2.46)

Example 19 Successive global rotations, global position. Numerical example of position of the tip point of a bar after several rotations about the global principal axes. The end point P of the arm shown in Fig. 2.8 is initially located at ⎡

⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ X1 0 0 0.0 ⎣ Y1 ⎦ = ⎣ l cos θ ⎦ = ⎣ 1 cos 75 ⎦ = ⎣ 0.26 ⎦ Z1 l sin θ 1 sin 75 0.97

(2.47)

The rotation matrix to find the new position of the end point after −29 deg rotation about the X-axis, followed by 30 deg about the Z-axis, and again 132 deg about the X-axis is

2.2 Successive Rotation About Global Cartesian Axes

45

Fig. 2.8 The arm of Example 19 G

and its new position is at

QB = QX,132 QZ,30 QX,−29 ⎡ ⎤⎡ ⎤ 1 0 0 cos π6 − sin π6 0 ⎦ ⎣ sin π cos π 0 ⎦ = ⎣ 0 cos 132π − sin 132π 180 180 6 6 132π 132π 0 0 1 0 sin 180 cos 180 ⎡ ⎤ 1 0 0 −29π ⎦ × ⎣ 0 cos −29π − sin 180 180 −29π −29π 0 sin 180 cos 180 ⎡ ⎤ 0.87 −0.44 −0.24 = ⎣ −0.33 −0.15 −0.93 ⎦ 0.37 0.89 −0.27

⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ X2 0.87 −0.44 −0.24 0.0 −0.35 ⎣ Y2 ⎦ = ⎣ −0.33 −0.15 −0.93 ⎦ ⎣ 0.26 ⎦ = ⎣ −0.94 ⎦ Z2 0.37 0.89 −0.27 0.97 −0.031

(2.48)

⎡

(2.49)

Example 20 Twelve independent triple global rotations. Triple global rotations are just enough to move a rigid body to any desired orientation. Inverse trigonometric functions are multi-valued, and this fact makes inverse kinematics problem complicated. Consider a rigid body in a final orientation after a series of rotations about global axes. We may transform its body coordinate frame B from the coincident position with a global frame G to any final orientation by only three rotations about the global axes provided that no two consequence rotations are about the same axis. In general, there are 12 different independent combinations of triple rotations about the global axes. They are 1 − QX,γ QY,β QZ,α 2 − QY,γ QZ,β QX,α 3 − QZ,γ QX,β QY,α 4 − QZ,γ QY,β QX,α 5 − QY,γ QX,β QZ,α 6 − QX,γ QZ,β QY,α 7 − QX,γ QY,β QX,α 8 − QY,γ QZ,β QY,α 9 − QZ,γ QX,β QZ,α 10 − QX,γ QZ,β QX,α 11 − QY,γ QX,β QY,α 12 − QZ,γ QY,β QZ,α

(2.50)

46

2 Rotation Kinematics

In other words, we may move a rigid body to any given final orientation by minimum three rotations about the global principal axes. There are 12 independent options for such maneuver. The value of the rotations α, β, and γ is different in the given 12 maneuvers for the same final orientation. The expanded form of the 12 global axes triple rotations is presented in Appendix A. As an example, assume we are given a transformation matrix G QB relating the final orientation of a rigid body to the global coordinate frame. ⎡ ⎤ 0.87 −0.44 −0.24 G QB = ⎣ −0.33 −0.15 −0.93 ⎦ (2.51) 0.37 0.89 −0.27 Selecting the first sequence of three rotations of case 1 − QX,γ QY,β QZ,α yields G

QB = QX,γ QY,β QZ,α ⎡ ⎤ cα cβ −cβ sα sβ = ⎣ cγ sα + cα sβ sγ cα cγ − sα sβ sγ −cβ sγ ⎦ sα sγ − cα cγ sβ cα sγ + cγ sα sβ cβ cγ

(2.52)

The element Q13 gives β, the ratio Q12 /Q11 gives α, and the ratio Q23 /Q33 gives γ . sin β = −0.24 0.44 0.87 0.93 tan γ = −0.27 tan α =

β = − (−1)k 0.242 + kπ rad α = 0.468 + kπ rad γ = −1.288 + kπ rad

(2.53) (2.54) (2.55)

It is a problem of trigonometric functions that their inverse functions are multi-valued. There is no straightforward method to determine the right solutions matching the problem in hand. Trial and error is usually the easiest way to find the right solution, as the matching one is associated with k = 0 or k = ±1. In this problem, we may check to find the matching solutions as follows: sin β = −0.24 tan α =

0.44 0.87

tan γ =

0.93 −0.27

β = − (−1) 0.242 + π rad = 3.383 rad α = 0.468 + π rad = 3.6096 rad γ = −1.288 rad

(2.56) (2.57) (2.58)

Therefore, we will make the same overall transformation matrix (2.51) by turning the rigid body 3.6096 rad about the Z-axis, then 3.383 rad about the Y -axis, and then −1.288 rad about the X-axis. G

QB = QX,γ QY,β QZ,α = QX,−1.29 QY,3.38 QZ,3.61 ⎡ ⎤ 0.86659 −0.43803 −0.23907 ⎢ ⎥ = ⎣ −0.33077 −0.14547 −0.93243 ⎦ 0.37365 0.88711 −0.27095

(2.59)

Example 21  Problem of inverse of trigonometric functions. It is always needed to examine all answers of inverse trigonometric functions to determine the correct ones. The general solutions of sin x = y, cos x = y, and tan x = y are arcsin y = (−1)k x + kπ

(2.60)

arccos y = ±x + 2kπ

(2.61)

arctan y = x + kπ

(2.62)

k = 0, 1, 2, 3, · · ·

2.2 Successive Rotation About Global Cartesian Axes

47

A big problem in mathematics that specifically affects kinematics of mechanisms and robots is the multi-valued inverse trigonometric functions. There must always be a checking process to realize which solution is the one that matches with the problem in hand. As a standard guidance, the inverse trigonometric functions usually denote the principal value. The principal values of the inverse trigonometric functions are defined as π π ≤ arcsin y ≤ 2 2 −1 ≤ y ≤ 1 0 ≤ arccos y ≤ π π π −∞ ≤ y ≤ ∞ − ≤ arctan y ≤ 2 2 −∞ ≤ y ≤ ∞ 0 ≤ arccot y ≤ π

(2.63)

π 0 ≤ arccsc y ≤ 2 π y ≤ −1 − ≤ arccsc y ≤ 0 2 π 1 ≤ y 0 ≤ arcsec y ≤ 2 π ≤ arcsec y ≤ π y ≤ −1 2

(2.64)

−1 ≤ y ≤ 1

−

1≤y

Example 22  Problem of inverse function symbol. Exponent −1 is not a correct sign to indicate inverse of a function. Exponent −1 stands for radiomimetic inverse. 1 x −1 = (2.65) x Imposing more than one operation to a symbol is a mistake making students and computers confused. f −1 (y) = y = f (x)

1 f (y)

(2.66)

x = f −1 (y)

(2.67)

Therefore, using exponent −1 to indicate inverse functions is not wise. It was wrongly introduced and accepted to show inverse functions by exponent −1. However, inverse of a function is an operator such as derivative and integral and needs to have a proper symbol. Let us for the moment use In as the symbol of inverse function operator. The inverse trigonometric functions also have special names. If sin x = y

cos x = y

tan x = y

(2.68)

then x = arcsin y = Inv sin y

(2.69)

x = arccos y = Inv cos y

(2.70)

x = arctan y = Inv tan y

(2.71)

As an example, let us find the inverse of a function Inv y. If y = f (x) = 2x + 3 then Inv y = because x=

x=

x−3 2

y−3 2

y−3 2

(2.72)

(2.73)

(2.74)

48

2 Rotation Kinematics

and we switch x and y to have y=

x−3 2

(2.75)

If y = 2x

(2.76)

Inv y = log2 x

(2.77)

then If y = x2

x = Inv y =

√

y

(2.78)

then dx 1 1 = √ = dy 2 y 2x

dy = 2x dx

dy dx 1 = 2x =1 dx dy 2x

(2.79) (2.80)

Example 23 Order of rotation and order of matrix multiplication. Order of rotation must match with order of rotation transformation matrix multiplication. Changing the order of global rotation matrices is equivalent to changing the order of rotations. Assume the position of a  T point P of a rigid body B to be at B rP = 1 2 3 . Its global position after rotation 30 deg about X-axis and then 60 deg about Y -axis is at G

rP

 1

= QY,60 QX,30 B rP ⎡ ⎤⎡ ⎤⎡ ⎤ cos π3 0 sin π3 1 0 0 1 =⎣ 0 1 0 ⎦ ⎣ 0 cos π6 − sin π6 ⎦ ⎣ 2 ⎦ − sin π3 0 cos π3 0 sin π6 cos π6 3 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0.5 0.433 0.75 1 3.616 =⎣ 0 0.866 −0.5 ⎦ ⎣ 2 ⎦ = ⎣ 0.232 ⎦ −0.866 0.25 0.433 3 0.933

(2.81)

If we change the order of rotations, then its position would be at G

rP

 2

= QX,30 QY,60 B rP ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0.5 0 0.866 1 3.0981 = ⎣ 0.433 0.866 −0.25 ⎦ ⎣ 2 ⎦ = ⎣ 1.4151 ⎦ −0.75 0.5 0.433 3 1.549

(2.82)

    These two final positions of P are d = G rP 1 − G rP 2 = 1.4309 apart. Example 24  An unsolved problem, repeated rotation about global axes. The general form of this problem has never been solved in mathematics. If we turn a body frame B about X-axis γ rad, where γ =

2π n

n∈N

(2.83)

then we need to repeat the rotation n times to turn the body back to its original configuration. We can check it by multiplying QX,γ by itself until we achieve an identity matrix. So, any body point of B will be mapped to the same point in the global frame. To show this, we may find QX,γ to the power m as

2.2 Successive Rotation About Global Cartesian Axes

49

⎡

Qm X,γ

⎡

⎤m

1

0 0 1 0 0 ⎢ 2π 2π ⎢ = ⎣ 0 cos γ − sin γ ⎦ = ⎢ 0 cos n − sin n ⎣ 2π 2π 0 sin γ cos γ 0 sin cos n n ⎤ ⎡ 1 0 0 ⎢ 2π ⎥ 2π ⎥ ⎢ = ⎢ 0 cos m n − sin m n ⎥ ⎣ 2π 2π ⎦ 0 sin m cos m n n

⎤m ⎥ ⎥ ⎥ ⎦

(2.84)

If m = n, then we have an identity matrix. ⎡ QnX,γ

1

0

⎤

0

⎢ 2π 2π ⎢ = ⎢ 0 cos n n − sin n n ⎣ 2π 2π 0 sin n cos n n n

⎡ ⎤ 100 ⎥ ⎥ ⎣ ⎥ = 0 1 0⎦ ⎦ 001

(2.85)

Repeated rotation about any other global axis provides the same result. Let us now rotate B about two global axes repeatedly, such as turning α about Z-axis followed by a rotation γ about X-axis, such that 2π 2π {n1 , n2 } ∈ N α= γ = (2.86) n1 n2 We may guess that repeating the rotations n = n1 × n2 times will turn B back to its original configuration, but it is not always a right solution. As an example, consider α = 2π and γ = 2π . We need 13 times combined rotations to achieve the original 3 4 configuration. ⎡

G

QB = QX,2π /4 QZ,2π/3

⎤ −0.5 −0.866 03 0 =⎣ 0 0 −1 ⎦ 0.866 03 −0.5 0

⎡

G

Q13 B

⎤ 0.9997 −0.01922 −0.01902 = ⎣ 0.01902 0.99979 −0.0112 ⎦ ≈ I 0.01922 0.01086 0.9998

(2.87)

(2.88)

We may turn B to its original configuration by lower number of combined rotations if n1 and n2 have a common divisor. For example, if n1 = n2 = 4, we only need to apply the combined rotations three times. ⎡

G

QB = QX,2π /4 QZ,2π /4

⎤ 0 −1 0 = ⎣ 0 0 −1 ⎦ 1 0 0

⎤ 1.0 0 0 G 3 QB = ⎣ 0 1.0 0 ⎦ = I 0 0 1.0

(2.89)

⎡

(2.90)

There is an unsolved problem. In a general case, determination of the minimum required number n to repeat a general combined rotation G QB to turn back to the original orientation is an unsolved question. G

QB =

m  j =1

QXi ,αj

i = 1, 2, 3

(2.91)

50

2 Rotation Kinematics

Fig. 2.9 Global roll, pitch, and yaw rotations

αj = G

2π nj

QnB = [I ]

m, nj ∈ N

(2.92)

n =?

(2.93)

Example 25 Global roll–pitch–yaw angles. Rotation about the X-axis, then Y -axis, and then Z-axis has special name as the same rotations about the body axes, but not as important. The rotation about the X-axis of the global coordinate frame is called a roll, the rotation about the Y -axis is called a pitch, and the rotation about the Z-axis is called a yaw. The global roll–pitch–yaw rotation matrix is G

QB = QZ,γ QY,β QX,α ⎡ ⎤ cβ cγ −cα sγ + cγ sα sβ sα sγ + cα cγ sβ = ⎣ cβ sγ cα cγ + sα sβ sγ −cγ sα + cα sβ sγ ⎦ −sβ cβ sα cα cβ

(2.94)

Figure 2.9 illustrates 45 deg roll, pitch, and yaw rotations about the axes of a global coordinate frame. Given the roll, pitch, and yaw angles, we can calculate the overall rotation matrix using Eq. (2.94). Also we are able to compute the equivalent roll, pitch, and yaw angles when a rotation matrix is given. Suppose that rij indicates the element of row i and column j of the roll–pitch–yaw rotation matrix (2.94), then the roll angle α is r32 r33

(2.95)

β = − arcsin r31

(2.96)

α = arctan tan and the pitch angle β is and the yaw angle γ is γ = arctan

r21 r11

(2.97)

provided that cos β = 0. Example 26 Determination of roll–pitch–yaw angles. A numerical example for determination of global roll–pitch–yaw angles for a given rotated rigid body. Let us determine the required roll–pitch–yaw angles that move the B-frame to an orientation at which the x-axis of the body frame B is parallel to u, while y-axis lays in (X, Y )-plane. u = Iˆ + 2Jˆ + 3Kˆ

(2.98)

2.2 Successive Rotation About Global Cartesian Axes

51

Because x-axis must be along u, we have G

1 2 3 u = √ Iˆ + √ Jˆ + √ Kˆ |u| 14 14 14

ıˆ =

= 0.267Iˆ + 0.534Jˆ + 0.802Kˆ

(2.99) (2.100)

and because y-axis is in (X, Y )-plane, we have G

  jˆ = Iˆ · jˆ Iˆ + Jˆ · jˆ Jˆ = cos θ Iˆ + sin θ Jˆ

(2.101)

The axes G ıˆ and G jˆ must be orthogonal, therefore, ⎡

⎤ ⎡ ⎤ 0.267 cos θ ⎣ 0.534 ⎦ · ⎣ sin θ ⎦ = 0 0.802 0 0.267 cos θ + 0.534 sin θ = 0 θ = −0.464 rad = −26.56 deg

(2.102) (2.103) (2.104)

We may find G kˆ by a cross product. ⎡

⎤ ⎡ ⎤ ⎡ ⎤ 0.267 0.894 0.358 Gˆ k = G ıˆ × G jˆ = ⎣ 0.534 ⎦ × ⎣ −0.447 ⎦ = ⎣ 0.717 ⎦ 0.802 0 −0.597

(2.105)

Hence, the transformation matrix G QB is ⎡

Iˆ · ıˆ ⎢ ˆ G QB = ⎣ J · ıˆ Kˆ · ıˆ

Iˆ · jˆ Jˆ · jˆ Kˆ · jˆ

⎤ ⎡ ⎤ Iˆ · kˆ 0.267 0.894 0.358 ⎥ Jˆ · kˆ ⎦ = ⎣ 0.534 −0.447 0.717 ⎦ 0.802 0 −0.597 Kˆ · kˆ

(2.106)

Now it is possible to determine the required roll–pitch–yaw angles to move the body coordinate frame B from the coincidence orientation with G to the final orientation. α = arctan

r32 0 = arctan =0 r33 −0.597

β = − arcsin r31 = − arcsin 0.802 ≈ −0.93 rad γ = arctan

G

0.534 r21 = arctan ≈ 1.1071 rad r11 0.267

QB = QZ,γ QY,β QX,α ⎡ ⎤ 0.267 39 −0.894 41 −0.358 53 = ⎣ 0.534 71 0.447 26 −0.716 97 ⎦ 0.801 62 0 0.597 83

(2.107) (2.108) (2.109)

(2.110)

52

2.3

2 Rotation Kinematics

Rotation About Local Cartesian Axes

Consider a rigid body B with a local coordinate frame B(Oxyz) that is originally coincident with a global coordinate frame G(OXY Z). Point O is the origin of both coordinate frames and is the only point of the body B to be fixed in the global frame G. If the body undergoes a rotation ϕ about the z-axis of its local coordinate frame, as can be seen in the top view shown in Fig. 2.10, then coordinates of any point of the rigid body in local and global coordinate frames are related by the following equation: B r = Az,ϕ G r (2.111) where B r is the B-expression of the position vector of the point P in B, and G r is the G-expression of the same vector, ⎡

⎤ X G r = ⎣Y ⎦ Z and Az,ϕ is the z-rotation matrix.

⎡ ⎤ x B r = ⎣y ⎦ z

⎡

Az,ϕ

⎤ cos ϕ sin ϕ 0 = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1

(2.112)

(2.113)

Similarly, rotation θ about the y-axis and rotation ψ about the x-axis of the local frame relate the local and global coordinates of point P by the following equations: B

r = Ay,θ G r

(2.114)

B

r = Ax,ψ r

(2.115)

G

where Ay,θ is the y-rotation matrix and Ax,ψ is the x-rotation matrix. ⎡

Ay,θ

⎤ cos θ 0 − sin θ =⎣ 0 1 0 ⎦ sin θ 0 cos θ ⎡

Ax,ψ

⎤ 1 0 0 = ⎣ 0 cos ψ sin ψ ⎦ 0 − sin ψ cos ψ

(2.116)

(2.117)

Proof Assume that vector r indicates the position of a point P of the rigid body B where it is initially at P1 . The rigid body turns α about the z-axis of the body coordinate frame B, and hence, point P goes from P1 to P2 indicated by vector r2 . Using ˆ along the axes of local coordinate frame B(Oxyz) and (Iˆ, Jˆ, K) ˆ along the axes of global coordinate the unit vectors (ˆı , jˆ, k) frame G(OXY Z), the initial and final position vectors r1 and r2 in both coordinate frames can be expressed by B

r1 = x1 ıˆ + y1 jˆ + z1 kˆ

(2.118)

G

r1 = X1 Iˆ + Y1 Jˆ + Z1 Kˆ

(2.119)

B

r2 = x2 ıˆ + y2 jˆ + z2 kˆ

(2.120)

G

ˆ r2 = X2 Iˆ + Y2 Jˆ + Z2 K.

(2.121)

The vectors B r1 and B r2 are the initial and final positions of the vector r expressed in body coordinate frame B(Oxyz), and G r1 and G r2 are the initial and final positions of the vector r expressed in the global coordinate frame G(OXY Z). The components of B r2 can be found if we have the components of G r2 . Using Eqs. (2.120) and (2.121) and the definition of the inner product, we may write

2.3 Rotation About Local Cartesian Axes

53

Fig. 2.10 Position vectors of point P before and after rotation of the local frame about the z-axis of the local frame

or equivalently

x2 = ıˆ · r2 = ıˆ · X2 Iˆ + ıˆ · Y2 Jˆ + ıˆ · Z2 Kˆ

(2.122)

y2 = jˆ · r2 = jˆ · X2 Iˆ + jˆ · Y2 Jˆ + jˆ · Z2 Kˆ

(2.123)

z2 = kˆ · r2 = kˆ · X2 Iˆ + kˆ · Y2 Jˆ + kˆ · Z2 Kˆ

(2.124)

⎤ ⎡ ıˆ · Iˆ ıˆ · Jˆ x2 ⎢ ⎣ y2 ⎦ = ⎣ jˆ · Iˆ jˆ · Jˆ z2 kˆ · Iˆ kˆ · Jˆ ⎡

⎤⎡ ⎤ ıˆ · Kˆ X2 ⎥ jˆ · Kˆ ⎦ ⎣ Y2 ⎦ . Z2 kˆ · Kˆ

(2.125)

The elements of the z-rotation matrix Az,ϕ are the direction cosines of G r2 with respect to axes of B(Oxyz). So, the elements of the matrix in Eq. (2.125) are ıˆ · Iˆ = cos ϕ jˆ · Iˆ = − sin ϕ kˆ · Iˆ = 0

ıˆ · Jˆ = sin ϕ jˆ · Jˆ = cos ϕ kˆ · Jˆ = 0

ıˆ · Kˆ = 0 jˆ · Kˆ = 0 kˆ · Kˆ = 1

(2.126)

Combining Eqs. (2.125) and (2.126), we find the components of B r2 by multiplying z-rotation matrix Az,ϕ and vector G r2 . r2 = Az,ϕ G r2 ⎡ ⎤ ⎡ ⎤⎡ ⎤ x2 cos ϕ sin ϕ 0 X2 ⎣ y2 ⎦ = ⎣ − sin ϕ cos ϕ 0 ⎦ ⎣ Y2 ⎦ z2 Z2 0 0 1 B

(2.127) (2.128)

Equation (2.127) indicates that after rotation about the z-axis of the local coordinate frame, the position vector in the local frame is equal to Az,ϕ times the position vector in the global frame. Hence, after rotation about the z-axis, we are able to find the coordinates of any point of a rigid body in local coordinate frame, if we have its coordinates in the global frame. Similarly, rotation θ about the y-axis and rotation ψ about the x-axis are described by the y-rotation matrix Ay,θ and the x-rotation matrix Ax,ψ , respectively. The rotation matrices Az,ϕ , Ay,θ , and Ax,ψ , ⎡

Az,ϕ

⎤ cos ϕ sin ϕ 0 = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1 ⎡

Ay,θ

⎤ cos θ 0 − sin θ =⎣ 0 1 0 ⎦ sin θ 0 cos θ

(2.129)

(2.130)

54

2 Rotation Kinematics

Fig. 2.11 Arm of Example 28

⎡

Ax,ψ

⎤ 1 0 0 = ⎣ 0 cos ψ sin ψ ⎦ 0 − sin ψ cos ψ

(2.131)

are called basic body rotation matrices. We usually refer to the first, second, and third rotations about the axes of the body coordinate frame by ϕ, θ, and ψ, respectively. All of the basic global rotation matrices Az,ϕ , Ay,θ , and Ax,ψ transform a G-expression vector to its B-expression. We use local and body coordinate frames equally.  Example 27 Local rotation, local position. Local rotation, global position. A numerical example to determine position of a given point after rotation. If a body coordinate frame Oxyz has been rotated 60 deg about the z-axis and a point P in the global coordinate frame OXY Z is at (4, 3, 2), then its coordinates in the body coordinate frame Oxyz are ⎡ π π ⎤ ⎡ ⎤ cos sin 0 ⎡ 4 ⎤ ⎡ 4.60 ⎤ x 3 3 ⎥ ⎢ ⎣ y ⎦ = ⎢ − sin π cos π 0 ⎥ ⎣ 3 ⎦ = ⎣ −1.96 ⎦ ⎣ ⎦ 3 3 z 2 2.0 0 0 1

(2.132)

If a local coordinate frame Oxyz has been rotated 60 deg about the z-axis and a point P in the local coordinate frame Oxyz is at (4, 3, 2), its position in the global coordinate frame OXY Z is at ⎡ π π ⎤T ⎡ ⎤ ⎡ ⎤ ⎤ cos sin 0 4 X −0.60 3π 3 ⎢ ⎥ ⎣ Y ⎦ = ⎢ − sin cos π 0 ⎥ ⎣ 3 ⎦ = ⎣ 4.96 ⎦ ⎣ ⎦ 3 3 Z 2 2.0 0 0 1 ⎡

(2.133)

Example 28 Successive local rotation, global position. Global position of the tip point of a robotic arm after few rotations. The arm shown in Fig. 2.11 has two actuators. The first actuator rotates the arm −90 deg about y-axis, and then the second actuator rotates the arm 90 deg about x-axis. If, before the rotations, the end point P is at B

 T rP = 9.5 −10.1 10.1

(2.134)

then its position in the global coordinate frame is at G

 −1 B −1 B rP = Ax,π /2 Ay,−π /2 rP = A−1 y,−π/2 Ax,π /2 rP ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 −1 0 9.5 10.1 = ⎣ 0 0 −1 ⎦ ⎣ −10.1 ⎦ = ⎣ −10.1 ⎦ 1 0 0 10.1 9.5

(2.135)

2.4 Successive Rotation About Local Cartesian Axes

2.4

55

Successive Rotation About Local Cartesian Axes

Consider a point P in a rigid body B(Oxyz) at position vector r. Having the final global position vector G r of P , we can determine its local position vector B r after a series of sequential rotations A1 , A2 , A3 , . . ., An about the local axes, by B

r=

B

AG G r

(2.136)

where B

AG = An · · · A3 A2 A1

(2.137)

B

The matrix AG is the local rotation matrix that maps the global coordinates to their corresponding local coordinates.

Proof Assume the body coordinate frame B(Oxyz) is initially coincident with the global coordinate frame G(OXY Z). The rigid body rotates about a local axis such that a local rotation matrix A1 relates the global coordinates of a body point P at G r to the associated local coordinates. B r = R1 G r (2.138) Let us introduce an intermediate space-fixed frame G1 coincident with the new position of the body coordinate frame, G1

r≡

B

r

(2.139)

and give the rigid body a second rotation about a local coordinate axis. Then another local rotation matrix A2 relates the coordinates in the intermediate fixed frame to the corresponding local coordinates: B

r = A2 G1 r

(2.140)

To relate the final coordinates of the point, we must first transform its global coordinates to the intermediate fixed frame and then transform to the original body frame. Substituting (2.138) in (2.140) shows that B

r = A2 A1 G r

(2.141)

Following the same procedure, we can determine the final global position of a body point after a series of sequential rotations A1 , A2 , A3 , . . ., An as (2.137). Rotation about the local coordinate axes is conceptually more interesting than rotation about global coordinate axes. It is because in a sequence of rotations every rotation is about an axis, which has been moved to its new global position during the previous rotation. The multiplication of rotation transformation matrices is associative. A3 A2 A1 = A3 [A2 A1 ] = [A3 A2 ] A1

(2.142) 

Example 29 Successive local rotation, local position. A numerical example to determine coordinates of a body point of a given global point after several rotations. A local coordinate frame B(Oxyz) that initially is coincident with a global coordinate frame G(OXY Z) undergoes a rotation ϕ = 30 deg about the z-axis, then θ = 30 deg about the x-axis, and then ψ = 30 deg about the y-axis. The local coordinates of a point P located at X = 5, Y = 30, and Z = 10 can be found by  The local rotation matrix is

xyz

T

 T = Ay,ψ Ax,θ Az,ϕ 5 30 10

(2.143)

56

2 Rotation Kinematics

⎡

B

AG = Ay,30 Ax,30 Az,30

⎤ 0.63 0.65 −0.43 = ⎣ −0.43 0.75 0.50 ⎦ 0.65 −0.125 0.75

and coordinates of P in the local frame are ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ x 0.63 0.65 −0.43 5 18.28 ⎣ y ⎦ = ⎣ −0.43 0.75 0.50 ⎦ ⎣ 30 ⎦ = ⎣ 25.33 ⎦ z 0.65 −0.125 0.75 10 7.0

(2.144)

(2.145)

Example 30 Twelve independent triple local rotations. Any triple local rotations is just enough to move a rigid body to any desired orientation. Inverse trigonometric functions are multi-valued, and this fact makes inverse kinematics problem complicated. Leonhard Euler (1707–1783) proved that any two independent orthogonal coordinate frames with a common origin can be related by a sequence of three rotations about the local coordinate axes, where no two successive rotations may be about the same axis. In other words, we may transform a body coordinate frame B from the coincident position with a global frame G to any final orientation by minimum three rotations about the local axes provided that no two consequence rotations are about the same axis. In general, there are 12 different independent combinations of triple rotation about local axes. They are 1 − Ax,ψ Ay,θ Az,ϕ 2 − Ay,ψ Az,θ Ax,ϕ 3 − Az,ψ Ax,θ Ay,ϕ 4 − Az,ψ Ay,θ Ax,ϕ 5 − Ay,ψ Ax,θ Az,ϕ 6 − Ax,ψ Az,θ Ay,ϕ 7 − Ax,ψ Ay,θ Ax,ϕ 8 − Ay,ψ Az,θ Ay,ϕ 9 − Az,ψ Ax,θ Az,ϕ 10 − Ax,ψ Az,θ Ax,ϕ 11 − Ay,ψ Ax,θ Ay,ϕ 12 − Az,ψ Ay,θ Az,ϕ

(2.146)

The expanded form of the 12 local axes’ triple rotation is presented in Appendix B. As an example, assume we are given a transformation matrix G AB relating the final orientation of a rigid body to the global coordinate frame. ⎡ ⎤ 0.63 0.65 −0.43 G AB = ⎣ −0.43 0.75 0.50 ⎦ (2.147) 0.65 −0.125 0.75 Selecting the first sequence of three rotations of case 1 − Ax,ψ Ay,θ Az,ϕ yields G

AB = Ax,ψ Ay,θ Az,ϕ ⎡ ⎤ cθ cϕ cθ sϕ −sθ = ⎣ −cψ sϕ + cϕ sθ sψ cϕ cψ + sθ sϕ sψ cθ sψ ⎦ sϕ sψ + cϕ sθ cψ −cϕ sψ + sθ cψ sϕ cθ cψ

(2.148)

The element A13 gives θ, the ratio A12 /A11 gives ϕ, and the ratio A23 /A33 gives ψ. sin θ = 0.43 0.65 0.63 0.50 tan ψ = 0.75 tan ϕ =

θ = (−1)k 0.444 + kπrad

(2.149)

ϕ = 0.801 + kπrad

(2.150)

ψ = 0.588 + kπ rad

(2.151)

In this problem, we may check to find the matching solutions as follows:

2.4 Successive Rotation About Local Cartesian Axes

57

θ = (−1) 0.444 + π rad = 2.697 rad

(2.152)

ϕ = 0.801 + π rad = 3.943 rad

(2.153)

ψ = 0.588 + π rad = 3.7296 rad

(2.154)

Therefore, we will make the same overall transformation matrix (2.147) by turning the rigid body 3.943 rad about the z-axis, then 2.697 rad about the y-axis, and then 3.7296 rad about the x-axis. G

AB = Ax,ψ Ay,θ Az,ϕ = Ax,3.73 Ay,2.69 Az,3.94 ⎡ ⎤ 0.62806 0.6485 −0.43009 ⎢ ⎥ = ⎣ −0.43172 0.75023 0.50078 ⎦ 0.64742 −0.12885 0.75116

(2.155)

Example 31  Repeated rotation about body axes. A numerical example. Consider a body frame B that turns ϕ about the z-axis. If n times fraction of 2π ϕ=

2π n

n∈N

(2.156)

then we need to repeat the rotation n times to get back the body B to the original configuration. We can check it by multiplying Rz,ϕ by itself until an identity matrix is achieved. To show this, we may find that

m Rz,ϕ

⎡

⎤m 2π 2π cos sin 0 cos ϕ sin ϕ 0 ⎢ n n ⎥ ⎢ 2π ⎥ 2π = ⎣ − sin ϕ cos ϕ 0 ⎦ = ⎢ cos 0⎥ ⎣ − sin ⎦ n n 0 0 1 0 0 1 ⎡ ⎤ 2π m 2πm sin 0⎥ ⎢ cos n n ⎢ ⎥ 2π m 2πm =⎢ cos 0⎥ ⎣ − sin ⎦ n n 0 0 1 ⎡

and when m = n, we have

⎤m

⎡

n Rz,ϕ

⎤ 2π 2π ⎡ ⎤ sin n 0 cos n 100 ⎢ ⎥ n n ⎢ 2π 2π ⎥ =⎢ = ⎣0 1 0⎦ cos n 0⎥ ⎣ − sin n ⎦ n n 001 0 0 1

(2.157)

(2.158)

Repeated rotation about any other body axis provides the same result. Example 32  Open problem. An unsolved problem, repeated rotation about local axes. The general form of this problem has never been solved in mathematics. Consider a body frame B that turns ϕ about the xi -axis followed by a rotation θ about the xj -axis and ψ about the xk -axis such that 2π 2π 2π {n1 , n2 , n3 } ∈ N ϕ= θ= ψ= (2.159) n1 n2 n3 Although it seems that we need to repeat the rotations n = n1 × n2 × n3 times to get back to the original configuration, it is not true in general. The determination of the required number n to repeat a general combined rotation to get back to the original orientation has not solved yet and is still an open question: B

RG =

m  j =1

Rxi ,ϕ j

i = 1, 2, 3

(2.160)

58

2 Rotation Kinematics

Fig. 2.12 Local roll–pitch–yaw angles

ϕj = B

2π nj

m, nj ∈ N

n RG = [I ]

(2.161)

n =?

(2.162)

Example 33 Local roll–pitch–yaw angles. Orientation expression of rigid bodies about local principal axes is very applied method in study of all types of vehicle dynamics. Rotation about the x-axis of the local frame is called roll or bank, rotation about y-axis of the local frame is called pitch or attitude, and rotation about the z-axis of the local frame is called yaw, spin, or heading. The local roll–pitch–yaw angles are shown in Fig. 2.12. The local roll–pitch–yaw rotation matrix is B

AG = Az,ψ Ay,θ Ax,ϕ ⎡ ⎤ cθ cψ cϕ sψ + sθ cψvsϕ sϕ sψ − cϕ sθ cψ ⎢ ⎥ = ⎣ −cθ sψ cϕ cψ − sθ sϕ sψ cψ sϕ + cϕ sθvsψ ⎦ sθ −cθsϕ cθ cϕ

G

QB =

B

(2.163)

 T ATG = Az,ψ Ay,θ Ax,ϕ = ATx,ϕ ATy,θ ATz,ψ

= QX,ϕ QY,θ QZ,ψ ⎡ ⎤ cθ cψ −cθ sψ sθ ⎢ ⎥ = ⎣ cϕ sψ + sθ cψ sϕ cϕ cψ − sθ sϕ sψ −cθ sϕ ⎦ sϕ sψ − cϕ sθ cψ cψ sϕ + cϕ sθ sψ cθ cϕ

(2.164)

Note the difference  between roll–pitch–yaw and Euler angles, although we show both utilizing ϕ, θ, and ψ. Having a rotation matrix B QG = rij , provided that cos θ = 0, we are able to determine the equivalent local roll, pitch, and yaw angles by θ = arcsin r31

ϕ = − arctan

r32 r33

ψ = − arctan

r21 r11

(2.165)

Example 34  Angular velocity and local roll–pitch–yaw rate. This is the first time introducing angular velocity without definition. Angular velocity is expressed as time rate of roll–pitch–yaw angles. Using the roll–pitch–yaw frequencies, the angular velocity G ωB of a body B with respect to the global reference frame G is ˆ ˙ eˆϕ + θ˙ eˆθ + ψ˙ eˆψ (2.166) G ωB = ωx ıˆ + ωy jˆ + ωz k = ϕ Relationships between the components of G ωB in body frame and roll–pitch–yaw components are found when the local roll  T unit vector eˆϕ and pitch unit vector eˆθ are transformed to the body frame. The roll unit vector eˆϕ = 1 0 0 transforms to the body frame after rotation θ and then rotation ψ.

2.5 Euler Angles

59

B

eˆϕ = Az,ψ Ay,θ

⎡ ⎤ ⎡ ⎤ 1 cos θ cos ψ ⎣ 0 ⎦ = ⎣ − cos θ sin ψ ⎦ 0 sin θ

(2.167)

 T The pitch unit vector eˆθ = 0 1 0 transforms to the body frame after rotation ψ.

B

eˆθ = Az,ψ

⎡ ⎤ ⎡ ⎤ 0 sin ψ ⎣ 1 ⎦ = ⎣ cos ψ ⎦ 0 0

(2.168)

 T The yaw unit vector eˆψ = 0 0 1 is already along the local z-axis. Hence, G ωB can be expressed in body frame B (Oxyz) as ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ωx cos θ cos ψ sin ψ 0 ⎥ ⎢ ⎢ ⎥ ˙⎢ ⎥ ˙⎢ ⎥ B ω = = ϕ ˙ + θ + ψ ω − cos θ sin ψ cos ψ ⎣ y⎦ ⎣ ⎦ ⎣ ⎦ ⎣0⎦ G B ωz sin θ 0 1 ⎡ ⎤⎡ ⎤ cos θ cos ψ sin ψ 0 ϕ˙ ⎢ ⎥⎢ ⎥ = ⎣ − cos θ sin ψ cos ψ 0 ⎦ ⎣ θ˙ ⎦ sin θ 0 1 ψ˙ ⎡

(2.169)

and therefore, G ωB in global frame G (OXY Z) in terms of local roll–pitch–yaw frequencies is ⎤ ⎡ ⎤ ⎡ ⎤ ωx θ˙ sin ψ + ϕ˙ cos θ cos ψ ωX ⎥ ⎥ ⎥ ⎢ G B −1 ⎢ B −1 ⎢ ˙ ˙ cos θ sin ψ ⎦ G ωB = ⎣ ωY ⎦ = AG ⎣ ωy ⎦ = AG ⎣ θ cos ψ − ϕ ψ˙ + ϕ˙ sin θ ωZ ωz ⎡ ⎤ ϕ˙ + ψ˙ sin θ ⎢˙ ⎥ = ⎣ θ cos ϕ − ψ˙ cos θ sin ϕ ⎦ θ˙ sin ϕ + ψ˙ cos θ cos ϕ ⎡ ⎤⎡ ⎤ 1 0 sin θ ϕ˙ ⎢ ⎥⎢ ⎥ = ⎣ 0 cos ϕ − cos θ sin ϕ ⎦ ⎣ θ˙ ⎦ 0 sin ϕ cos θ cos ϕ ψ˙ ⎡

2.5

(2.170)

Euler Angles

The rotation about the Z-axis of the global coordinate is called precession, the rotation about the x-axis of the local coordinate is called nutation, and the rotation about the z-axis of the local coordinate is called spin. The precession–nutation–spin rotation angles are also called Euler angles. Euler angles rotation matrix has many application in rigid body kinematics. The kinematics and dynamics of axisymmetric rigid bodies have simpler and more understandable expression based on Euler angles. The Euler angle rotation matrix B AG to transform a position vector from G(OXY Z) to B(Oxyz) B

r=

B

AG G r

(2.171)

is B

AG = Az,ψ Ax,θ Az,ϕ ⎡

⎤

cϕ cψ − cθ sϕ sψ cψ sϕ + cθ cϕ sψ sθ sψ = ⎣ −cϕ sψ − cθ cψ sϕ −sϕ sψ + cθ cϕ cψ sθ cψ ⎦ sθ sϕ −cϕ sθ cθ

(2.172)

60

2 Rotation Kinematics

Fig. 2.13 First Euler angle

and hence, the Euler angle rotation matrix G QB to transform a position vector from B(Oxyz) to G(OXY Z) G

r=

G

QB B r

(2.173)

is G

QB =

B

A−1 G =

B

 T ATG = Az,ψ Ax,θ Az,ϕ

⎤ cϕ cψ − cθ sϕ sψ −cϕ sψ − cθ cψ sϕ sθ sϕ ⎥ ⎢ = ⎣ cψ sϕ + cθ cϕ sψ −sϕ sψ + cθ cϕ cψ −cϕ sθ ⎦ sθ sψ sθ cψ cθ ⎡

(2.174)

Proof To find the Euler angle rotation matrix B RG to go from the global frame G(OXY Z) to the final body frame B(Oxyz), we employ a temporary body frame B  (Ox  y  z ) as shown in Fig. 2.13 that before the first rotation coincides with the global frame. Let there be at first a rotation ϕ about the z -axis. The first rotation ϕ about the Z-axis relates the coordinate systems by  G r = G QB  B r (2.175) Using matrix inversion, we find the body position for a given global position vector: B

r=

G

G Q−1 r= B

B

AG G r

(2.176) 

Therefore, the first rotation is equivalent to a rotation about the local z-axis by looking for B r: B

B

r=

B

AG G r ⎡

⎤

cos ϕ sin ϕ 0 AG = Az,ϕ = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1

(2.177) (2.178)

Next we assume the frame B  (Ox  y  z ) to be a new fixed global frame and introduce a new body frame B  (Ox  y  z ). Before the second rotation, the two frames coincide. Then, we apply a rotation θ about x  -axis as shown in Fig. 2.14. The transformation between B  (Ox  y  z ) and B  (Ox  y  z ) is B 

B 

A

r=

B

B 



AB  B r ⎡

= Ax,θ

⎤

1 0 0 ⎣ = 0 cos θ sin θ ⎦ 0 − sin θ cos θ

(2.179) (2.180)

Finally we consider the frame B  (Ox  y  z ) as a new fixed global frame and consider the final body frame B(Oxyz) to coincide with B  before the third rotation. We now execute a ψ rotation about the z -axis as shown in Fig. 2.15. The transformation between B  (Ox  y  z ) and B(Oxyz) is

2.5 Euler Angles

61

Fig. 2.14 Second Euler angle

Fig. 2.15 Third Euler angle

B

B

r=

B



AB  B r ⎡

⎤

cos ψ sin ψ 0 AB  = Az,ψ = ⎣ − sin ψ cos ψ 0 ⎦ 0 0 1

(2.181) (2.182)

By the rule of composition of rotations, the transformation from G(OXY Z) to B(Oxyz) is B

r=

B

AG G r

(2.183)

where B

AG = Az,ψ Ax,θ Az,ϕ ⎤ ⎡ cϕ cψ − cθ sϕ sψ cψ sϕ + cθ cϕ sψ sθ sψ ⎥ ⎢ = ⎣ −cϕ sψ − cθ cψ sϕ −sϕ sψ + cθ cϕ cψ sθ cψ ⎦ sθ sϕ −cϕ sθ cθ

(2.184)

and therefore, G

QB =

B

⎡

A−1 G =

B

 T ATG = Az,ψ Ax,θ Az,ϕ

⎤ cϕ cψ − cθ sϕ sψ −cϕ sψ − cθ cψ sϕ sθ sϕ ⎢ ⎥ = ⎣ cψ sϕ + cθ cϕ sψ −sϕ sψ + cθ cϕ cψ −cϕ sθ ⎦ sθ sψ sθ cψ cθ

(2.185)

62

2 Rotation Kinematics

Given the angles of precession ϕ, nutation θ, and spin ψ, we can determine the overall rotation matrix using Equation (2.184). Also we are able to calculate the equivalent precession, nutation, and spin angles when a rotation matrix is given. If rij indicates the element of row i and column j of the precession–nutation–spin rotation matrix (2.185), provided that sin θ = 0, we have θ = arccos r33 (2.186) r31 ϕ = − arctan (2.187) r32 r13 (2.188) ψ = arctan r23 The transformation matrix (2.184) is called a local Euler rotation matrix, and (2.185) is called a global Euler rotation matrix.  Example 35 Euler angle rotation matrix. A numerical example for local and global Euler angles transformation. The Euler or precession–nutation–spin rotation matrix for ϕ = 79.15 deg, θ = 41.41 deg, and ψ = −40.7 deg would be found by substituting ϕ = 1.38 rad, θ = 0.72 rad, and ψ = −0.71 rad in Eq. (2.184) B

AG = Az,−0.71 Ax,0.72 Az,1.38 ⎡ ⎤ 0.623 0.652 −0.431 = ⎣ −0.436 0.747 0.501 ⎦ 0.649 −0.124 0.75

(2.189)

and the inverse of the transformation will be G

QB =

B

⎡

A−1 G

⎤

(2.190)

0.623 −0.436 0.65 = ⎣ 0.653 0.748 −0.124 ⎦ −0.431 0.501 0.751 Example 36 Euler angles of a local transformation matrix. A numerical example to determine Euler angles to provide the same rotation matrix made of given rotations. The local rotation matrix after rotation of 30 deg about the z-axis, then 30 deg about the x-axis, and then 30 deg about the y-axis is B

AG = Ay, π6 Ax, π6 Az, π6 ⎡ ⎤ 0.63 0.65 −0.43 = ⎣ −0.43 0.75 0.50 ⎦ 0.65 −0.125 0.75

(2.191)

and therefore, the local coordinates of a sample point at X = 5, Y = 30, and Z = 10 are ⎤ ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ x 0.63 0.65 −0.43 5 18.35 ⎣ y ⎦ = ⎣ −0.43 0.75 0.50 ⎦ ⎣ 30 ⎦ = ⎣ 25.35 ⎦ z 0.65 −0.125 0.75 10 7.0

(2.192)

The Euler angles of the corresponding precession–nutation–spin rotation matrix are θ = arccos (r33 ) = arccos 0.75 = 0.72 rad = 41.4 deg ϕ = − arctan ψ = arctan

r31 0.65 = − arctan = 1.38 rad = 79.1 deg r32 −0.125

r13 −0.43 = arctan = −0.71 rad = −40.7 deg r23 0.50

(2.193) (2.194) (2.195)

2.5 Euler Angles

63

Hence, Ay, π6 Ax, π6 Az, π6 = Az,ψ Ax,θ Az,ϕ when ϕ = 1.38 rad, θ = 0.72 rad, and ψ = −0.71 rad. B

AG = Az,−0.71 Ax,0.72 Az,1.38 ⎡ ⎤ 0.62306 0.65253 −0.43128 = ⎣ −0.43545 0.74741 0.50176 ⎦ 0.64976 −0.12482 0.74982

(2.196)

In other words, the rigid body attached to the local frame moves to the final configuration by undergoing either three consecutive rotations ϕ = 79.1 deg, θ = 41.4 deg, and ψ = −40.7 deg about z, x, and z axes, respectively, or three consecutive rotations 30 deg, 30 deg, and 30 deg about z, x, and y axes. Example 37 Relative rotation matrix of two bodies. A numerical example to relate two sets of Euler angles rotations. Consider a rigid body B1 with an orientation matrix B1 AG made by Euler angles ϕ = 30 deg, θ = −45 deg, ψ = 60 deg, and another rigid body B2 having ϕ = 10 deg, θ = 25 deg, ψ = −15 deg, with respect to the global frame. To find the relative rotation matrix B1 AB2 to map the coordinates of second body frame B2 to the first body frame B1 , we need to find the individual rotation matrices first. B1

AG = Az,60 Ax,−45 Az,30 ⎡ ⎤ 0.127 0.78 −0.612 = ⎣ −0.927 −0.127 −0.354 ⎦ −0.354 0.612 0.707

(2.197)

B2

AG = Az,10 Ax,25 Az,−15 ⎡ ⎤ 0.992 −0.0633 −0.109 = ⎣ 0.103 0.907 0.408 ⎦ 0.0734 −0.416 0.906

(2.198)

The desired rotation matrix B1 AB2 may be found by B1

AB2 =

B1

AG G AB2

(2.199)

which is equal to B1

AB2 =

B1

⎡

AG B2 A−1 G =

B1

AG B2 ATG

⎤

(2.200)

0.14353 0.47084 −0.87026 = ⎣ −0.87277 −0.35498 −0.33587 ⎦ −0.46737 0.80748 0.36049 Example 38  Euler angle rotation matrix for small angles. It shows what happens when Euler angles are very small. The Euler rotation matrix B AG = Az,ψ Ax,θ Az,ϕ for very small Euler angles ϕ, θ, and ψ is approximated by ⎡

⎤ 1 γ 0 B AG = ⎣ −γ 1 θ ⎦ 0 −θ 1

(2.201)

γ =ϕ+ψ

(2.202)

where Therefore, in case of small angles of rotation, the angles ϕ and ψ are indistinguishable.

64

2 Rotation Kinematics

Fig. 2.16 Application of Euler angles in describing the configuration of a top

If only θ → 0, then the Euler rotation matrix B AG = Az,ψ Ax,θ Az,ϕ approaches to ⎡

⎤ cϕ cψ − sϕ sψ cψ sϕ + cϕ sψ 0 B AG = ⎣ −cϕ sψ − cψ sϕ −sϕ sψ + cϕ cψ 0 ⎦ 0 0 1 ⎡ ⎤ cos (ϕ + ψ) sin (ϕ + ψ) 0 = ⎣ − sin (ϕ + ψ) cos (ϕ + ψ) 0 ⎦ 0 0 1

(2.203)

and therefore, the angles ϕ and ψ are indistinguishable even if the value of ϕ and ψ is finite. Hence, the Euler set of angles in rotation matrix (2.172) is not unique when θ = 0. Example 39 Euler angles application in motion of rigid bodies. Euler angles are the best way to express orientation of a top. The Zxz Euler angles are good parameters to describe the configuration of a rigid body with a fixed point. The Euler angles to show the configuration of a top are shown in Fig. 2.16 as an example. The Zxz Euler angles seem to be natural parameters to express the configuration of rotating axisymmetric rigid bodies. Euler angles that show the expression of a top in Fig. 2.16 are a good example. The rotation of the top about its axis of symmetry is called spin ψ. The angle between the axis of symmetry and the Z-axis is called nutation θ, and the rotation of the axis of symmetry about the Z-axis is called precession ϕ. The motion of the Earth will also be expressed by Euler angles better. Example 40  Angular velocity vector in terms of Euler frequencies. Angular velocity is expressed as time rate of Euler angles.   An Eulerian local frame E O, eˆϕ , eˆθ , eˆψ can be introduced by defining unit vectors eˆϕ , eˆθ , and eˆψ as shown in Fig. 2.17. Although the Eulerian frame is not necessarily orthogonal, it is very useful in rigid body kinematic analysis. The angular velocity vector G ωB of the body frame B(Oxyz) with respect to the global frame G(OXY Z) can be expressed in the nonorthogonal Euler angles frame E as the sum of three Euler angle rate vectors. E G ωB

= ϕ˙ eˆϕ + θ˙ eˆθ + ψ˙ eˆψ

˙ and ψ, ˙ are called Euler frequencies. The rate of Euler angles, ϕ, ˙ θ,

(2.204)

2.5 Euler Angles

65

Fig. 2.17 Euler angles frame eˆϕ , eˆθ , eˆψ

To find G ωB in body frame, we must express the unit vectors eˆϕ , eˆθ , and eˆψ shown in Fig. 2.17, in the body frame B. The unit vector eˆϕ = Kˆ is in the global frame G.  T eˆϕ = 0 0 1 = Kˆ (2.205) It can be transformed to the body frame after three rotations. ⎡

⎤ sin θ sin ψ B eˆϕ = B AG Kˆ = Az,ψ Ax,θ Az,ϕ Kˆ = ⎣ sin θ cos ψ ⎦ cos θ

(2.206)

The unit vector eˆθ = ıˆ is in the intermediate frame Ox  y  z ,  T eˆθ = 1 0 0 = ıˆ and it needs to get two rotations Ax,θ and Az,ψ to be transformed to the body frame. ⎡ ⎤ cos ψ B B   eˆθ = AOx  y  z ıˆ = Az,ψ Ax,θ ıˆ = ⎣ − sin ψ ⎦ 0

(2.207)

(2.208)

The unit vector eˆψ is already in the body frame.  T eˆψ = 0 0 1 = kˆ

(2.209)

Therefore, G ωB is expressed in body coordinate frame. ⎡

⎤ ⎡ ⎤ ⎡ ⎤ sin θ sin ψ cos ψ 0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ B ˙ ⎣ sin θ cos ψ ⎦ + θ˙ ⎣ − sin ψ ⎦ + ψ˙ ⎣ 0 ⎦ G ωB = ϕ cos θ 0 1     = ϕ˙ sin θ sin ψ + θ˙ cos ψ ıˆ + ϕ˙ sin θ cos ψ − θ˙ sin ψ jˆ   + ϕ˙ cos θ + ψ˙ kˆ

(2.210)

Therefore, components of G ωB in body frame B (Oxyz) are related to the Euler angle frame E (Oϕθψ) by the following equation, in which, the transformation matrix B AE is not orthogonal and hence is not a rotation matrix.

66

2 Rotation Kinematics

⎡

B G ωB

⎤

=

B

⎡

AE

E G ωB

⎤⎡

(2.211)

⎤

ωx sin θ sin ψ cos ψ 0 ϕ˙ ⎣ ωy ⎦ = ⎣ sin θ cos ψ − sin ψ 0 ⎦ ⎣ θ˙ ⎦ cos θ 0 1 ψ˙ ωz

(2.212)

Then, G-expression of the angular velocity G ωB can be expressed in the global frame using an inverse transformation of Euler rotation matrix. ⎡

⎤ ϕ˙ sin θ sin ψ + θ˙ cos ψ ⎥ G B −1 B B −1 ⎢ ˙ sin θ cos ψ − θ˙ sin ψ ⎦ G ω B = AG G ω B = AG ⎣ ϕ ϕ˙ cos θ + ψ˙     = θ˙ cos ϕ + ψ˙ sin θ sin ϕ Iˆ + θ˙ sin ϕ − ψ˙ cos ϕ sin θ Jˆ   + ϕ˙ + ψ˙ cos θ Kˆ

(2.213)

The components of G ωB in the global coordinate frame G (OXY Z) are related to the Euler angle coordinate frame E (Oϕθψ) by the following relationship, in which, the transformation matrix G QE is again a non-orthogonal matrix. ⎡

G G ωB

⎤

=

G

⎡

QE E G ωB

⎤⎡

(2.214)

⎤

ωX 0 cos ϕ sin θ sin ϕ ϕ˙ ⎣ ωY ⎦ = ⎣ 0 sin ϕ − cos ϕ sin θ ⎦ ⎣ θ˙ ⎦ ωZ 1 0 cos θ ψ˙

(2.215)

Example 41  Euler frequencies based on a Cartesian frequencies. Relationship between angular velocity expression by Euler frequencies and Cartesian frequencies. The vector BG ωB that indicates the angular velocity of a rigid body B with respect to the global frame G expressed in frame B is related to the Euler frequencies by B G ωB

=

B G ωB

ωx sin θ sin ψ cos ψ 0 ϕ˙ = ⎣ ωy ⎦ = ⎣ sin θ cos ψ − sin ψ 0 ⎦ ⎣ θ˙ ⎦ ωz cos θ 0 1 ψ˙

B

⎡

AE

E G ωB

⎤

⎡

⎤⎡

⎤

(2.216) (2.217)

The coefficient matrix B AE is not an orthogonal matrix because its transpose is not equal to its inverse. B

B

B

ATE = ATE

A−1 E

B

⎡

A−1 E

⎤

sin θ sin ψ sin θ cos ψ cos θ cos ψ − sin ψ 0 ⎦ 0 0 1 ⎤ ⎡ sin ψ cos ψ 0 1 ⎣ = sin θ cos ψ − sin θ sin ψ 0 ⎦ sin θ − cos θ sin ψ − cos θ cos ψ 1 =⎣

(2.218) (2.219)

(2.220)

It is because the Euler angles coordinate frame E (Oϕθψ) is not an orthogonal frame. For the same reason, the matrix of coefficients that relates the Euler frequencies and the components of G G ωB is also not an orthogonal matrix. =

QE E G ωB ⎤ ⎡ ⎤⎡ ⎤ ⎡ 0 cos ϕ sin θ sin ϕ ϕ˙ ωX ⎥ ⎢ ⎥⎢ ˙ ⎥ ⎢ G = ω = ω 0 sin ϕ − cos ϕ sin θ θ ⎦ ⎦ ⎣ ⎦ ⎣ ⎣ B Y G 1 0 cos θ ψ˙ ωZ G G ωB

G

(2.221) (2.222)

2.5 Euler Angles

67

Therefore, the Euler frequencies based on local and global decomposition of the angular velocity vector G ωB must solely be found by the inverse of coefficient matrices E G ωB

⎡

=

B A−1 E G ωB ⎡

B

⎤

(2.223) ⎤⎡

⎤

sin ψ cos ψ 0 ϕ˙ ωx 1 ⎢ ⎥⎢ ⎢ ˙ ⎥ ⎥ = ⎣ sin θ cos ψ − sin θ sin ψ 0 ⎦ ⎣ ωy ⎦ ⎣θ ⎦ sin θ ωz − cos θ sin ψ − cos θ cos ψ 1 ψ˙

(2.224)

G Q−1 E G ωB ⎡ ⎤⎡ ⎡ ⎤ ⎤ − cos θ sin ϕ cos θ cos ϕ 1 ωX ϕ˙ 1 ⎢ ⎥⎢ ⎢ ˙ ⎥ ⎥ ⎣ sin θ cos ϕ sin θ sin ϕ 0 ⎦ ⎣ ωY ⎦ ⎣θ ⎦= sin θ sin ϕ − cos ϕ 0 ψ˙ ωZ E G ωB

=

G

Using (2.223) and (2.225), it can be verified that the transformation matrix B AG = B AE transformation matrix (2.184). The angular velocity vector can thus be expressed as

(2.225) (2.226)

G

Q−1 E would be the same as Euler

⎤ ωx B ˆ ⎣ ωy ⎦ G ωB = ıˆ jˆ k ωz 



⎡

⎤ ωX ˆ ⎣ ωY ⎦ G ωB = Iˆ Jˆ K ωZ ⎡ ⎤   ϕ˙ E ˆ ˆ ω = K eˆθ k ⎣ θ˙ ⎦ G B ψ˙ 



(2.227)

⎡

(2.228)

(2.229)

Example 42  Integrability of the angular velocity components. The definition of integrability and proving angular velocity is integrable if it is based on Euler frequencies. The integrability condition for an arbitrary total differential of f = f (x, y) df = f1 dx + f2 dy = is

∂f ∂f dx + dy ∂x ∂y

∂f1 ∂f2 = ∂y ∂x

(2.230)

(2.231)

The angular velocity components ωx , ωy , and ωz along the body coordinate axes x, y, and z cannot be integrated to obtain the associated angles because they are not total differentials. As an example, ωx dt = sin θ sin ψ dϕ + cos ψ dθ ∂ (sin θ sin ψ) ∂ cos ψ

= ∂θ ∂ϕ

(2.232) (2.233)

However, the integrability condition (2.231) is satisfied by the Euler frequencies. From (2.224), we have  sin ψ cos ψ  ωy dt (ωx dt) + sin θ sin θ   dθ = cos ψ (ωx dt) − sin ψ ωy dt

dϕ =

dψ =

 (ωz dt) − cos θ sin ψ − cos θ cos ψ  ωy dt + (ωx dt) + sin θ sin θ sin θ

(2.234) (2.235) (2.236)

68

2 Rotation Kinematics

These equations indicate that sin ψ ∂ϕ = sin θ ∂ (ωx dt) cos ψ =

∂θ ∂ (ωx dt)

− cos θ sin ψ ∂ψ = sin θ ∂ (ωx dt)

cos ψ ∂ϕ  =  sin θ ∂ ωy dt − sin ψ =

(2.237)

∂θ   ∂ ωy dt

(2.238)

− cos θ cos ψ ∂ψ  =  sin θ ∂ ωy dt

(2.239)

∂ψ 1 = sin θ ∂ (ωz dt)

(2.240)

and therefore, ∂   ∂ ωy dt



sin ψ sin θ



1 = sin2 θ



∂ψ ∂θ  − cos θ sin ψ   cos ψ sin θ  ∂ ωy dt ∂ ωy dt



1 (− cos ψ cos θ cos ψ + cos θ sin ψ sin ψ) sin2 θ − cos θ cos 2ψ = sin2 θ

=

∂ ∂ (ωx dt)



cos ψ sin θ

=

−1 sin2 θ

 sin ψ sin θ

∂θ ∂ψ + cos θ cos ψ ∂ (ωx dt) ∂ (ωx dt)

1 (sin ψ cos θ sin ψ − cos θ cos ψ cos ψ) sin2 θ − cos θ cos 2ψ = sin2 θ

(2.241)

=

(2.242)

It can be checked that dθ and dψ are also integrable (Rimrott, 1989). Example 43  Cardan angles and frequencies. The definition of Cardan angles and relationship between angular velocity expression by Euler frequencies and Cardan frequencies. The system of Euler angles is singular at θ = 0, and as a consequence, ϕ and ψ become coplanar and indistinguishable. From the 12 angle systems of Appendix B, each with certain names, characteristics, advantages, and disadvantages, the rotations about three different axes such as B AG = Az,ψ Ay,θ Ax,ϕ are called Cardan angles after the Italian mathematician Geronimo Cardano (1501–1576) or Bryant angles. The Cardan angle system is not singular at θ = 0 and has some application in mechatronics and attitude analysis of satellites in a central force field. B

AG = Az,ψ Ay,θ Ax,ϕ ⎡ ⎤ cθ cψ cϕ sψ + sθ cψ sϕ sϕ sψ − cϕ sθ cψ = ⎣ −cθ sψ cϕ cψ − sθ sϕ sψ cψ sϕ + cϕ sθ sψ ⎦ sθ −cθ sϕ cθ cϕ

(2.243)

The angular velocity G ωB of a rigid body with respect to the global frame can either be expressed in terms of the components along the axes of B(Oxyz) or in terms of the Cardan frequencies along the axes of the non-orthogonal Cardan frame. The angular velocity in terms of Cardan frequencies is ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 0 0 ˙ Az,ψ Ay,θ ⎣ 0 ⎦ + θ˙ Az,ψ ⎣ 1 ⎦ + ψ˙ ⎣ 0 ⎦ G ωB = ϕ 0 0 1

(2.244)

2.6 Local Axes Versus Global Axes Rotation

69

and therefore, ⎤ ⎡ cos θ cos ψ ωx ⎣ ωy ⎦ = ⎣ − cos θ sin ψ sin θ ωz ⎡ ⎤ ⎡ cos ψ ϕ˙ ⎢ cos θ ⎣ θ˙ ⎦ = ⎣ sin ψ ψ˙ − tan θ cos ψ

⎤⎡ ⎤ sin ψ 0 ϕ˙ cos ψ 0 ⎦ ⎣ θ˙ ⎦ 0 1 ψ˙ ⎤⎡ ⎤ sin ψ ωx − 0 ⎥⎣ ⎦ cos θ ω y cos ψ 0 ⎦ ω z tan θ sin ψ 1

⎡

In case of small Cardan angles, we have

(2.246)

⎡

⎤ 1 ψ −θ B AG = ⎣ −ψ 1 ϕ ⎦ θ −ϕ 1 ⎡

⎤ ⎡ ωx 1 ψ ⎣ ωy ⎦ = ⎣ −ψ 1 ωz θ 0 ⎡ ⎤ ⎡ ϕ˙ ψ −ψ ⎣ θ˙ ⎦ = ⎣ ψ 1 ψ˙ −θ 0

and

2.6

(2.245)

(2.247)

⎤⎡ ⎤ 0 ϕ˙ 0 ⎦ ⎣ θ˙ ⎦ 1 ψ˙ ⎤⎡ ⎤ 0 ωx 0 ⎦ ⎣ ωy ⎦ 1 ωz

(2.248)

(2.249)

Local Axes Versus Global Axes Rotation

The global rotation matrix G QB is equal to the inverse of the local rotation matrix B AG and vice versa, G

QB =

B

A−1 G

B

AG =

G

Q−1 B

(2.250)

where G B

−1 −1 −1 QB = A−1 1 A2 A3 · · · An

AG =

Q−1 1

Q−1 2

Q−1 3

(2.251)

· · · Q−1 n

(2.252)

Also, premultiplication of the global rotation matrix is equal to postmultiplication of the local rotation matrix. Proof Consider a sequence of global rotations and their resultant rotation matrix r to G r. G r = G QB B r

G

QB to transform a position vector

B

(2.253)

The global position vector G r can also be transformed to B r using a local rotation matrix B AG . B

r=

B

AG G r

(2.254)

Combining Eqs. (2.253) and (2.254) yields G

r=

G

B

r=

B

QB

B G

AG G r

(2.255)

B

(2.256)

AG G QB = I

(2.257)

AG QB r

and hence, G

QB

B

AG =

B

70

2 Rotation Kinematics

Therefore, the global and local rotation matrices are inverse of each other. QB =

B

A−1 G

Q−1 B =

B

AG

G G

(2.258)

Assume G QB = Qn · · · Q3 Q2 Q1 and B AG = An · · · A3 A2 A1 , and we have G

QB =

B

−1 −1 −1 −1 A−1 G = A1 A2 A3 · · · An

(2.259)

B

AG =

G

Q−1 B

(2.260)

=

Q−1 1

Q−1 2

Q−1 3

· · · Q−1 n

and Eq. (2.257) becomes Qn · · · Q2 Q1 An · · · A2 A1 = An · · · A2 A1 Qn · · · Q2 Q1 = I

(2.261)

and therefore, −1 −1 −1 Qn · · · Q3 Q2 Q1 = A−1 1 A2 A3 · · · An −1 −1 −1 An · · · A3 A2 A1 = Q−1 1 Q2 Q3 · · · Qn

(2.262)

−1 −1 −1 Q−1 1 Q2 Q3 · · · Qn Qn · · · Q3 Q2 Q1 = I

(2.263)

−1 −1 −1 A−1 1 A2 A3 · · · An An · · · A3 A2 A1 = I

(2.264)

or

Hence, the effect of in order rotations about the global coordinate axes is equivalent to the effect of the same rotations about the local coordinate axes performed in the reverse order. G B

−1 −1 −1 QB = A−1 1 A2 A3 · · · An

AG =

Q−1 1

Q−1 2

Q−1 3

· · · Q−1 n

(2.265) (2.266) 

Example 44 Global position and postmultiplication of rotation matrix. A numerical example to show how post and premultiplication by rotation matrix.  T The local position of a point P after rotation is at B r = 1 2 3 . If the local rotation matrix to transform G r to B r is given as ⎡ ⎤ ⎡ ⎤ cos ϕ sin ϕ 0 cos 30 sin 30 0 B Az,ϕ = ⎣ − sin ϕ cos ϕ 0 ⎦ = ⎣ − sin 30 cos 30 0 ⎦ (2.267) 0 0 1 0 0 1 then we may find the global position vector G r by postmultiplication B Az,ϕ and the local position vector B rT , ⎤ cos 30 sin 30 0 G T r = B rT B Az,ϕ = 1 2 3 ⎣ − sin 30 cos 30 0 ⎦ 0 0 1   = −0.13 2.23 3.0 



⎡

(2.268)

B instead of premultiplication of B A−1 z,ϕ by r. G

r=

B

⎡

B A−1 z,ϕ r

⎤⎡ ⎤ ⎡ ⎤ cos 30 − sin 30 0 1 −0.13 = ⎣ sin 30 cos 30 0 ⎦ ⎣ 2 ⎦ = ⎣ 2.23 ⎦ 0 0 1 3 3.0

(2.269)

2.7 General Transformation

2.7

71

General Transformation

Consider a general situation in which two coordinate frames, G(OXY Z) and B(Oxyz) with a common origin O, are employed to express the components of a given vector r. There is always a transformation matrix G RB to map the components of r from the reference frame B(Oxyz) to the other reference frame G(OXY Z). G

r=

G

RB B r

(2.270)

We may also reverse the equation, B r = G RB−1 G r, to map the components of r from the reference frame G(OXY Z) to the other reference frame B(Oxyz) by introducing B RG B

r=

B

RG G r

(2.271)

where B RG = G RB−1 = G RBT

G B

RB = RG = 1

(2.272) (2.273)

When the coordinate frames B and G are orthogonal, the rotation matrix G RB is called an orthogonal matrix. The transpose R T and inverse R −1 of an orthogonal matrix [R] are equal: R T = R −1

(2.274)

Because of the matrix orthogonality condition, only three of the nine elements of a transformation matrix independent.

G

RB are

Proof Decomposition of the unit vectors of G(OXY Z) along the axes of B(Oxyz) ˆ kˆ Iˆ = (Iˆ · ıˆ)ˆı + (Iˆ · jˆ)jˆ + (Iˆ · k)

(2.275)

ˆ kˆ Jˆ = (Jˆ · ıˆ)ˆı + (Jˆ · jˆ)jˆ + (Jˆ · k)

(2.276)

ˆ kˆ Kˆ = (Kˆ · ıˆ)ˆı + (Kˆ · jˆ)jˆ + (Kˆ · k)

(2.277)

introduces the transformation matrix G RB to map the local frame to the global frame ⎡

⎤ ⎡ Iˆ Iˆ · ıˆ ⎢ ˆ⎥ ⎢ ˆ = ⎣ J ⎦ ⎣ J · ıˆ Kˆ Kˆ · ıˆ

Iˆ · jˆ Jˆ · jˆ Kˆ · jˆ

⎤⎡ ⎤ Iˆ · kˆ ıˆ ⎥⎣ ⎦ ˆ ˆ j J ·k ⎦ ˆ = kˆ Kˆ · kˆ

⎡ ⎤ ıˆ G RB ⎣ jˆ ⎦ kˆ

(2.278)

where ⎡

⎤ Iˆ · ıˆ Iˆ · jˆ Iˆ · kˆ ⎢ ⎥ G RB = ⎣ Jˆ · ıˆ Jˆ · jˆ Jˆ · kˆ ⎦ Kˆ · ıˆ Kˆ · jˆ Kˆ · kˆ ⎤ ⎡ ˆ cos(Iˆ, ıˆ) cos(Iˆ, jˆ) cos(Iˆ, k) ⎢ ˆ ⎥ = ⎣ cos(Jˆ, ıˆ) cos(Jˆ, jˆ) cos(Jˆ, k) ⎦ ˆ ˆ ıˆ) cos(K, ˆ jˆ) cos(K, ˆ k) cos(K, Each column of G RB is decomposition of a unit vector of the local frame B(Oxyz) in the global frame G(OXY Z).

(2.279)

72

2 Rotation Kinematics G

RB =



G

ıˆ G jˆ G kˆ

 (2.280)

Similarly, each row of G RB is decomposition of a unit vector of the global frame G(OXY Z) in the local frame B(Oxyz). ⎡

⎤ I ⎢ ⎥ G RB = ⎣ B JˆT ⎦ B ˆT K B ˆT

(2.281)

The elements of G RB are directional cosines of the axes of G(OXY Z) in frame B(Oxyz) or B in G. This set of nine directional cosines completely defines the orientation of B(Oxyz) in G(OXY Z) and can be used to map the coordinates of any point (x, y, z) to its corresponding coordinates (X, Y, Z). Alternatively, using the method of unit vector decomposition to develop the matrix B RG yields B

B

r=

B

⎡

RG G r =

RB−1 G r ⎤ ıˆ · Kˆ ⎥ jˆ · Kˆ ⎦ kˆ · Kˆ

G

(2.282)

ıˆ · Iˆ ıˆ · Jˆ ⎢ RG = ⎣ jˆ · Iˆ jˆ · Jˆ kˆ · Iˆ kˆ · Jˆ ⎡ ⎤ ˆ cos(ˆı , Iˆ) cos(ˆı , Jˆ) cos(ˆı , K) ⎢ ˆ ⎥ = ⎣ cos(jˆ, Iˆ) cos(jˆ, Jˆ) cos(jˆ, K) ⎦ ˆ Iˆ) cos(k, ˆ Jˆ) cos(k, ˆ K) ˆ cos(k,

(2.283)

It shows that the inverse of a transformation matrix is equal to the transpose of the transformation matrix, G

RB−1 =

G

RBT

(2.284)

or G

RB ·

G

RBT = I

(2.285) G

A matrix with condition (2.284) is called an orthogonal matrix. Orthogonality of RB comes from the fact that it maps an orthogonal coordinate frame to another orthogonal coordinate frame. Applying Eq. (2.285) yields ⎡ ⎤⎡ ⎤ ⎡ ⎤ r11 r12 r13 r11 r21 r31 100 ⎣ r21 r22 r23 ⎦ ⎣ r12 r22 r32 ⎦ = ⎣ 0 1 0 ⎦ (2.286) r31 r32 r33 r13 r23 r33 001 2 2 2 + r12 + r13 =1 r11

+

(2.288)

2 2 2 r31 + r32 + r33 =1

(2.289)

r11 r21 + r12 r22 + r13 r23 = 0

(2.290)

r21 r31 + r22 r32 + r23 r33 = 0

(2.291)

r31 r11 + r32 r12 + r33 r13 = 0

(2.292)

2 r22

+

(2.287)

=1

2 r21

2 r23

Therefore, the inner product of any two different rows of G RB is zero, and the inner product of any row of G RB by itself is unity. These constraint relations are also true for columns of G RB and evidently for rows and columns of B RG . Because of six independent constraints among the nine elements of G RB , an orthogonal transformation matrix G RB has only three independent elements. The orthogonality condition can be summarized by the equation

2.7 General Transformation

73

Fig. 2.18 Body and global coordinate frames of Example 45

3 

rij rik = δ j k

j, k = 1, 2, 3

(2.293)

i=1

where rij is the element of row i and column j of the transformation matrix R, and δ j k is Kronecker’s delta.  δ ij = δ j i =

1 0

i=j i = j

(2.294)

It states that δ j k = 1 if j = k and δ j k = 0 if j = k. Equation (2.293) provides six independent relations that must be satisfied by the nine directional cosines. Therefore, there are only three independent directional cosines. Because of the constraint equations, the independent elements of the matrix R cannot all be in the same row or column or any diagonal. Because of Eq. (2.285), the determinant of a transformation matrix is equal to one,

G

RB = ±1 because

G

RB ·

G











2 RBT = G RB · G RBT = G RB · G RB = G RB = 1

(2.295)

(2.296)

Using linear algebra and column vectors G ıˆ, G jˆ, and G kˆ of G RB , we know that 

G

RB = G ıˆ · G jˆ × G kˆ

(2.297)

and because the coordinate system is right-handed, we have G jˆ × G kˆ = G ıˆ, and therefore,

G

RB = G ıˆT · G ıˆ = +1

(2.298) 

Example 45 Elements of transformation matrix. A numerical example to determine rotation matrix by comprehending indirect information. The position vector r of a point P may be expressed in terms of its components with respect to either G (OXY Z) or ˆ and we are B (Oxyz) frame. Body and global coordinate frames are shown in Fig. 2.18. If G r = 100Iˆ − 50Jˆ + 150K, looking for components of r in the B (Oxyz) frame, then we need to find the proper rotation matrix B RG first. The row elements of B RG are the direction cosines of the axes of Oxyz in the coordinate frame G (OXY Z). The x-axis lies in the XZ-plane at 40 deg from the X-axis, and the angle between y and Y is 60 deg. Therefore,

74

2 Rotation Kinematics

⎡

⎤ ⎡ ⎤ cos 40 0 sin 40 ıˆ · Iˆ ıˆ · Jˆ ıˆ · Kˆ ⎢ ⎥ B RG = ⎣ jˆ · Iˆ jˆ · Jˆ jˆ · Kˆ ⎦ = ⎣ jˆ · Iˆ cos 60 jˆ · Kˆ ⎦ kˆ · Iˆ kˆ · Jˆ kˆ · Kˆ kˆ · Iˆ kˆ · Jˆ kˆ · Kˆ ⎡ ⎤ 0.766 0 0.643 = ⎣ jˆ · Iˆ 0.5 jˆ · Kˆ ⎦ kˆ · Iˆ kˆ · Jˆ kˆ · Kˆ

(2.299)

T = I , we obtain a set of equations to find the missing and by using the orthogonality condition, B RG G RB = B RG B RG elements. ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0.766 0 0.643 0.766 r21 r31 100 ⎣ r21 0.5 r23 ⎦ ⎣ 0 0.5 r32 ⎦ = ⎣ 0 1 0 ⎦ (2.300) 001 r31 r32 r33 0.643 r23 r33

0.766 r21 + 0.643 r23 = 0 0.766 r31 + 0.643 r33 = 0 2 2 + r23 + 0.25 = 1 r21

r21 r31 + 0.5r32 + r23 r33 = 0 2 2 2 r31 + r32 + r33 =1

(2.301)

These set of five coupled algebraic equations are nonlinear, and hence, they provide us with multiple solutions. B

RG

⎡

 1

⎤ 0.766 0 0.643 = ⎣ 0.5568 0.5 −0.6633 ⎦ −0.32146 0.866 0.38296

⎤ 0.766 0 0.643 RG 2 = ⎣ 0.5568 0.5 0.6633 ⎦ 0.32146 0.866 −0.38296 ⎡ ⎤ 0.766 0 0.643 B  RG 3 = ⎣ 0.5568 0.5 −0.6633 ⎦ 0.32146 −0.866 −0.38296 ⎡ ⎤ 0.766 0 0.643 B  RG 4 = ⎣ −0.5568 0.5 0.6633 ⎦ −0.32146 −0.866 0.38296 B

(2.302)

⎡



(2.303)

(2.304)

(2.305)

  The second solution B RG 2 will be eliminated because it does not fit the orthogonality B  condition (2.285). Considering the acute angle between the z and Z axes, we need a positive r33 and that eliminates RG 3 . Also, considering the obtuse angle B  between the z and X axes, we need a negative r31 and that eliminates RG 1 . Therefore, the right transformation matrix between G (OXY Z) and B (Oxyz) will be ⎡

⎤ 0.766 0 0.643 B RG = ⎣ 0.5568 0.5 −0.6633 ⎦ −0.32146 0.866 0.38296 and then we can find the components of B r.

(2.306)

2.7 General Transformation

75

⎡

⎤⎡ ⎤ 0.766 0 0.643 100 B r = B RG G r = ⎣ 0.5568 0.5 −0.6633 ⎦ ⎣ −50 ⎦ −0.32146 0.866 0.38296 150 ⎡ ⎤ 173.05 = ⎣ −68.82 ⎦ −18.0

(2.307)

Example 46 Two points transformation matrix. A numerical example of determination of rotation matrix by indirect information. The global position vector of two points, P1 and P2 , of a rigid body B are ⎡

G

rP1

⎤ 1.077 = ⎣ 1.365 ⎦ 2.666

⎡

G

rP2

⎤ −0.473 = ⎣ 2.239 ⎦ −0.959

(2.308)

The origin of the body B (Oxyz) is fixed on the origin of G (OXY Z), and the points P1 and P2 are lying on the local x-axis and y-axis, respectively. ˆ To find G RB , we use the local unit vectors G ıˆ and G jˆ to obtain G k. ⎡ ⎤ 0.338 r P G ıˆ = G 1 = ⎣ 0.429 ⎦

rP 1 0.838 G

⎡ ⎤ −0.191 r P G jˆ = G 2 = ⎣ 0.902 ⎦

rP 2 −0.387 G

⎤ −0.922 Gˆ k = ıˆ × jˆ = ⎣ −0.029 ⎦ 0.387

(2.309)

⎡

(2.310)

Hence, the transformation matrix using the coordinates of two points G rP1 and G rP2 would be G

RB =



G

ıˆ G jˆ G kˆ



⎡

⎤ 0.338 −0.191 −0.922 = ⎣ 0.429 0.902 −0.029 ⎦ 0.838 −0.387 0.387

(2.311)

Example 47 Length invariant of a position vector. The proof of an invariant property of vectors under rotation transformation. Expressing a vector in different frames utilizing rotation matrices does not affect the length and direction properties of the vector. Therefore, the length of a vector is an invariant.

|r| = G r = B r

(2.312)

The length invariant property can be shown by |r|2 =

G T G

=

B T B

r

r

r=

G

T RB B r

G

RB B r =

B T G

r

RBT G RB B r

r

(2.313)

Example 48  Group property of transformations. The definition of group and showing rotation transformations make a group. A set S together with a binary operation ⊗ defined on elements of S is called a group (S, ⊗) if it satisfies the following four axioms: 1. Closure: If s1 , s2 ∈ S, then s1 ⊗ s2 ∈ S. 2. Identity: There exists an identity element s0 such that s0 ⊗ s = s ⊗ s0 = s for ∀s ∈ S.

76

2 Rotation Kinematics

3. Inverse: For each s ∈ S, there exists a unique inverse s −1 ∈ S such that s −1 ⊗ s = s ⊗ s −1 = s0 . 4. Associativity: If s1 , s2 , s3 ∈ S, then (s1 ⊗ s2 ) ⊗ s3 = s1 ⊗ (s2 ⊗ s3 ). Three dimensional coordinate transformations make a group if we define the set of rotation matrices by ! S = R ∈ R3×3 : RR T = R T R = I, |R| = 1 .

(2.314)

Therefore, the elements of the set S are transformation matrices Ri , the binary operator ⊗ is matrix multiplication, the identity matrix is I, and the inverse of element R is R −1 = R T . S is also a continuous group because 5. The binary matrix multiplication is a continuous operation and 6. The inverse of any element in S is a continuous function of that element. Therefore, S is a differentiable manifold. A group that is a differentiable manifold is called a Lie group. Example 49  Transformation with determinant −1. All physical rotation matrices have +1 as an eigenvalue. If it is −1, then the matrix would be a reflection. An orthogonal matrix with determinant +1 corresponds to a rotation as described in Eq. (2.295). In contrast, an orthogonal matrix with determinant −1 describes a reflection. It transforms a right-handed coordinate system into a left-handed and vice versa. This transformation does not correspond to any possible physical action on rigid bodies. Example 50 Alternative proof for transformation matrix. Re-definition of rotation matrices based on unit vectors of the two coordinate frames. Starting with an identity ⎡ ⎤   ıˆ (2.315) ıˆ jˆ kˆ ⎣ jˆ ⎦ = 1 ˆk we may write

⎡

⎤ ⎡ ⎤ ⎡ ⎤ Iˆ Iˆ   ıˆ ⎢ ˆ⎥ ⎢ ˆ⎥ ⎣ J ⎦ = ⎣ J ⎦ ıˆ jˆ kˆ ⎣ jˆ ⎦ kˆ Kˆ Kˆ

(2.316)

Triple matrix multiplication can be performed in different order. A (BC) = (AB) C Therefore,

⎤ ⎡ Iˆ Iˆ · ıˆ ⎢ ˆ⎥ ⎢ ˆ ⎣ J ⎦ = ⎣ J · ıˆ Kˆ Kˆ · ıˆ ⎡

where

Iˆ · jˆ Jˆ · jˆ Kˆ · jˆ

⎤⎡ ⎤ ıˆ Iˆ · kˆ ⎥⎣ ⎦ ˆ ˆ J · k ⎦ jˆ = kˆ Kˆ · kˆ

⎤ Iˆ   ⎢ ⎥ G RB = ⎣ Jˆ ⎦ ıˆ jˆ kˆ Kˆ

(2.317) ⎡ ⎤ ıˆ G ⎣ RB jˆ ⎦ kˆ

(2.318)

⎡

(2.319)

Following the same method, we can show that ⎡ ⎤ ıˆ   B RG = ⎣ jˆ ⎦ Iˆ Jˆ Kˆ kˆ

(2.320)

2.7 General Transformation

77

Fig. 2.19 Rotation of a rotated rigid body about the Z-axis

Example 51  Rotation of a rotated body. How to express rotation of a B-frame that is not coincident by the G-frame. Consider a rigid body with a fixed point at a general position B0 that is not necessarily coincident with the global frame G. Assume the rotation matrix G R0 between B0 and G is known. If the body rotates from this initial position, then the transformation matrix G RB between the final position of the body and the global frame is G

RB =

G

R0 0 RB

(2.321)

The matrix 0 RB is the transformation between initial and final positions of the body with the assumption that B0 is a fixed position. The principal assumption in the theory of rotation kinematics is that the body coordinate frame B always begins to rotate from a coincident configuration with the global frame G. If the body frame is not initially on the global frame, we show it by B0 and assume that the body is already rotated from the coincident configuration with G. The rotated body B0 is then going to turn a second rotation. Figure 2.19 illustrates a body frame B at its final position after a rotation of α about the Z-axis from an initial position B0 . The initial position of the body is after a first rotation of α 0 about the Z-axis. In this example, both the first and second rotations are about the global Z-axis. To prevent confusion and keep the previous definitions, we define three transformation matrices G R0 , 0 RB , and G RB . The matrix G R0 indicates the transformation between G and the initial position of the body at B0 . The matrix 0 RB is the transformation between the initial position of the body at B0 and the final position of the body B, and the matrix G RB is the transformation between G and the final position of the body B. Let us assume G R0 is given as ⎡ ⎤ cos α 0 − sin α 0 0 G R0 = ⎣ sin α 0 cos α 0 0 ⎦ (2.322) 0 0 1 The matrix G RB can be found using the regular method. Consider a body frame starts from G and goes to the final position by two rotations α 0 and α: G

RB = RZ,α0 RZ,α = G R0 0 RB ⎡ ⎤ cos (α + α 0 ) − sin (α + α 0 ) 0 = ⎣ sin (α + α 0 ) cos (α + α 0 ) 0 ⎦ 0 0 1

(2.323)

Therefore, the rotation matrix B R0 will be ⎡

⎤ cos α sin α 0 B R0 = G RBT G R0 = ⎣ − sin α cos α 0 ⎦ 0 0 1

(2.324)

78

2 Rotation Kinematics

This is correct for a general case in which the first and second rotations are about the same axis. To see the application, let us assume that a point P to be at B r = xˆı + y jˆ + zkˆ (2.325) Its global position is r=

G

r=

B

G

and its position in the initial frame B0 is 0

RB B r

(2.326)

R0T B r

(2.327)

When the body is at its initial position, the global coordinates of P are G0

r=

G

R0 0 r

(2.328)

Example 52 Rotation about X of a rotated body about Z. A numerical example for rotation of a rotated body about global axes. A rigid body is already rotated 30 deg about the Z-axis and is at B0 : ⎡

π ⎤ π − sin 0 ⎢ π6 π6 ⎥ G ⎥ R0 = ⎢ cos 0⎦ ⎣ sin 6 6 0 0 1 cos

(2.329)

The body rotates 60 deg about the X-axis from the position B0 . The rotation matrix G RB is given as ⎡

G

Therefore,

RB = RX, π6 RZ, π3

⎤ 0.86603 −0.5 0 = ⎣ 0.35355 0.61237 −0.70711 ⎦ 0.35355 0.61237 0.70711

⎤ 0.92678 −0.12683 −0.35356 0 RB = G R0T G RB = ⎣ −0.12683 0.78033 −0.61238 ⎦ 0.35355 0.61237 0.70711

(2.330)

⎡

(2.331)

Example 53 Rotation about x of a rotated body about Z. A numerical example for rotation about body axes of a rotated body about global axes. Consider a rigid body that is already rotated 45 deg about the Z-axis and is at B0 : ⎡

π π ⎤ ⎡ ⎤ − sin 0 0.707 −0.707 0 4 4 ⎢ ⎥ π π G ⎥ R0 = ⎢ cos 0 ⎦ ≈ ⎣ 0.707 0.707 0 ⎦ ⎣ sin 4 4 0 0 1 0 0 1 cos

(2.332)

The body rotates 45 deg about the x-axis from the position B0 . Considering B0 as a fixed frame, we can determine 0 RB : ⎡ 0

RB =

B

1

0

0

⎤T

π π ⎥ ⎢ T 0 cos sin ⎥ π = ⎢ R0T = Rx, 4π 4 ⎦ ⎣ 4 π 0 − sin cos 4 4

⎤ 0.70711 −0.5 0.5 G RB = G R0 0 RB = ⎣ 0.70711 0.5 −0.5 ⎦ 0 0.70711 0.70711

(2.333)

⎡

(2.334)

2.8  Active and Passive Transformation

79

Assume a triangle in the body frame with corners at O, P1 , and P2 where B

r1 = 1ˆı

(2.335)

B

r2 = 1ˆı + 1jˆ + 1kˆ

(2.336)

The global coordinates of P1 and P2 are ⎡ ⎤ ⎡ ⎤ 1 0.707 11 G r1 = G RB B r1 = G RB ⎣ 0 ⎦ = ⎣ 0.707 11 ⎦ 0 0 ⎡ ⎤ ⎡ ⎤ 1 0.707 11 G r2 = G RB B r2 = G RB ⎣ 1 ⎦ = ⎣ 0.707 11 ⎦ 1 1.414 2

(2.337)

(2.338)

The coordinates of the current positions of P1 and P2 in B0 are ⎡ ⎤ ⎡ ⎤ 1 1 0 r1 = 0 RB B r1 = 0 RB ⎣ 0 ⎦ = ⎣ 0 ⎦ 0 0 ⎡ ⎤ ⎡ ⎤ 1 1 0 r2 = 0 RB B r2 = 0 RB ⎣ 1 ⎦ = ⎣ 0 ⎦ 1 1.414 2

2.8

(2.339)

(2.340)

 Active and Passive Transformation

Rotation of a local frame B in a global frame G is an active rotation if the position vector B r of a point P is fixed in the local frame B and rotates with B. Rotation of a local frame B in a global frame G is called a passive rotation if the position vector G r of a point P is fixed in the global frame and does not rotate with the local frame. The passive and active transformations are mathematically equivalent. The coordinates of a point P can be transformed from one coordinate to the other by a proper rotation matrix. In an active rotation, the observer is standing in a global frame G and calculating the position of body points in G. However, in a passive rotation, the observer is standing on the body frame B and calculating the position of global points in B. If R1 = G RB is an active rotation matrix about an axis, such as the Z-axis, then G r = R1 B r = RZ,α B r (2.341) and if R2 =

B

RG is the passive rotation matrix of the same rotation, then G

r = R2T B r = RZ,−α B r

(2.342)

In robotics, we usually work with active rotations and examine the rotation of a robot’s lines in a global frame. Proof Assume a local frame B(Oxyz) that is initially coincident with a global frame G(OXY Z) performs a rotation. In an active rotation, a body point P will move with B. The body coordinates of P will remain unchanged, while its global coordinate will be found by the proper rotation matrix R1 = G RB . Let us assume that the axis of rotation is the Z-axis. G

r = R1 B r =

G

RB B r = RZ,α B r

(2.343)

In a passive rotation, the global coordinates of point P will remain unchanged. We may switch the roll of frames B and G to consider the passive rotation as an active rotation of −α for G in B about the z-axis. The coordinate of P in B will be found by the proper rotation matrix R2 = B RG : B

r = R2 G r =

B

RG G r = Rz,−α G r

(2.344)

80

2 Rotation Kinematics

Therefore, the global coordinates of P in the passive rotation may be found from (2.344) as T B r = R2T B r = Rz,−α r

(2.345)

r = R2T B r = RZ,−α B r

(2.346)

G

T However, because of RZ,α = Rz,α , we have G

 Example 54  Active and passive rotation about X-axis. A numerical example for active and passive rotations. Consider local and global frames B and G that are coincident. A body point P is at B r. ⎡ ⎤ 1 B r = ⎣2⎦ 1

(2.347)

A rotation of 90 deg about the X-axis will move the point to G r. G

r = RX,90 B r ⎡ 1 0 0 π π ⎢ − sin 0 cos =⎢ 2 ⎣ π π2 0 sin cos 2 2

(2.348)

⎤

⎡ ⎤ ⎡ ⎤ 1 ⎥ 1 ⎥ ⎣ 2 ⎦ = ⎣ −1 ⎦ ⎦ 2 1

Now assume that P is fixed in G. When B rotates 90 deg about X-axis, the coordinates of P in the local frame will change to B r = RX,−90 G r (2.349) ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 0 0 1 −π −π ⎥ 1 ⎢ 0 cos − sin ⎥⎣ ⎦ ⎣ 1 ⎦ =⎢ 2 2 ⎦ 2 = ⎣ −π −π 1 −2 0 sin cos 2 2

2.9

Summary

This chapter is about Rotation Kinematics in which we learn the following two objectives: 1. To learn how to determine the transformation matrix between two Cartesian coordinate frames B and G with a common origin by applying rotations about principal axes of either frames 2. To decompose a given transformation matrix to a series of principal rotations Two Cartesian coordinate frames B and G with a common origin are related by nine directional cosines of a frame in the other. The conversion of coordinates in the two frames can be casted in a matrix transformation r = G RB B r ⎡ ⎤ ⎡ Iˆ · ıˆ Iˆ · jˆ X2 ⎣ Y2 ⎦ = ⎢ ⎣ Jˆ · ıˆ Jˆ · jˆ Z2 Kˆ · ıˆ Kˆ · jˆ G

⎤⎡

(2.350) ⎤

Iˆ · kˆ x2 ⎥ Jˆ · kˆ ⎦ ⎣ y2 ⎦ z2 Kˆ · kˆ

(2.351)

2.9 Summary

where

81

⎡

⎤ ˆ cos(Iˆ, ıˆ) cos(Iˆ, jˆ) cos(Iˆ, k) ⎢ G ˆ ⎥ RB = ⎣ cos(Jˆ, ıˆ) cos(Jˆ, jˆ) cos(Jˆ, k) ⎦ ˆ ıˆ) cos(K, ˆ jˆ) cos(K, ˆ k) ˆ cos(K,

(2.352)

The transformation matrix G RB is orthogonal. That means the determinant of G RB is one, and its inverse is equal to its transpose.

G

RB = 1 G

RB−1 =

G

(2.353) RBT

(2.354)

The orthogonality condition generates six equations between the nine elements of G RB that shows only three elements of RB are independent. Any relative orientation of B in G can be achieved by three consecutive principal rotations about the coordinate axes in either frames, B or G. If B is the body coordinate frame, and G is the globally fixed frame, the global principal rotation transformation matrices are ⎡ ⎤ 1 0 0 ⎢ ⎥ RX,γ = G RB = ⎣ 0 cos γ − sin γ ⎦ (2.355)

G

0 sin γ cos γ ⎡ ⎢ RB = ⎣

⎤

⎥ 0 ⎦ − sin β 0 cos β ⎡ ⎤ cos α − sin α 0 ⎢ ⎥ = G RB = ⎣ sin α cos α 0 ⎦ 0 0 1

RY,β =

RZ,α

cos β 0 sin β

G

0

1

(2.356)

(2.357)

and the body principal rotation transformation matrices are ⎡ Rx,ψ =

B

1

0

⎤

⎢ ⎥ RG = ⎣ 0 cos ψ sin ψ ⎦ 0 − sin ψ cos ψ ⎡

Ry,θ =

0

B

cos θ 0 − sin θ

⎢ RG = ⎣ 0

1

0

(2.358)

⎤ ⎥ ⎦

(2.359)

sin θ 0 cos θ ⎡ Rz,ϕ =

B

cos ϕ sin ϕ 0

⎤

⎢ ⎥ RG = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1

(2.360)

The global and local rotation transformations are inverse of each other. T RX,γ = Rx,γ

(2.361)

T RY,β = Ry,β

(2.362)

T RZ,α = Rz,α

(2.363)

82

2.10

2 Rotation Kinematics

Key Symbols

a a˜ A B c d eˆϕ , eˆθ , eˆψ E f, f1 , f2 G I = [I ] ıˆ, jˆ, kˆ ı˜, j˜, k˜ Iˆ, Jˆ, Kˆ l m n N O Oϕθψ P Q r rij R R s S t u v x, y, z X, Y, Z

A general vector Skew symmetric matrix of the vector a Transformation matrix of rotation about a local axis Body coordinate frame, local coordinate frame cos Distance between two points Coordinate axes of E, local roll–pitch–yaw coordinate axes Eulerian local frame A function of x and y Global coordinate frame, fixed coordinate frame Identity matrix Local coordinate axes unit vectors Skew symmetric matrices of the unit vector ıˆ, jˆ, kˆ Global coordinate axes unit vectors Length Number of repeating rotation Fraction of 2π, number of repeating rotation The set of natural numbers Common origin of B and G Euler angle frame A body point, a fixed point in B, a partial derivative Transformation matrix of rotation about a global axis, a partial derivative Position vector The element of row i and column j of a matrix Rotation transformation matrix, radius of a circle The set of real numbers sin, a member of S A set Time A general axis Velocity vector Local coordinate axes Global coordinate axes

Greek α, β, γ δ ij ϕ, θ, ψ ˙ ψ˙ ϕ, ˙ θ, ωx , ωy , ωz ω

Rotation angles about global axes Kronecker’s delta Rotation angles about local axes, Euler angles Euler frequencies Angular velocity components Angular velocity vector

Symbol [ ]−1 [ ]T ⊗ (S, ⊗)

Inverse of the matrix [ ] Transpose of the matrix [ ] A binary operation A group

Exercises

83

Exercises 1. Notation and symbols. Describe the meaning of these notations. a- G r b- G rP c- B rP d- G RB e- G RBT f- B RG −1 g- B RG h- G dB i- 2 d1 j- QX k- QY,β l- Q−1 Y,45 ˆ m- k n- Jˆ o- ATz,ϕ p- eˆψ q- ı˜ r- I 2. Body point and global rotations. The point P is at B rP = [1, 2, 1]T in a body coordinate B(Oxyz). Find the final global position of P after, (a) A rotation of 30 deg about the X-axis, followed by a 45 deg rotation about the Z-axis to move P from P1 to P2 . (b) A rotation of 30 deg about the Z-axis, followed by a 45 deg rotation about the X-axis to move P from P1 to P3 . (c)  Point P will move on a sphere. Let us name the initial global position of P by P1 , the second position by P2 , and the third position by P3 . Determine the angles of P1 OP2 , P2 OP3 , and P3 OP1 . (d)  Determine the area of the triangle made by points P1 , P2 , and P3 . 3. Body point after global rotation. Find the position of a point P in the local coordinate, if it is moved to G rP = [1, 3, 2]T after (a) A rotation of 60 deg about Z-axis (b) A rotation of 60 deg about X-axis (c) A rotation of 60 deg about Y -axis (d) Rotations of 60 deg about Z-axis, 60 deg about X-axis, and 60 deg about Y -axis 4. Invariant of a vector. A point was at B rP = [1, 2, z]T . After a rotation of 60 deg about X-axis, followed by a 30 deg rotation about Z-axis, it is at ⎡ ⎤ X G rP = ⎣ Y ⎦ (2.364) 2.933 Find z, X, and Y . 5. Global rotation of a cube. Figure 2.20 illustrates the original position of a cube with a fixed point at D and edges of length l = 1. Determine: (a) Coordinates of the corners after rotation of 30 deg about the X-axis. (b) Coordinates of the corners after rotation of 30 deg about the Y -axis. (c) Coordinates of the corners after rotation of 30 deg about the Z-axis. (d) Coordinates of the corners after rotation of 30 deg about the X-axis, then 30 deg about the Y -axis, and then 30 deg about the Z-axis. 6. Constant length vector. (a) Show that the length of a vector will not change by rotation.

G G B

r = RB r

(2.365)

(b) Show that the distance between two body points will not change by rotation.

B



p 1 − B p 2 = G RB B p 1 −

G

RB B p 2

(2.366)

7. Coordinate invariant of inner product. Knowing r1 · r2 = rT1 r2

(2.367)

prove that the inner product of two vectors does not depend on the choice of coordinate frames in which they are expressed.

84

2 Rotation Kinematics

Fig. 2.20 A cube at its initial position

8. Repeated global rotations. Rotate B rP = [2, 2, 3]T , 60 deg about the X-axis, followed by 30 deg about the Z-axis. Then, repeat the sequence of rotations for 60 deg about the X-axis, followed by 30 deg about the Z-axis. After how many rotations will point P be back to its initial global position? 9.  Alternative motions to reach an orientation. The coordinates of a body point P in B and G frames are ⎡

⎤ 1.23 B rP = ⎣ 4.56 ⎦ 7.89

⎡

⎤ 4.56 G rP = ⎣ 7.89 ⎦ 1.23

(2.368)

Determine: (a) If it is possible to transform B rP to G rP ? (b) A transformation matrix G RB between B rP and G rP . (c) Euler angles to transform B rP to G rP . (d) Global roll–pitch–yaw to transform B rP to G rP . (e) Body roll–pitch–yaw to transform B rP to G rP . 10.  Repeated global rotations. How many rotations of α = π/m deg about X-axis, followed by β = π/n deg about Z-axis, are needed to bring a body point to its initial global position, if m, n ∈ N? 11. Triple global rotations. Verify the equations in Appendix A. 12.  Special triple rotation. Assume that the first triple rotation in Appendix A brings a body point back to its initial global position. What are the possible values of the angles α = 0, β = 0, and γ = 0? 13.  Combination of triple rotations. Any triple rotation in Appendix A can move a body point to its new global position. Assume α 1 , β 1 , and γ 1 for the case 1 − QX,γ 1 QY,β 1 QZ,α1 are given. What can α 2 , β 2 , and γ 2 be (in terms of α 1 , β 1 , and γ 1 ) to get the same global position using the case 2 − QY,γ 2 QZ,β 2 QX,α2 ? 14. Global roll–pitch–yaw rotation angles. Calculate the role, pitch, and yaw angles for the following rotation matrix: ⎡

⎤ 0.53 −0.84 0.13 B RG = ⎣ 0.0 0.15 0.99 ⎦ −0.85 −0.52 0.081

(2.369)

Exercises

85

15.  Rotation matrix and spherical trigonometry. A plane cutting a sphere and passing through the center of the sphere is called a great circle. Any other plane intersecting the sphere but not passing through the center cuts the surface in a small circle. A spherical triangle is made up when three great circles intersect. If we are given any three points on the surface of a sphere, we can join them by great circle arcs to form a spherical triangle. Consider a vector B r in a body coordinate frame B. The tip point of B r will move on a sphere when the body frame performs multiple rotations. The vector B r goes to three positions r1 , r2 , and r3 . The vector r1 indicates the original position of a body point, r2 indicates the position vector of the body point after a rotation about the Z-axis, and r3 indicates the position vector of the point after another rotation about the X-axis. However, these rotations may be about any arbitrary axes. The tip point of the vectors r1 , r2 , and r3 makes a spherical triangle that lies on a sphere with radius R = |r1 | = |r2 | = |r3 |. Assume ABC is a triangle on a sphere of unit radius R = 1. Let us show the angles of the triangle by α, β, and γ and the length of its sides by a, b, and c. The arc lengths a, b, and c are, respectively, equal to the plane angles BOC, AOC, and AOB for the unit sphere. Show that cos a = cos b cos c + sin b sin c cos α

(2.370)

cos b = cos c cos a + sin c sin a cos β

(2.371)

cos c = cos a cos b + sin a sin b cos γ

(2.372)

sin α sin β sin γ = = (2.373) sin a sin b sin c 16.  Back to the initial orientation and Appendix A. Assume we turn a rigid body B using the first set of Appendix A. How can we turn it back to its initial orientation by applying (a) The first set of Appendix A (b) The second set of Appendix A (c) The third set of Appendix A (d)  Assume that we have turned a rigid body B by α 1 = 30 deg, β 1 = 30 deg, and γ 1 = 30 deg using the first set of Appendix A. We want to turn B back to its original orientation. Which one of the second or third set of Appendix A does it faster? Let us assume that the fastest set is the one with minimum sum of s = α 2 + β 2 + γ 2 . 17.  Back to the original orientation and Appendix B. Assume we turn a rigid body B using the first set of Appendix A. How can we turn it back to its initial orientation by applying (a) The first set of Appendix B (b) The second set of Appendix B (c) The third set of Appendix B (d)  Assume that we have turned a rigid body B by α = 30 deg, β = 30 deg, and γ = 30 deg using the first set of Appendix A. We want to turn B back to its original orientation. Which one of the first, second, or third set of Appendix B does it faster? Let us assume that the fastest set is the one with minimum sum of s = ϕ + θ + ψ. 18. Two local rotations. Find the global coordinates of a body point at B rP = [2, 2, 3]T after (a) A rotation of 60 deg about x-axis followed by 60 deg about z-axis (b) A rotation of 60 deg about z-axis followed by 60 deg about x-axis (c) A rotation of 60 deg about z-axis followed by 60 deg about x-axis, and a rotation of 60 deg about z-axis 19. Local rotation of a cube. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Determine: (a) Coordinates of the corners after rotation of 30 deg about x-axis. (b) Coordinates of the corners after rotation of 30 deg about y-axis. (c) Coordinates of the corners after rotation of 30 deg about z-axis. (d) Coordinates of the corners after rotation of 30 deg about x-axis, then 30 deg about y-axis, and then 30 deg about z-axis.

86

2 Rotation Kinematics

20. Global and local rotation of a cube. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Determine: (a) Coordinates of the corners after rotation of 30 deg about x-axis followed by rotation of 30 deg about X-axis. (b) Coordinates of the corners after rotation of 30 deg about y-axis followed by rotation of 30 deg about X-axis. (c) Coordinates of the corners after rotation of 30 deg about z-axis followed by rotation of 30 deg about X-axis. (d) Coordinates of the corners after rotation of 30 deg about x-axis, then 30 deg about X-axis, and then 30 deg about x-axis. (e) Coordinates of the corners after rotation of 30 deg about x-axis, then 30 deg about Y -axis, and then 30 deg about z-axis. 21. Body point, local rotation. What is the global coordinates of a body point at B rP = [2, 2, 3]T , after (a) A rotation of 60 deg about the x-axis (b) A rotation of 60 deg about the y-axis (c) A rotation of 60 deg about the z-axis 22. Unknown rotation angle 1. Transform B rP = [2, 2, 3]T to G rP = [2, YP , 0]T by a rotation about x-axis and determine YP and the angle of rotation. 23. Unknown rotation angle 2. √ √ Consider a point P at B rP = [2, 3, 2]T . Determine: √ √ (a) The required principal global rotations in order X, Y, Z, to move P to G rP = [√2, 2, √3]T . (b) The required principal global rotations in order Z, Y, Z, to move P to G rP = [ √2, 2, √3]T . (c) The required principal global rotations in order Z, X, Z, to move P to G rP = [ 2, 2, 3]T . 24. Triple local rotations. Verify the equations in Appendix B. 25. Combination of local and global rotations. Find the final global position of a body point at B rP = [10, 10, −10]T after (a) A rotation of 45 deg about the x-axis followed by 60 deg about the Z-axis. (b) A rotation of 45 deg about the z-axis followed by 60 deg about the Z-axis. (c) A rotation of 45 deg about the x-axis followed by 45 deg about the Z-axis and 60 deg about the X-axis. 26. Combination of global and local rotations. Find the final global position of a body point at B rP = [10, 10, −10]T after (a) A rotation of 45 deg about the X-axis followed by 60 deg about the z-axis. (b) A rotation of 45 deg about the Z-axis followed by 60 deg about the z-axis. (c) A rotation of 45 deg about the X-axis followed by 45 deg about the x-axis and 60 deg about the z-axis. 27. Repeated local rotations. Rotate B rP = [2, 2, 3]T , 60 deg about the x-axis, followed by 30 deg about the z-axis. Then repeat the sequence of rotations for 60 deg about the x-axis, followed by 30 deg about the z-axis. After how many rotations will point P move back to its initial global position? 28.  Repeated local rotations. How many rotations of α = π /m deg about the x-axis, followed by β = π /n deg about the z-axis, are needed to bring a body point to its initial global position if m, n ∈ N? 29.  Remaining rotation. Find the result of the following sequence of rotations: G

RB = ATy,θ ATz,ψ ATy,−θ

30. Angles from rotation matrix. Find the angles ϕ, θ , and ψ if the rotation transformation matrices of Appendix B are given. 31. Euler angles from rotation matrix. (a) Check if the following matrix G RB is a rotation transformation:

(2.374)

Exercises

87

⎡

⎤ 0.53 −0.84 0.13 G RB = ⎣ 0.0 0.15 0.99 ⎦ −0.85 −0.52 0.081

(2.375)

(b) Find the Euler angles for G RB . (c) Find the local roll–pitch–yaw angles for G RB . 32. Equivalent Euler angles to two rotations. Find the Euler angles corresponding to the following rotation matrix: (a) B RG = Ay,45 Ax,30 (b) B RG = Ax,45 Ay,30 (c) B RG = Ay,45 Az,30 33. Equivalent Euler angles to three rotations. Find the Euler angles corresponding to the following rotation matrix: (a) B RG = Az,60 Ay,45 Ax,30 (b) B RG = Az,60 Ay,45 Az,30 (c) B RG = Ax,60 Ay,45 Ax,30 34.  A cube rotation and forbidden space of z < 0. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Assume that none of the corners is allowed to have a negative z-components at any time. (a) Present a series of global principal rotations to make the line F H parallel to z-axis. (b) Present a series of global principal rotations to make the line DB on the z-axis and point A in (Z, Y )-plane. (c) Present a series of local principal rotations to make the line F H parallel to z-axis. (d) Present a series of local principal rotations to make the line DB on the z-axis and point A in (Z, Y )-plane. 35.  Local and global positions, Euler angles. Find the conditions between the Euler angles, (a) To transform G rP = [1, 1, 0]T to B rP = [0, 1, 1]T (b) To transform G rP = [1, 1, 0]T to B rP = [1, 0, 1]T 36.  Equivalent Euler angles to a triple rotations. Find the Euler angles for the rotation matrix of the fourth case in Appendix B. 4 − Az,ψ  Ay,θ  Ax,ϕ 

(2.376)

37.  Integrability of Euler frequencies. Show that dϕ and dψ are integrable, if ϕ and ψ are first and third Euler angles. 38.  Cardan angles for Euler angles. (a) Find the Cardan angles for a given set of Euler angles. (b) Find the Euler angles for a given set of Cardan angles. 39.  Cardan frequencies for Euler frequencies. (a) Find the Euler frequencies in terms of Cardan frequencies. (b) Find the Cardan frequencies in terms of Euler frequencies. 40.  Transformation matrix and three rotations. Figure 2.20 illustrates the original position of a cube with a fixed point at D and edges of length l = 1. Assume a new orientation in which points D and F are on Z-axis and point A is in (X, Z)-plane. Determine: (a) Transformation matrix between initial and new orientations. (b) Euler angles to move the cube to its new orientation. (c) Global roll–pitch–yaw angles to move the cube to its new orientation. (d) Local roll–pitch–yaw angles to move the cube to its new orientation. 41.  Alternative maneuvers. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Assume a new orientation in which points D and F are on Z-axis and point A is in (X, Z)-plane. Determine: (a) Angles for maneuver Y − X − Z as first–second–third rotations. (b) Angles for maneuver Y − Z − X as first–second–third rotations. (c) Angles for maneuver y − x − z as first–second–third rotations.

88

2 Rotation Kinematics

(d) Angles for maneuver y − z − x as first–second–third rotations. (e) Angles for maneuver y − Z − x as first–second–third rotations. (f) Angles for maneuver Y − z − X as first–second–third rotations. (g) Angles for maneuver x − X − x as first–second–third rotations. 42. Elements of rotation matrix. The elements of rotation matrix G RB are ⎡

⎤ ˆ cos(Iˆ, ıˆ) cos(Iˆ, jˆ) cos(Iˆ, k) ⎢ G ˆ ⎥ RB = ⎣ cos(Jˆ, ıˆ) cos(Jˆ, jˆ) cos(Jˆ, k) ⎦ ˆ ıˆ) cos(K, ˆ jˆ) cos(K, ˆ k) ˆ cos(K,

(2.377)

Find G RB if G rP1 = [0.7071, −1.2247, 1.4142]T is a point on the x-axis, and G rP2 = [2.7803, 0.38049, −1.0607]T is a point on the y-axis. 43. Linearly independent vectors. A set of vectors a1 , a2 , · · ·, an is considered linearly independent if the equation k1 a1 + k2 a2 + · · · + kn an = 0

(2.378)

in which k1 , k2 , · · ·, kn are unknown coefficients, has only one solution. k1 = k2 = · · · = kn = 0

(2.379)

Verify that the unit vectors of a body frame B(Oxyz), expressed in the global frame G(OXY Z), are linearly independent. 44. Product of orthogonal matrices.   A matrix R is called orthogonal if R −1 = R T where R T ij = Rj i . Prove that the product of two orthogonal matrices is also orthogonal. 45. Vector identity. The formula (a + b)2 = a 2 + b2 + 2ab for scalars is equivalent to (a + b)2 = a · a + b · b + 2a · b

(2.380)

for vectors. Show that this formula is equal to (a + b)2 = a · a + b · b + 2 G RB a · b if a is a vector in local frame and b is a vector in global frame. 46. Rotation as a linear operation. Show that R (a × b) = Ra × Rb

(2.381)

(2.382)

where R is a rotation matrix and a and b are two vectors defined in a coordinate frame. 47. Scalar triple product. Show that for three arbitrary vectors a, b, and c, we have a· (b × c) = (a × b) · c

(2.383)

48.  Euler angles and minimization distances. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Assume a new orientation in which points D and F are on Z-axis and point A is in (X, Y )-plane. Determine: (a) Transformation matrix between initial and new orientations. (b) Euler angles to move the cube to its new orientation.

Exercises

89

Fig. 2.21 A cube with sides of length l = 1

(c) Choose three non-coplanar corners and determine their position using Euler transformation matrix with unknown Euler angles. Define the distance between the initial and final positions of the points as d1 , d2 and d3 . Is it possible to determine the Euler angles by minimizing a sum of distances objective function J = d12 + d22 + d32 ? 49.  Continues rotation. Figure 2.20 illustrates the initial position of a cube with a fixed point at D and edges of length l = 1. Assume that the cube is turning about x-axis with angular speed of ω1 and at the same time it is turning about Z-axis with angular speed of ω2 . Determine the path of motion of point F . What is the path for ω1 = ω2 , ω1 = 2ω2 , ω1 = 3ω2 , and ω1 = 4ω2 ? 50.  Project: Quickest triple global rotations. Consider a body frame B coincident with a global frame G. The frame B undergoes a rotation of 45 deg about the X-axis followed by a rotation of 30 deg about the Z-axis, then 30 deg about the Y -axis, and 60 deg about the X-axis. If the body was supposed to be moved to the final orientation by only three principal global rotations of Appendix A, which one is the most efficient triple rotation? Assume efficiency means minimization of the sum of total rotation angles s. s = |α| + |β| + |γ |

(2.384)

51.  Project: Quickest triple body rotations. Consider a body frame B coincident with a global frame G. The frame B undergoes a rotation of 45 deg about the X-axis followed by a rotation of 30 deg about the Z-axis, then 30 deg about the Y -axis, and 60 deg about the X-axis. If the body was supposed to be moved to the final orientation by only three principal body rotations of Appendix B, which one is the most efficient triple rotation? Assume efficiency means minimization of the sum of total rotation angles s. s = |ϕ| + |θ | + |ψ|

(2.385)

52.  Project: Rotations of a cube. Consider a cube with sides of length l = 1 as is shown in Fig. 2.21. The corners of the cube are at A (1, 0, 0), B (1, 1, 0), C (0, 1, 0), D (0, 0, 0), E (1, 0, 1), F (1, 1, 1), G (0, 1, 1), H (0, 0, 1). Let us call this original configuration the rest position. (a) Move the longest diagonal DF to lay on the Z-axis only by two rotations about global principal axes. There are multiple solutions. Either one is acceptable. Determine and report the coordinates of all corners. (b) If the x-axis is not in (XZ)-plane, turn the cube at the end of case a about the Z-axis until the x-axis is in (XZ)-plane. Determine and report the coordinates of all corners. (c) Return the cube at the end of case b to its rest position only by two rotations about body principal axes. Determine the axes and angles for this maneuver with the minimum sum of total rotation angles.

3

Orientation Kinematics

Any finial orientation of a rigid body with a fixed point O, after a finite number of rotations is equivalent to a unique φ about a fixed axis u. ˆ Also any rotation φ of a rigid body with a fixed point O about a fixed axis uˆ can be decomposed into three rotations about three given non-coplanar axes including the global or body principal exes. Determination of the angle and axis is called the orientation kinematics of rigid bodies.

Fig. 3.1 Axis of rotation uˆ when it is coincident with the local z-axis

3.1

Axis–Angle Rotation

Assume a body frame B(Oxyz) rotates φ about a line indicated by a unit vector uˆ with direction cosines u1 , u2 , u3 . ⎡

⎤ u1 uˆ = u1 Iˆ + u2 Jˆ + u3 Kˆ = ⎣ u2 ⎦ u3  u21 + u22 + u23 = 1

(3.1)

(3.2)

This is called axis–angle representation of a rotation. Two parameters are necessary to define the unit vector uˆ through O, and one is necessary to define the amount of rotation φ of the rigid body about u. ˆ The axis–angle transformation matrix G RB that maps the coordinates in the local frame B(Oxyz) to the corresponding coordinates in the global frame G(OXY Z), © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_3

91

92

3 Orientation Kinematics G

r=

G

RB

B

(3.3)

r

is ⎡

⎤ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ G RB = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ

(3.4)

RB = Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ

(3.5)

G

with inverse B

RG =

G

RBT = Ru,−φ = I cos φ + uˆ uˆ T vers φ − u˜ sin φ ˆ

where, vers φ = versine φ = 1 − cos φ = 2 sin 2

φ 2

(3.6)

(3.7)

and u˜ is the skew symmetric matrix corresponding to the vector u. ˆ ⎤ u1 uˆ = ⎣ u2 ⎦ u3 ⎡

A matrix u˜ is skew symmetric if:

⎡

⎤ 0 −u3 u2 u˜ = ⎣ u3 0 −u1 ⎦ −u2 u1 0 u˜ T = −u˜

(3.8)

(3.9)

The transformation matrix (3.4) is the most general rotation matrix for a body frame B with a fixed point O, rotating with respect to a global frame G. If the axis of rotation (3.1) coincides with a global coordinate axis Z, Y , or X, then Eqs. (2.20), (2.21), or (2.22) will be reproduced. Given a transformation matrix G RB we can obtain the axis uˆ and angle φ of the rotation by  1 G RB − G RBT 2 sin φ  1  G  cos φ = tr RB − 1 2 u˜ =

(3.10) (3.11)

Proof Interestingly, the effect of rotation φ about an axis uˆ is equivalent to a sequence of rotations about the axes of the body frame such that the body frame is first rotated to bring one of its axes, say the z-axis, into coincidence with the rotation axis u, ˆ followed by a rotation φ about that local axis, then the reverse of the first sequence of rotations. The remaining rotation after these actions would be a rotation φ about u. ˆ ˆ the global frame G (OXY Z), and the rotated local frame Figure 3.1 illustrates an axis of rotation uˆ = u1 Iˆ + u2 Jˆ + u3 K, B (Oxyz) when the local z-axis is coincident with u. ˆ The local frame B (Oxyz) undergoes a sequence of rotations ϕ about the z-axis and θ about the y-axis to bring the local z-axis into coincidence with the rotation axis u, ˆ followed by rotation φ about u, ˆ and then perform the sequence backward by turning −θ about the y-axis and then −ϕ about the z-axis. Therefore, using (2.259), the rotation matrix G RB to map coordinates in body frame to their coordinates in global frame after rotation φ about uˆ is G

RB =

B

−1 RG =

B

T RG = Ru,φ ˆ

 T = Az,−ϕ Ay,−θ Az,φ Ay,θ Az,ϕ = ATz,ϕ ATy,θ ATz,φ ATy,−θ ATz,−ϕ

(3.12)

3.1 Axis–Angle Rotation

93

but we have

u2 u1 sin ϕ =  cos ϕ =  2 2 2 u + u2 u1 + u22  1 sin θ = u21 + u22 cos θ = u3 sin θ sin ϕ = u2 sin θ cos ϕ = u1

(3.13)

and hence, RB = Ru,φ ˆ ⎡ ⎤ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ

G

(3.14)

The matrix (3.14) can be decomposed to ⎡

Ru,φ ˆ

⎤ ⎡ ⎤ 100 u1   = cos φ ⎣ 0 1 0 ⎦ + (1 − cos φ) ⎣ u2 ⎦ u1 u2 u3 001 u3 ⎡ ⎤ 0 −u3 u2 + sin φ ⎣ u3 0 −u1 ⎦ −u2 u1 0

(3.15)

to be equal to G

RB = Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ

(3.16)

Equation (3.5) is called the Rodriguez rotation formula or the Euler–Lexell–Rodriguez formula, after French mathematician Olinde Rodriguez (1795–1851), Swiss mathematician Leonhard Euler (1707–1783), Finnish–Swedish astronomer Anders Johan Lexell (1740–1784). To show the rules (3.10) and (3.11), we expand G RB − G RBT to determine the axis of rotation uˆ ⎡

⎤ 0 −2 (sin φ) u3 2 (sin φ) u2 G RB − G RBT = ⎣ 2 (sin φ) u3 0 −2 (sin φ) u1 ⎦ −2 (sin φ) u2 2 (sin φ) u1 0 ⎡ ⎤ 0 −u3 u2 = 2 sin φ ⎣ u3 0 −u1 ⎦ = 2u˜ sin φ −u2 u1 0 and expand tr

G

(3.17)

 RB to provide the angle of rotation φ. tr

G

 RB = r11 + r22 + r33 = 3 cos φ + u21 (1 − cos φ) + u22 (1 − cos φ) +u23 (1 − cos φ)

  = 3 cos φ + u21 + u22 + u23 − u21 + u22 + u23 cos φ = 2 cos φ + 1

(3.18)

94

3 Orientation Kinematics

The Rodriguez rotation formula may also be reported in literature by the following equivalent forms: φ Ru,φ = I + u˜ sin φ + 2u˜ 2 sin 2 ˆ 2  φ φ φ Ru,φ I cos + u˜ sin = I + 2u˜ sin ˆ 2 2 2

(3.19) (3.20)

Ru,φ = I + u˜ sin φ + u˜ 2 vers φ ˆ   Ru,φ = I−uˆ uˆ T cos φ + u˜ sin φ + uˆ uˆ T ˆ

(3.21)

Ru,φ = I + u˜ + u˜ sin φ − u˜ cos φ ˆ

(3.23)

2

2

(3.22)

The inverse of an angle-axis rotation is G

RBT =

B

RG = Ru,−φ = I cos φ + uˆ uˆ T vers φ − u˜ sin φ ˆ

(3.24)

It means orientation of B in G, when B is rotated φ about u, ˆ is the same as the orientation of G in B, when B is rotated −φ about u. ˆ The 3 × 3 real orthogonal transformation matrix R is also called a rotator and the skew symmetric matrix u˜ is called a spinor, or the Euler axis or the eigenaxis of rotation. We can verify that u˜ uˆ = 0

(3.25)

uˆ uˆ − I = u˜

(3.26)

rT u˜ r = 0

(3.27)

uˆ × r = u˜ r = −˜r uˆ = −r × uˆ

(3.28)

2

T

      The angle-axis representation of rotation is not unique. Rotation θ, uˆ is equal to rotation −θ, −uˆ , and θ + 2π , uˆ .



ˆ Simplifying the axis–angle rotation matrix for a special known case axis of Example 55 Axis–angle rotation when uˆ = K. rotation. If the local frame B (Oxyz) rotates about the Z-axis, then  T uˆ = Kˆ = 0 0 1

(3.29)

and the transformation matrix (3.4) reduces to ⎡

⎤ 0 vers φ + cos φ 0 vers φ − 1 sin φ 0 vers φ + 0 sin φ G RB = ⎣ 0 vers φ + 1 sin φ 0 vers φ + cos φ 0 vers φ − 0 sin φ ⎦ 0 vers φ − 0 sin φ 0 vers φ + 0 sin φ 1 vers φ + cos φ ⎡ ⎤ cos φ − sin φ 0 = ⎣ sin φ cos φ 0 ⎦ 0 0 1 which is equivalent to the rotation matrix about the Z-axis of global frame in (2.20).

(3.30)

3.1 Axis–Angle Rotation

95

Fig. 3.2 A cube with a fixed point at A

Example 56 Rotation about a rotated local axis. Axis of rotation if it is a body axis after a rotation about global axes. If the body coordinate frame B (Oxyz) rotates ϕ about the global Z-axis, then the x-axis would be along uˆ x . ⎡

cos ϕ − sin ϕ G ⎣ uˆ x = RZ,ϕ ıˆ = sin ϕ cos ϕ 0 0

⎤⎡ ⎤ ⎡ ⎤ 0 1 cos ϕ 0 ⎦ ⎣ 0 ⎦ = ⎣ sin ϕ ⎦ 1 0 0

(3.31)

Rotation θ about uˆ x can be defined by Rodriguez’s formula (3.4). ⎤ sin ϕ sin θ cos2 ϕ vers θ + cos θ cos ϕ sin ϕ vers θ = ⎣ cos ϕ sin ϕ vers θ sin2 ϕ vers θ + cos θ − cos ϕ sin θ ⎦ − sin ϕ sin θ cos ϕ sin θ cos θ ⎡

G

Ruˆ x ,θ

(3.32)

Now, rotation ϕ about the global Z-axis followed by rotation θ about the local x-axis is transformed by G

RB =

G

⎡

Ruˆ x ,θ G RZ,ϕ

⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(3.33)

  that must be equal to Ax,θ Az,ϕ −1 = ATz,ϕ ATx,θ . Example 57 Axis and angle of rotation. Determine all important points of a rigid body after a given axis–angle rotation. Consider a cubic rigid body with a fixed point at A and a unit length of edges as is shown in Fig. 3.2. If we turn the cube 45 deg about u,  T u= 111 (3.34) then we can find the global coordinates of its corners using Rodriguez transformation matrix. π φ= 4

⎡ ⎤ 0.577 35 u uˆ = √ = ⎣ 0.577 35 ⎦ 3 0.577 35

(3.35)

96

3 Orientation Kinematics

Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ ⎡ ⎤ 0.804 74 −0.310 62 0.505 88 = ⎣ 0.505 88 0.804 74 −0.310 62 ⎦ −0.310 62 0.505 88 0.804 74

(3.36)

The local coordinates of the corners are B

rB B rC x 1 1 y 0 1 z 0 0

B

rD 0 1 0

B

rE 0 0 1

B

rF 1 0 1

B

rG 1 1 1

B

rH 0 1 1

(3.37)

B and therefore, using G r = Ru,φ r the global coordinates of the corners after the rotation will be ˆ G

rB G rC G rD G rE G rF G rG G rH X 0.804 0.495 −0.31 0.505 1.310 1 0.196 Y 0.505 1.31 0.804 −0.31 0.196 1 0.495 Z −0.31 0.196 0.505 0.804 0.495 1 1.31

(3.38)

Point G is on the axis of rotation, so its coordinates will not change. Points B, D, F , and H are in a symmetric plane indicated by u. ˆ Therefore, they will move on a circle. To check this fact, we may find the midpoint of BH , or F D and see if it is on the u-axis. ˆ Let us call the midpoint of the cube by P . ⎡ ⎤ 0.5     1 1 B B B rP = rB + B rH = rF + B rD = ⎣ 0.5 ⎦ 2 2 0.5 ⎡ ⎤ 0.5     1 1 G G G rP = rB + G rH = rF + G rD = ⎣ 0.5 ⎦ 2 2 0.5

(3.39)

(3.40)

Example 58 Axis and angle of a rotation matrix. Numerical example for determination of axis–angle rotation for a set of given Euler angles. A body coordinate frame B undergoes three Euler rotations (ϕ, θ , ψ) = (30, 45, 60) deg with respect to a global frame G. The rotation matrix to transform coordinates of B to G is G

 T T T T T RG = Rz,ψ Rx,θ Rz,ϕ = Rz,ϕ Rx,θ Rz,ψ ⎡ ⎤ 0.126 83 −0.926 78 0.353 55 = ⎣ 0.780 33 −0.126 83 −0.612 37 ⎦ 0.612 37 0.353 55 0.707 11

RB =

B

(3.41)

The unique angle-axis of rotation for this rotation matrix can then be found by Eqs. (3.10) and (3.11).  φ = arccos

 1  G  tr RB − 1 2

= arccos (−0.146 45) = 1.7178 rad = 98 deg

(3.42)

3.1 Axis–Angle Rotation

97

G  1 RB − G RBT 2 sin φ ⎡ ⎤ 0.0 −0.862 85 −0.130 82 = ⎣ 0.862 85 0.0 −0.488 22 ⎦ 0.130 82 0.488 22 0.0

u˜ =

⎤ 0.488 22 uˆ = ⎣ −0.130 82 ⎦ 0.862 85

(3.43)

⎡

(3.44)

As a double check, we may verify the angle-axis rotation formula and derive the same rotation matrix. G

RB = Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ ⎡ ⎤ 0.12682 −0.92677 0.35354 = ⎣ 0.78032 −0.12683 −0.61237 ⎦ 0.61236 0.35355 0.70709

(3.45)

Example 59  Skew symmetric characteristic of rotation matrix. Time derivative of orthogonality condition gives a skew symmetric combination of rotation matrices that defines angular velocity. Time derivative of the orthogonality condition of rotation matrix (2.285) G

RBT G RB = I

(3.46)

leads to d G T G  RB RB = G R˙ TB G RB + dt G T G T RB R˙ B = − G RBT G R˙ B

G

RBT G R˙ B = 0

(3.47) (3.48)

  showing that G RBT G R˙ B is a skew symmetric matrix. A matrix is skew symmetric if its transpose is equal to its negative. If   we show the rotation matrix by its elements, G RB = rij , then G R˙ B = r˙ ij . Rewriting the orthogonality condition as G

RB G RBT = I

(3.49)

provides another form of the identity. G

Let us show the skew symmetric matrix

R˙ B G RBT + G

G

RB G R˙ BT = 0 T R˙ B G RBT = − G RB G R˙ BT G

(3.50) (3.51)

 RBT G R˙ B by ω, ˜ ω˜ =

G

RBT

G

R˙ B

(3.52)

then, we find the following equation for the time derivative of rotation matrix: G

R˙ B =

G

RB ω˜

(3.53)

where ω is the vector of angular velocity of the frame B(Oxyz) with respect to frame G(OXY Z), and ω˜ is its skew symmetric matrix expression of ω.

98

3 Orientation Kinematics

Example 60  Differentiating a rotation matrix with respect to a parameter. The skew symmetric characteristic of derivative of rotation matrix is correct for derivative with respect to any parameter. Suppose that a rotation matrix R is a function of a variable τ ; hence, R = R(τ ). To find the differential of R with respect to τ , we use the orthogonality characteristic R RT = I

(3.54)

dR T dR T R +R =0 dτ dτ

(3.55)

# " dR T dR T T R + R =0 dτ dτ

(3.56)

and take derivative of both sides

which can be rewritten in the following form:

showing that [ dR R T ] is a skew symmetric matrix. dτ Example 61  Eigenvalues and eigenvectors of G RB . Any rotation matrix has a real eigenvalue of +1 and its associated real eigenvector uˆ which is the axis of rotation φ. The other two eigenvalues are complex conjugate and their associated eigenvectors are complex vectors making an orthogonal triad with u. ˆ Consider a rotation matrix G RB . Applying a rotation on the axis of rotation uˆ cannot change the direction of u. ˆ RB uˆ = λuˆ

(3.57)

G

RB − λI = 0

(3.58)

− λ3 + tr(G RB )λ2 − tr(G RB )λ + 1 = 0

(3.59)

    (λ − 1) λ2 − λ tr(G RB ) − 1 + 1 = 0

(3.60)

G

Therefore, the transformation equation implies that

The characteristic equation of this determinant is

Factoring the left-hand side, yields

It shows that λ1 = 1 is always an eigenvalue of G RB . Hence, there exist a real vector u, ˆ such that every point on the line indicated by the vector n1 = uˆ remains fixed and invariant under transformation G RB . The rotation angle of this rotation φ is defined by  1 1 G cos φ = tr( RB ) − 1 = (r11 + r22 + r33 − 1) (3.61) 2 2 The remaining eigenvalues are λ2 = eiφ = cos φ + i sin φ λ3 = e

−iφ

= cos φ − i sin φ

(3.62) (3.63)

and their associated eigenvectors are v and v¯ , where v¯ is the complex conjugate of v. Because G RB is orthogonal, n1 , v, and v¯ are also orthogonal. The eigenvectors v and v¯ span a plane perpendicular to the axis of rotation n1 . A real basis for this plane can be found by using the following vectors: 1 |v + v¯ | 2 i n3 = |v − v¯ | 2

n2 =

(3.64) (3.65)

3.1 Axis–Angle Rotation

99

The basis n2 and n3 transform to G

G

1 1

|λ2 v + λ3 v¯ | = eiφ v + eiφ v 2 2 = v cos φ + v¯ sin φ

(3.66)

i 1

|λ2 v − λ3 v¯ | = eiφ v − eiφ v 2 2 = −v cos φ + v¯ sin φ

(3.67)

RB n 2 =

RB n 3 =

Therefore, the effect of transformation G RB is to rotate vectors in the plane spanned by n2 and n3 through angle φ about n1 , while vectors along n1 are invariant. Example 62  Final rotation formula. The rotation kinematics is based on the assumption that before applying a rotation, the body B and global G frames are coincident. Here we learn about breaking this assumption, and changes to axis-angle rotation formula. The assumption to apply any rotation of a body B in a fixed frame G is that the coordinate frames B and G should be coincident before the rotation. We can imagine a situation in which B and G are not coincident and we want to rotate B about a globally fixed axis G u. ˆ Consider a global frame G and a body frame B0 at a non-coincident configuration. The body frame is supposed to turn φ about an axis G uˆ from its current position at B0 . G

uˆ = u1 Iˆ + u2 Jˆ + u3 Kˆ  u21 + u22 + u23 = 1

(3.68) (3.69)

We can always assume that the body B has come to the position B0 , from a coincident configuration with G, by a rotation α about z0 followed by a rotation β about x0 and then a rotation γ about z0 . Consider the body frame B at the coincident position with B0 . When we apply a sequence of rotations ϕ about the z-axis and θ about the y-axis on the body frame, the local z-axis will coincide with the rotation axis G u. ˆ Let us imagine B at this time and indicate it by B1 . Then we apply the rotation φ about z ≡ uˆ and perform the sequence of rotations −θ about the y-axis and −ϕ about the z-axis. The resultant of this maneuver would be a rotation φ of B about u, ˆ starting from B0 . The initial relative orientation of the body must be known; therefore, the transformation G R0 between B0 and G is a given matrix. ⎡ ⎤ b11 b12 b13   G R0 = bij = ⎣ b21 b22 b23 ⎦ (3.70) b31 b32 b33 Having G R0 , we can determine the angles α, β, and γ . G

R0 = Rz0 ,γ Rx0 ,β Rz0 ,α ⎡ ⎤ cα cγ − cβ sα sγ cγ sα + cα cβ sγ sβ sγ = ⎣ −cα sγ − cβ cγ sα cα cβ cγ − sα sγ sβ cγ ⎦ sα sβ −cα sβ cβ

α = − arctan

b31 b32

β = arccos b33

γ = arctan

b13 b23

(3.71)

(3.72)

The transformation matrix between B0 and B comes from Rodriguez formula (3.5). However, G uˆ must be expressed in B0 to apply the Rodriguez formula.

100

3 Orientation Kinematics 0 0

uˆ =

G

R0T

G

uˆ

(3.73)

RB = I cos φ + 0 uˆ 0 uˆ T vers φ + 0 u˜ sin φ   T = I cos φ + G R0T G uˆ G R0T G uˆ vers φ +

G

R0T u˜ G R0 sin φ

= I cos φ + +

G

G

R0T

G

uˆ G uˆ T

G

R0 vers φ

R0T u˜ G R0 sin φ

(3.74)

The transformation matrix G RB between the final position of the body and global frame would be G

RB =

R0 0 RB  = G R0 I cos φ + G

R0T G uˆ G uˆ T G R0 vers φ  + G R0T u˜ G R0 sin φ   = G R0 cos φ + G uˆ G uˆ T G R0 vers φ + u˜ G R0 sin φ =

G

Ru,φ ˆ

G

G

R0

(3.75)

We call this equation the final rotation formula. It determines the transformation matrix between a body frame B and the global frame G after the rotation φ of B about uˆ = G u, ˆ starting from a given in coincident position with global frame B0 = G, expressed by a given transformation matrix G R0 . As an example, consider a body that is rotated 30 deg about the Z axis and is at B0 . π ⎤ ⎡ π ⎤ − sin 0 0.866 −0.5 0 6 6 ⎢ ⎥ π π G ⎥ R0 = ⎢ cos 0 ⎦ ≈ ⎣ 0.5 0.866 0 ⎦ ⎣ sin 6 6 0 0 1 0 0 1 ⎡

cos

(3.76)

The body is then supposed to turn 90 deg about G u. ˆ φ=

π 2

G

uˆ = Iˆ

(3.77)

Therefore, ⎛⎡

⎤ ⎡ ⎤ 100 100  π π G RB = ⎝⎣ 0 1 0 ⎦ cos + ⎣ 0 0 0 ⎦ 1 − cos 2 2 001 000 ⎡ ⎤ ⎞⎡ ⎤ 00 0 0.866 −0.5 0 π + ⎣ 0 0 −1 ⎦ G R0 sin ⎠ ⎣ 0.5 0.866 0 ⎦ 2 01 0 0 0 1 ⎡ ⎤ 0.866 −0.5 0 0 −1 ⎦ =⎣ 0 0.5 0.866 0

(3.78)

A body point at B r = 2ˆı will be seen at G

r=

G

⎡

RB B r

⎤⎡ ⎤ ⎡ ⎤ 0.866 −0.5 0 2 1.732 =⎣ 0 0 −1 ⎦ ⎣ 0 ⎦ = ⎣ 0 ⎦ 0.5 0.866 0 0 1

(3.79)

3.1 Axis–Angle Rotation

101

Example 63  Rotation of a rotated body. A numerical example for final rotation formula (3.75). A rigid body B has already turned 30 deg about Y -axis. We need to turn the body 45 deg about u. ˆ uˆ =

G

1 1 1 uˆ = √ Iˆ + √ Jˆ + √ Kˆ 3 3 3

(3.80)

Because of the first rotation, we have ⎡

π π 0 sin 6 6 ⎢ G R0 = ⎢ 0 1 0 ⎣ π π − sin 0 cos 6 6 cos

⎤

⎡ ⎤ 0.866 0 0.5 ⎥ ⎥=⎣ 0 1 0 ⎦ ⎦ −0.5 0 0.866

(3.81)

Using the final rotation formula (3.75), we are able to determine the required rotation transformation matrix, G RB . G

G

  Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ ⎤ ⎡ 0.804 74 −0.310 62 0.505 88 = ⎣ 0.505 88 0.804 74 −0.310 62 ⎦ −0.310 62 0.505 88 0.804 74

RB =

G

⎡

Ru,φ ˆ

G

(3.82)

R0

⎤ 0.443 99 −0.310 62 0.840 47 = ⎣ 0.593 41 0.804 74 −1.606 5 × 10−2 ⎦ −0.671 37 0.505 88 0.541 62

(3.83)

Example 64 Orthogonality of Rodriguez rotation matrix. Here is to examine that Rodriguez rotation matrix is orthogonal and discover a few identities. The orthogonality characteristic of the rotation matrix must be consistent with the Rodriguez formula as well. We show that we can multiply Eqs. (3.5) and (3.6): G

RB B RG = Ru,φ ˆ Ru,−φ ˆ   = I cos φ + uˆ uˆ T vers φ + u˜ sin φ   × I cos φ + uˆ uˆ T vers φ − u˜ sin φ = I cos2 φ + uˆ uˆ T vers φ cos φ − u˜ sin φ cos φ +uˆ uˆ T vers φ cos φ + uˆ uˆ T uˆ uˆ T vers φ vers φ −u˜ uˆ uˆ T vers φ sin φ + u˜ sin φ cos φ +uˆ uˆ T u˜ sin φ vers φ − u˜ u˜ sin2 φ = I

That is because of

⎤ u21 u1 u2 u1 u3 uˆ uˆ T = ⎣ u1 u2 u22 u2 u3 ⎦ u1 u3 u2 u3 u23 ⎡ 2 ⎤ u1 u1 u2 u1 u3 uˆ uˆ T uˆ uˆ T = ⎣ u1 u2 u22 u2 u3 ⎦ = uˆ uˆ T u1 u3 u2 u3 u23 ⎡ ⎤ 000 u˜ uˆ uˆ T = uˆ uˆ T u˜ = ⎣ 0 0 0 ⎦ 000

(3.84)

⎡

(3.85)

(3.86)

(3.87)

102

3 Orientation Kinematics

Example 65  Euler rotation theorem 66. Expression and proof of Euler theorems about rigid body rotation. Motion of a rigid body with a fixed point can be summarized in Euler rigid body rotation theorem proven in 1775 by Swiss mathematician Leonhard Euler (1707–1783). Theorem 66 A rigid body rotation about a point is equivalent to a rotation about a line passing through the point. This theorem was originally stated by Euler as below: Theorem 67 Quomodocunque sphaera circa centrum suum conuertatur, semper assignari potest diameter, cuius directio in situ translato conueniat cum situ initiali. When a sphere is moved around its center it is always possible to find a diameter whose direction in the displaced position is the same as in the initial position. Proof Assume a body has been rotated about the fixed point O. Let us select an arbitrary segment A1 B1 in the initial position of the body that is not passing through O. Then A2 B2 would be the corresponding segment at the final position. We draw the plane of symmetry π of segment A1 B1 and A2 B2 by points O and midpoints of A1 A2 and B1 B2 . The planes OA1 B1 and OA2 B2 intersect at a line l, which is also in the plane of symmetry π. The line l passes through O because l is the locus of points equidistant from A1 and A2 as well as B1 and B2 . While OA1 = OA2 and OB1 = OB2 , let us pick a point C on l different than O. The tetrahedrons OCA1 B1 and OCA2 B2 are equal and superposable. This is because the vertices O, C, A1 and O, C, A2 are placed symmetrically with respect to π . Hence, if the tetrahedrons OCA1 B1 and OCA2 B2 were not superposable, the vertices B1 and B2 would have to be placed asymmetrically with respect to π. If we now rotate the rigid body about the axis l so that A1 falls on A, then the tetrahedron OCA1 B1 will fall on the tetrahedron OCA2 B2 . After this rotation of the body we will know the positions of three points O, A2 , B2 of the body and therefore we know the position of every point of the body. A consequence of the Euler theorem is that: During a rotation of a body about a point, there exists a certain line in the body having the property that its points do not change their position. Another consequence of the Euler theorem is that: If a rigid body makes two successive rotations about two axes passing through the fixed point O, then the body can be displaced from its initial position to its final position by means of one rotation about an axis passing through O. Therefore, the combination of two rotations about an axis passing through one point is a rotation about an axis passing through the same point.  Example 68  Euler rotation theorem 2. Expression and proof of Euler theorem about rigid body rotation. Theorem 69 A rigid body can be displaced from an initial position to a final position by means of two successive rotations. Proof Let P1 be a point of the rigid body B at its initial position B1 , and P2 the corresponding point in the final position B2 . We rotate the body 180 deg about the axis l, which is the axis of symmetry of the segment P1 P2 . By this rotation, point P1 will fall on point P2 . The body will assume the position B  , which has the point P2 in common with position B2 . Consequently, we can go from position B  to B2 by a rotation about an axis l passing through P2 . 

3.2

 Order-Free Rotation

Most industrial multibodies with a common fixed point are similar to spherical wrists with one, two, three, or more rotary actuators that can work independently. Practically, the order of actuation of the motors is not important if motor number i is carrying all other bodies numbers i + 1, i + 2, · · · . The orientation of the body would be the same, no matter which motor acts first. We introduce a theorem to generalize and simplify the applied rotation transformation of multibodies. Here is the mathematical explanation.

3.2  Order-Free Rotation

103

Theorem Order-free rotations. Consider n rigid bodies with coordinate frames B1 , B2 , · · · , Bn with a common fixed point. The body B1 carries the bodies B2 , B3 , · · · , Bn and turns α 1 about a fixed axis in the global coordinate frame G. The body B2 carries B3 , B4 , · · · , Bn and T turns α 2 about a fixed axis in B1 , and so on. The transformation matrix G Rn = G Rn n Rn−1 · · · 2 R1 = n RG is independent of the order of rotations α 1 , α 2 , · · · , α n . Proof Consider a globally fixed coordinate frame G (OXY Z) and two body frames B1 (Ox1 y1 z1 ) and B2 (Ox2 y2 z2 ), all with a common origin O. The body B1 caries B2 and rotates with respect to G. The body B2 rotates in B1 . The first rotation α of B1 in G is about a G-fixed axis G uˆ 1 . The second motion is a rotation β of B2 about a B1 -fixed axis 1 uˆ 2 , or equivalently about 2 uˆ 2 in B2 . The frames B2 , B1 , G are coincident before rotations. A transformation matrix 2 R1 for rotation β = α 2 about a body axis 2 uˆ 2 relates B2 and B1 . Let us show this rotation matrix by Rβ , and call it the β-rotation. 2

R1 = R 2 uˆ 2 , β = Rβ

(3.88)

Now assume that a γ -rotation of R 2 uˆ 2 , γ similar to (3.88) has happened and an α-rotation of R2 uˆ 1 ,α is going to follow. To determine the overall transformation matrix 1 RG to relate the frame B1 to the global frame G, we apply a rotation −γ on B2 about the local axis 2 uˆ 2 to bring B2 on B1 and G. The rotation α = α 1 will turn B1 and B2 together, considering B2 is carried by B1 . The axis G uˆ 1 would coincide with an axis 2 uˆ 1 that would be about a local axis of B2 . A rotation α of B2 about 2 uˆ 1 will equivalently rotate B1 and B2 in G. Then, we apply the rotation α about 2 uˆ 1 and then apply a reverse rotation γ about 2 uˆ 2 to remove the effect of the rotation −γ about 2 uˆ 2 . Let us show the transformation matrix 1 RG for rotation α by Rα , and call it the α-rotation. 1 RG = R2 uˆ 2 ,γ R2 uˆ 1 ,α R2 uˆ 2 ,−γ = Rα (3.89) Therefore a rotation Rα of B1 in G is being considered as a combination of three rotations from B2 viewpoint. A rotation −γ of B2 with respect to B1 , followed by a rotation α of B2 and B1 with respect to G, and then a rotation γ of B2 with respect to B1 . The rotations α and β can be interchanged and performed in any order. The final transformation matrix will not be altered by changing the order of rotations α and β. To show this fact, let us assume that the frames B1 and B2 are on G. Applying the rotation α and then the rotation β provides the following transformation matrix: 2

  RG = Rβ Rα = R2 uˆ 2 ,β R2 uˆ 2 ,0 R2 uˆ 1 ,α R2 uˆ 2 ,−0   = R2 uˆ 2 ,β I R2 uˆ 1 ,α I = R2 uˆ 2 ,β R2 uˆ 1 ,α

(3.90)

By changing the order of rotations and applying the rotation β first and then α, we find the same transformation matrix. 2

  RG = Rα Rβ = R2 uˆ 2 ,β R2 uˆ 1 ,α R2 uˆ 2 ,−β R2 uˆ 2 ,β = R2 uˆ 2 ,β R2 uˆ 1 ,α

(3.91)

Now consider 3 R2 as a transformation matrix for rotation of B3 about 2 uˆ 3 in B2 , and 2 RG as the matrix for rotation of B2 in G. Employing the same procedure, we can find the order-free transformation 3 RG . 3

RG = 3 R2 2 RG = 2 RG 3 R2

(3.92)

We can similarly expand and apply the proof of the order-free rotations theorem to any number of bodies that are similarly connected. 

104

3 Orientation Kinematics

Fig. 3.3 An inspection camera

Example 70  Order-free rotations in directional control systems. A detection device needs only two rotary actuators. The axes of actuators are better to be set orthogonal. Here the order-free rotation theorem is examined on an inspecting camera. Consider the inspection camera in Fig. 3.3 with a direction control system to direct the camera to a point. To have a directional control system, we need to control the angles of a flat plate and direct an axis on the plate or its normal vector to a desired direction. Figure 3.3 shows that the two motors of the directional control system are independent. Therefore, we may command motors 1 and 2 to turn in different order or together. The final configuration of the camera must be the same if we command motor 1 to turn α and motor 2 to turn β in any order. To show this fact, we must define the transformation matrices such that the matrix multiplication is independent of the order of the matrices. Consider the camera frame B8 is not coincident with the fixed Sina frame B9 . We may always bring the local z8 -axis of B8 on the global z9 -axis of B9 by a rotation −β about the y8 -axis. Now rotation α about the globally fixed z9 -axis becomes a local rotation about the z8 -axis. A reverse rotation β about the y8 -axis eliminates the first and third rotations and a rotation α about the fixed z9 -axis remains: 8

R9 = RZ,α = Ry,β Rz,α Ry,−β ⎡ ⎤⎡ ⎤⎡ ⎤ cβ 0 −sβ cα −sα 0 c (−β) 0 −s (−β) ⎦ = ⎣ 0 1 0 ⎦ ⎣ sα cα 0 ⎦ ⎣ 0 1 0 sβ 0 cβ 0 0 1 s (−β) 0 c (−β) ⎡ ⎤ cαc2 β + s 2 β −cβsα cαcβsβ − cβsβ ⎦ =⎣ cβsα cα sαsβ 2 2 cαcβsβ − cβsβ −sαsβ c β + cαs β

(3.93)

It reduces to the principal Rz,α for β = 0. Let us apply a rotation α to the camera frame B8 about the z9 -axis from a coincident configuration with the Sina frame B9 followed by a rotation β about the y8 -axis: 8

R9 = Ry8 ,β RzT9 ,α ⎡ ⎤⎡ cos β 0 − sin β cos α − sin α =⎣ 0 1 0 ⎦ ⎣ sin α cos α sin β 0 cos β 0 0 ⎡ ⎤ cos α cos β cos β sin α − sin β = ⎣ − sin α cos α 0 ⎦ cos α sin β sin α sin β cos β

(3.94) ⎤T

0 0⎦ 1

Now let us turn the camera frame B8 an angle β from a coincident configuration with frame B9 about the y8 -axis followed by a rotation α about the z9 -axis. However, the rotation about the z9 -axis must be considered as (3.93):

3.2  Order-Free Rotation

105 8

R9 = RzT9 ,α Ry8 ,β ⎤⎡ ⎤ ⎡ cβ 0 −sβ cαc2 β + s 2 β −cβsα cαcβsβ − cβsβ ⎦⎣ 0 1 0 ⎦ =⎣ cβsα cα sαsβ 2 2 sβ 0 cβ cαcβsβ − cβsβ −sαsβ c β + cαs β ⎡ ⎤ cos α cos β − cos β sin α − sin β = ⎣ sin α cos α 0 ⎦ cos α sin β − sin α sin β cos β

(3.95)

The final rotation matrix (3.95) is the same as (3.94). So, it is immaterial which motor turns first provided that the correct matrices are used. Therefore, we may define the transformation matrix between the camera frame B8 and frame B9 by matrix multiplication in any order, 8 R9 = RzT9 ,α Ry8 ,β = Ry8 ,β RzT9 ,α (3.96) where,

⎡

RzT9 ,α

⎤ cαc2 β + s 2 β −cβsα cαcβsβ − cβsβ ⎦ cβsα cα sαsβ =⎣ 2 2 cαcβsβ − cβsβ −sαsβ c β + cαs β ⎡

and Ry8 ,β

⎤ cos β 0 − sin β =⎣ 0 1 0 ⎦ sin β 0 cos β

(3.97)

(3.98)

However, to evaluate 8 R9 , we must substitute the current value of β in RzT9 ,α . To aim the camera at from a point to the other, let us assume the camera frame B8 of the directional control system has already been rotated and is not coincident with the Sina frame B9 . If the rotation α 1 happened before rotation β 1 , the rotation matrix between B8 and B9 would be ⎡

⎤ cos α 1 cos β 1 − cos β 1 sin α 1 − sin β 1 ⎦ sin α 1 cos α 1 0 R1 = 8 R9 = ⎣ cos α 1 sin β 1 − sin α1 sin β 1 cos β 1

(3.99)

otherwise, we may use Eq. (3.96) to reverse the order of rotations. To aim the camera to a new direction, we may turn the camera β 2 about y8 and α 2 about z9 . The rotation matrix between the camera and the base would then be 8

  R9 = R1 Ry8 ,β 2 RzT9 ,α2 = R1 Ry8 ,β 2 Ry8 ,β 1 Rz8 ,α2 Ry8 ,−β 1 = Ry8 ,β 1 Rz8 ,α1 Ry8 ,β 2 Ry8 ,β 1 Rz8 ,α2 Ry8 ,−β 1 = Ry8 ,β 1 Rz8 ,α1 Ry8 ,β 1 +β 2 Rz8 ,α2 Ry8 ,−β 1

(3.100)

or if we change the order of second rotations β 2 and α 2 and use Eq. (3.93), then 8

  R9 = R1 RzT9 ,α2 Ry8 ,β 2 = R1 Ry8 ,β 1 +β 2 Rz8 ,α2 Ry8 ,−β 1 −β 2 Ry8 ,β 2 = Ry8 ,β 1 Rz8 ,α1 Ry8 ,β 1 +β 2 Rz8 ,α2 Ry8 ,−β 1 −β 2 Ry8 ,β 2 = Ry8 ,β 1 Rz8 ,α1 Ry8 ,β 1 +β 2 Rz8 ,α2 Ry8 ,−β 1

(3.101)

Example 71  Spherical wrist and order-free rotations. Spherical wrist is a good example for real dynamic system with order-free rotations. A spherical wrist, as is shown in Fig. 3.4, will act by three independent rotary actuators. The independency of the actuators means we may run the three actuators in any arbitrary order. This freedom requires that the transformation matrix between B0 and B2 be order free and independent of the order of rotations. Let us assume none of the angles is zero, and hence, the coordinate frames B0 , B1 , B2 are not coincident. The rotation ψ of B2 is always about the local z2 -axis. This rotation will not move any axis of B2 .

106

3 Orientation Kinematics

Fig. 3.4 Eulerian spherical wrist or type roll–pitch–roll

⎡

2

R1 = Rz2 ,ψ = Rz,ψ

⎤ cos ψ sin ψ 0 = ⎣ − sin ψ cos ψ 0 ⎦ 0 0 1

(3.102)

To find the order-free transformation matrix for rotation θ about x1 , we turn B2 an angle −ψ about z2 to bring x2 on x1 . Now rotation θ about x1 becomes a local rotation about x2 . A reverse rotation ψ about z2 eliminates the first and third rotations and a rotation θ about x1 remains: 2

R1 = Rx1 ,θ = Rz2 ,ψ Rx2 ,θ Rz2 ,−ψ = Rz,ψ Rx,θ Rz,−ψ ⎡ ⎤⎡ ⎤⎡ ⎤ cψ sψ 0 1 0 0 c (−ψ) s (−ψ) 0 = ⎣ −sψ cψ 0 ⎦ ⎣ 0 cθ sθ ⎦ ⎣ −s (−ψ) c (−ψ) 0 ⎦ 0 0 1 0 −sθ cθ 0 0 1 ⎡ 2 ⎤ c ψ + cθ s 2 ψ cθ cψsψ − cψsψ sθsψ = ⎣ cθcψsψ − cψsψ cθ c2 ψ + s 2 ψ cψsθ ⎦ −sθsψ −cψsθ cθ

(3.103)

The matrix (3.103) is the order-free transformation for turning θ about x1 . We can examine and check that 2

R1 = Rx1 ,θ Rz2 ,ψ = Rz2 ,ψ Rx1 ,θ

(3.104)

provided that we use (3.103) for Rx1 ,θ and substitute the current value of ψ before multiplication. Assuming a rotation ψ about z2 and then a rotation θ about x1 , we find 2

R1 = Rx1 ,θ Rz2 ,ψ ⎡ 2 ⎤⎡ ⎤ c ψ + cθs 2 ψ cθ cψsψ − cψsψ sθsψ cψ sψ 0 = ⎣ cθcψsψ − cψsψ cθ c2 ψ + s 2 ψ cψsθ ⎦ ⎣ −sψ cψ 0 ⎦ −sθsψ −cψsθ cθ 0 0 1 ⎡ ⎤ cos ψ cos θ sin ψ sin θ sin ψ = ⎣ − sin ψ cos θ cos ψ cos ψ sin θ ⎦ 0 − sin θ cos θ

(3.105)

Now assuming a rotation θ about x1 and then a rotation ψ about z2 , we find 2

R1 = Rz2 ,ψ Rx1 ,θ ⎡ ⎤⎡ 2 ⎤ cψ sψ 0 c 0 + cθ s 2 0 cθ c0s0 − c0s0 sθs0 = ⎣ −sψ cψ 0 ⎦ ⎣ cθ c0s0 − c0s0 cθ c2 0 + s 2 0 c0sθ ⎦ 0 0 1 −sθs0 −c0sθ cθ ⎡ ⎤ cos ψ cos θ sin ψ sin θ sin ψ = ⎣ − sin ψ cos θ cos ψ cos ψ sin θ ⎦ 0 − sin θ cos θ

(3.106)

3.2  Order-Free Rotation

107

because we substitute ψ = 0 in Rx1 ,θ to evaluate Rz2 ,ψ Rx1 ,θ . To find the order-free transformation matrix for rotation ϕ about x0 , we turn B2 an angle −ψ about the local z2 to bring x2 on x1 . Another rotation −θ about x2 brings z2 on z0 . Now rotation ϕ about x0 becomes a local rotation about x2 . A reverse rotation θ about x2 and then ψ about z2 eliminates the first, second, fourth, and fifth rotations and a rotation ϕ about x0 remains: 1

R0 = Rx0 ,θ = Rz2 ,ψ Rx2 ,θ Rz2 ,ϕ Rx2 ,−θ Rz2 ,−ψ ⎤ ⎡ r11 r12 r13 = Rz,ψ Rx,θ Rz,ϕ Rx,−θ Rz,−ψ = ⎣ r21 r22 r23 ⎦ r31 r32 r33

(3.107)

where r11 = − (1 − cos ϕ) cos2 ψ sin2 θ cos2 θ + 1

(3.108)

r21 = (1 − cos ϕ) sin2 θ cos ψ sin ψ − cos θ sin ϕ

(3.109)

r31 = ((1 − cos ϕ) cos θ sin ψ + cos ψ sin ϕ) sin θ

(3.110)

r12 = (1 − cos ϕ) sin2 θ cos ψ sin ψ + cos θ sin ϕ

(3.111)

r22 = cos ϕ + sin2 θ cos2 ψ (1 − cos ϕ)

(3.112)

r32 = ((1 − cos ϕ) cos θ cos ψ − sin ψ sin ϕ) sin θ

(3.113)

r13 = ((1 − cos ϕ) cos θ sin ψ − sin ϕ cos ψ) sin θ

(3.114)

r23 = ((1 − cos ϕ) cos θ cos ψ + sin ψ sin ϕ) sin θ

(3.115)

r33 = cos ϕ + (1 − cos ϕ) cos θ

(3.116)

2

The matrix (3.107) is the order-free transformation for turning ϕ about z0 . We can check that the order of rotations ϕ, θ, ψ are not important and we have 2

R0 = Rx1 ,θ Rz2 ,ψ Rz0 ,ϕ = Rz2 ,ψ Rz0 ,ϕ Rx1 ,θ = Rz0 ,ϕ Rx1 ,θ Rz2 ,ψ = Rz0 ,ϕ Rz2 ,ψ Rx1 ,θ = Rz2 ,ψ Rx1 ,θ Rz0 ,ϕ = Rx1 ,θ Rz0 ,ϕ Rz2 ,ψ

(3.117)

We need to substitute the current values of ϕ, θ, and ψ before multiplying any transformation matrices.

Example 72  Order-free Euler angle matrix. Here is to show order-free rotation on Euler angles. The Euler angle transformation with the order ϕ, θ, ψ is B

RG = Rz,ψ Rx,θ Rz,ϕ ⎡ ⎤ cϕcψ − cθsϕsψ cψsϕ + cθ cϕsψ sθsψ = ⎣ −cϕsψ − cθ cψsϕ −sϕsψ + cθ cϕcψ sθ cψ ⎦ sθsϕ −cϕsθ cθ

(3.118)

108

3 Orientation Kinematics

where,

⎡

⎤ cos ϕ sin ϕ 0 Rz,ϕ = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1 ⎡ ⎤ 1 0 0 Rx,θ = ⎣ 0 cos θ sin θ ⎦ 0 − sin θ cos θ ⎡ ⎤ cos ψ sin ψ 0 Rz,ψ = ⎣ − sin ψ cos ψ 0 ⎦ 0 0 1

(3.119)

(3.120)

(3.121)

These are in-order rotation matrices associated with ϕ, θ, ψ. The rotation ϕ transforms the global coordinate frame to B  , rotation θ transforms B  to B  , and rotation ψ transforms B  to B. To make them to be order free, we should define them as a rotation about a local axis of the final coordinate frame. The rotation ψ is already about the local z-axis and is order free. The order-free rotation θ is Rx,θ = Rz,ψ Rx,θ Rz,−ψ ⎡ 2 ⎤ c ψ + cθ s 2 ψ cθ cψsψ − cψsψ sθsψ = ⎣ cθcψsψ − cψsψ cθ c2 ψ + s 2 ψ cψsθ ⎦ −sθsψ −cψsθ cθ

(3.122)

The matrix (3.122) is the order-free transformation for rotation θ about x: B

RG = Rx,θ Rz,ψ = Rz,ψ Rx,θ

(3.123)

where, B

RG = Rx,θ Rz,ψ ⎡ 2 ⎤⎡ ⎤ c ψ + cθ s 2 ψ cψ sψ 0 (cθ − 1) sθsψ = ⎣ (cθ − 1) cψsψ cθ c2 ψ + s 2 ψ cψsθ ⎦ ⎣ −sψ cψ 0 ⎦ −sθsψ −cψsθ cθ 0 0 1 ⎡ ⎤ cos ψ cos θ sin ψ sin θ sin ψ = ⎣ − sin ψ cos θ cos ψ cos ψ sin θ ⎦ 0 − sin θ cos θ

B

RG = Rz,ψ Rx,θ ⎡ ⎤⎡ 2 ⎤ cψ sψ 0 c 0 + cθ s 2 0 (cθ − 1) c0s0 sθs0 = ⎣ −sψ cψ 0 ⎦ ⎣ (cθ − 1) c0s0 cθ c2 0 + s 2 0 c0sθ ⎦ 0 0 1 −sθs0 −c0sθ cθ ⎡ ⎤ cos ψ cos θ sin ψ sin θ sin ψ = ⎣ − sin ψ cos θ cos ψ cos ψ sin θ ⎦ 0 − sin θ cos θ

(3.124)

(3.125)

The order-free transformation matrix for rotation ϕ is Rz,ϕ = Rz,ψ Rx,θ Rz,ϕ Rx,−θ Rz,−ψ ⎡ ⎤ r11 r12 r13 = ⎣ r21 r22 r23 ⎦ r31 r32 r33

(3.126)

3.3  Euler Parameters

109

where, r11 = − (1 − cos ϕ) cos2 ψ sin2 θ cos2 θ + 1

(3.127)

r21 = (1 − cos ϕ) sin2 θ cos ψ sin ψ − cos θ sin ϕ

(3.128)

r31 = ((1 − cos ϕ) cos θ sin ψ + cos ψ sin ϕ) sin θ

(3.129)

r12 = (1 − cos ϕ) sin2 θ cos ψ sin ψ + cos θ sin ϕ

(3.130)

r22 = cos ϕ + sin2 θ cos2 ψ (1 − cos ϕ)

(3.131)

r32 = ((1 − cos ϕ) cos θ cos ψ − sin ψ sin ϕ) sin θ

(3.132)

r13 = ((1 − cos ϕ) cos θ sin ψ − sin ϕ cos ψ) sin θ

(3.133)

r23 = ((1 − cos ϕ) cos θ cos ψ + sin ψ sin ϕ) sin θ

(3.134)

r33 = cos ϕ + (1 − cos ϕ) cos2 θ

(3.135)

The matrix (3.126) is the order-free transformation for rotation ϕ about z. Using the order-free matrices (3.121), (3.122), and (3.126) we find the Euler angle transformation matrix (3.122) by rotations ϕ, θ, ψ in any order: B

RG = Rx,θ Rz,ψ Rz,ϕ = Rz,ψ Rz,ϕ Rx,θ = Rz,ϕ Rx,θ Rz,ψ = Rz,ϕ Rz,ψ Rx,θ = Rz,ψ Rx,θ Rz,ϕ = Rx,θ Rz,ϕ Rz,ψ

(3.136)

Equations (3.127)–(3.135) are the same as (3.108)–(3.116), and it justifies why we call the spherical wrist of Fig. 3.4 an Eulerian wrist.

3.3

 Euler Parameters

Assume φ to be the angle of rotation of a body coordinate frame B(Oxyz) about uˆ = u1 Iˆ + u2 Jˆ + u3 Kˆ relative to a global frame G(OXY Z). The existence of such an axis of rotation is the analytical representation of the Euler’s theorem about rigid body rotation: Euler Rigid Body Rotation Theorem The most general displacement of a rigid body with one fixed point is a rotation about an axis. To find the axis and angle of rotation we introduce the Euler parameters e0 , e1 , e2 , e3 such that e0 is a scalar and e1 , e2 , e3 are components of a vector e, e0 = cos

φ 2

φ e = e1 Iˆ + e2 Jˆ + e3 Kˆ = uˆ sin 2

(3.137) (3.138)

The Euler parameters have a constraint among them. e12 + e22 + e32 + e02 = e02 + eT e = 1

(3.139)

110

3 Orientation Kinematics

Then, the transformation matrix G RB to satisfy the equation G r = G

G

RB B r can be derived based on Euler parameters

  RB = Ru,φ = e02 − e2 I + 2e eT + 2e0 e˜ ˆ ⎡ 2 ⎤ e0 + e12 − e22 − e32 2 (e1 e2 − e0 e3 ) 2 (e0 e2 + e1 e3 ) = ⎣ 2 (e0 e3 + e1 e2 ) e02 − e12 + e22 − e32 2 (e2 e3 − e0 e1 ) ⎦ 2 (e1 e3 − e0 e2 ) 2 (e0 e1 + e2 e3 ) e02 − e12 − e22 + e32

(3.140)

where e˜ is the skew symmetric matrix corresponding to vector e. ⎤ 0 −e3 e2 e˜ = ⎣ e3 0 −e1 ⎦ −e2 e1 0 ⎡

(3.141)

Given a transformation matrix G RB we may obtain Euler parameters:  1  G  tr RB + 1 4  1 G RB − G RBT e˜ = 4e0

e02 =

(3.142) (3.143)

and determine the angle φ and axis of rotation u. ˆ  1  G  tr RB − 1 2  1 G u˜ = RB − G RBT 2 sin φ

cos φ =

(3.144) (3.145)

Euler parameters provide a well-suited, redundant, and non-singular rotation description for arbitrary and large rotations. Assume a rotation transformation matrix G RB is given. ⎡

⎤ r11 r12 r13 G RB = ⎣ r21 r22 r23 ⎦ r31 r32 r33

(3.146)

It is then possible to calculate the Euler parameters e0 , e1 , e2 , e3 and indicate the axis and angle of rotation to derive G RB by utilizing any one of the following four sets of equations: 1

1 + r11 + r22 + r33 2 1 r32 − r23 1 r13 − r31 e1 = e2 = 4 e0 4 e0

e0 = ±

1

1 + r11 − r22 − r33 2 1 r21 + r12 1 r31 + r13 e2 = e3 = 4 e1 4 e1

e3 =

1 r21 − r12 4 e0

(3.147)

e0 =

1 r32 + r23 4 e1

(3.148)

e1 =

1 r21 + r12 4 e2

(3.149)

e1 = ±

1

1 − r11 + r22 − r33 2 1 r32 + r23 1 r13 − r31 e3 = e0 = 4 e2 4 e2 e2 = ±

3.3  Euler Parameters

111

Fig. 3.5 Axis and angle of rotation

1

1 − r11 − r22 + r33 2 1 r21 − r12 1 r31 + r13 e0 = e1 = 4 e3 4 e3 e3 = ±

e2 =

1 r32 + r23 4 e3

(3.150)

Although Eqs. (3.147)–(3.150) present four different sets of solutions, their resulting Euler parameters e0 , e1 , e2 , e3 are identical. Therefore, numerical inaccuracies can be minimized by using the set with maximum divisor. The plus and minus signs indicate that rotation φ about uˆ is equivalent to rotation −φ about −u. ˆ Proof Figure 3.5 depicts a point P of a rigid body with position vector r, and the unit vector uˆ along the axis of rotation ON fixed in the global frame. The point moves to P  with position vector r after an active rotation φ about u. ˆ To obtain the relationship between r and r , we express r by the following vector equation: −−→ −−→ −−→ r = ON + NQ + QP 

(3.151)

From Fig. 3.5 we may describe Eq. (3.151) utilizing r, r , u, ˆ and φ.       r = r · uˆ uˆ + uˆ × r × uˆ cos φ − r×uˆ sin φ         = r · uˆ uˆ + r − r · uˆ uˆ cos φ + uˆ × r sin φ

(3.152)

Rearranging Eq. (3.152) leads to a new form of the Rodriguez rotation formula.     r = r cos φ + (1 − cos φ) uˆ · r uˆ + uˆ × r sin φ

(3.153)

Using the Euler parameters in (3.137) and (3.138), along with the following trigonometric relations: φ −1 2 φ φ sin φ = 2 sin cos 2 2 φ 1 − cos φ = 2 sin2 2 cos φ = 2 cos2

(3.154) (3.155) (3.156)

112

3 Orientation Kinematics

converts the Rodriguez formula (3.153) to a more useful form.   r = r 2e02 − 1 + 2e (e · r) + 2e0 (e × r) Defining new notations for r =

G

r and r = G

B

(3.157)

r, we may rewrite the equation,

   r = e02 − e2 B r + 2e eT

B

   r + 2e0 e˜ B r

(3.158)

that allows us to factor out the position vector B r and extract the Euler parameter transformation matrix G RB G

  RB = e02 − e2 I + 2e eT + 2e0 e˜

(3.159)

where, G

B r =G RB B r = Ru,φ r ˆ

(3.160)

To prove the equations of the angle φ and axis of rotation uˆ for a given transformation matrix RB , let us calculate the trace of G RB and find e0 and φ,   tr G RB = 4e02 − 1 = 2 cos φ + 1 (3.161) G

and calculate G RB −

G

RBT to find e and u. ˆ ⎡

⎤ 0 −4e0 e3 4e0 e2 G RB − G RBT = ⎣ 4e0 e3 0 −4e0 e1 ⎦ −4e0 e2 4e0 e1 0 ⎡ ⎤ 0 −u3 u2 = 2 sin φ ⎣ u3 0 −u1 ⎦ −u2 u1 0 ⎤ r32 − r23 1 ⎣ e˜ = r13 − r31 ⎦ 4e0 r21 − r12 ⎤ ⎡ r − r23 1 ⎣ 32 uˆ = r13 − r31 ⎦ 2 sin φ r21 − r12

(3.162)

⎡

(3.163)

(3.164)

Comparing (3.140) and (3.146) shows that for the first set of Eqs. (3.147), e0 can be found by summing the diagonal elements   r11 , r22 , and r33 to have tr G RB = 4e02 − 1. To find e1 , e2 , and e3 we need to simplify r32 − r23 , r13 − r31 , and r21 − r12 , respectively. The other sets of solutions (3.148)–(3.150) can also be found by comparison.  Example 73  Axis–angle rotation G RB for a φ and u. ˆ Numerical example for Euler parameters based on a given axis–angle of rotations.  √ Euler parameters for rotation φ = 30 deg about uˆ = Iˆ + Jˆ + Kˆ / 3 are π = 0.966 12  φ e = uˆ sin = e1 Iˆ + e2 Jˆ + e3 Kˆ = 0.149 Iˆ + Jˆ + Kˆ 2

e0 = cos

(3.165) (3.166)

therefore, the corresponding transformation matrix G RB is G

  RB = e02 − e2 I + 2e eT + 2e0 e˜ ⎡ ⎤ 0.91 −0.244 0.333 = ⎣ 0.333 0.91 −0.244 ⎦ −0.244 0.333 0.91

(3.167)

3.3  Euler Parameters

113

Example 74  Euler parameters and Euler angles relationship. Comparing the Euler angles rotation matrix (2.172) and the Euler parameter transformation matrix (3.140) we determine the following relationships between Euler angles and Euler parameters: θ ψ +ϕ cos 2 2 θ ψ −ϕ e1 = sin cos 2 2 e0 = cos

θ ψ −ϕ sin 2 2 θ ψ +ϕ e3 = cos sin 2 2 e2 = sin

(3.168) (3.169)

(3.170) (3.171)

or 2 (e2 e3 + e0 e1 ) sin θ   2   −1 θ = cos 2 e0 + e32 − 1

ϕ = cos−1

ψ = cos−1

−2 (e2 e3 − e0 e1 ) sin θ

(3.172) (3.173) (3.174)

Example 75  Rotation matrix for angle of rotation φ = kπ . Euler parameter transformation matrix for ±90 deg rotations. When the angle of rotation is φ = kπ , k = ±1, ±3, ..., then e0 = 0. Therefore, the Euler parameter transformation matrix (3.140) becomes ⎡ 2 1 ⎤ e1 − 2 e1 e2 e1 e3 G RB = 2 ⎣ e1 e2 e22 − 12 e2 e3 ⎦ (3.175) e1 e3 e2 e3 e32 − 12 which is a symmetric matrix and indicates that rotation φ = kπ , and φ = −kπ are equivalent. Example 76  Vector of infinitesimal rotation. For infinitesimal rotations, the axis of rotation uˆ is axis of angular velocity ω. Consider the Rodriguez rotation formula (3.153) for a differential rotation dφ   r = r + uˆ × r dφ

(3.176)

In this case the difference between r and r is also very small, dr = r − r = dφ uˆ × r

(3.177)

and hence, a differential rotation dφ about an axis indicated by the unit vector uˆ is a vector along uˆ with magnitude dφ. Dividing both sides by dt leads to r˙ = φ˙ uˆ × r = ω × r

(3.178)

ω = φ˙ uˆ

(3.179)

which represents the global velocity vector of any point in a rigid body rotating about u. ˆ

114

3 Orientation Kinematics

Example 77  Exponential form of rotation eφ u˜ . Axis–angle of rotation can be expressed by an exponential function equivalently. It is an interesting expression with not much advantages over traditional methods. Consider a point P in the body frame B with a position vector r. If the rigid body has an angular velocity ω, then the velocity of P in the global coordinate frame is P r = ω × r = ωr ˜ (3.180) This is a first-order linear differential equation that may be integrated to have ˜ r(0) r(t) = eωt

(3.181)

˜ where r(0) is the initial position vector of P , and eωt is a matrix exponential.

˜ = I + ωt ˜ + eωt

˜ 2 ˜ 3 (ωt) (ωt) + + ··· 2! 3!

(3.182)

The angular velocity ω has a magnitude ω and direction indicated by a unit vector u. ˆ Therefore, ω = ωuˆ

(3.183)

ω˜ = ωu˜

(3.184)

ωt ˜ = ωt u˜ = φ u˜

(3.185)

and hence ˜ = eφ u˜ eωt



= I+ φ−

φ3 φ5 + − ··· 3! 5!



 u˜ +

φ2 φ4 φ6 − + ··· 2! 4! 6!

(3.186)

u˜ 2

or equivalently eφ u˜ = I + u˜ sin φ + u˜ 2 (1 − cos φ)

(3.187)

It is an alternative form of the Rodriguez formula showing that eφ u˜ is the rotation transformation to map B r = r(0) to G r = r(t). Expanding eφ u˜ gives ⎤ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ ⎡

eφ u˜

(3.188)

which is equal to the axis–angle Eq. (3.4), and therefore, eφ u˜ is equivalent to the rotation matrix G RB . eφ u˜ = Ru,φ = ˆ

G

RB = I cos φ + uˆ uˆ T vers φ + u˜ sin φ

(3.189)

To show that eφ u˜ ∈ S, where S is the set of rotation matrices, S = R ∈ R3×3 : RR T = I, |R| = 1

!

(3.190)

we have to show that R = eφ u˜ has the orthogonality property R T R = I and its determinant is |R| = 1. The orthogonality can be verified by the fact that R T = R −1 and consequently RR T = I.  −1  T T eφ u˜ = e−φ u˜ = eφ u˜ = eφ u˜

(3.191)

From orthogonality, it follows that |R| = ±1, and from continuity of exponential function, it follows that e0 = 1. Therefore, |R| = 1.

3.3  Euler Parameters

115

Fig. 3.6 Illustration of a rotation of a rigid body to derive a new form of the Rodriguez rotation formula in Example 78

Example 78  New form of Rodriguez rotation formula.Rodriguez rotation formula may also be expressed by vectors. Considering Fig. 3.6 we may write φ

−−→

φ

−−→

cos MP  = sin NM 2 2

−−→ −−→ −−→ −−→



NM MP  = MP  uˆ × NM to find

 cos

φ 2



 −−→ φ −−→ MP = sin uˆ × NM. 2

(3.192) (3.193)

(3.194)

Now using the following expressions: −−→ −−→ −→ 2MP  = NP  − NP −−→ −−→ −→ 2NM = NP  + NP −−→ −→ NP − NP = r − r −−→ −→   uˆ × NP  + NP = uˆ × r + r

(3.195) (3.196) (3.197) (3.198)

we can write an alternative form of the Rodriguez rotation formula cos or

   φ φ  r − r = sin uˆ × r + r 2 2

(3.199)

   φ r − r = tan uˆ × r + r 2

(3.200)



Example 79  Rodriguez vector. A combination of axis and angle of rotation may be used to define a vector to represent rotations. Then the rotation vector would be the only operator to apply rotations. The vector w is called the Rodriguez vector associated to a rotation (u, φ). w=

e φ = uˆ tan e0 2

e0 = cos

1 φ 1 =√ =√ 2 1 + wT w 1 + w2

(3.201) (3.202)

116

3 Orientation Kinematics

wi

ei = √

1 + wT w

cos φ =

1 − w2 1 + w2

wi

=√

i = 1, 2, 3

1 + w2

sin φ =

2w 1 + w2

(3.203) (3.204)

The Rodriguez formula (3.140) can be converted to a new form based on Rodriguez vector. G

  RB = Ru,φ = e02 − e2 I + 2e eT + 2e0 e˜ ˆ =

   1 1 − wT w I + 2wwT + 2w˜ 1 + wT w

(3.205)

The combination of two rotations, w and w , is equivalent to a single rotation w. w=

w + w − w × w 1 − w · w

(3.206)

Example 80  Elements of G RB . Employing permutation symbol  ij k and Kronecker delta δ ij , to express elements of rotation matrices. The expression is good for tensor calculus and contracting long equations. Introducing Levi-Civita density or permutation symbol  ij k ,

 ij k

⎧ ⎨ 1 ij k = 123 = 231 = 312 = 0 i = j, j = k, k = i ⎩ −1 ij k = 321 = 213 = 132 =

1 (i − j )(j − k)(k − i) 2

i, j, k = 1, 2, 3

(3.207)

(3.208)

and recalling Kronecker delta (2.294) δ ij = 1 if i = j, and δ ij = 0 if i = j

(3.209)

we can redefine the elements of the Rodriguez rotation matrix. rij = δ ij cos φ + (1 − cos φ) ui uj +  ij k uk sin φ

(3.210)

Example 81  Euler parameters from G RB . Numerical example to determine Euler parameters for a given rotation matrix. A transformation matrix G RB is given. ⎡

⎤ 0.5449 −0.5549 0.6285 G RB = ⎣ 0.3111 0.8299 0.4629 ⎦ −0.7785 −0.0567 0.6249

(3.211)

To calculate the corresponding Euler parameters, we use Eq. (3.147) and find tr

G

 RB = r11 + r22 + r33 = 0.5449 + 0.8299 + 0.6249 = 1.9997

therefore, e0 =

    tr G RB + 1 /4 = 0.866

(3.212)

(3.213)

and e1 = −0.15

e2 = 0.406

e3 = 0.25

(3.214)

3.4  Quaternions

117

Example 82  Euler parameters when we have one of them. Symmetric equations to determine Euler parameters from a rotation matrix. Consider the Euler parameter rotation matrix (3.140) corresponding to rotation φ about an axis indicated by a unit vector u. ˆ The off-diagonal elements of G RB 1 (r32 − r23 ) 4 1 e0 e3 = (r21 − r12 ) 4 1 e1 e3 = (r13 + r31 ) 4 e0 e1 =

1 (r13 − r31 ) 4 1 e1 e2 = (r12 + r21 ) 4 1 e2 e3 = (r23 + r32 ) 4 e0 e2 =

(3.215)

can be utilized to calculate ei , i = 0, 1, 2, 3 if we have one of them. Example 83  Euler parameters by Stanley method. A numerical efficient method to determine Euler parameters for a given rotation matrix. Following an effective method developed by Stanley (1978), we may first find the four ei2   1 1 + tr G RB 2   1 e12 = 1 + 2r11 − tr G RB 4 e02 =

  1 1 + 2r22 − tr G RB 4   1 1 + 2r33 − tr G RB e32 = 4 e22 =

(3.216) (3.217)

(3.218) (3.219)

and take the positive square root of the largest ei2 . Then the other ei are found by dividing the appropriate three of the six equations (3.215) by the largest ei .

3.4

 Quaternions

A global quaternion q is defined as a scalar+vector quantity, where q0 is a scalar and q is a vector. q = q0 + q = q0 + q1 Iˆ + q2 Jˆ + q3 Kˆ

(3.220)

A quaternion can also be shown by a four-element vector ⎡

⎤ q0 ⎢ q1 ⎥ ⎥ q=⎢ ⎣ q2 ⎦ q3

(3.221)

q = q0 + q 1 i + q 2 j + q 3 k

(3.222)

i 2 = j 2 = k 2 = ij k = −1

(3.223)

ij = −j i = k

(3.224)

in a flag form where, i, j, k are flags and defined as follows:

118

3 Orientation Kinematics

j k = −kj = i

(3.225)

ki = −ik = j

(3.226)

Addition and multiplication of two quaternions q and p are quaternions:

q + p = (q0 + q) + (p0 + p) =q0 + q1 i + q2 j + q3 k + p0 + p1 i + p2 j + p3 k = (q0 + p0 ) + (q1 + p1 ) i + (q2 + p2 ) j + (q3 + p3 ) k

(3.227)

qp = (q0 + q) (p0 + p) = q0 p0 + q0 p + p0 q + qp = q0 p0 + q0 p + p0 q + q × p − q · p = (q0 p0 − q1 p1 − q2 p2 − q3 p3 ) + (p0 q1 + p1 q0 − p2 q3 + p3 q2 ) i + (p0 q2 + p1 q3 + p2 q0 − p3 q1 ) j + (p0 q3 − p1 q2 + p2 q1 + p3 q0 ) k

(3.228)

where qp is the quaternion vector product and is equal to the outer vector product minus the inner vector product of q and p. qp = q × p − q · p

(3.229)

Quaternion addition is associative and commutative, q +p = p+q

(3.230)

q + (p + r) = (q + p) + r

(3.231)

but quaternion multiplication is not commutative, qp = pq

(3.232)

However, quaternion multiplication is associative and distributes over addition, (pq) r = p (qr)

(3.233)

(p + q) r = pr + qr

(3.234)

q ∗ = q0 − q = q0 − q1 Iˆ − q2 Jˆ − q3 Kˆ

(3.235)

A quaternion q has a conjugate q ∗ .

Therefore, qq ∗ = (q0 + q) (q0 − q) = q0 q0 + q0 q − q0 q − qq = q0 q0 + q · q − q × q = q02 + q12 + q22 + q32 = |q|2

(3.236)

and hence, we may define the quaternion inverse and quaternion division: |q| =



qq ∗ = q02 + q12 + q22 + q32

(3.237)

3.4  Quaternions

119

1 q∗ = q |q|2

(3.238)

pq −1 =

pq ∗ |q|2

(3.239)

q −1 p =

q ∗p |q|2

(3.240)

q −1 =

−1 ∗ A quaternion  q is a unit quaternion if |q|  = 1.  When a quaternion q is a unit, then we have q = q . Let e φ, uˆ be a unit quaternion, e φ, uˆ = 1, based on Euler parameters, we have

  e φ, uˆ = e0 + e = e0 + e1 Iˆ + e2 Jˆ + e3 Kˆ = cos

(3.241)

φ φ + sin uˆ 2 2

The r = 0 + r is a quaternion corresponding to a vector r. The vector r after a rotation φ about uˆ would be     r = e φ, uˆ r e∗ φ, uˆ

(3.242)

which is equivalent to

    r = e φ, uˆ B r e∗ φ, uˆ (3.243)   φ φ can be defined by a quaternion e φ, uˆ = cos 2 + sin 2 u, ˆ and consequently, two consecutive Therefore, a rotation Ru,φ ˆ rotations R = R2 R1 are defined by       (3.244) e φ, uˆ = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1   A rotation notation is shown by its scalar and vector, equal to the angle and unit vector on axis of rotation, e1 φ 1 , uˆ 1 , e2 φ 2 , uˆ 2 , · · · while e0 , e1 , e2 , e3 are Euler parameters. If we show a quaternion by its equivalent a 4 × 4 matrix, G

⎡

q0 ⎢ q1 ← → q =⎢ ⎣ q2 q3 it provides the important orthogonality property.

−q1 q0 q3 −q2

−q2 −q3 q0 q1

⎤ −q3 q2 ⎥ ⎥ −q1 ⎦ q0

← → → q −1 = ← q T

(3.245)

(3.246)

The matrix quaternion (3.245) can also be represented by " # q0 −qT ← → q = q q0 I3 − q˜ where,

⎤ 0 −q3 q2 q˜ = ⎣ q3 0 −q1 ⎦ −q2 q1 0

(3.247)

⎡

(3.248)

→ Employing the matrix quaternion ← q , we can show quaternion multiplication by matrix multiplication. ⎡

q0 ⎢ q1 → qp = ← q p=⎢ ⎣ q2 q3

−q1 q0 q3 −q2

−q2 −q3 q0 q1

⎤⎡ ⎤ −q3 p0 ⎢ p1 ⎥ q2 ⎥ ⎥⎢ ⎥ −q1 ⎦ ⎣ p2 ⎦ q0 p3

(3.249)

120

3 Orientation Kinematics

The matrix expression of quaternions relates quaternion manipulations and matrix manipulations. If p, q, and v are three quaternions such that qp = v (3.250) then,

← → → → q ← p =← v

(3.251)

Hence, the quaternion representation of transformation between coordinate frames (3.244) can also be defined by matrix multiplication. ← → ←− → ←− → ←−  −−→ ← −−→ ←−  −−→ ←  −−→ G r = e φ, uˆ B r e∗ φ, uˆ = e φ, uˆ B r e φ, uˆ T (3.252) Proof Sir William Rowan Hamilton (1805–1865), an Irish mathematician, suggested we have three different numbers that are all square roots of −1, labeled i, j, k and called quaternion. i 2 = j 2 = k 2 = ij k = −1

(3.253)

Quaternion may be considered as an expansion of complex numbers. Quaternions represent rotations better than Euler angles as they do not have singularity of Euler  angles  for ±π /2.   To prove that a unit quaternion e φ, uˆ can work as a rotation matrix G RB , let us consider a quaternion e φ, uˆ as in Eq. (3.241) and employ the quaternion multiplication (3.228) to calculate ere∗ . re∗ = e0 r + r × e∗ − r · e∗ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ r1 r1 −e1 r1 −e1 = e0 ⎣ r2 ⎦ + ⎣ r2 ⎦ × ⎣ −e2 ⎦ − ⎣ r2 ⎦ · ⎣ −e2 ⎦ r3 r3 −e3 r3 −e3 ⎤ ⎡ e0 r1 + e2 r3 − e3 r2 = (e1 r1 + e2 r2 + e3 r3 ) + ⎣ e0 r2 − e1 r3 + e3 r1 ⎦ e0 r3 + e1 r2 − e2 r1 ⎤ ⎡ ⎤ e1 e0 r1 + e2 r3 − e3 r2 ere∗ = e0 (e1 r1 + e2 r2 + e3 r3 ) − ⎣ e2 ⎦ · ⎣ e0 r2 − e1 r3 + e3 r1 ⎦ e3 e0 r3 + e1 r2 − e2 r1 ⎡ ⎤ ⎡ ⎤ e1 e0 r1 + e2 r3 − e3 r2 +e0 ⎣ e0 r2 − e1 r3 + e3 r1 ⎦ + (e1 r1 + e2 r2 + e3 r3 ) ⎣ e2 ⎦ e0 r3 + e1 r2 − e2 r1 e3 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ e1 e0 r1 + e2 r3 − e3 r2 r1 + ⎣ e2 ⎦ × ⎣ e0 r2 − e1 r3 + e3 r1 ⎦ = G RB ⎣ r2 ⎦ e3 e0 r3 + e1 r2 − e2 r1 r3

(3.254)

⎡

(3.255)

which G RB is equivalent to the Euler parameter transformation matrix (3.140). ⎤ e02 + e12 − e22 − e32 2 (e1 e2 − e0 e3 ) 2 (e0 e2 + e1 e3 ) G RB = ⎣ 2 (e0 e3 + e1 e2 ) e02 − e12 + e22 − e32 2 (e2 e3 − e0 e1 ) ⎦ 2 (e1 e3 − e0 e2 ) 2 (e0 e1 + e2 e3 ) e02 − e12 − e22 + e32 ⎡

(3.256)

     ∗ Using a similar method we can also show that r = e φ, u ˆ r e φ, u ˆ is the inverse transformation of r =   ∗  e φ, uˆ r e φ, uˆ , which is equivalent to     B r = e∗ φ, uˆ G r e φ, uˆ (3.257)

3.4  Quaternions

121

    Now assume e1 φ 1 , uˆ 1 and e2 φ 2 , uˆ 2 are the quaternions corresponding to the rotation matrix Ruˆ 1 ,φ 1 and Ruˆ 2 ,φ 2 , respectively. The first rotation maps B1 r to B2 r, and the second rotation maps B2 r to B3 r. Therefore,   r = e1 φ 1 , uˆ 1   B3 r = e2 φ 2 , uˆ 2 B2

  r e1∗ φ 1 , uˆ 1   B2 r e2∗ φ 2 , uˆ 2 B1

(3.258) (3.259)

which implies B3

    r = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1     = e φ, uˆ B1 r e∗ φ, uˆ

showing that

    r e1∗ φ 1 , uˆ 1 e2∗ φ 2 , uˆ 2

B1

(3.260)

      e φ, uˆ = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1

(3.261)

It is the quaternion equation corresponding to R = R2 R1 . Simplifying Eq. (3.255) recovers Rodriguez formula.     r = e φ, uˆ r e∗ φ, uˆ   = e02 − e · e r + 2e0 (e × r) + 2e (e · r)

(3.262)

Using matrix definition of quaternions, we have ⎡

e0 ←− ⎢ e1  −−→ e φ, uˆ = ⎢ ⎣ e2 e3 ⎡ ← → ⎢ B r =⎢ ⎣

←−  −−→ e φ, uˆ T

Therefore,

−e1 e0 e3 −e2

−e2 −e3 e0 e1

⎤ −e3 e2 ⎥ ⎥ −e1 ⎦ e0

⎤ 0 − B r1 − B r2 − B r3 B r1 0 − B r3 B r2 ⎥ ⎥ B r2 B r3 0 − B r1 ⎦ B r3 − B r2 B r1 0 ⎡ ⎤ e0 e1 e2 e3 ⎢ −e1 e0 e3 −e2 ⎥ ⎥ =⎢ ⎣ −e2 −e3 e0 e1 ⎦ −e3 e2 −e1 e0

⎤ 0 − G r1 − G r2 − G r3 → ←− ←− ⎢ G r1 0 − G r3 G r2 ⎥  −−→ ←  −−→ G ⎥ r = e φ, uˆ B r e φ, uˆ T = ⎢ ⎣ G r2 G r3 0 − G r1 ⎦ G r3 − G r2 G r1 0

(3.263)

(3.264)

(3.265)

⎡

(3.266)

where G

r1 =

B

  r1 e02 + e12 − e22 − e32 + B r2 (2e1 e2 − 2e0 e3 )

+ B r3 (2e0 e2 + 2e1 e3 ) G

r2 =

B

(3.267)

  r1 (2e0 e3 + 2e1 e2 ) + B r2 e02 − e12 + e22 − e32

+ B r3 (2e2 e3 − 2e0 e1 )

(3.268)

122

3 Orientation Kinematics

G

r3 =

r1 (2e1 e3 − 2e0 e2 ) + B r2 (2e0 e1 + 2e2 e3 )   + B r3 e02 − e12 − e22 + e32 B

(3.269) 

Equation (3.266) is compatible with Eqs. (3.252) and (3.244).

Example 84  Composition of rotations using quaternions. How to combine rotations by rotation quaternion and determine the equivalent rotation quaternion. it easier to calculate the composition of rotations. If the quaternion Using quaternions to represent rotations makes  e1 φ 1 , uˆ 1 represents the rotation Ruˆ 1 ,φ 1 and e2 φ 2 , uˆ 2 represents Ruˆ 2 ,φ 2 , then the product       e φ, uˆ = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1

(3.270)

  e φ, uˆ = Ruˆ 2 ,φ 2 Ruˆ 1 ,φ 1 r      = Ruˆ 2 ,φ 2 e1 φ 1 , uˆ 1 r e1∗ φ 1 , uˆ 1         = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1 r e1∗ φ 1 , uˆ 1 e2∗ φ 2 , uˆ 2           = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1 r e1∗ φ 1 , uˆ 1 e2∗ φ 2 , uˆ 2          ∗ = e2 φ 2 , uˆ 2 e1 φ 1 , uˆ 1 r e1 φ 1 , uˆ 1 e2 φ 2 , uˆ 2

(3.271)

represents Ruˆ 2 ,φ 2 Ruˆ 1 ,φ 1 because:

Example 85  Principal global rotation matrices. Rotation quaternion for rotation about global axes. The associated quaternion to the principal global rotation matrices RZ,α , RY,β , and RX,γ are ⎡ ⎤⎞ ⎛ 0  α α e α, Kˆ = ⎝cos , sin ⎣ 0 ⎦⎠ 2 2 1 ⎛ ⎡ ⎤⎞ 0  β β e β, Jˆ = ⎝cos , sin ⎣ 1 ⎦⎠ 2 2 0 ⎛ ⎡ ⎤⎞ 1  γ γ e γ , Iˆ = ⎝cos , sin ⎣ 0 ⎦⎠ 2 2 0

(3.272)

(3.273)

(3.274)

Employing (3.272)–(3.274), we are able to derive the principal transformation matrices. As an example, let us find RZ,α :   RB = e02 − e2 I + 2e eT + 2e0 e˜  α α α α = cos2 − sin2 I + 2Kˆ Kˆ T + 2 cos sin K˜ 2 2 2 2 ⎡ ⎤ cos α − sin α 0 = ⎣ sin α cos α 0 ⎦ 0 0 1

RZ,α =

G

(3.275)

  Example 86  Inner automorphism property of e φ, uˆ . Introducing automorphism property and showing rotation quaternion acts based  on automorphism property. Because e φ, uˆ is a unit quaternion,     e∗ φ, uˆ = e−1 φ, uˆ (3.276)

3.4  Quaternions

123

we may write G

  r = e φ, uˆ

B

  r e−1 φ, uˆ

(3.277)

In abstract algebra, a mapping of the form r = q r q −1 , calculated by multiplying on the left by an element and on the right by its inverse, is called an inner automorphism. Thus, G r is the inner automorphism of B r based on the rotation quaternion  e φ, uˆ . Example 87 Global roll–pitch–yaw quaternions. An example of showing combined rotation matrices by combination of rotation quaternions. The global roll–pitch–yaw rotations about the global coordinate axes X, Y , and Z is

G

RB = RZ,γ RY,β RX,α ⎡ ⎤ cβ cγ −cα sγ + cγ sα sβ sα sγ + cα cγ sβ = ⎣ cβ sγ cα cγ + sα sβ sγ −cγ sα + cα sβ sγ ⎦ −sβ cβ sα cα cβ

(3.278)

   The roll quaternion e α, Iˆ , pitch quaternion e β, Jˆ , and yaw quaternion e γ , Kˆ are given in (3.272)–(3.274).   Multiplying these quaternions creates the global roll–pitch–yaw quaternion e φ, uˆ :      e φ, uˆ = e γ , Kˆ e β, Jˆ e α, Iˆ α β γ α β γ cos cos + sin sin sin 2 2 2 2 2 2 ⎡ γ α α β γ β cos cos sin − cos sin sin ⎢ 2 2 2 2 2 2 ⎢ α γ β β α γ ⎢ + ⎢ cos cos sin + cos sin sin ⎢ 2 2 2 2 2 2 ⎣ β γ γ α β α cos cos sin − cos sin sin 2 2 2 2 2 2

= cos

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(3.279)

As a double check, we must get the same matrix (3.278) by substituting (3.279) in (3.256). Example 88  Polar expression of quaternions. Although we are only interested in rotation quaternions, general quaternions can be expressed in general polar forms. Here is how. Consider a quaternion q in flag and polar forms. q = q0 + q1 i + q2 j + q3 k = r (cos θ + u sin θ )

(3.280) 0 ≤ θ ≤ 2π

q1 i + q 2 j + q 3 k u=± q12 + q22 + q32  r = |q| = q02 + q12 + q22 + q32

cos θ =

q0 r

sin θ =

 ± q12 + q22 + q32 r

(3.281)

(3.282)

(3.283)

(3.284)

124

3 Orientation Kinematics

cot θ = ±

q0 r

tan θ = ±

r q0

(3.285)

For example, can be shown in polar form.

3.5

q =3+i+j +k

(3.286)

 √ π i+j +k π q = 2 3 cos + sin √ 6 6 3

(3.287)

 Spinors and Rotators

Finite rotations can be expressed in two general ways: first, using 3 × 3 real orthogonal matrices R, which is called rotator. Rotator is an abbreviation for rotation tensor; and second, using 3 × 3 real skew symmetric matrices u˜ called spinor. Spinor is an abbreviation for spin tensor. A rotator is a linear operator that maps B r to G r when a rotation axis uˆ and a rotation angle φ are given. G

r=

G

RB

B

r

(3.288)

The spinor u˜ is corresponding to the unit vector u, ˆ which, along with φ, can be utilized to describe a rotation. G

  RB = I cos φ + uˆ uˆ T vers φ + u˜ sin φ

(3.289)

Spinors and rotators are functions of each other so R must be expandable in a Taylor series of u. ˜ R = I + c1 u˜ + c2 u˜ 2 + c3 u˜ 3 + · · ·

(3.290)

However, because of (3.294), all powers of order three or higher may be eliminated. The rotator R is a linear function of I, u, ˜ and u˜ 2 : R = I + a(λu) ˜ + b(λu) ˜ 2 ⎡ ⎤ −bλ2 (u22 + u23 ) + 1 −aλu3 + bλ2 u1 u2 aλu2 + bλ2 u1 u3 = ⎣ aλu3 + bλ2 u1 u2 −bλ2 (u21 + u23 ) + 1 −aλu1 + bλ2 u2 u3 ⎦ −aλu2 + bλ2 u1 u3 aλu1 + bλ2 u2 u3 −bλ2 (u21 + u22 ) + 1 where λ is the spinor normalization factor, and a = a(φ) and b = b(φ) are scalar functions of the rotation angle φ. Table 3.1 collects some representations of rotator R as a function of the coefficients a, b, and the spinor λu. ˜ Table 3.1 Rotator R as a function of spinors a sin φ 2 cos2

b sin2 φ 2

2 cos φ2 1 φ sin φ

λ 1

φ 2

2 cos2

φ 2

2 2 φ2

sin2 φ2

R

tan

φ 2

sin φ

φ 2

I + sin φ u˜ + 2 sin2 φ2 u˜ 2 I + 2 cos2 φ2 [tan φ2 u˜ + tan2 φ2 u˜ 2 ] = [I+ tan φ2 u][I− ˜ tan φ2 u] ˜ −1 φ φ 2 φ 2 I + 2 cos 2 sin 2 u˜ + 2 sin 2 u˜ I + sin φ u˜ + 2 sin2 φ2 u˜ 2

(3.291)

3.5  Spinors and Rotators

125

Proof Square of u, ˜ calculated by direct multiplication, is ⎡

⎤ −u22 − u23 u1 u2 u1 u3 u˜ 2 = ⎣ u1 u2 −u21 − u23 u2 u3 ⎦ u1 u3 u2 u3 −u21 − u22 = −u˜ u˜ T = −u˜ T u˜ = uˆ uˆ T − u2 I

(3.292)

This is a symmetric matrix whose trace is tr[u˜ 2 ] = −2|u| ˆ 2 = −2u2 = −2(u21 + u22 + u23 )

(3.293)

and its eigenvalues are 0, −u2 , −u2 . In other words, u˜ satisfies its own characteristic equation. u˜ 2 = −u2 I , u˜ 3 = −u2 u˜ , · · · , u˜ n = −u2 u˜ n−2 , n ≥ 3

(3.294)

The odd powers of u˜ are skew symmetric with distinct purely imaginary eigenvalues, while even powers of u˜ are symmetric with repeated real eigenvalues. A rotator is a function of a spinor, so R can be expanded in a Taylor series of u. ˜ R = I + c1 u˜ + c2 u˜ 2 + c3 u˜ 3 + · · ·

(3.295)

However, because of (3.294), all powers of order 3 or higher can be eliminated. Hence, R is a quadratic function of u: ˜ R = I + a(λu) ˜ + b(λu) ˜ 2

(3.296)

where the parameter λ is the spinor normalization factor, and a = a(φ, u) and b = b(φ, u) are scalar functions of rotation angle φ and vector u. ˜ Assuming λ = 1, we may find tr R = 1 + 2 cos φ, which, because of (3.291), is equal to tr R = 1 + 2 cos φ = 3 − 2bu2 and therefore, b=

1 − cos φ 2 φ = 2 sin2 u2 u 2

(3.297)

(3.298)

Now the orthogonality condition    I = R T R = I − a u˜ + bu˜ 2 I + a u˜ + bu˜ 2 = I + (2b − a 2 )u˜ 2 + b2 u˜ 4 = I + (2b − a 2 − b2 u2 )u˜ 2 yields, a=

1 2b − b2 u2 = sin φ u

(3.299)

(3.300)

and therefore, R = I+

1 2 φ sin φ u˜ + 2 sin2 u˜ 2 u u 2

= I + sin φ u˜ + ver φ u˜ 2

(3.301)

From a numerical analysis viewpoint, the sine-squared form is preferred to avoid the cancelation in calculating 1 − cos φ for small φ. Replacing a and b in (3.296) provides the explicit rotator in terms of u˜ and φ.

126

3 Orientation Kinematics

  ⎤ u21 + u22 + u23 cφ 2u1 u2s 2 φ2 − u3 sφ 2u1 u3 s 2 φ2 + u2 sφ R = ⎣ 2u1 u2 s 2 φ2 + u3 sφ u22 + u23 + u21 cφ 2u2 u3 s 2 φ2 − u1 sφ ⎦   2u1 u3 s 2 φ2 − u2 sφ 2u2 u3 s 2 φ2 + u1 sφ u23 + u21 + u22 cφ ⎡

This is equivalent to Eq. (3.14). If λ = 1 and λ = 0, then a=

1 sin φ λu

b=

2 φ sin2 (λu)2 2

(3.302)

(3.303) 

which do not affect R. Example 89  Eigenvalues of a spinor. Numerical example for eigenvalues and eigenvectors of a spinor. Consider the axis of rotation indicated by ⎡ ⎤ 6 u = ⎣2⎦ 3

u = |u| = 7

(3.304)

The associated spin matrix u˜ and its square u˜ 2 are ⎡

⎤ 0 −3 2 u˜ = ⎣ 3 0 −6 ⎦ −2 6 0 ⎤ −13 12 18 u˜ 2 = ⎣ 12 −45 6 ⎦ 18 6 −40

(3.305)

⎡

(3.306)

where the eigenvalues of u˜ are (0, 7i, −7i) while those of u˜ 2 are (0, −49, −49).

3.6

 Problems in Representing Rotations

There are a number of different methods to express rotations, however only a few of them are fundamentally distinct. The coordinates required to completely describe the orientation of a rigid body relative to a reference frames are called attitude coordinates. There are two inherent problems in representing rotations, both related to incontrovertible properties of rotations. 1. Rigid body rotations do not commute. 2. Rigid body rotations cannot map smoothly in three dimensional Euclidean space. The non-commutativity of rotations forces us to obey the order of rotations. The lack of a smooth mapping in three dimensional Euclidean space means we cannot smoothly represent every kind of rotation by using only one set of three numbers. Any set of three rotational coordinates contains at least one geometrical orientation where the coordinates are singular. Kinematic singularity is the orientation at which coordinates are undefined or not unique. The problem is similar to defining a coordinate system to locate a point on the Earth’s surface. Using longitude and latitude becomes problematic at the north and south poles, where a small displacement can produce a radical change in longitude. We cannot find a superior system because it is not possible to smoothly wrap a sphere with a plane. Similarly, it is not possible to smoothly wrap the space of spatial rotations with three dimensional Euclidean space. Singularity is the reason why we sometimes describe rotations by using four numbers. We may use only three-number systems and expect to see the resulting singularity, or use four numbers, and cope with the redundancy. The choice depends on the application and method of calculation. For computer applications, the redundancy is not an important problem, so most algorithms use representations with extra numbers. However, engineers prefer to work with the minimum set of numbers. Therefore, there is no unique and superior method for representing rotations.

3.6  Problems in Representing Rotations

3.6.1

127

 Rotation Matrix

The rotation matrix representation based on directional cosines is the most useful representation method of rigid body rotations for many applications. The two reference frames G and B, having a common origin, are defined through orthogonal ˆ and {B} = {ˆı , jˆ, k}. ˆ The rotation or transformation matrix between the right-handed sets of unit vectors {G} = {Iˆ, Jˆ, K} two frames is found by using the orthogonality condition of B and G and describing the unit vectors of one of them in the other: ˆ kˆ Iˆ = (Iˆ · ıˆ)ˆı + (Iˆ · jˆ)jˆ + (Iˆ · k) ˆ kˆ = cos(Iˆ, ıˆ)ˆı + cos(Iˆ, jˆ)jˆ + cos(Iˆ, k)

(3.307)

ˆ kˆ Jˆ = (Jˆ · ıˆ)ˆı + (Jˆ · jˆ)jˆ + (Jˆ · k) ˆ kˆ = cos(Jˆ, ıˆ)ˆı + cos(Jˆ, jˆ)jˆ + cos(Jˆ, k)

(3.308)

ˆ kˆ Kˆ = (Kˆ · ıˆ)ˆı + (Kˆ · jˆ)jˆ + (Kˆ · k) ˆ ıˆ)ˆı + cos(K, ˆ jˆ)jˆ + cos(K, ˆ k) ˆ kˆ = cos(K,

(3.309)

Therefore, having the rotation matrix G RB , ⎡

⎤ Iˆ · ıˆ Iˆ · jˆ Iˆ · kˆ ⎢ ⎥ G RB = ⎣ Jˆ · ıˆ Jˆ · jˆ Jˆ · kˆ ⎦ Kˆ · ıˆ Kˆ · jˆ Kˆ · kˆ ⎤ ⎡ ˆ cos(Iˆ, ıˆ) cos(Iˆ, jˆ) cos(Iˆ, k) ⎢ ˆ ⎥ = ⎣ cos(Jˆ, ıˆ) cos(Jˆ, jˆ) cos(Jˆ, k) ⎦ ˆ ˆ ˆ ˆ cos(K, ıˆ) cos(K, jˆ) cos(K, k)

(3.310)

would be enough to find the coordinates of a point in the reference frame G, when its coordinates are given in the other frame B. G r = G RB B r (3.311) The rotation matrices convert the composition of rotations to matrix multiplication. It is simple and convenient especially when rotations are about the principal axes of global or body coordinate frames. Orthogonality is the most important and useful property of rotation matrices, which shows that the inverse of a rotation matrix is equivalent to its transpose, G RB−1 = G RBT . The null rotation is represented by the identity matrix, I. The primary disadvantage of rotation matrices is that there are so many numbers, which often make rotation matrices hard to interpret. Numerical errors may build up until a normalization is necessary.

3.6.2

 Axis–Angle

Axis–angle representation, described by the Rodriguez formula, is a direct result of the Euler rigid body rotation theorem, explained in Example (98). In axis–angle method a rotation is described by the magnitude of rotation, φ, with the positive right-hand direction about an axis directed by the unit vector, u. ˆ G

RB = Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ

(3.312)

Converting the axis–angle representation to matrix form is done by expanding. ⎡

⎤ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ G RB = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ

(3.313)

128

3 Orientation Kinematics

Converting the matrix representation to axis–angle form is shown in Example 57 using matrix manipulation. However, it is easier if we convert the matrix to a quaternion and then convert the quaternion to axis–angle form. Axis–angle representation of rotation has some problems. First, the rotation axis is indeterminate when φ = 0. Second, the axis–angle representation is a two-to-one mapping system because, = Ru,φ R−u,−φ ˆ ˆ

(3.314)

Ru,φ+2kπ = Ru,φ ˆ ˆ

(3.315)

and it is redundant because for any integer k, we have

However, both of these problems can be improved by restricting φ to some suitable range such as [0, π] or [− π2 , π2 ]. Finally, axis–angle representation is not efficient to find the composition of two rotations and determine the equivalent axis–angle of rotations.

3.6.3

 Euler Angles

Euler angles are also employed to describe the rotation matrix of rigid bodies utilizing only three numbers. G

T  RB = Rz,ψ Rx,θ Rz,ϕ ⎡ ⎤ cϕcψ − cθ sϕsψ −cϕsψ − cθcψsϕ sθsϕ = ⎣ cψsϕ + cθ cϕsψ −sϕsψ + cθ cϕcψ −cϕsθ ⎦ sθsψ sθcψ cθ

(3.316)

Euler angles and rotation matrices are not generally one-to-one, and also, they are not convenient representations of rotations or good enough for constructing composite rotations. The angles ϕ and ψ are not distinguishable when θ → 0. The equivalent rotation matrix is directly obtained by matrix multiplication, while the inverse conversion, from rotation matrix to a set of Euler angles is not straightforward. It is also not applicable when sin θ = 0. Employing (3.316) we can find the Euler angles as follows: ϕ = − arctan

r13 r23

θ = arccos r33 r31 ψ = arctan r32

(3.317) (3.318) (3.319)

It is possible to use a more efficient method that handles all cases uniformly. The main idea is to work with sum and difference of ϕ and ψ σ = ϕ+ψ

(3.320)

υ = ϕ−ψ

(3.321)

and then, σ −υ 2 σ +υ ψ= 2 ϕ=

(3.322) (3.323)

3.6  Problems in Representing Rotations

129

Therefore, r11 + r22 = cos σ (1 + cos θ)

(3.324)

r11 − r22 = cos υ(1 − cos θ)

(3.325)

r21 − r12 = sin σ (1 + cos θ)

(3.326)

r21 + r12 = sin υ(1 − cos θ),

(3.327)

which leads to σ = tan−1 υ = tan−1

r21 − r12 r11 + r22 r21 + r12 r11 − r22

(3.328) (3.329)

This approach resolves the problem at sin θ = 0. At θ = 0, we can find σ , but υ is undetermined, and at θ = π, we can find υ, but σ is undetermined. The undetermined values are consequence of arctan 00 . Besides this singularity, both σ and υ are uniquely determined. The middle rotation angle, θ, can also be found using arctan operator.  θ = arctan

r13 sin ϕ − r23 cos ϕ r33

(3.330)

The main advantage of Euler angles is that they use only three numbers. They are integrable, and they provide a good visualization of spatial rotation with no redundancy. Euler angles are used in dynamic analysis of spinning bodies. The other combinations of Euler angles as well as roll–pitch–yaw angles have the same kind of problems, and similar advantages.

3.6.4

 Quaternion

Quaternion uses four numbers to represent a rotation according to a special rules for addition and multiplication. Rotation quaternion is a unit quaternion that may be described by Euler parameters, or the axis and angle of rotation.   e φ, uˆ = e0 + e = e0 + e1 ıˆ + e2 jˆ + e3 kˆ = cos

φ φ + sin uˆ 2 2

(3.331)

We can also define a 4 × 4 matrix to represent a quaternion ⎡

q0 ⎢ q1 ← → q =⎢ ⎣ q2 q3

−q1 q0 q3 −q2

−q2 −q3 q0 q1

⎤ −q3 q2 ⎥ ⎥ −q1 ⎦ q0

(3.332)

which provides the important orthogonality property. → ← → q −1 = ← q T

(3.333)

→ The matrix quaternion (3.332) can also be represented by 4 × 4 matrix quaternion, ← q , " # q −qT ← → q = 0 q q0 I3 − q˜

(3.334)

130

3 Orientation Kinematics

where,

⎡

⎤ 0 −q3 q2 q˜ = ⎣ q3 0 −q1 ⎦ −q2 q1 0

(3.335)

→ Employing the 4 × 4 matrix quaternion, ← q , we can show quaternion multiplication by matrix multiplication. ⎡

q0 ⎢ q1 → qp = ← q p=⎢ ⎣ q2 q3

−q1 q0 q3 −q2

−q2 −q3 q0 q1

⎤⎡ ⎤ −q3 p0 ⎢ p1 ⎥ q2 ⎥ ⎥⎢ ⎥ −q1 ⎦ ⎣ p2 ⎦ q0 p3

(3.336)

The matrix expression of quaternions ties the quaternion manipulations and matrix manipulations. If p, q, and v are three quaternions and qp = v (3.337) then,

← → → → q ← p =← v

(3.338)

Hence, quaternion representation of transformation between coordinate frames, G

  r = e φ, uˆ

B

  r e∗ φ, uˆ

(3.339)

may also be defined by matrix multiplication. ← → ←− → ←− → ←−  −−→ ← −−→ ←−  −−→ ←  −−→ G r = e φ, uˆ B r e∗ φ, uˆ = e φ, uˆ B r e φ, uˆ T

3.6.5

(3.340)

 Euler Parameters

Euler parameters are the elements of rotation quaternions. Therefore, there is a direct conversion between rotation quaternion   and Euler parameters, which in turn are related to angle-axis parameters. We can obtain the axis and angle of rotation φ, u ˆ ,   from Euler parameters or from rotation quaternion e φ, uˆ . φ = 2 tan−1 uˆ =

e |e|

|e| e0

(3.341) (3.342)

Unit quaternion provides a suitable base for expressing spatial rotations, although it needs normalization due to the error pile-up problem. In general, in most applications quaternions offer superior computational efficiency. Leonhard Euler (1707–1783) was the first to derive the Rodriguez formula, while Benjamin Rodriguez (1795–1851) was the first to discover the Euler parameters. William Hamilton (1805–1865) introduced quaternions, although Friedrich Gauss (1777–1855) discovered them but never published.

Example 90  Taylor expansion of rotation matrix. Time varying rotation matrices can be expanded by Taylor series. Such expansion provides several interesting identities. Assume the rotation matrix R = R(t) is a time dependent transformation between coordinate frames B and G. The body frame B is coincident with G at t = 0. Therefore, R(0) = I, and we may expand the elements of R in a Taylor series expansion 1 1 R(t) = I + R1 t + R2 t 2 + R3 t 3 + · · · (3.343) 2! 3! in which Ri , (i = 1, 2, 3, · · · ) is a constant matrix. The rotation matrix R(t) is orthogonal for all t, hence,

3.6  Problems in Representing Rotations

131

RR T = I  I + R1 t +

1 R2 t 2 + · · · 2!

(3.344)

 I + R1T t +

1 T 2 R t + ··· 2! 2

=I

(3.345)

The coefficient of t i , (i = 1, 2, 3, · · · ) must vanish on the left-hand side. This gives us R1 + R1T = 0 R2 +

(3.347)

R3 + 3R2 R1T + 3R1 R2T + R3T = 0

(3.348)

or in general

+

(3.346)

=0

2R1 R1T

R2T

n   n i=0

i

Rn−i RiT = 0

(3.349)

where, R0 = R0T = I

(3.350)

Equation (3.346) shows that R1 is a skew symmetric matrix, and therefore, R1 R1T = −R12 = C1 is symmetric. Now Eq. (3.347) R2 + R2T = −2R1 R1T = −[R1 R1T + [R1 R1T ]T ] = 2C1 (3.351) leads to R2 = C1 + [C1 − R2T ] R2T

(3.352)

= C1 + [C1 − R2 ] = C1 + [C1 −

R2T ]T

(3.353)

that shows [C1 − R2T ] is skew symmetric because we must have [C1 − R2T ] + [C1 − R2T ]T = 0

(3.354)

Therefore, we have a symmetric matrix product: [C1 − R2T ][C1 − R2T ]T = −[C1 − R2T ]2

(3.355)

The next step shows R3 + R3T = −3[R1 R2T + R2 R1T ] = −3[R1 R2T + [R1 R2T ]T ] = 3[R1 [R12 − R2T ] + [R12 − R2T ]R1 ] = 2C2

(3.356)

that leads to   R3 = C2 + C2 − R3T

(3.357) 

 T T

R3T = C2 + [C2 − R3 ] = C2 + C2 − R3

(3.358)

  that shows C2 − R3T is also skew symmetric because we must have 

  T C2 − R3T + C2 − R3T = 0.

(3.359)

132

3 Orientation Kinematics

Therefore, the matrix product



C2 − R3T



C2 − R3T

T

 2 = − C2 − R3T

(3.360)

is also symmetric. Continuing this procedure shows that the expansion of a rotation matrix R(t) around the unit matrix can be written in the form of  1  C1 + [C1 − R2T ] t 2 2!    1 C2 + C2 − R3T t 3 + · · · + 3!

R(t) = I + C1 t +

(3.361)

T ] are skew symmetric matrices and, where Ci are symmetric and [Ci − Ri+1

Ci =

 1 T Ri−1 + R1−1 2

(3.362)

Therefore, the expansion of an inverse rotation matrix can be written as  1  C1 + [C1 − R2T ] t 2 2!    1 C2 + C2 − R3T t 3 + · · · + 3!

R T (t) = I + C1 t +

3.7

(3.363)

 Composition and Decomposition of Rotations

Determination of a rotation to be equivalent to some given rotations and determination of some rotations to be equivalent to a given rotation matrix are challenging problems when the axes of rotations are not orthogonal. This problem is called the composition and decomposition of rotations.

3.7.1

 Composition of Rotations

Rotation φ 1 about uˆ 1 of a rigid body with a fixed point followed by a rotation φ 2 about uˆ 2 can be composed to a unique rotation φ 3 about uˆ 3 . In other words, when a rigid body rotates from an initial position to a middle position, B2 r = B2 RB1 B1 r, and then rotates to a final position, B3 r = B3 RB2 B2 r, the middle position can be skipped to rotate directly to the final position, B3 r = B3 RB1 B1 r. Proof Let us rewrite the Rodriguez rotation formula (3.200) as 

   r − r = w × r + r

(3.364)

using the Rodriguez vector w. w = tan

φ uˆ 2

(3.365)

Rotation w1 followed by rotation w2 are (r2 − r1 ) = w1 × (r2 + r1 )

(3.366)

(r3 − r2 ) = w2 × (r3 + r2 )

(3.367)

The right-hand side of the first one is perpendicular to w1 and the second one is perpendicular to w2 . Hence, inner product of the first one with w1 and the second one with w2 show that w1 · r2 = w1 · r1

(3.368)

3.7  Composition and Decomposition of Rotations

133

w2 · r3 = w2 · r2

(3.369)

and outer product of the first one with w2 and the second one with w1 show that w2 × (r2 − r1 ) − w1 × (r3 − r2 ) = w1 [w2 · (r2 + r1 )] − (w1 · w2 ) (r2 + r1 ) − w2 [w1 · (r3 + r2 )] + (w1 · w2 ) (r3 + r2 )

(3.370)

Rearranging while using Eqs. (3.368) and (3.369) gives us w2 × (r2 − r1 ) − w1 × (r3 − r2 ) = (w2 × w1 ) × (r1 + r3 ) + (w1 · w2 ) (r3 − r1 )

(3.371)

which can be written as (w1 + w2 ) × r2 = w2 × r1 + w1 × r3 + (w2 × w1 ) × (r1 + r3 ) + (w1 · w2 ) (r3 − r1 )

(3.372)

Adding Eqs. (3.366) and (3.367) to obtain (w1 + w2 ) × r2 yields (w1 + w2 ) × r2 = r3 − r1 − w1 × r1 − w2 × r3

(3.373)

Therefore, we obtain the required Rodriguez rotation formula to rotate r1 to r3 r3 − r1 = w3 × (r3 + r1 )

(3.374)

w1 + w2 + w2 × w1 1 − w1 · w2

(3.375)

where, w3 =

 Example 91 Equivalent rotation to two individual rotations. Vector expression of rotations and combination of rotations. A rigid body B that undergoes two rotations. First rotation is α = 30 deg about u, ˆ 1  uˆ = √ Iˆ + Jˆ + Kˆ 3 followed by a second rotation β = 45 deg about v. ˆ

vˆ = Kˆ

(3.376)

(3.377)

To determine the equivalent single rotation, we define the vectors u and v, ⎡√ ⎤ ⎡ ⎤ 0.1547 √3/3 α π /6 u = tan uˆ = ⎣ √3/3 ⎦ tan = ⎣ 0.1547 ⎦ 2 2 0.1547 3/3 ⎡ ⎤ ⎡ ⎤ 0 0 β π/4 ⎦ v = tan uˆ = ⎣ 0 ⎦ tan =⎣ 0 2 2 1 0.41421 and calculate the w from (3.375).

⎡ ⎤ 0.23376 u+v+v×u ⎣ w= = 9.6827 × 10−2 ⎦ 1−u·v 0.60786

(3.378)

(3.379)

(3.380)

134

3 Orientation Kinematics

The equivalent Rodriguez vector w may be decomposed to determine the axis and angle of rotations wˆ and φ. ⎡

⎤ ⎡ ⎤ 0.23376 0.355 φ w = ⎣ 9.68 × 10−2 ⎦ = 0.658 ⎣ 0.147 ⎦ = tan wˆ 2 0.60786 0.923 φ = 2 arctan 0.658 = 1.1645 rad ≈ 66.72 deg

3.7.2

(3.381) (3.382)

 Decomposition of Rotations

A rotation φ 1 of a rigid body with a fixed point about uˆ 1 can be decomposed into three successive rotations about three G G ˆ and cˆ through unique angles α, β, and γ . Let G Ra,α arbitrary axes a, ˆ b, ˆ , Rb,β ˆ , and Rc,γ ˆ be any three successive rotation ˆ and cˆ through non-vanishing values α, β, and γ . Then, any other matrices about non-coaxis non-coplanar unit vectors a, ˆ b, G G G rotation G Ru,φ ˆ can be expressed in terms of Ra,α ˆ , Rb,β ˆ , and Rc,γ ˆ , if α, β, and γ are properly chosen numbers. G

Ru,φ = ˆ

G

G G Rc,γ Rb,β Ra,α ˆ ˆ ˆ

(3.383)

Proof Using the definition of quaternion rotations, we may write G

r=

G

  B Ru,φ r = e φ, uˆ ˆ

B

  r e∗ φ, uˆ

(3.384)

Let us assume r1 indicate the position vector r before rotation, and r2 , r3 , and r4 indicate the position vector r after rotation Ra,α ˆ , Rb,β ˆ , and Rc,γ ˆ , respectively. Hence, r2 = a r1 a ∗

(3.385)

r3 = b r2 b∗

(3.386)

r4 = c r3 c∗

(3.387)

r4 = e r1 e

∗

(3.388)

      ˆ γ , cˆ , and φ, uˆ are associated rotation quaternions. where, α, aˆ , (β, b),   α α a α, aˆ = a0 + a = cos + sin aˆ 2 2 ˆ = b0 + b = cos β + sin β bˆ b(β, b) 2 2   γ γ c γ , cˆ = c0 + c = cos + sin cˆ 2 2   φ φ e φ, uˆ = e0 + e = cos + sin uˆ 2 2

(3.389) (3.390) (3.391) (3.392)

We define the following scalars to simplify the equations: α = C1 2 α sin = S1 2 b2 c3 − b3 c2 = f1 S2 S3

β γ φ = C2 cos = C3 cos = C 2 2 2 β γ φ sin = S2 sin = S3 sin = S 2 2 2 b3 c1 − b1 c3 b1 c2 − b2 c1 = f2 = f3 S2 S3 S2 S3

c2 a3 − c3 a2 = g1 S3 S1

a3 c1 − a1 c3 = g2 S3 S1

cos

cos

a1 c2 − a2 c1 = g3 S3 S1

(3.393) (3.394) (3.395) (3.396)

3.7  Composition and Decomposition of Rotations a2 b3 − a3 b2 = h1 S1 S2

135 a3 b1 − a1 b3 = h2 S1 S2

a1 b2 − a2 b1 = h3 S1 S2

b · c = n1 S2 S3

(3.398)

c · a = n2 S3 S1

(3.399)

a · b = n3 S1 S2

(3.400)

(a × b) · c = n4 S1 S2 S3

Direct substitution shows that

(3.397)

r4 = er1 e∗ = cbar1 a ∗ b∗ c∗

(3.401)

(3.402)

and therefore, e = cba = c (b0 a0 − b · a + b0 a + a0 b + b × a) = c0 b0 a0 − a0 b · c − b0 c · a − c0 a · b + (a × b) ·c +a0 b0 c + b0 c0 a + c0 a0 b +a0 (b × c) + b0 (c × a) + c0 (b × a) − (a · b) c − (b · c) a + (c · a) b.

(3.403)

Hence, e0 = c0 b0 a0 − a0 n1 S2 S3 − b0 n2 S3 S1 − c0 n3 S1 S2 + n4 S1 S2 S3

(3.404)

and e = a0 b0 c + b0 c0 a + c0 a0 b +a0 (b × c) + b0 (c × a) + c0 (b × a) −n1 S2 S3 a + n2 S3 S1 b−n3 S1 S2 c

(3.405)

which generate four equations C1 C2 C3 − n 1 C1 S 2 S 3 − n 2 S 1 C2 S 3 + n 3 S 1 S 2 C3 − n 4 S 1 S 2 S 3 = C

(3.406)

a1 S1 C2 C3 + b1 C1 S2 C3 + c1 C1 C2 S3 +f1 C1 S2 S3 + g1 S1 C2 S3 + h1 S1 S2 C3 −n1 a1 S1 S2 S3 + n2 b1 S1 S2 S3 −n3 c1 S1 S2 S3 = u1 S

(3.407)

a2 S1 C2 C3 + b2 C1 S2 C3 + c2 C1 C2 S3 +f2 C1 S2 S3 + g2 S1 C2 S3 + h2 S1 S2 C3 −n1 a2 S1 S2 S3 + n2 b2 S1 S2 S3 −n3 c2 S1 S2 S3 = u2 S

(3.408)

a3 S1 C2 C3 + b13 C1 S2 C3 + c3 C1 C2 S3 +f3 C1 S2 S3 + g3 S1 C2 S3 + h3 S1 S2 C3 −n1 a3 S1 S2 S3 + n2 b3 S1 S2 S3 −n3 c3 S1 S2 S3 = u3 S Since e02 + e12 + e22 + e32 = 1, only the first equation and two out of the other three equations, along with

(3.409)

136

3 Orientation Kinematics

C1 =

 1 − S12

C2 =



1 − S22

C3 =

 1 − S32

(3.410) 

must be utilized to determine C1 , C2 , C3 , S1 , S2 , and S3 .

Example 92  Decomposition of a vector in a non-orthogonal coordinate frame. Decomposition of a rotation matrix into rotations about three arbitrary axes is an interesting and complicated rotation theorem. This is an example to show the equation is compatible with rotations about principal axes. Let a, b, and c be any three non-coplanar, non-vanishing vectors. Any other vector r can be expressed in terms of a, b, and c, in the following form, provided u, v, and w are properly chosen numbers: r = ua + vb + wc

(3.411)

ˆ coordinate system is a Cartesian coordinate system, then decomposition reduces to orthogonality If (a, b, c) = (Iˆ, Jˆ, K) condition. ˆ Kˆ r = (r · Iˆ)Iˆ + (r · Jˆ)Jˆ + (r · K) (3.412) To show this, we start with finding the vector inner product of Eq. (3.411) by (b × c), r · (b × c) = ua · (b × c) + vb · (b × c) + wc · (b × c) (3.413) and noting that (b × c) is perpendicular to both b and c, consequently. we have r · (b × c) = ua · (b × c)

(3.414)

Therefore, u= where,

[rbc] [abc]

(3.415)



a1 b1 c1

[abc] = a · b × c = a · (b × c) =

a2 b2 c2

a3 b3 c3

(3.416)

Similarly v and w would be v=

[rca] [abc]

w=

[rab] [abc]

(3.417)

Hence, r= that can also be written as

[rbc] [rca] [rab] a+ b+ c [abc] [abc] [abc]

   b×c c×a a×b r = r· a + r· b + r· c [abc] [abc] [abc]

(3.418)

(3.419)

Multiplying (3.419) by [abc] gives a symmetric equation. [abc] r − [bcr] a + [cra] b − [rab] c = 0

(3.420)

3.8 Summary

137

ˆ which is a mutually orthogonal system of unit vectors, then If the (a, b, c) coordinate system is a Cartesian system (Iˆ, Jˆ, K),   IˆJˆKˆ = 1

Iˆ × Jˆ = Kˆ

Jˆ × Kˆ = Iˆ

Kˆ × Iˆ = Jˆ

(3.421)

and Eq. (3.419) becomes equal to (3.412).    r = r·Iˆ Iˆ + r·Jˆ Jˆ + r·Kˆ Kˆ

3.8

(3.422)

Summary

The objectives of this chapter are 1. To determine the transformation matrix G RB between two Cartesian coordinate frames B and G with a common origin  T when B is turning φrad about a given axis indicated by the unit vector G uˆ = u1 u2 u3 . 2. To determine the axis G uˆ and angle φ of rotation for a given transformation matrix G RB . There are several methods to express rotation of a body frame B with respect to the global frame G. The most applied method is the Rodriguez rotation formula that provides us with a 3 × 3 transformation matrix made of three independent elements. One element to indicate the angle φrad and two elements to indicate the unit vector on the axis of rotation u. ˆ RB = Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ ⎤ ⎡ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ G

φ 2

vers φ = versine φ = 1 − cos φ = 2 sin2

(3.423)

(3.424)

The skew symmetric matrix u˜ is the matrix expression of the vector u. ˆ ⎡

⎤ u1 uˆ = ⎣ u2 ⎦ u3

⎤ 0 −u3 u2 u˜ = ⎣ u3 0 −u1 ⎦ −u2 u1 0 ⎡

(3.425)

On the other hand, we can determine the angle φ and axis of rotation uˆ from a given transformation matrix G RB .  1  G  tr RB − 1 2  1 G u˜ = RB − G RBT 2 sin φ

cos φ =

(3.426) (3.427)

The transformation matrix, angle and axis of rotation, as well as Rodriguez rotation formula can also be defined by Euler parameters e0 , e1 , e2 , e3 . e0 = cos

φ 2

e = e1 Iˆ + e2 Jˆ + e3 Kˆ = uˆ sin e12 + e22 + e32 + e02 = e02 + eT e = 1

(3.428) φ 2

(3.429) (3.430)

138

3 Orientation Kinematics G

  RB = Ru,φ = e02 − e2 I + 2e eT + 2e0 e˜ ˆ (3.431)

  Another method to present a rotation transformation between B and G is the unit quaternions e φ, uˆ .   e φ, uˆ = e0 + e = e0 + e1 Iˆ + e2 Jˆ + e3 Kˆ = cos

φ φ + sin uˆ 2 2

  e∗ φ, uˆ = e0 − e = e0 − e1 Iˆ − e2 Jˆ − e3 Kˆ G

  r = e φ, uˆ

B

  r e∗ φ, uˆ

(3.432) (3.433) (3.434)

3.9 Key Symbols

3.9

139

Key Symbols

A B C c e e0 , e1 , e2 , e3 G i, j, k I = [I ] ıˆ, jˆ, kˆ ı˜, j˜, k˜ Iˆ, Jˆ, Kˆ n l O P p, q, r r rij R R s S t uˆ u˜ v w x, y, z X, Y, Z

Transformation matrix of rotation about a local axis Body coordinate frame, local coordinate frame Constant value, cosine of half angle cos Unit quaternion, rotation quaternion, exponential Euler parameters Global coordinate frame, fixed coordinate frame Flags of a quaternion Identity matrix Local coordinate axes unit vectors Skew symmetric matrices of the unit vector ıˆ, jˆ, kˆ Global coordinate axes unit vectors Eigenvectors of R Length Common origin of B and G A body point, a fixed point in B General quaternions Position vector The element of row i and column j of a matrix Rotation transformation matrix The set of real numbers sin Sine of half angle Time A unit vector on axis of rotation Skew symmetric matrix of the vector uˆ Velocity vector, eigenvectors of R Rodriguez vector Local coordinate axes Global coordinate axes

Greek α, β, γ  ij k δ ij λ φ ϕ, θ, ψ ω ω˜

Rotation angles about global axes Permutation symbol Kronecker’s delta Eigenvalues of R Angle of rotation about uˆ Rotation angles about local axes, Euler angles Angular velocity vector Skew symmetric matrix of the vector ω

Symbol tr vers [ ]−1 [ ]T ← → q

Trace operator 1 − cos Inverse of the matrix [ ] Transpose of the matrix [ ] Matrix form of a quaternion q

140

3 Orientation Kinematics

Exercises 1. Notation and symbols. Describe the meaning of the following notations: a − uˆ

b − vers φ c − u˜

−1 g − B RG h − dφ

m − Jˆ

d − Ru,φ e − e0 ˆ

i − q0 + q j − ij

→ n−← q

p − |q|

o − q˜

k − q∗

f−e   l − e φ, uˆ

q − R3×3

2. Invariant axis of rotation. Show that the axis of rotation uˆ is fixed in B(Oxyz) or G(OXY Z). 3. z-axis–angle rotation matrix. Expand G

RB =

B

−1 RG =

B

T RG = Ru,φ ˆ

 T = Az,−ϕ Ay,−θ Az,φ Ay,θ Az,ϕ = ATz,ϕ ATy,θ ATz,φ ATy,−θ ATz,−ϕ

(3.435)

RB = Ru,φ = ˆ ⎤ ⎡ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ

(3.436)

and verify the axis–angle rotation matrix. G

4. Axis–angle decomposition. A body frame B turns 30 deg about X-axis and then 45 deg about Z-axis. (a) Determine the rotation transformation matrix G RB . (b) Determine the angle and axis of rotation to provide the same G RB . (c) Determine the Euler angles for G RB . (d) Determine the Euler parameters for G RB . 5.  x-axis–angle and y-axis–angle rotation matrix. (a) Find the axis–angle rotation matrix by transforming the x-axis on the axis of rotation u. ˆ (b) Find the axis–angle rotation matrix by transforming the y-axis on the axis of rotation u. ˆ 6. Singular axis–angle rotation. Determine the angle and axis of rotation for the given G RB . ⎡

G

RB = Ru,φ ˆ

⎤ 0 0 1 = ⎣ 0 −1 0 ⎦ 1 0 0

(3.437)

7. Axis–angle rotation and Euler angles.  T (a) Find the Euler angles corresponding to the rotation 45 deg about u = 1 1 1 . (b) Determine the axis and angle of rotation for a combined rotation of 45 deg about x-axis, then 45 deg about y-axis, and then 45 deg about z-axis. (c) Find the Euler angles corresponding to the rotations in section b. (d) Determine the required angles to turn about x, then y, and then z axes to have the same final orientation as section a.

Exercises

141

Fig. 3.7 A cube with edge length l = 1 at original configuration

8. Euler angles between two local frames. The Euler angles between the coordinate frame B1 and G are 20 deg, 35 deg, and −40 deg. The Euler angles between the coordinate frame B2 and G are 60 deg, −30 deg, and −10 deg. Find the angle and axis of rotation that transform B2 to B1 . 9. Global rotation of a cube. Figure 3.7 illustrates the original position of a cube with a fixed point at A and edges of length l = 1. (a) Turn the cube 45 deg about AC and determine the global coordinates of the corners. (b) Turn the cube 45 deg about AH and determine the global coordinates of the corners. (c) Turn the cube 45 deg about AG and determine the global coordinates of the corners. 10. Axis–angle of a series of rotation. The cube in Fig. 3.7 with a fixed point at A has an edge length of l = 1. (a) Determine the angle and axis of rotation when we turn the cube 45 deg about x-axis followed by a rotation of 45 deg about y-axis. (b) Determine the angle and axis of rotation when we turn the cube 45 deg about x-axis followed by a rotation of 45 deg about AH . 11. Global decomposition of the rotation of a cube. The cube in Fig. 3.7 with a fixed point at A has an edge length of l = 1. (a) Decompose a rotation of 45 deg about AG into a rotation about X-axis, then Y -axis, and then Z-axis. (b) Decompose a rotation of 45 deg about AG into a rotation about Y -axis, then Z-axis, and then X-axis. (c) Decompose a rotation of 45 deg about AG into a rotation about Z-axis, then Y -axis, and then X-axis. (d)  Decompose a rotation of 45 deg about AG into a rotation about AC and then AH . 12. −Z volume in a rotation of a cube. The cube in Fig. 3.7 with a fixed point at A has an edge length of l = 1. (a) How much should be the angle of rotation φ for the axis AG to move AE on X-axis? (b)  Calculate the volume of the cube that gets a negative Z coordinate during the rotation φ. 13. Rotation of a cube about a fixed and a body axis. The cube in Fig. 3.7 with a fixed point at A has an edge length of l = 1. (a) Turn the cube 45 deg about AC and then 45 deg about AH . Determine the global coordinates of the corners after the rotations. (b) Turn the cube 45 deg about AC and then 45 deg about AG. Determine the global coordinates of the corners after the rotations. (c)  Turn the cube 45 deg about AC, then 45 deg about AG, and then 45 deg about AH . Determine the global coordinates of the corners after the rotations. 14.  Angular velocity vector.    Use the definition G RB = rij and G R˙ B = r˙ij , and find the angular velocity vector ω, where ω˜ = G R˙ B G RBT .

142

3 Orientation Kinematics

15. Sum of two orthogonal matrices. Show that the sum of two orthogonal matrices is not, in general, an orthogonal matrix, but their product is. 16. Characteristic equation of G RB . Expand the following determinant to derive

G

RB − λI = 0

(3.438)

G

to derive the characteristic equation of RB . Prove r11 = r22 r33 − r23 r32

(3.439)

r22 = r33 r11 − r31 r13

(3.440)

r33 = r11 r22 − r12 r21

(3.441)

− λ3 + tr(G RB )λ2 − tr(G RB )λ + 1 = 0

(3.442)

    (λ − 1) λ2 − λ tr(G RB ) − 1 + 1 = 0

(3.443)

to reduce the characteristic equation to

and

17. Equivalent vector cross product. T T   Show that if a = a1 a2 a3 and b = b1 b2 b3 are two arbitrary vectors, and a˜ is the skew symmetric matrix corresponding to a, ⎡ ⎤ 0 −a3 a2 a ˜= ⎣ a3 0 −a1 ⎦ (3.444) −a2 a1 0 then a˜ b = a × b

(3.445)

18.  Combined angle-axis rotations. The rotation φ 1 about uˆ 1 followed by rotation φ 2 about uˆ 2 is equivalent to a rotation φ about u. ˆ Find the angle φ and axis uˆ in terms of φ 1 , uˆ 1 , φ 2 , and uˆ 2 . 19.  Rodriguez vector. Using the Rodriguez rotation formula show that r − r = tan

  φ uˆ × r + r 2

(3.446)

20.  Equivalent Rodriguez rotation matrices. Show that the Rodriguez rotation matrix G

RB = I cos φ + uˆ uˆ T vers φ + u˜ sin φ

(3.447)

RB = I + (sin φ) u˜ + (vers φ) u˜ 2

(3.448)

can also be written as G

21.  Rotation matrix and Rodriguez formula. Knowing the alternative definition of the Rodriguez formula G

RB = I + (sin φ) u˜ + (vers φ) u˜ 2

(3.449)

and u˜ 2n−1 = (−1)n−1 u˜ u˜ 2n = (−1)n−1 u˜ 2

(3.450) (3.451)

Exercises

143

examine the following equation: G

RBT G RB =

G

RB G RBT

(3.452)

22.  Rodriguez formula application. Use the alternative definition of the Rodriguez formula G

RB = I + (sin φ) u˜ + (vers φ) u˜ 2

and find the global position of a body point at B

 T r= 134

(3.453)

(3.454)

after a rotation of 45 deg about the axis indicated by uˆ =



√1 √1 √1 3 3 3

T (3.455)

23. Associative property of rotation transformation matrices multiplication. Using Rodriguez rotation formula, prove that multiplication of rotation transformation matrices is associative. R3 R2 R1 = R3 [R2 R1 ] = [R3 R2 ] R1

(3.456)

24. Axis and angle of rotation. Find the axis and angle of rotation for the following transformation matrix: ⎡ ⎢ R=⎣

√ 1 3 6 4 4 √ √4 − 6 2 6 4 4 4 √ 1 − 6 3 4 4 4

⎤ ⎥ ⎦

(3.457)

25.  Axis of rotation multiplication. Show that and

u˜ 2k+1 = (−1)k u˜

(3.458)

  u˜ 2k = (−1)k I − uˆ uˆ T

(3.459)

26.  Stanley method. Find the Euler parameters of the following rotation matrix based on the Stanley method: ⎡

⎤ 0.5449 −0.5549 0.6285 G RB = ⎣ 0.3111 0.8299 0.4629 ⎦ −0.7785 −0.0567 0.6249 27.  Stanley method. The cube in Fig. 3.7 with a fixed point at A has an edge length of l = 1. (a) Determine the matrix G RB for a rotation of 45 deg about AG. (b) Determine the Euler angles for G RB of section a. (c) Determine the Euler parameters for G RB of section a. (d) Determine the Euler parameters from G RB of section a using Stanley method. 28.  Rotation matrices identity. Show that if A, B, and C are three rotation matrices then, (a) (AB) C = A (BC) = ABC

(3.460)

(3.461)

(b) (A + B)T = AT + B T

(3.462)

144

3 Orientation Kinematics

(c) (AB)T = B T AT (d)



A−1

T

 −1 = AT

(3.463) (3.464)

29.  Angle-axis of rotation and Euler angles. Compare the Euler angles rotation matrix with the angle-axis rotation matrix and find the angle and axis of rotation based on Euler angles. What are the axis and angle of rotation in terms of Euler angles, and what are the Euler angles for a given angle and axis of rotation? 30.  Repeating global-local rotations. Rotate B rP = [6, 2, −3]T , 60 deg about the Z-axis, followed by 30 deg about the x-axis. Then repeat the sequence of rotations for 60 deg about the Z-axis, followed by 30 deg about the x-axis. After how many rotations will point P be back to its initial global position? 31.  Repeating global-local rotations. How many rotations of α = π /m deg about the X-axis, followed by β = π /k deg about the z axis are needed to bring a body point to its initial global position, if m, k ∈ N? 32.  Small rotation angles. Show that for very small angles of rotation ϕ, θ, and ψ about the axes of the body coordinate frame, the first and third rotations are indistinguishable when they are about the same axis. 33.  Inner automorphism property of a˜ . If R is a rotation matrix and a is a vector, show that ' R a˜ R T = Ra

(3.465)

34.  Angle-derivative of principal rotation matrices. Show that dRZ,α ˜ Z,α = KR dα dRY,β = J˜RY,β dβ dRX,γ = I˜RX,γ dγ

(3.466) (3.467) (3.468)

35.  Euler angles, Euler parameters. Compare the Euler angles rotation matrix and Euler parameter transformation matrix to verify the following relationships between Euler angles and Euler parameters: θ ψ +ϕ cos 2 2 θ ψ −ϕ = sin cos 2 2 θ ψ −ϕ = sin sin 2 2 θ ψ +ϕ = cos sin 2 2 2 (e2 e3 + e0 e1 ) = cos−1 sin θ   2   −1 = cos 2 e0 + e32 − 1

e0 = cos

(3.469)

e1

(3.470)

e2 e3 ϕ θ

ψ = cos−1

−2 (e2 e3 − e0 e1 ) sin θ

(3.471) (3.472) (3.473) (3.474) (3.475)

Exercises

145

36.  Quaternion definition.   Find the unit quaternion e φ, uˆ associated to √ √ T  √ uˆ = 1/ 3 1/ 3 1/ 3

φ=

π 3

(3.476)

    and find the result of e φ, uˆ ıˆ e∗ φ, uˆ . 37.  Quaternion product. (a) Calculate pq, qp, p∗ q, qp∗ , p∗ p, qq ∗ , p∗ q ∗ , and p∗ rq ∗ if: p = 3 + i − 2j + 2k

(3.477)

q = 2 − i + 2j + 4k

(3.478)

r = −1 + i + j − 3k

(3.479)

(b) The dot product of two quaternions q and p is defined as p · q = p0 q0 + p · q

(3.480)

If p · q = 0, then q and p are orthogonal quaternions. Calculate, p · q, q · p, r · q. (c) Prove these identities. p·q =

 1 ∗ p q + q ∗p 2

|q|2 = q02 + q · q

(3.481) (3.482)

38.  Quaternion inverse. −1 −1 (a) Calculate q −1 , p−1 , p−1 q −1 , q −1 p∗ , p∗ p−1 , q −1 q ∗ , and p∗ q ∗ if: p = 3 + i − 2j + 2k

(3.483)

q = 2 − i + 2j + 4k

(3.484)

(b) Compute the right and left quotients for the given quaternions p and q. pq −1 =

pq ∗ |q|2

q −1 p =

q ∗p |q|2

(3.485)

39.  Quaternion and angle-axis rotation. (a) Find the unit quaternion associated to p and q, and find the angle and axis of rotation for each of the unit quaternions. p = 3 + i − 2j + 2k

(3.486)

q = 2 − i + 2j + 4k

(3.487)

(b) Use the unit quaternion p p= and find the global position of B

1+i−j +k 2

 T r = 2 −2 6

(3.488)

(3.489)

146

3 Orientation Kinematics

40.  Quaternion matrix. Use the unit quaternion matrices associated to p = 3 + i − 2j + 2k

(3.490)

q = 2 − i + 2j + 4k

(3.491)

r = −1 + i + j − 3k

(3.492)

← → ← →→ ← ← → ← →← → ← → →← → ← →→ ← → → → → → q , → q p∗ , p∗ ← p , → q q ∗ , p∗ q ∗ , and p∗ ← r q∗ . and find ← p ← r ← q ,← q ← p , p∗ ← 41.  Quaternion properties. Show the following properties. q and p are quaternions q = q0 + q 1 i + q 2 j + q 3 k p = p0 + p1 i + p2 j + p3 k and a ∈ R is a real number, and n ∈ N is a natural number. (p ± q)∗ = p∗ ± q ∗  ∗ ∗ q =q ∗

∗ ∗

(3.493) (3.494)

(pq) = q p

(3.495)

(aq)∗ = aq ∗

(3.496)

  |q + p|2 + |q − p|2 = 2 |q|2 + |p|2

(|q| + |p|)2 = |q|2 + 2 qp∗ + |p|2

(3.497)

q 2n = (−1)n |q|2n

(3.499)

∞  qn n=0

n!

= cos |q| +

q sin |q| |q|

(3.498)

(3.500)

42.  Quaternion equations. Find the quaternion = q0 + q1 i + q2 j + q3 k in the following equations: q 2 = −j  ∗ 2 q = i−j 1 q − q∗ + k = 0 4 1 ∗ qq − ij k + q 2 = 0 2  2 q q − q∗ = i + j + k 43.  Euler angles and quaternion. Find quaternion components in terms of Euler angles, and Euler angles in terms of quaternion components.

(3.501) (3.502) (3.503) (3.504) (3.505)

Exercises

147

44.  bac-cab rule application. (a) Use the Levi-Civita density  ij k to prove the bac-cab rule.

(b) Use the bac-cab rule to show that

a × (b × c) = b (a · c) − c (a · b)

(3.506)

    a = nˆ a · nˆ + nˆ × a × nˆ

(3.507)

where nˆ is any unit vector. What is the geometric significance of this equation? 45.  Two rotations are not enough. Show that, in general, it is impossible to move a point P (X, Y, Z) from the initial position P (Xi , Yi , Zi ) to the final position P (Xf , Yf , Zf ) only by two rotations about the global axes. 46.  Three rotations are enough. Show that, in general, it is possible to move a point P (X, Y, Z) from the initial position P (Xi , Yi , Zi ) to the final position P (Xf , Yf , Zf ) by three rotations about different global axes. 47.  Closure property. Show the closure property of transformation matrices. If q1 , q2 ∈ S, then q1 ⊗ q2 ∈ S

(3.508)

A set S together with a binary operation ⊗ defined on elements of S is called a group (S, ⊗). Closure property is one of the necessary axioms to have a group. 48.  Skew symmetric matrices. T T   Use a = a1 a2 a3 and b = b1 b2 b3 to show that (a) ˜ a˜ b = −ba (3.509) (b)

(c)



a + b = a˜ + b˜

(3.510)

a˜(b = baT − abT

(3.511)

aT a˜ T = −aT a˜ = 0

(3.512)

a˜ b˜ = baT − aT b I

(3.513)



49.  Skew symmetric matrix multiplication. Verify that (a) (b) 50.  Skew symmetric matrix derivative. Show that

·

a˜ = ( a˙

(3.514)   51.  Time derivative of A = a, a˜ .   Assume that a is a time dependent vector and A = a, a˜ is a 3 × 4 matrix. What is the time derivative of C = AAT ? 52.  Rotation for a rotated position. Consider a rigid body B at a position that is not coincident with the global coordinate frame G. The directional cosines of ıˆ, jˆ are ⎡ ⎤ ⎡ ⎤ 0.25 0.5 ıˆ = ⎣ 0.25 ⎦ jˆ = ⎣ cos(jˆ, Jˆ) ⎦ (3.515) ˆ ˆ cos(ˆı , K) cos(jˆ, K) Calculate the missing terms, and determine the global coordinates of a body point P at B rP

148

3 Orientation Kinematics B

after rotation of 45 deg about u.

 T rP = 1 2 3

(3.516)

 T u= 111

(3.517)

53.  Rotation from a rotated position. (a) Calculate the transformation matrix for rotation 30 deg about z-axis, followed by a rotation 45 deg about x-axis. (b) Assume the body B is given to us after the first rotation of 30 deg about z-axis in section a. Determine the axis uˆ that at this time coincides with x-axis and turns the body 45 deg about u. ˆ (c) Calculate the transformation matrix for rotation 30 deg about z-axis, followed by a rotation 45 deg about x-axis, and then, 60 deg about z-axis. (d) Assume the body B is given to us after the second rotation in section b. Determine the axis uˆ that at this time coincides with z-axis and turns the body 60 deg about u. ˆ 54.  Global roll–pitch–yaw quaternions. Derive matrix (3.278) by substituting (3.279) in (3.256). 55.  Project: Axis–angle rotations for body axes rotations. Consider a body frame B coincident with a global frame G. (a) The frame B undergoes a rotation of 45 deg about the X-axis followed by a rotation of 90 deg about the Z-axis. Determine the axis uˆ 1 and angle φ 1 to move B to the final orientation only by one rotation. (b) The frame B undergoes a rotation of 90 deg about the Z-axis followed by a rotation of 45 deg about the X-axis. Determine the axis uˆ 2 and angle φ 2 to move B to the final orientation only by one rotation. (c) The frame B undergoes a rotation of φ 1 about uˆ 1 followed by a rotation φ 2 about uˆ 2 . Turn B to its coincident position with G by turning it φ 3 about uˆ 3 . Determine φ 3 about uˆ 3 . 56.  Project: Axis–angle and incremental body axes rotations. Consider a body frame B coincident with a global frame G. (a) The frame B undergoes a rotation of 45 deg about the X-axis followed by a rotation of 90 deg about the Z-axis. Determine the axis uˆ and angle φ to move B to the final orientation only by one rotation. (b) The frame B undergoes a rotation of 45/2 deg about the X-axis followed by a rotation of 90/2 deg about the Z-axis and then a rotation of 45/2 deg about the X-axis followed by a rotation of 90/2 deg about the Z-axis. Determine the axis uˆ and angle φ to move B to the final orientation only by one rotation. (c) The frame B undergoes a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis, then a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis, and then a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis. Determine the axis uˆ and angle φ to move B to the final orientation only by one rotation. (d) Keep doing this process of turning frame B a rotation of 45/n deg about the X-axis followed by a rotation of 90/n deg about the Z-axis, and repeat the rotations n times, n = 1, 2, 3, · · · , 45. Determine the axis uˆ n and angle φ n to move B to the final orientation only by one rotation for every n. How uˆ n and angle φ n are related to each other? Plot φ n versus n. Plot directional cosines of uˆ n versus n.

4

Motion Kinematics

The most general motion of a rigid body B in a global frame G is made by a rotation φ about an axis u, ˆ plus a displacement d. Kinematically, the rigid body motion may be expressed by a 3 × 3 rotation matrix plus a 3 × 1 displacement vector. It may also be expressed by a 4 × 4 homogenous transformation matrix. This chapter will review all methods to express rigid body motions. Every rigid body represents a link of a robot.

Fig. 4.1 Rotation and translation of a local frame with respect to a global frame

4.1

Rigid Body Motion

Consider a rigid body with an attached body coordinate frame B (oxyz) moving in a fixed global coordinate frame G(OXY Z). The rigid body can rotate in the global frame, while the origin point o of the body frame B can translate relative to the origin O of G as shown in Fig. 4.1. If the displacement vector G d indicates the position of the moving origin o relative to the fixed origin O, then the coordinates of a body point P in local and global frames are related by the following equation: G

rP =

G

RB B rP +

G

(4.1)

d

where ⎡

⎤ XP G rP = ⎣ YP ⎦ ZP

⎡

⎤ xP B rP = ⎣ yP ⎦ zP

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_4

⎡

⎤ Xo G d = ⎣ Yo ⎦ Zo

(4.2)

149

150

4 Motion Kinematics

The vector G d is called the displacement or translation of B with respect to G, and G RB is the rotation matrix to transform B r to G r when G d = 0. Such a combination of a translation and a rotation in Eq. (4.1) is called rigid motion. Decomposition of a rigid motion into a rotation and a translation is the simplest method for representing spatial motion. We show the translation by a vector, and the rotation by a coordinate transformation matrix. Proof Figure 4.1 illustrates a rotated and translated body frame B in the global frame G. The most general rotation is represented by the Rodriguez rotation formula (3.153), which needs an axis of rotation uˆ and an angle of rotation φ. The translation G d displaces the whole rigid body parallel to G d such that all points of the body have the same displacement. Hence, the most general displacement of a rigid body is expressed by the following equation and has two independent parts: a rotation and a translation. 







G r = B r cos φ + (1 − cos φ) uˆ · B r uˆ + uˆ × B r sin φ + G d

= G RB B r + G d

(4.3)

Equation (4.3) shows that the most general displacement of a rigid body is a rotation about an axis and a translation along an axis. The choice of the point of reference o is arbitrary, but when this point is chosen and the body coordinate frame is set up, the rotation and translation are uniquely determined. Based on translation and rotation, the position of a body can be uniquely determined by six independent parameters: three translation components Xo , Yo , Zo ; and three rotational components. The three rotational components can be any two coordinates of the unit vector uˆ and the angle of rotation φ. If a body moves in such a way that its rotational components remain constant, the motion is a pure translation; and if it moves in such a way that Xo , Yo , Zo remain constant, the motion is a pure rotation. Therefore, a rigid body has three translational and three rotational degrees of freedom.  Example 93 Translation and rotation of a body coordinate frame. Numerical example to apply rigid body motion. A body coordinate frame B(oxyz), that is originally coincident with global coordinate frame G(OXY Z), rotates 45 deg  T  T about the X-axis and translates to 3 5 7 . Find, the global position of a point at B r = x y z . G

r=

G

⎡

RB B r +

G

d

⎤

⎡ ⎤ ⎡ ⎤ 3 π⎥ x π ⎢ 0 cos − sin ⎥ ⎣ y ⎦ + ⎣ 5 ⎦ =⎢ ⎣ π4 π4 ⎦ z 7 0 sin cos 4 4 ⎡ ⎤ x +√ 3 √ = ⎣ 12 √2y − 12 √2z + 5 ⎦ 1 2y + 12 2z + 7 2 1

0

0

√

 √ 2 2 y− z + 5 Jˆ = (x + 3) Iˆ + 2 2 √  √ 2 2 + y+ z + 7 Kˆ 2 2

(4.4)

Example 94 Moving body coordinate frame. A numerical example of rotation and translation of a rigid body. Figure 4.2 illustrates a point P at B rP = 0.1ˆı + 0.3jˆ + 0.3kˆ in a body frame B, which is rotated 50 deg about the Z-axis and translated −1 along X, 0.5 along Y , and 0.2 along the Z axes. Find the position G rP of P in global coordinate frame.

4.1 Rigid Body Motion

151

Fig. 4.2 A translating and rotating body B in a global coordinate frame G

G

rP =

G

⎡

RB B rP +

G

d

⎤ 50π 50π ⎡ ⎤ ⎡ ⎤ −1 ⎢ cos 180 − sin 180 0 ⎥ 0.1 ⎢ ⎥ ⎣ 0.3 ⎦ + ⎣ 0.5 ⎦ 50π 50π =⎢ cos 0⎥ ⎣ sin ⎦ 180 180 0.3 0.2 0 0 1 ⎡ ⎤ −1.166 = ⎣ 0.769 ⎦ 0.5

(4.5)

Example 95 Rotation of a translated rigid body. Numerical example to apply rigid body motion. Point P of a rigid body B has an initial position vector B rP . B

 T rP = 1 2 3

(4.6)

 T If the body rotates 45 deg about the x-axis and then translates to G d = 4 5 6 , find the final global position of P . G

r=

B

⎡

T B Rx,π /4 rP +

G

d ⎤

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 0 0 4 5.0 π π⎥ 1 ⎢ 0 cos − sin ⎥ ⎣ 2 ⎦ + ⎣ 5 ⎦ = ⎣ 4.29 ⎦ =⎢ ⎣ π4 π4 ⎦ 3 6 9.53 0 sin cos 4 4

(4.7)

The rotation occurs first when G d = 0 and then translation happens. Example 96 Arm rotation plus elongation. Numerical example to apply rigid body motion illustrated by a robotic arm. Position vector of point P1 at the tip of a P R arm shown in Fig. 4.3a is at: G

rP1 =

B

 T rP1 = 1350 0 900 mm

(4.8)

The arm rotates 60 deg about the global Z-axis and elongates by d = 720.2ˆı mm. The final configuration of the arm is shown in Fig. 4.3b. The new position vector of P is G rP2 = G RB B rP1 + G d (4.9)

152

4 Motion Kinematics

Fig. 4.3 A polar RP arm

where G RB = RZ,60 is the rotation matrix to transform rP2 to rP1 when G d = 0, ⎡

π ⎤ π − sin 0 ⎢ π3 π3 ⎥ G ⎥ RB = ⎢ cos 0⎦ ⎣ sin 3 3 0 0 1 cos

(4.10)

and G d is the translation vector of o with respect to O in the global frame. The translation vector in the body coordinate  T frame is B d = 720.2 0 0 so G d would be found by a transformation. G

d=

RB B d ⎡ π π ⎤ cos − sin 0 ⎡ 720.2 ⎤ ⎡ 360.10 ⎤ ⎢ π3 ⎥ π3 ⎥ =⎢ cos 0 ⎦ ⎣ 0.0 ⎦ = ⎣ 623.71 ⎦ ⎣ sin 3 3 0.0 0.0 0 0 1 G

(4.11)

Therefore, the final global position of the tip of the arm is at: rP2 = G RB B rP1 + G d ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤ c60 −s60 0 1350 360.1 1035.1 = ⎣ s60 c60 0 ⎦ ⎣ 0 ⎦ + ⎣ 623.7 ⎦ = ⎣ 1792.8 ⎦ 0 0 1 900 0.0 900.0 G

(4.12)

Example 97 Composition of transformations. How to combine two rigid body motions and determine an equivalent rigid body motion. Assume 2 r indicates the rigid motion of body B1 with respect to body B2 , and G r indicates the rigid motion of body B2 with respect to frame G. 2 G

r = 2 R1 1 r + 2 d 1

(4.13)

r=

(4.14)

G

R2 2 r +

G

d2

Composition of these two rigid motions can be defined by a third rigid motion, which will be calculated by substituting the expression for 2 r into the equation for G r.

4.2 Homogenous Transformation

153

Fig. 4.4 Representation of a point P in coordinate frames B and G

G

2

 R1 1 r + 2 d 1 +

r=

G

R2

=

G

R2 2 R1 1 r+ G R2 2 d1 +

=

G

R1 r + 1

G

G

d2

G

d2 (4.15)

d1

Therefore, G

R1 =

G

R2 2 R1

d1 =

G

R2 d 1 +

G

2

(4.16) G

d2

(4.17)

which shows that the transformation from frame B1 to frame G can be done by a rotation G R1 and a translation G d1 . Example 98  Euler rigid body theorem. Expression and proof of Euler rigid body motion theorem. Rigid body motion is well expressed by Leonhard Euler (1707-1783) theorem.

4.2

Homogenous Transformation

A rigid body with coordinate frame B is moving in a globally fixed coordinate frame G. The position vector of an arbitrary point P of the rigid body is denoted by B rP and G rP in the frames, as is shown in Fig. 4.4. The translation vector G d indicates the position of origin o of the body frame B in the global frame G. The general motion of a rigid body B (oxyz) in the global frame G (OXY Z) is a combination of rotation G RB and translation G d. G

r=

G

RB B r +

G

d

(4.18)

Combining a rotation matrix plus a vector can be expressed better by homogenous transformation matrix. Introducing a 4 × 4 homogenous transformation matrix G TB helps us show a rigid motion by a single matrix transformation: G

r=

G

TB B r

(4.19)

where ⎡

⎤ r11 r12 r13 Xo ⎢ r21 r22 r23 Yo ⎥ G ⎥ TB = ⎢ ⎣ r31 r32 r33 Zo ⎦ 0 0 0 1 # "G " G # RB G d RB G d ≡ ≡ 000 1 0 1

(4.20)

154

4 Motion Kinematics

⎡

⎤ XP ⎢ YP ⎥ G ⎥ r=⎢ ⎣ ZP ⎦ 1

⎡

⎤ xP ⎢ yP ⎥ B ⎥ r=⎢ ⎣ zP ⎦ 1

⎡

⎤ Xo ⎢ Yo ⎥ G ⎥ d=⎢ ⎣ Zo ⎦ 1

(4.21)

The homogenous transformation matrix G TB is a 4 × 4 matrix that maps a homogenous position vector from one frame to another. This extension of matrix representation of rigid motions simplifies numerical calculations significantly. Representation of an n-component position vector by an (n + 1)-component vector is called homogenous coordinate representation. The appended element is a scale factor, w; hence, in general, homogenous representation of a position vector  T r = x y z is ⎡ ⎤ ⎡ ⎤ wx r1 ⎢ wy ⎥ ⎢ r2 ⎥ ⎥ ⎢ ⎥ r=⎢ (4.22) ⎣ wz ⎦ = ⎣ r3 ⎦ w w Using homogenous coordinates shows that the absolute values of the four coordinates are not important. Instead, it is the three ratios, r1 /w, r2 /w, and r3 /w, that are important because, provided w = 0, and w = ∞, we have ⎡

⎤ ⎡ ⎤ wx x ⎢ wy ⎥ ⎢ y ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ wz ⎦ = ⎣ z ⎦ w 1

(4.23)

Therefore, the homogenous vector wr refers to the same point as r does. If w = 1, then the homogenous coordinates of a position vector are the same as physical coordinates of the vector and the space is the Euclidean configuration space (Jazar, 2011). Hereafter, if no confusion exists and w = 1, we will use regular vectors, and their homogenous representation equivalently. Proof We append a 1 to the coordinates of a point and define 4 × 1 homogenous position vectors as follows: ⎡

⎤ XP ⎢ YP ⎥ G ⎥ rP = ⎢ ⎣ ZP ⎦ 1

⎡

⎤ xP ⎢ yP ⎥ B ⎥ rP = ⎢ ⎣ zP ⎦ 1

⎡

⎤ Xo ⎢ Yo ⎥ G ⎥ d=⎢ ⎣ Zo ⎦ 1

(4.24)

Employing the homogenous transformation matrix to transform homogenous expressions of position vector of a body point P from B-frame to G-frame yields rP = G TB B rP ⎤ ⎡ ⎤⎡ ⎤ XP r11 r12 r13 Xo xP ⎢ YP ⎥ ⎢ r21 r22 r23 Yo ⎥ ⎢ yP ⎥ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎣ ZP ⎦ = ⎣ r31 r32 r33 Zo ⎦ ⎣ zP ⎦ 1 0 0 0 1 1 ⎡ Xo + r11 xP + r12 yP + r13 zP ⎢ Yo + r21 xP + r22 yP + r23 zP =⎢ ⎣ Zo + r31 xP + r32 yP + r33 zP 1 G

(4.25)

⎡

⎤ ⎥ ⎥ ⎦

(4.26)

4.2 Homogenous Transformation

155

However, the standard method of rotation

G

RB plus displacement G d reduces to:

rP = G RB B rp + G d ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ XP r11 r12 r13 xP Xo ⎣ YP ⎦ = ⎣ r21 r22 r23 ⎦ ⎣ yP ⎦ + ⎣ Yo ⎦ ZP r31 r32 r33 zP Zo ⎡ ⎤ Xo + r11 xP + r12 yP + r13 zP = ⎣ Yo + r21 xP + r22 yP + r23 zP ⎦ Zo + r31 xP + r32 yP + r33 zP G

(4.27)

(4.28)

which is compatible with the definition of homogenous vector and homogenous transformation. The advantage of homogenous transformation is combination of rotation and translation and expressing rigid body motions by matrix calculus. The main disadvantage of homogenous transformation matrix is that its inverse is not equal to its transpose.  Example 99 Rotation and translation of a body coordinate frame. Numerical example of a rigid body motion. A body coordinate frame B(oxyz), that is originally coincident with global coordinate frame G(OXY Z), rotates 45 deg  T about the X-axis and translates to 3 5 7 1 . Find the matrix representation of the global position of a body point at  T B r= x y z 1 . G

r=

TB B r ⎡ ⎤ ⎡1 0 0 X π π ⎢Y ⎥ ⎢ 0 cos − sin 4 4 ⎥=⎢ =⎢ ⎢ ⎣ Z ⎦ ⎣ 0 sin π cos π 4 4 1 0 0 0 ⎡ ⎤ x+3 ⎢ ⎥ ⎢ 0.707y − 0.707z + 5 ⎥ =⎢ ⎥ ⎣ 0.707y + 0.707z + 7 ⎦ 1 G

⎤⎡ ⎤ x 5⎥ y⎥ ⎥⎢ ⎥ ⎥⎢ ⎣ 7⎦ z ⎦ 1 1 3

(4.29)

Example 100 An axis–angle rotation and a translation. Numerical example or rigid body motion applied on a cube and determination coordinates of critical points. A cubic rigid body with a unit length of edges sits at the corner of the first quadrant as is shown in Fig. 4.5. If we turn the cube 45 deg about u and translate it by G d, determine the coordinates of the corners of the cube after the rigid body motion.  T u= 111  T G d= 111 The axis uˆ and angle φ of rotation are

(4.31)

⎡

φ=

π 4

⎤ 0.577 35 u ⎢ ⎥ uˆ = √ = ⎣ 0.577 35 ⎦ 3 0.577 35

(4.30)

(4.32)

The Rodriguez transformation matrix of the rotation will be Ru,φ = I cos φ + uˆ uˆ T vers φ + u˜ sin φ ˆ ⎡ ⎤ 0.804 74 −0.310 62 0.505 88 ⎢ ⎥ = ⎣ 0.505 88 0.804 74 −0.310 62 ⎦ −0.310 62 0.505 88 0.804 74

(4.33)

156

4 Motion Kinematics

Fig. 4.5 A cubic rigid body with a unit length of edges

Translating the cube by G d makes the following homogenous transformation matrix G TB . ⎡

⎤ 0.804 74 −0.310 62 0.505 88 1 ⎢ 0.505 88 0.804 74 −0.310 62 1 ⎥ ⎢ ⎥ G TB = ⎢ ⎥ ⎣ −0.310 62 0.505 88 0.804 74 1 ⎦ 0 0 0 1

(4.34)

The local coordinates of the corners of the cube are B

rA x 0 y 0 z 0 Using the transformation equation G r =

G

B

rB B rC 1 1 0 1 0 0

B

rD 0 1 0

B

rE 0 0 1

B

rF 1 0 1

B

rG 1 1 1

B

rH 0 1 1

(4.35)

TB B r, the global coordinates of the corners after the motion will be

B

rA B rB B rC B rD G rE G rF G rG G rH X 1 1.805 1.49 0.689 1.505 2.311 2 1.195 Y 1 1.506 2.31 1.805 0.689 1.195 2 1.494 Z 1 0.689 1.195 1.506 1.804 1.494 2 2.31

(4.36)

Example 101 Decomposition of G TB into translation and rotation. How to decompose a rigid body motion into combination of a rotation and a translation matrices. homogenous transformation matrix G TB can be decomposed into a matrix multiplication of a pure rotation matrix G RB and a pure translation matrix G DB . We use R to indicate rotation matrix and D to indicate displacement equivalent to translation. G

TB =

G

⎡

DB G RB

⎤⎡

1 0 0 Xo r11 ⎢ 0 1 0 Yo ⎥ ⎢ r21 ⎥⎢ =⎢ ⎣ 0 0 1 Zo ⎦ ⎣ r31 000 1 0 ⎡ ⎤ r11 r12 r13 Xo ⎢ r21 r22 r23 Yo ⎥ ⎥ =⎢ ⎣ r31 r32 r33 Zo ⎦ 0 0 0 1

r12 r22 r32 0

r13 r23 r33 0

⎤

0 0⎥ ⎥ 0⎦ 1

(4.37)

4.2 Homogenous Transformation

157

Therefore, a homogenous transformation can be achieved by a pure rotation first, followed by a pure translation. The formula for decomposing transformation matrix G TB to a rotation G RB and a translation G DB is Rotation first, translation second (RF DS), G TB = G DB G RB Decomposition of a homogenous transformation to translation and rotation is not interchangeable G

TB =

G

DB G RB =

G

RB G DB

(4.38)

However, according to the definition of G RB and G DB , we have G

TB =

G

DB G RB =

G

DB +

G

RB − I =

G

RB +

G

DB − I

(4.39)

If a rigid body with its coordinate frame B(oxyz), that is originally coincident with global coordinate frame G(OXY Z), translates by G d, G

T  d = dX dY dZ

(4.40)

the motion of the rigid body is a pure translation and the transformation matrix for a point of a rigid body in the global frame is G

r=

TB B r = G DB B r ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ x x + dX X 1 0 0 dX ⎢ Y ⎥ ⎢ 0 1 0 dY ⎥ ⎢ y ⎥ ⎢ y + dY ⎥ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ =⎢ ⎣ Z ⎦ = ⎣ 0 0 1 dZ ⎦ ⎣ z ⎦ = ⎣ z + dZ ⎦ 1 000 1 1 1 G

⎡

(4.41)

Example 102 Rotation about and translation along a global and local axes. Numerical examples for working with decomposition of homogenous transformation matrix of rotation and translation along a local or global axes. A point P is located at B r = (0, 0, 20) in a body coordinate frame. If the rigid body rotates 30 deg about the global X-axis and the origin of the body frame translates to (X, Y, Z) = (50, 0, 60), then the coordinates of the point in the global frame are ⎤⎡1 0 0 1 0 0 50 π π ⎢0 1 0 0 ⎥⎢ 0 cos − sin 6 6 ⎢ ⎥⎢ ⎢ ⎣ 0 0 1 60 ⎦ ⎣ 0 sin π cos π 6 6 000 1 0 0 0 ⎡

⎤ 0 ⎡ 0 ⎤ ⎡ 50 ⎤ ⎢ ⎥ 0⎥ 0 ⎥ ⎥⎢ ⎥ = ⎢ −10 ⎥ ⎥⎢ ⎣ ⎦ ⎣ 77.3 ⎦ 0 ⎦ 20 1 1 1

(4.42)

A point P is located at (0, 0, 20) in a body coordinate frame. If the origin of the body frame translates to (X, Y, Z) = (50, 0, 60) and rotates 30 deg about the local x-axis, then the coordinates of the point in global frame are ⎤⎡1 0 0 1 0 0 50 ⎢ π ⎢ 0 1 0 0 ⎥ ⎢ 0 cos 6 sin π6 ⎢ ⎥ ⎣ 0 0 1 60 ⎦ ⎢ ⎣ 0 − sin π6 cos π6 000 1 0 0 0 ⎡

⎤T 0 0⎥ ⎥ ⎥ 0⎦ 1

⎡

⎤ ⎡ ⎤ 0 50 ⎢ 0 ⎥ ⎢ −10 ⎥ ⎢ ⎥=⎢ ⎥ ⎣ 20 ⎦ ⎣ 77.3 ⎦ 1 1

Example 103 Translation. Numerical example for a pure translational homogenous transformation matrix. If a body point at  T B r = −1 0 2 1 is translated to G r without rotation, find the displacement vector d. G

(4.44)

 T r = 0 10 −5 1

(4.45)

 T d = dX dY dZ 1

(4.46)

G G

(4.43)

158

4 Motion Kinematics

The corresponding transformation is

⎤⎡ ⎤ ⎤ ⎡ 1 2 1 0 0 dX ⎢ 10 ⎥ ⎢ 0 1 0 dY ⎥ ⎢ 4 ⎥ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎣ −5 ⎦ = ⎣ 0 0 1 dZ ⎦ ⎣ 2 ⎦ 000 1 1 1 ⎡

(4.47)

Therefore, 1 + dX = 2

4 + dY = 10

2 + dZ = −5

(4.48)

and dX = 1

dY = 6

dZ = −7

(4.49)

Example 104 Pure rotation and pure translation. Principal global rotation and translation homogenous matrices are collected here for reference. A set of basic homogenous transformations for translation along and rotation about X, Y , and Z axes are given below. ⎡

G

G

TB = DX,a

TB = RX,γ

⎤ 100a ⎢0 1 0 0⎥ ⎥ =⎢ ⎣0 0 1 0⎦ 0001 ⎡ 1 0 0 ⎢ 0 cos γ − sin γ =⎢ ⎣ 0 sin γ cos γ 0 0 0 ⎤ 1000 ⎢0 1 0 b⎥ ⎥ =⎢ ⎣0 0 1 0⎦ 0001 ⎡ cos β 0 sin β ⎢ 0 1 0 =⎢ ⎣ − sin β 0 cos β 0 0 0

(4.50) ⎤ 0 0⎥ ⎥ 0⎦ 1

(4.51)

⎡

G

G

TB = DY,b

TB = RY,β

⎤ 1000 ⎢0 1 0 0⎥ ⎥ =⎢ ⎣0 0 1 c⎦ 0001 ⎡ cos α − sin α ⎢ sin α cos α =⎢ ⎣ 0 0 0 0

(4.52) ⎤ 0 0⎥ ⎥ 0⎦ 1

(4.53)

⎡

G

G

TB = DZ,c

TB = RZ,α

(4.54) ⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(4.55)

Example 105 Homogenous transformation as a vector addition. Position of a point P in B-frame, which is at G d in G-frame makes it possible to show rigid body motion by vectors. As is shown in Fig. 4.4, the position of point P can be expressed by a vector addition. G

rP =

G

d + B rP

(4.56)

4.2 Homogenous Transformation

159

A vector equation is only meaningful when all the vectors are expressed in the same coordinate frame. Hence, we need to transform either B rP to G or G d to B. Therefore, the applied vector equation is G

rP =

G

RB B rP +

G

(4.57)

d

or B

RG G rP =

B

RG G d + B rP

(4.58)

The first one defines a homogenous transformation from B to G, rP =

G TB B rP # "G RB G d G TB = 0 1 G

(4.59) (4.60)

and the second one defines a transformation from G to B. B B

rP =

TG =

B

TG G rP

"B

#

RG − RG d = 0 1 B

G

"G

RBT 0

−

G

RBT G d 1

#

(4.61) (4.62)

Example 106  Point at infinity and direction line. The meaning of the fourth element of homogenous expression of vectors to be 0. Points at infinity have a convenient representation with homogenous coordinates. Consider the scale factor w as the fourth coordinate of a point, then the homogenous representation of the point is given by: ⎡ ⎤ ⎡ ⎤ x x/w ⎢ y ⎥ ⎢ y/w ⎥ ⎢ ⎥=⎢ ⎥ (4.63) ⎣ z ⎦ ⎣ z/w ⎦ w 1 When w tends to zero, the point goes to infinity, and we may adapt the convention that the homogenous coordinate ⎡ ⎤ x ⎢y ⎥ ⎢ ⎥ (4.64) ⎣z⎦ 0 represents a point at infinity. More importantly, a point at infinity indicates a direction. In this case, it indicates all lines  T parallel to the vector r = x y z , which intersect at a point at infinity. The homogenous coordinate transformation of points at infinity introduces a neat decomposition of the homogenous transformation matrices. ⎡ ⎤ r11 r12 r13 Xo ⎢ r21 r22 r23 Yo ⎥ G ⎥ TB = ⎢ (4.65) ⎣ r31 r32 r33 Zo ⎦ 0 0 0 1 The first three columns have zero as the fourth coordinate. Therefore, they represent points at infinity, which are the directions corresponding to the three coordinate axes. The fourth column has one as the fourth coordinate and represents the location of the origin of coordinate frame.

Example 107  The most general homogenous transformation. Inhomogenous transformation matrices are important tools in computer graphics. The homogenous transformation matrix (4.20), that represents a rigid body motion, is a special case of the general homogenous transformation. The most general homogenous transformation, which has been extensively used in the field of computer graphics, is

160

4 Motion Kinematics

A

# RB (3 × 3) A d (3 × 1) p (1 × 3) w (1 × 1) # " rotation translation = perspective scale factor

TB =

"A

(4.66)

For the purpose of robotics, we always take the last row vector of [T ] to be (0, 0, 0, 1). However, the more general form of (4.66) could be useful, for example, when a graphical simulator or a vision system is added to the overall robotic system. The upper left 3 × 3 submatrix A RB denotes the orientation of a moving frame B with respect to a reference frame A. The upper right submatrix 3 × 1 is A d and denotes the position of the origin of the moving frame B relative to the reference frame A. The lower left submatrix 1 × 3 is p and denotes a perspective transformation, and the lower right element w is a scaling factor. Consider an identity transformation. If we change the main diagonal elements of the homogenous transformation matrix to any other number than 1, the matrix will define scalingtransformation.  Assume to show the scaling factors in x, y, z, by sx , sy , sz , respectively, the point (x, y, z) will be mapped to sx x, sy y, sz z . If scale factors are greater than 1, the transformation will be magnification and if the scale factors are less than 1, the transformation will be shrinking. Scale factor 0 is not defined. ⎡

⎤ ⎡ sx sx x ⎢ sy y ⎥ ⎢ 0 ⎢ ⎥ ⎢ ⎣ sz z ⎦ = ⎣ 0 0 0

0 sy 0 0

0 0 sz 0

⎤⎡ ⎤ 0 x ⎢y ⎥ 0⎥ ⎥⎢ ⎥ 0⎦⎣ z ⎦ 1 0

(4.67)

Every negative values on the main diagonal elements will show reflection associated to its coordinate. Adjusting the negative coefficients enables us to make reflection with respect to any principal plane we wish. A reflection with respect to origin will be made by all negative diagonal elements. ⎡

⎤ ⎡ ⎤⎡ ⎤ −x −1 0 0 0 x ⎢ −y ⎥ ⎢ 0 −1 0 0 ⎥ ⎢ y ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ −z ⎦ = ⎣ 0 0 −1 0 ⎦ ⎣ z ⎦ 0 0 0 0 1 0

(4.68)

Reflection about the x-axis will be made by ⎡

⎤ ⎡ ⎤⎡ ⎤ x 1 0 0 0 x ⎢ −y ⎥ ⎢ 0 −1 0 0 ⎥ ⎢ y ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ −z ⎦ = ⎣ 0 0 −1 0 ⎦ ⎣ z ⎦ 0 0 0 0 1 0

(4.69)

and reflection about the (x, y)-plane will be ⎡

⎤ ⎡ ⎤⎡ ⎤ 10 0 0 x x ⎢ y ⎥ ⎢0 1 0 0⎥⎢y ⎥ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎣ −z ⎦ = ⎣ 0 0 −1 0 ⎦ ⎣ z ⎦ 00 0 1 0 0

(4.70)

Off-diagonal elements will be used to make shear transformation in graphic. For example, a shear transformation in y and z coordinates, proportional to x, is made by change in off-diagonal elements of the first column. ⎡

⎤ ⎡ x 1 ⎢ y + cx x ⎥ ⎢ cx ⎢ ⎥ ⎢ ⎣ z + cx x ⎦ = ⎣ cx 0 0

⎤⎡ ⎤ 000 x ⎥ ⎢ 1 0 0⎥⎢y ⎥ ⎥ 0 1 0⎦⎣ z ⎦ 0 001

(4.71)

Example 108 Rigid body based on corner coordinates. Numerical example of calculating position of corners of a box, using homogenous transformation method, after rotation and translation.

4.2 Homogenous Transformation

161

Fig. 4.6 Expressing the motion of a rigid body in terms of some body points

Employing the coordinates of sharp corners of a rigid body, we may express a rigid body by an array of homogenous coordinates of the corners in body coordinate frame B. Figure 4.6a illustrates a box. The coordinates of the corners of the box in its body frame B are collected in the matrix [P ]: B

  P = r1 r2 r3 r4 r5 r6 r7 r8 ⎡ ⎤ 11001100 ⎢0 3 3 0 0 3 3 0⎥ ⎥ =⎢ ⎣0 0 0 0 5 5 5 5⎦ 11111111

(4.72)

The configuration of the box after a rotation of −90 deg about the Z-axis and a translation of three units along the X-axis is shown in Fig. 4.6b. The new coordinates of the corners in the global frame G can be found by multiplying the corresponding transformation matrix and [P ]: G

TB = DX,3 RZ,90     ⎤ ⎡ ⎤⎡ 1003 cos  − π2  − sin − π2 0 0 ⎢ 0 1 0 0 ⎥ ⎢ sin − π cos − π 0 0 ⎥ 2 2 ⎥⎢ ⎥ =⎢ ⎣0 0 1 0⎦⎣ 0 0 1 0⎦ 0 0 01 0001 ⎡ ⎤ 0 103 ⎢ −1 0 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0⎦ 0 001

(4.73)

Therefore, the global coordinates of corners 1–8 after the motion are G

P =

G

⎡

TB B P

⎤⎡ ⎤ 0 103 11001100 ⎢ −1 0 0 0 ⎥ ⎢ 0 3 3 0 0 3 3 0 ⎥ ⎥⎢ ⎥ =⎢ ⎣ 0 0 1 0⎦⎣0 0 0 0 5 5 5 5⎦ 0 001 11111111 ⎡ ⎤ 3 6 63 3 6 63 ⎢ −1 −1 0 0 −1 −1 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 0 0 5 5 5 5⎦ 1 1 11 1 1 11

(4.74)

162

4.3

4 Motion Kinematics

Inverse and Reverse Homogenous Transformation

The advantage of simplicity to work with homogenous transformation matrices come with the penalty of losing the orthogonality property. If we show a rigid body motion by the homogenous transformation G TB , "

G

I Gd TB = 0 1

#"G

# "G # RB 0 RB G d = 0 1 0 1

(4.75)

then the inverse of homogenous transformation matrix G TB is B

TG =

G

TB−1

=

"G

RB G d 0 1

#−1 =

"G

RBT − G RBT G d 0 1

# (4.76)

and the reverse motion of G TB would be G T−B G

T−B =

"G

RBT 0 0 1

#"

# "G T G T G # I − Gd RB − RB d = 0 1 0 1

(4.77)

showing that G

TB−1 G TB = I4

(4.78)

G

T−B G TB = I4

(4.79)

where I4 is the identity matrix of rank 4. The homogenous transformation matrix G TB presents rigid body motions very well; however, a shortcoming is that they lose the orthogonality property and, hence, its inverse is not equal to its transpose: G

TB−1 =

G

TBT

(4.80)

Although it is traditional to use the inverse matrix notation G TB−1 for B TG , G

TB−1 =

B

TG

(4.81)

calculating G TB−1 must be done according to Eq. (4.76), and not by regular matrix inversion of a 4 × 4 matrix. The matrix inversion notation makes equations consistent with the multiplication of a matrix [T ] by its inverse, [T ]−1 , because G

TB−1 G TB =

B

TG G TB = I4

(4.82)

Proof 1. Figure 4.7 depicts a rotated and translated body frame B(oxyz) with respect to a global frame G(OXY Z). Transformation of the coordinates of a point P from the global frame to the body frame will be expressed by homogenous transformation matrix B TG , which is the inverse of the transformation G TB . To find B TG , let us start with expression of G r and definition of G TB for mapping B r to G r r = G RB B r + G d "G # "G #"B # r RB G d r = 1 0 1 1 G

(4.83) (4.84)

4.3 Inverse and Reverse Homogenous Transformation

163

Fig. 4.7 A rotated and translated body frame B(oxyz) with respect to a global frame G(OXY Z)

and calculate B r B

r=

G

RB−1 (G r −

G

d) =

G

RBT G r −

G

RBT G d

(4.85)

to express the transformation matrix B TG for mapping G r to B r. B

TG =

G

TB−1

=

"G

RBT − G RBT G d 0 1

# (4.86)

A matrix multiplication indicates that G

G

"

# RB G d = 0 1 0 1 "G T G # " # RB RB G RBT G d − G RBT G d I 0 = = 3 = I4 0 1 0 1

TB−1 G TB

RBT − G RBT G d

G

(4.87) Decomposing a homogenous motion G TB by a rotation G RB plus a translation G DB , "

G

TB =

G

DB

G

I Gd RB = 0 1

#"G

RB 0 0 1

# (4.88)

we define the reverse motion by a translation − G DB followed by a reverse rotation G R−B = G

T−B =

G

R−B





− DB = G

"G

RBT 0 0 1

#"

I − Gd 0 1

G

RBT .

# (4.89)

The result of a rigid body motion G TB and then the reverse motion G T−B would also be an identity matrix, indicating no resultant motion. T−B G TB = G TB G T−B "G T #" #" G #"G # RB 0 I − Gd I d RB 0 = 0 1 0 1 0 1 0 1 "G T #" #"G # RB 0 I3 0 RB 0 = = I4 0 1 0 1 0 1 G

(4.90) 

164

4 Motion Kinematics

Proof 2. B TG can also be found by a geometric expression. Using the inverse of the rotation matrix G RB , G

RB−1 =

G

RBT =

B

RG

(4.91)

and transforming G d to the body frame B, and then reversing B d to indicate the origin of the global frame with respect to the origin of the body frame − B d = − B RG G d = − G RBT G d (4.92) allow us to define the homogenous transformation B TG . B

TG =

"B

# "G T G T G # RG − B d RB − RB d = 0 1 0 1

We use the notation B

TG =

G

TB−1

(4.93)

(4.94)

and remember that the inverse of a homogenous transformation matrix must be calculated according to Eq. (4.76), and not by regular inversion of a 4 × 4 matrix. This notation is consistent with the multiplication of a T matrix by its inverse T −1 . G

TB−1 G TB = I4

(4.95) 

Example 109 Inverse of a homogenous transformation matrix. It is a numerical example for calculation of inverse of a homogenous transformation matrix. Assume that ⎡ ⎤ 0.643 −0.766 0 −1 "G # ⎢ 0.766 0.643 0 0.5 ⎥ RB G d G ⎥= TB = ⎢ (4.96) ⎣ 0 0 1 0.2 ⎦ 0 1 0 0 0 1 then

⎡

⎤ 0.643 −0.766 0 G RB = ⎣ 0.766 0.643 0 ⎦ 0 0 1

⎡

⎤ −1 G d = ⎣ 0.5 ⎦ 0.2

(4.97)

and, therefore, B

# RBT − G RBT G d TG = = 0 1 ⎡ ⎤ 0.643 0.766 0 0.26 ⎢ −0.766 0.643 0 −1.087 ⎥ ⎥ =⎢ ⎣ 0 0 1 −0.2 ⎦ 0 0 0 1 G

TB−1

"G

(4.98)

Example 110 Transformation matrix and coordinate of points. It is a numerical example to determine transformation matrix when coordinates of few point are given before and after motion. It is possible and sometimes convenient to express a rigid body motion in terms of known displacement of specified points of the body. Assume A, B, C, D are four points with the known coordinates at two different positions: A1 (2, 4, 1)

B1 (2, 6, 1)

C1 (1, 5, 1)

D1 (3, 5, 2)

(4.99)

A2 (5, 1, 1)

B2 (7, 1, 1)

C2 (6, 2, 1)

D2 (6, 2, 3)

(4.100)

4.3 Inverse and Reverse Homogenous Transformation

165

There must be a transformation matrix T to map the initial positions to the final positions. ⎡

⎤ ⎡ ⎤ 2213 5766 ⎢4 6 5 5⎥ ⎢1 1 2 2⎥ ⎢ ⎥ ⎢ ⎥ [T ] ⎢ ⎥=⎢ ⎥ ⎣1 1 2 2⎦ ⎣1 1 1 3⎦ 1111 1111

(4.101)

It is easy to find [T ] by matrix multiplication. ⎡

⎤⎡ ⎤−1 5766 2213 ⎢1 1 2 2⎥⎢4 6 5 5⎥ ⎢ ⎥⎢ ⎥ [T ] = ⎢ ⎥⎢ ⎥ ⎣1 1 1 3⎦⎣1 1 2 2⎦ 1111 1111 ⎡ ⎤⎡ ⎤ 5766 0 −1/2 −1/2 7/2 ⎢1 1 2 2⎥⎢ 0 1/2 −1/2 −3/2 ⎥ ⎢ ⎥⎢ ⎥ =⎢ ⎥⎢ ⎥ ⎣ 1 1 1 3 ⎦ ⎣ −1/2 0 1/2 1/2 ⎦ 1111 1/2 0 1/2 −3/2 ⎡ ⎤ 010 1 ⎢0 0 1 0 ⎥ ⎢ ⎥ =⎢ ⎥ ⎣ 1 0 1 −2 ⎦ 000 1

(4.102)

What if we had more than 4 points? In that case, we only pick four points to determine [T ]. Example 111 Quick inverse transformation. An easier method to determine inverse of homogenous matrices by breaking it into displacement and rotation. For numerical calculation, it is more practical to decompose a transformation matrix into rotation [R] and displacement [D] and take advantage of the inverse of matrix multiplication. T −1 = [DR] −1 = R −1 D −1 = R T D −1

(4.103)

Consider a homogenous matrix [T ] ⎡

r11 ⎢ r21 [T ] = ⎢ ⎣ r31 0

r12 r22 r32 0

r13 r23 r33 0

⎤ ⎡ ⎤⎡ 1 0 0 r14 r14 r11 ⎢ 0 1 0 r24 ⎥ ⎢ r21 r24 ⎥ ⎥=⎢ ⎥⎢ r34 ⎦ ⎣ 0 0 1 r34 ⎦ ⎣ r31 1 000 1 0

r12 r22 r32 0

r13 r23 r33 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(4.104)

therefore, T −1 = [DR] −1 = R −1 D −1 = R T D −1 ⎡ ⎤⎡ ⎤ 1 0 0 −r14 r11 r21 r31 0 ⎢ r12 r22 r32 0 ⎥ ⎢ 0 1 0 −r24 ⎥ ⎥⎢ ⎥ =⎢ ⎣ r13 r23 r33 0 ⎦ ⎣ 0 0 1 −r34 ⎦ 0 0 0 1 000 1 ⎡ ⎤ r11 r21 r31 −r11 r14 − r21 r24 − r31 r34 ⎢ r12 r22 r32 −r12 r14 − r22 r24 − r32 r34 ⎥ ⎥ =⎢ ⎣ r13 r23 r33 −r13 r14 − r23 r24 − r33 r34 ⎦ 0 0 0 1

(4.105)

166

4 Motion Kinematics

Example 112  Inverse of a general matrix. How to determine inverse of large matrices by breaking it into four smaller matrices. Ideal for computerized calculations. Let [T ] be defined as a general matrix combined by four submatrices [A], [B], [C], and [D]. " [T ] =

AB CD

# (4.106)

Then, the inverse of T is given by: T −1 =

"

A−1 + A−1 BE −1 CA−1 −A−1 BE −1 −E −1 CA−1 E −1

#

[E] = D − CA−1 B

(4.107) (4.108)

In case of the most general homogenous transformation matrix, A

"A

# RB (3 × 3) A d (3 × 1) TB = p (1 × 3) w (1 × 1) " # rotation translation = perspective scale factor

(4.109)

we have [T ] =

A

TB

(4.110)

[A] =

A

RB

(4.111)

A

d

[B] =



[C] = p1 p2 p3



[D] = [w1×1 ] = w

(4.112) (4.113) (4.114)

and, therefore, [E] = [E1×1 ] = E   = w − p1 p2 p3 A RB−1 A d   = w − p1 p2 p3 A RBT A d = 1 − p1 (d1 r11 + d2 r21 + d3 r31 ) +p2 (d1 r12 + d2 r22 + d3 r32 ) +p3 (d1 r13 + d2 r23 + d3 r33 )  1  g1 g2 g3 E g1 = p1 r11 + p2 r12 + p3 r13

− E −1 CA−1 =

(4.115)

(4.116)

g2 = p1 r21 + p2 r22 + p3 r23 g3 = p1 r31 + p2 r32 + p3 r33 − A−1 BE −1

⎤ ⎡ d r + d2 r21 + d3 r31 1 ⎣ 1 11 =− d1 r12 + d2 r22 + d3 r32 ⎦ E d1 r13 + d2 r23 + d3 r33

(4.117)

4.4 Combined Homogenous Transformation

167

A−1 + A−1 BE −1 CA−1 = [F ] ⎡ ⎤ f11 f12 f13 = ⎣ f21 f22 f23 ⎦ f31 f32 r33

(4.118)

where f11 = r11 + f12 = r21 + f13 = r31 + f21 = r12 + f22 = r22 + f23 = r32 + f31 = r13 + f32 = r23 + f33 = r33 +

1 E 1 E 1 E 1 E 1 E 1 E 1 E 1 E 1 E

(p1 r11 + p2 r12 + p3 r13 ) (d1 r11 + d2 r21 + d3 r31 ) (p1 r21 + p2 r22 + p3 r23 ) (d1 r11 + d1 r21 + d1 r31 ) (p1 r31 + p2 r32 + p3 r33 ) (d1 r11 + d2 r21 + d3 r31 ) (p1 r11 + p2 r12 + p3 r13 ) (d1 r12 + d2 r22 + d3 r32 ) (p1 r21 + p2 r22 + p3 r23 ) (d1 r12 + d2 r22 + d3 r32 ) (p1 r31 + p2 r32 + p3 r33 ) (d1 r12 + d2 r22 + d3 r32 ) (p1 r11 + p2 r12 + p3 r13 ) (d1 r13 + d2 r23 + d3 r33 ) (p1 r21 + p2 r22 + p3 r23 ) (d1 r13 + d2 r23 + d3 r33 ) (p1 r31 + p2 r32 + p3 r33 ) (d1 r13 + d2 r23 + d3 r33 )

(4.119)

In the case of a coordinate homogenous transformation, [T ] =

A



TB

[C] = 0 0 0



[A] =

A

RB

[B] =

[D] = [1]

A

d

(4.120) (4.121)

we have [E] = [1] and, therefore, A

4.4

TB−1

=

"A

RBT − A RBT A d 0 1

(4.122) # (4.123)

Combined Homogenous Transformation

Figure 4.8 illustrates three reference frames: A, B, and C. The transformation matrices to transform coordinates from frame B to A and from frame C to B are "A # RB A d 1 A (4.124) TB = 0 1 "B # RC B d 2 B TC = (4.125) 0 1 Hence, the transformation matrix from C to A is

168

4 Motion Kinematics

Fig. 4.8 Three coordinate frames to analyze compound transformations

A

TC =

A

TB TC = B

"A

RB A d 1 0 1

#"B

"A

RB B RC A RB B d 2 + A d 1 0 1 # "A RC A d 2 = 0 1 =

RC B d 2 0 1 #

#

(4.126)

and, therefore, the inverse transformation is C

"B

A

RBT − B RCT

A

RB B d 2 + A d 1 TA = 0 1 # "B T A T B T B RC RB − RC d2 − A RCT A d1 = 0 1 "A T A T A # RC − RC d 2 = 0 1 RCT

A

RBT

#

(4.127)

The value of homogenous coordinates are better appreciated when several displacements occur in succession, which, for instance, can be written as G T4 = G T1 1 T2 2 T3 3 T4 (4.128) rather than: G

R4 4 rP + G d4       = G R1 1 R2 2 R3 3 R4 4 rP + 3 d4 + 2 d3 + 1 d2 + G d1

(4.129)

Example 113 Homogenous transformation for multiple frames. How to combine rigid body motions when more than two coordinate frames are involved. Figure 4.9 depicts a point P in a local frame B2 (x2 y2 z2 ). The coordinates of P in the global frame G(OXY Z) can be found by using the homogenous transformation matrices. The position of P in frame B2 (x2 y2 z2 ) is indicated by 2 rP . Therefore, its position in frame B1 (x1 y1 z1 ) is ⎡

⎤ ⎡ ⎤ x1 " 1 1 # x2 ⎢ y1 ⎥ ⎢ ⎥ ⎢ ⎥ = R2 d 2 ⎢ y 2 ⎥ ⎣ z1 ⎦ 0 1 ⎣ z2 ⎦ 1 1

(4.130)

4.4 Combined Homogenous Transformation

169

Fig. 4.9 Point P in a local frame B2 (x2 y2 z2 )

Fig. 4.10 Rotation about an axis not going through origin

and, hence, its position in the global frame G(OXY Z) would be ⎡

⎤ ⎡ ⎤ X " G G # x1 ⎢Y ⎥ ⎥ R1 d 1 ⎢ ⎢ ⎥= ⎢ y1 ⎥ ⎣Z⎦ 0 1 ⎣ z1 ⎦ 1 1 ⎤ x2 ⎥ R1 G d 1 R2 1 d 2 ⎢ ⎢ y2 ⎥ = ⎣ 0 1 0 1 z2 ⎦ 1 ⎡ ⎤ x2 "G 1 G 1 # ⎥ R1 R2 R1 d 2 + G d 1 ⎢ ⎢ y2 ⎥ = ⎣ 0 1 z2 ⎦ 1 "G

#"1

#

⎡

(4.131)

Example 114  Rotation about an axis not going through origin. This is a good application of homogenous transformation matrix to determine rotation of a rigid body about a non-central axis. This example is a reference for rigid body rotational motions about any arbitrary axis. The homogenous transformation matrix can also be used to present rotation about an axis not going through the origin. Such an axis is called an off-center axis. Figure 4.10 indicates an angle of rotation, φ, around an axis u, ˆ passing through a point P at a position G dQ . Let us set a local frame B at point P parallel to the global frame G. Then, a rotation around uˆ can be expressed as a translation along −d, to bring the body frame B to the global frame G, followed by a rotation φ about u, ˆ and a reverse translation along d to move origin of the body frame B back to where it was.

170

4 Motion Kinematics G

⎡

TB = Dd,d ˆ Ru,φ ˆ Dd,−d ˆ " #" #" # Id Ru,φ 0 I −d ˆ = 01 0 1 0 1 " # Ru,φ d − Ru,φ ˆ ˆ d = 0 1

(4.132)

⎤ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ ⎢ ⎥ ⎢ Ru,φ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎥ ˆ = ⎣ u1 u2 vers φ + u3 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ u21 vers φ + cφ

d − Ru,φ ˆ d= ⎤ ⎡ d1 (1 − u21 ) vers φ − u1 vers φ (d2 u2 + d3 u3 ) + sφ (d2 u3 − d3 u2 ) ⎥ ⎢ ⎢ d (1 − u2 ) vers φ − u vers φ (d u + d u ) + sφ (d u − d u ) ⎥ 2 3 3 1 1 3 1 1 3 ⎦ ⎣ 2 2 d3 (1 − u23 ) vers φ − u3 vers φ (d1 u1 + d2 u2 ) + sφ (d1 u2 − d2 u1 )

(4.133)

(4.134)

Example 115  A rotating cylinder. Numerical example for off-center rigid body rotation. Imagine a cylinder with radius R = 2 that is going to turn about the axis uˆ at d. ⎡ ⎤ 0 uˆ = ⎣ 0 ⎦ 1

⎡ ⎤ 2 d = ⎣0⎦ 0

(4.135)

If the cylinder turns 90 deg about its axis, then every point on the periphery of the cylinder will move 90 deg on circular paths parallel to (x, y)-plane. The transformation of this motion is "

G

TB = Dd,d ˆ Ru,φ ˆ Dd,−d ˆ

⎤⎡ π 1002 c 2 −s π2 ⎢0 1 0 0⎥⎢sπ cπ 2 ⎥⎢ 2 =⎢ ⎣0 0 1 0⎦⎣ 0 0 0 0 0001 ⎡ ⎤ 0 −1 0 2 ⎢ 1 0 0 −2 ⎥ ⎥ =⎢ ⎣0 0 1 0 ⎦ 0 0 0 1 ⎡

#

"

I −d 2 0 1 0 1 ⎤⎡ ⎤ 00 1 0 0 −2 ⎢ ⎥ 0 0⎥ ⎥⎢0 1 0 0 ⎥ 1 0⎦⎣0 0 1 0 ⎦ 01 000 1

Id = 01

RK, ˆ π 0

#

(4.136)

Consider a point of cylinder that was on the origin. After the rotation, the point would be seen at: G

r=

G

⎡

TB G r

⎤⎡ ⎤ ⎡ ⎤ 0 −1 0 2 0 2 ⎢ 1 0 0 −2 ⎥ ⎢ 0 ⎥ ⎢ −2 ⎥ ⎥⎢ ⎥ ⎢ ⎥ =⎢ ⎣0 0 1 0 ⎦⎣0⎦ = ⎣ 0 ⎦ 0 0 0 1 1 1

(4.137)

As another example, let a body frame B sitting on G-frame turn 90 deg about an axis uˆ parallel to Kˆ that is at G d. Determine the new location of a body point P at B rP .

4.4 Combined Homogenous Transformation

171

Fig. 4.11 Rotating a body frame B about an off-center axis

 T rP = 1 0 0  T G d= 110

B

(4.138) (4.139)

The associated homogenous transformation G TB from (4.132) is G

TB = Dd,d ˆ Ru,φ ˆ Dd,−d ˆ ⎤⎡ ⎡ 1001 cos π2 − sin π2 ⎢ 0 1 0 1 ⎥ ⎢ sin π cos π 2 2 ⎥⎢ =⎢ ⎣0 0 1 0⎦⎣ 0 0 0 0 0001 ⎡ ⎤ 0 −1 0 2 ⎢1 0 0 0⎥ ⎥ =⎢ ⎣0 0 1 0⎦ 0 0 01

⎤⎡ ⎤ 00 1 0 0 −1 ⎢ ⎥ 0 0⎥ ⎥ ⎢ 0 1 0 −1 ⎥ 1 0⎦⎣0 0 1 0 ⎦ 01 000 1

(4.140)

After the rotation, the point P will be seen at G rP : ⎤⎡ ⎤ ⎡ ⎤ 0 −1 0 2 1 2 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ 1 0 0 0 0 G ⎥⎢ ⎥ = ⎢1⎥ rP = G TB B rP = ⎢ ⎣0 0 1 0⎦⎣0⎦ ⎣0⎦ 0 0 01 1 1 ⎡

(4.141)

and the origin o of the body coordinate frame B would be at ⎤⎡ ⎤ ⎡ ⎤ 0 −1 0 2 0 2 ⎢ ⎥ ⎢ ⎥ ⎢ 1 0 0 0⎥⎢0⎥ ⎢0⎥ G ⎥ ro = G TB B ro = ⎢ ⎣0 0 1 0⎦⎣0⎦ = ⎣0⎦ 0 0 01 1 1 ⎡

The final positions of point P , o, and frame B are shown in Fig. 4.11.

(4.142)

172

4 Motion Kinematics

Fig. 4.12 An RP R manipulator robot

Fig. 4.13 The SCARA robot RRP of Example 117

Example 116 End-effector of an RP R robot in a global frame. How to analyze a simple robotic arm, using rigid body motion and homogenous transformation matrices Point P is at tip of the end arm of the robot shown in Fig. 4.12. Position vector of P in frame B2 (x2 y2 z2 ) is 2 rP . Frame B2 (x2 y2 z2 ) at location G d1 can rotate about z2 and slide along y1 . Frame B1 (x1 y1 z1 ) can rotate about the Z-axis of the global frame G(OXY Z). The position of the origin of B1 is shown by 1 d2 in B2 . To determine the position of P in G(OXY Z), we add G d1 and G d2 and G rP . G

r=

G

R1 1 R2 2 rP +

=

G

T2 2 rP

where G

T1 =

"G

and G

T2 =

G

R1 1 d 2 +

G

d1 =

G

T1 1 T2 2 rP (4.143)

R1 G d 1 0 1 "G

# 1

T2 =

"1

R1 1 R2 G R1 1 d 2 + 0 1

R2 1 d 2 0 1

G

d1

# (4.144)

# (4.145)

Example 117 End-effector of a SCARA robot in a global frame. How to analyze relative DH homogenous matrices for a robotic arm. SCARA robot is an important robotic arm. Figure 4.13 depicts an RRP (SCARA) robot with a global coordinate frame G(OXY Z) attached to the base link along with the coordinate frames B1 (o1 x1 y1 z1 ) and B2 (o2 x2 y2 z2 ) attached to link (1) and the tip of link (3).

4.4 Combined Homogenous Transformation

173

The transformation matrix G T1 to map points in frame B1 to the base frame G is ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 G T1 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(4.146)

and the transformation matrix 1 T2 to map points in frame B2 to the frame B1 is ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ 1 −h ⎦ 0 1

(4.147)

Therefore, the transformation matrix G T2 to map points in the end-effector frame B2 to the base frame G is G

T2 =

G

⎡

T1 1 T2

⎤

(4.148)

c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l1 sθ 1 + l2 s (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎦ ⎣ 0 0 1 −h 0 0 0 1  T The origin of the last frame is at 2 ro2 = 0 0 0 1 in its local frame B2 . Hence, the position of o2 in the base coordinate frame is at ⎤ ⎡ l1 cos θ 1 + l2 cos (θ 1 + θ 2 ) ⎢ l1 sin θ 1 + l2 sin (θ 1 + θ 2 ) ⎥ G ⎥ (4.149) r2 = G T2 2 ro2 = ⎢ ⎦ ⎣ −h 1 Example 118 Object manipulation. Numerical example for motion of a rigid body and determining the coordinates of its corners after motion. The geometry of a rigid body may be represented by an array of the homogenous coordinates of some specific points of the body expressed in a local coordinate frame. Figure 4.14a illustrates the configuration of a wedge. The coordinates of the corners of the wedge in its body frame B are collected in the matrix P , standing for Points ⎡

⎤ 111000 ⎢0 0 3 3 0 0⎥ B ⎥ P =⎢ ⎣4 0 0 0 0 4⎦ 111111

(4.150)

The configuration of the wedge after a rotation of −90 deg about the Z-axis and a translation of three units along the X-axis is shown in Fig. 4.14b. The new coordinates of the corners in the global frame G can be found by multiplying the homogenous transformation matrix G TB by the P matrix. G

TB = DX,3 RZ,−π ⎡ ⎤⎡ 1003 cos −π − sin −π ⎢ 0 1 0 0 ⎥ ⎢ sin −π cos −π ⎥⎢ =⎢ ⎣0 0 1 0⎦⎣ 0 0 0001 0 0 ⎡ ⎤ 0 103 ⎢ −1 0 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0⎦ 0 001

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(4.151)

174

4 Motion Kinematics

Fig. 4.14 Describing the motion of a rigid body in terms of some body points

Fig. 4.15 Cylindrical coordinates of a point P

⎤⎡ ⎤ 0 103 111000 ⎢ −1 0 0 0 ⎥ ⎢ 0 0 3 3 0 0 ⎥ G ⎥⎢ ⎥ P = G TB B P = ⎢ ⎣ 0 0 1 0⎦⎣4 0 0 0 0 4⎦ 0 001 111111 ⎡ ⎤ 3 3 6 633 ⎢ −1 −1 −1 0 0 0 ⎥ ⎥ =⎢ ⎣ 4 0 0 0 0 4⎦ 1 1 1 111 ⎡

(4.152)

Example 119 Cylindrical coordinates. Cartesian–Cylindrical coordinate system transformation using homogenous transformation method. A set of cylindrical coordinates, as shown in Fig. 4.15, can be achieved by a translation r along the X-axis, followed by a rotation ϕ about the Z-axis, and finally a translation z parallel to the Z-axis. Therefore, the homogenous transformation matrix to go from cylindrical coordinates C(Orϕz) to Cartesian coordinates G(OXY Z) is

4.4 Combined Homogenous Transformation

175

Fig. 4.16 Spherical coordinates of a point P

G

TC = DZ,z RZ,ϕ DX,r ⎤⎡ ⎡ 1000 cos ϕ − sin ϕ ⎢ 0 1 0 0 ⎥ ⎢ sin ϕ cos ϕ ⎥⎢ =⎢ ⎣0 0 1 z⎦⎣ 0 0 0001 0 0 ⎤ ⎡ cos ϕ − sin ϕ 0 r cos ϕ ⎢ sin ϕ cos ϕ 0 r sin ϕ ⎥ ⎥ =⎢ ⎣ 0 0 1 z ⎦ 0 0 0 1

⎤⎡ ⎤ 00 100r ⎢ ⎥ 0 0⎥ ⎥⎢0 1 0 0⎥ ⎦ ⎣ 10 0 0 1 0⎦ 01 0001

(4.153)

As an example, consider a point P at (1, π3 , 2) in a cylindrical coordinate frame. Then, the Cartesian coordinates of P would be ⎤⎡ ⎤ ⎡ ⎤ ⎡ 0 0.5 cos π3 − sin π3 0 cos π3 ⎢ sin π cos π 0 sin π ⎥ ⎢ 0 ⎥ ⎢ 0.866 ⎥ 3 3 3 ⎥⎢ ⎥ = ⎢ ⎥ ⎢ (4.154) ⎣ 0 0 1 2 ⎦ ⎣ 0 ⎦ ⎣ 2.0 ⎦ 0 0 0 1 1 1.0 Example 120 Spherical coordinates. Cartesian–Spherical coordinate system transformation using homogenous transformation method. A set of spherical coordinates, as shown in Fig. 4.16, can be achieved by a translation r along the Z-axis, followed by a rotation θ about the Y -axis, and finally a rotation ϕ about the Z-axis. Therefore, the homogenous transformation matrix to go from spherical coordinates S(Orθϕ) to Cartesian coordinates G(OXY Z) is G

TS = RZ,ϕ RY,θ DZ,r ⎡ ⎤ cos θ cos ϕ − sin ϕ cos ϕ sin θ r cos ϕ sin θ ⎢ cos θ sin ϕ cos ϕ sin θ sin ϕ r sin θ sin ϕ ⎥ ⎥ =⎢ ⎣ − sin θ 0 cos θ r cos θ ⎦ 0 0 0 1

where

⎡

RZ,ϕ

cos ϕ − sin ϕ ⎢ sin ϕ cos ϕ =⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(4.155)

(4.156)

176

4 Motion Kinematics

⎡

cos θ 0 sin θ ⎢ 0 1 0 RY,θ = ⎢ ⎣ − sin θ 0 cos θ 0 0 0 ⎡ ⎤ 1000 ⎢0 1 0 0⎥ ⎥ DZ,r = ⎢ ⎣0 0 1 r ⎦ 0001

⎤ 0 0⎥ ⎥ 0⎦ 1

(4.157)

(4.158)

As an example, consider a point P at (2, π3 , π3 ) in a spherical coordinate frame. Then, the Cartesian coordinates of P would be ⎡ π π ⎤⎡ ⎤ ⎡ ⎤ c 3 c 3 −s π3 c π3 s π3 2c π3 s π3 0 0.866 ⎢ c π s π c π s π s π 2s π s π ⎥ ⎢ 0 ⎥ ⎢ 1.5 ⎥ 3 3 3 3 3 ⎥⎢ ⎥ = ⎢ ⎢ 3 π3 ⎥ (4.159) ⎣ −s 0 c π3 2c π3 ⎦ ⎣ 0 ⎦ ⎣ 1.0 ⎦ 3 0 0 0 1 1 1.0

4.5

 Order-Free Transformation

A serial robotic manipulator with n controllable joints can be moved with any arbitrary order of joint actuations to achieve a desired set of relative joint variables. The final orientation and configuration of the links are independent of the order of activation of the joints. We introduce a theorem and develop order-free transformation matrices to show that under specific conditions, the order of transformations of such robotic multibody is immaterial. Theorem Order-free transformations Consider n connected links B1 , B2 , · · · , Bn such that the link B1 carries the links B2 , B3 , · · · , Bn . The link B1 can move with respect to G with one DOF that can be a rotation θ 1 about a fixed axis in G or a translation d1 along a fixed axis in G. The link B2 carries the links B3 , B4 , · · · , Bn and moves with respect to B1 that can be a rotation θ 2 about a fixed axis in B1 , or a translation d2 along a fixed axis in B1 , and so on. The homogenous transformation matrix G Tn = n TG−1 is independent of the order of motions θ 1 or d1 , θ 2 or d2 , · · · , θ n or dn . Proof Let us consider a global frame G (OXY Z) and two body frames B1 (Ox1 y1 z1 ) and B2 (Ox2 y2 z2 ). The body B1 carries B2 and moves with respect to G, and the body B2 moves in B1 . The transformation 1 TG of G to B1 is a rotation 1 RG plus a translation 1 DG . The only variable of this transformation is either a rotation α or a translation d1 about or along a globally fixed axis G uˆ 1 : 1 TG = 1 DG 1 RG = 1 Dd1 1 Rα (4.160) The second transformation 2 T1 of B1 to B2 is a rotation 2 R1 plus a translation 2 D1 . The only variable of 2 T1 is either a rotation β or a translation d2 about or along a fixed axis 1 uˆ 2 in B1 : 2

T1 = 2 D1 2 R1 = 2 Dd2 2 Rβ = Tβ

(4.161)

To make 1 TG an order-free transformation, we modify the first motion to be about or along a local axis 2 uˆ 2 in B2 . To redefine TG , we move B2 to B1 by a translation −d2 plus a rotation −β, both along and about their associated local axes in B2 . Now the axes of B2 are the same as B1 and 1 TG can be performed by a rotation α about a local axis in B2 plus a translation d1 along a local axis B2 . Then, we apply the inverse rotation β plus an inverse translation d2 about and along their axes to move B2 back to its position. The resultant of these rotations and translations would be an order-free 1 TG : 1

1

TG = 2 Dd2 2 Rβ 2 Dd1 2 Rα 2 R−β 2 D−d2 =

2

T1 TG T1−1 1

2

= Tα

(4.162) (4.163)

The transformation matrices (4.161) and (4.162) are indicating the order-free motions that can be performed in any order to calculate 1 TG . The final transformation matrix will not be altered by changing the order of motions Tα and Tβ . To examine this fact, let us assume that the frames B1 and B2 are on G. Applying the motion Tα and then the motion Tβ provides

4.5  Order-Free Transformation

177

Fig. 4.17 A 2R or RR planar manipulator

2

TG = 2 T1 1 TG = Tβ Tα   = 2 Dd2 2 Rβ 2 Dd2 2 Rβ 1 Dd1 1 Rα 2 R−β 2 D−d2   = 2 Dd2 2 Rβ 2 Dd2 2 Rβ 1 Dd1 1 Rα 2 R−β 2 D−d2   = 2 Dd2 2 Dd2 2 Rβ 1 Dd1 1 Rα 2 D−d2 = 2 Dd2 2 Rβ 1 Dd1 1 Rα

(4.164)

By changing the order of motions and applying first Tβ and then Tα , we find the same transformation matrix: 2

TG = 1 TG 2 T1 = Tα Tβ   = 2 Dd2 2 Rβ 1 Dd1 1 Rα 2 R−β 2 D−d2 2 Dd2 2 Rβ   = 2 Dd2 2 Rβ 1 Dd1 1 Rα I = 2 Dd2 2 Rβ 1 Dd1 1 Rα

(4.165)

Now consider 3 T2 a transformation matrix for motion of B3 in B2 and 2 TG a matrix for motion of B2 in G. Employing the same procedure, we can find the order-free transformation matrix 3 TG as: 3

TG = 3 T2 2 TG = 2 TG 3 T2

(4.166)

We can similarly expand and apply the proof of the theory of order-free transformations to any number of bodies that are similarly connected.  Example 121  Order-free transformations of a planar RR manipulator. Figure 4.17 illustrates an RR planar manipulator with two parallel revolute joints and variables θ 1 and θ 2 . Links (1) and (2) are both RR(0) and their transformation matrices 0 T1 , 1 T2 are ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(4.167)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(4.168)

178

4 Motion Kinematics

Employing 0 T1 and 1 T2 , we can find the forward kinematics 0 T2 only with the assumption that first, the grounded actuator M1 acts to change θ 1 and then, the second motor M2 acts to change θ 2 : 0

T2 = 0 T1 1 T2 ⎤ ⎡ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l2 c (θ 1 + θ 2 ) + l1 cθ 1 ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l2 s (θ 1 + θ 2 ) + l1 sθ 1 ⎥ ⎥ =⎢ ⎦ ⎣ 0 0 1 0 0 0 0 1

(4.169)

However, the actuators M1 and M2 are independent and may act in any order. To find the order-free transformations, we redefine the transformation matrices 0 T1 , 1 T2 to go from lower body to upper body: ⎡

1

T0 = 0 T1−1

⎤ cos θ 1 sin θ 1 0 −l1 ⎢ − sin θ 1 cos θ 1 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1 ⎡

2

T1 = 1 T2−1

⎤ cos θ 2 sin θ 2 0 −l2 ⎢ − sin θ 2 cos θ 2 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(4.170)

(4.171)

Using 0 T1 , 1 T2 , the forward kinematics of the manipulator can be found as 2

T0 = 2 T1 1 T0 = 0 T2−1 ⎤ ⎡ cos (θ 1 + θ 2 ) sin (θ 1 + θ 2 ) 0 −l2 − l1 cos θ 2 ⎥ ⎢ − sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 l1 sin θ 2 ⎥ =⎢ ⎦ ⎣ 0 0 1 0 0 0 0 1

(4.172)

We employ the rule (RF DS) to redefine the homogenous transformation matrices 0 T1 , and 1 T2 . The transformation 2 T1 = D1 2 R1 is made by a rotation θ 2 of B2 about the local axis z2 plus a translation l2 of B2 along the local axis x2 :

2

2

T1 = 2 D1 2 R1 = Dx,l2 Rz,θ 2 ⎤⎡ ⎤ ⎡ 1 0 0 −l2 cos θ 2 sin θ 2 0 0 ⎢ 0 1 0 0 ⎥ ⎢ − sin θ 2 cos θ 2 0 0 ⎥ ⎥⎢ ⎥ = Rz,α = ⎢ ⎣0 0 1 0 ⎦⎣ 0 0 1 0⎦ 000 1 0 0 01

(4.173)

The other transformation 1 T0 = 1 D0 1 R0 is made by a rotation θ 1 of B1 about the local axis z1 plus a translation l1 of B1 along the local axis x1 . To redefine 1 T0 , we move B2 to B1 by a translation l2 along the local axis x2 plus a rotation θ 2 about the local axis z2 . Now the axes of B2 are the same as B1 and 1 T0 can be performed by a rotation θ 1 about the local axis z2 plus a translation l1 along the local axis x2 . Then, we apply the inverse rotation θ 2 about z2 plus an inverse translation l2 along x2 to move B2 back to its position. The resultant of these rotations and translations would be 1 T0 : 1

T0 = Dx,l2 Rz,θ 2 Dx,l1 Rz2 ,θ 1 Rz,−θ 2 Dx,−l2 = 2 T1 1 T0 2 T1−1 ⎡ ⎤ cos θ 1 sin θ 1 0 l2 cos θ 1 − l1 cos θ 2 − l2 ⎢ − sin θ 1 cos θ 1 0 l1 sin θ 2 − l2 sin θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(4.174)

4.5  Order-Free Transformation

179

where

⎡

Dx,−l2

⎤ 1 0 0 l2 ⎢0 1 0 0 ⎥ ⎥ =⎢ ⎣0 0 1 0 ⎦ 0001

(4.175)

⎤ cos −θ 2 sin −θ 2 0 0 ⎢ − sin −θ 2 cos −θ 2 0 0 ⎥ ⎥ Rz,−θ 2 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎤ ⎡ cos θ 1 sin θ 1 0 0 ⎢ − sin θ 1 cos θ 1 0 0 ⎥ ⎥ Rz2 ,θ 1 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 1 0 0 −l1 ⎢0 1 0 0 ⎥ ⎥ Dx,l1 = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎡ ⎤ cos θ 2 sin θ 2 0 0 ⎢ − sin θ 2 cos θ 2 0 0 ⎥ ⎥ Rz,θ 2 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 1 0 0 −l2 ⎢0 1 0 0 ⎥ ⎥ Dx,l2 = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎡

(4.176)

(4.177)

(4.178)

(4.179)

(4.180)

The matrices (4.171) and (4.174) are order free and we can check the result of their product with different orders: 2

T0 = 1 T0 2 T1 ⎤⎡ ⎡ cθ 2 sθ 2 cθ 1 sθ 1 0 l2 cθ 1 − l1 cθ 2 − l2 ⎢ −sθ 1 cθ 1 0 l1 sθ 2 − l2 sθ 1 ⎥ ⎢ −sθ 2 cθ 2 ⎥⎢ =⎢ ⎦⎣ 0 ⎣ 0 0 0 1 0 0 0 0 0 0 1 ⎡ ⎤ c (θ 1 + θ 2 ) s (θ 1 + θ 2 ) 0 −l2 − l1 cθ 2 ⎢ −s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 ⎥ l1 sθ 2 ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0

2

0

0

(4.181) 0 0 1 0

⎤

−l2 0 ⎥ ⎥ 0 ⎦ 1

1

T0 = 1 T0 2 T1 ⎤ ⎤⎡ ⎡ cθ 1 sθ 1 0 0cθ 1 − l1 c0 − 0 cθ 2 sθ 2 0 −l2 ⎢ −sθ 2 cθ 2 0 0 ⎥ ⎢ −sθ 1 cθ 1 0 l1 s0 − 0sθ 1 ⎥ ⎥ ⎥⎢ =⎢ ⎦ ⎣ 0 0 1 0 0 1 0 ⎦⎣ 0 0 0 0 1 0 0 0 1 ⎡ ⎤ c (θ 1 + θ 2 ) s (θ 1 + θ 2 ) 0 −l2 − l1 cθ 2 ⎢ −s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 ⎥ l1 sθ 2 ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(4.182)

180

4 Motion Kinematics

Fig. 4.18 An articulated manipulator

Example 122  Order-free transformations of an articulated manipulator. An articulated manipulator is any serial multibody with three links and three joints to reach a point in three dimensional space. However, an articulated arm is usually referred to a three-link multibody that is similar to a human hand with a shoulder, arm, and forearm such as the one shown in Fig. 4.18. The first link of the arm is an RR(90) with a distance l1 between x0 and x1 , and, therefore, ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 sin θ 1 0 − cos θ 1 1 0 0 0

⎤ 0 0⎥ ⎥ l1 ⎦ 1

⎤ cos θ 1 sin θ 1 0 0 ⎢ 0 0 1 −l1 ⎥ ⎥ =⎢ ⎣ sin θ 1 − cos θ 1 0 0 ⎦ 0 0 0 1

(4.183)

⎡

1

T0 = 0 T1−1

(4.184)

The second link of the arm is an RR(90) with a distance d2 between x1 and x2 and a distance l2 between z1 and z2 : ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ 1 d2 ⎦ 0 1

⎡

2

T1 = 1 T2−1

⎤ cos θ 2 sin θ 2 0 −l2 ⎢ − sin θ 2 cos θ 2 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 −d2 ⎦ 0 0 0 1

(4.185)

(4.186)

The third link is not connected to any other link at its distal end, and we may set B3 at its proximal end and attach a takht frame at its tip point. We may also attach only one coordinate frame at the tip point and consider the link as an RR(90) with a distance d3 between x2 and x3 and a distance l3 between z2 and z3 :

4.5  Order-Free Transformation

181

⎡

cos θ 3 − sin θ 3 ⎢ sin θ 3 cos θ 3 2 T3 = ⎢ ⎣ 0 0 0 0

⎤ 0 l3 cos θ 3 0 l3 sin θ 3 ⎥ ⎥ 1 d3 ⎦ 0 1

(4.187)

⎡

3

T2 = 2 T3−1

⎤ cos θ 3 sin θ 3 0 −l3 ⎢ − sin θ 3 cos θ 3 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 −d3 ⎦ 0 0 0 1

(4.188)

The final to base coordinate frame of the manipulator is 0

T3 = 0 T1 1 T2 2 T3 ⎡ c (θ 2 + θ 3 ) cθ 1 −s (θ 2 + θ 3 ) cθ 1 sθ 1 ⎢ c (θ 2 + θ 3 ) sθ 1 −s (θ 2 + θ 3 ) sθ 1 −cθ 1 =⎢ ⎣ s (θ 2 + θ 3 ) c (θ 2 + θ 3 ) 0 0 0 0

⎡

⎤

⎤

0

(4.189)

dx dy ⎥ ⎥ 0 dz ⎦ 1

0

⎡

⎤

0d (l2 cθ 2 + l3 c (θ 2 + θ 3 )) cθ 1 + (d2 + d3 ) sθ 1 x ⎢0 ⎥ ⎢ ⎥ ⎣ dy ⎦ = ⎣ (l2 cθ 2 + l3 c (θ 2 + θ 3 )) sθ 1 − (d3 + d2 ) cθ 1 ⎦ 0d l1 + l2 sθ 2 + l3 s (θ 2 + θ 3 ) z

(4.190)

or 3

T0 = 3 T2 2 T1 1 T0 ⎡

c (θ 2 + θ 3 ) cθ 1 c (θ 2 + θ 3 ) sθ 1 s (θ 2 + θ 3 ) ⎢ −s (θ + θ ) cθ −s (θ + θ ) sθ c (θ + θ ) ⎢ 2 3 1 2 3 1 2 3 =⎢ ⎣ sθ 1 −cθ 1 0 0 0 0

(4.191) 3d x 3d y 3d z

⎤ ⎥ ⎥ ⎥ ⎦

1

⎡3

⎤ ⎡ ⎤ dx −l1 s (θ 2 + θ 3 ) − l2 cθ 3 − l3 ⎣ 3 dy ⎦ = ⎣ −l1 c (θ 2 + θ 3 ) + l2 sθ 3 ⎦ 3 dz −d2 − d3

(4.192)

To determine the order-free transformations 3 T2 , 2 T1 , 1 T0 , we begin with the final coordinate frame B3 . The transformation 3 T2 is already a motion about and along the axes of B3 . The order-free transformation of the second matrix is T1 = 3 T2 2 T1 3 T2−1 = 3 T2 2 T1 2 T3 ⎡ ⎤ cos θ 2 sin θ 2 0 l3 cos θ 2 − l2 cos θ 3 − l3 ⎢ − sin θ 2 cos θ 2 0 l2 sin θ 3 − l3 sin θ 2 ⎥ ⎥ =⎢ ⎦ ⎣ 0 0 1 −d2 0 0 0 1

(4.193)

T1 = 3 T2 2 T1 = 2 T1 3 T2 ⎡ ⎤ cos (θ 2 + θ 3 ) sin (θ 2 + θ 3 ) 0 −l3 − l2 cos θ 3 ⎢ − sin (θ 2 + θ 3 ) cos (θ 2 + θ 3 ) 0 ⎥ l2 sin θ 3 ⎥ =⎢ ⎣ 0 0 1 −d2 − d3 ⎦ 0 0 0 1

(4.194)

2

We may check to see that: 3

182

4 Motion Kinematics

However, when 2 T1 is the first motion, we must substitute the parameters l3 and θ 3 of the second motion equal to zero. The order-free transformation of the third matrix is 1

T0 = 3 T2 2 T1 1 T0 2 T1−1 3 T2−1 = 3 T2 2 T1 1 T0 1 T2 2 T3 ⎡

sγ + sθ 1 2s (θ 2 + θ 3 ) cγ + cθ 1 1⎢ sγ − sθ 1 cθ 1 − cγ 2c (θ 2 + θ 3 ) ⎢ = ⎢ 2 ⎣ 2s (θ 1 − θ 2 − θ 3 ) −2c (θ 1 − θ 2 − θ 3 ) 0 0 0 0

(4.195) 1d x 1d y 1d z

⎤ ⎥ ⎥ ⎥ ⎦

1

γ = θ 1 − 2θ 2 − 2θ 3

(4.196)

where 1

dx = 2 (d2 + d3 − l1 ) sin (θ 2 + θ 3 ) + l3 cos θ 1 − 2l2 cos θ 3 − 2l3 + (l2 + l3 ) cos (θ 1 − 2θ 2 − 2θ 3 ) + l2 cos (θ 1 + θ 3 )

1

(4.197)

dy = 2 (d2 + d3 − l1 ) cos (θ 2 + θ 3 ) − l3 sin θ 1 + 2l2 sin θ 3 + (l2 + l3 ) sin (θ 1 − 2θ 2 − 2θ 3 ) − l2 sin (θ 1 + θ 3 )

1

(4.198)

dz = 2 (l3 sin (θ 1 − θ 2 − θ 3 ) − d3 − d2 + l2 sin (θ 1 − θ 2 ))

(4.199) We may check to see that the transformation 3 T0 = 3 T2 2 T1 1 T0 can be found by multiplying 3 T2 , 2 T1 , 1 T0 in any order: 3

T0 = 3 T2 2 T1 1 T0 = 2 T1 3 T2 1 T0 = 3 T2 1 T0 2 T1 = 1 T0 3 T2 2 T1 = 2 T1 1 T0 3 T2 = 1 T0 2 T1 3 T2

(4.200)

It indicates that we can run the three motors of the manipulator in any arbitrary order. As long as the associated angles of rotation of the motors remain the same, the final configuration of the manipulator would be the same.

4.6

 Screw Coordinates

Any rigid body motion can be replaced by a single translation along an axis combined with a unique rotation about that axis. Such a motion is called screw motion. Consider the screw motion illustrated in Fig. 4.19. Screw is made by motion of a point P that rotates about the screw axis uˆ and simultaneously translates along the same axis u. ˆ Hence, any point on the screw axis moves along the axis, while any point off the axis moves along a helix. The angular rotation of the rigid body about the screw is called twist. A screw motion is indicated by its pitch, p, that is the ratio of translation, h, to rotation, φ. h (4.201) p= φ Hence, the rectilinear distance h through which the rigid body translates parallel to the axis of screw uˆ for a unit rotation φ is the pitch p. If p > 0, then the screw is right-handed, and if p < 0, it is left-handed. A screw motion sˇ is shown by sˇ (h, φ, u, ˆ s) and is indicated by a twist axis unit vector u, ˆ a location vector s, a twist angle φ, and a translation h (or a pitch p). The location vector s indicates the global position of a point on the screw axis. The twist angle φ, the twist axis u, ˆ and the pitch p (or translation h) are called screw parameters. The screw motion is another transformation method to present a general motion of a rigid body. Screw motion may be expressed by homogenous matrix screw transformation, which is a combination of rotation and translation about the screw axis.

4.6  Screw Coordinates

183

Fig. 4.19 A screw motion is translation along a line combined with a rotation about the line

If uˆ passes through the origin of the coordinate frame, then s = 0 and the screw motion is called central screw sˇ (h, φ, u). ˆ A reverse central screw is defined as sˇ (−h, −φ, u). ˆ A translation displacement Du,h along an axis u, ˆ combined with an ˆ angular rotation Ru,φ about the same axis appears repeatedly in robotic application. For a central screw motion, we have ˆ G

sˇB (h, φ, u) ˆ = Du,h Ru,φ ˆ ˆ

where

⎡

Du,h ˆ

1 ⎢0 ⎢ =⎢ ⎣0 0

0 1 0 0

0 0 1 0

(4.202)

⎤ hu1 hu2 ⎥ ⎥ ⎥ hu3 ⎦

(4.203)

1 Ru,φ ˆ = ⎡ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ ⎢ u u vers φ + u sφ u2 vers φ + cφ u u vers φ − u sφ ⎢ 1 2 3 2 3 1 2 ⎢ ⎣ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ 0 0 0

⎤ 0 0⎥ ⎥ ⎥ 0⎦

(4.204)

1

and, hence, G sˇ (h, φ, u) ˆ = B ⎡ 2 u1 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ ⎢ u u vers φ + u sφ u2 vers φ + cφ u u vers φ − u sφ ⎢ 1 2 3 2 3 1 2 ⎢ ⎣ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ 0 0 0

⎤ hu1 hu2 ⎥ ⎥ ⎥ hu3 ⎦

(4.205)

1

As a result, a central screw transformation matrix indicates a pure translation corresponds to φ = 0, and a pure rotation corresponds to h = 0 (or p = ∞). A general screw motion is expressed by a homogenous transformation matrix G TB , G

TB = =

G

sˇB (h, φ, u, ˆ s) =

"G

RB G d 0 1

"G

RB 0

G

s−

G

RB G s + huˆ 1

#

# (4.206)

where G

RB = I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ    G d = I − uˆ uˆ T (1 − cos φ) − u˜ sin φ G s + huˆ

(4.207) (4.208)

184

4 Motion Kinematics

Fig. 4.20 Screw motion of a rigid body

When the screw motion is not central and uˆ is not passing through the origin, and a screw motion to move a position vector p to p is denoted by   p = (p − s) cos φ + (1 − cos φ) uˆ · (p − s) uˆ   + uˆ × (p − s) sin φ + s + huˆ

(4.209)

=

G

RB (p − s) + s + huˆ

(4.210)

=

G

RB p + s −

(4.211)

G

RB s + huˆ

and, therefore, p = sˇ (h, φ, u, ˆ s) p = G

G

p = sˇ (h, φ, u, ˆ s) p = B

TB p

(4.212)

G

(4.213)

TB p

where G TB is the screw motion homogenous transformation matrix of Eq. (4.206). The location vector G s is the global position vector of the body frame B before screw motion. The vectors p and p are positions of a point P after and before screw motion, as are shown in Fig. 4.20. The screw axis is indicated by the unit vector u. ˆ Now a body point P moves from its first position to its second position P  by a rotation about u. ˆ Then it moves to P  by a translation h parallel to u. ˆ The initial position of P is pointed by p and its final position is pointed by p . Proof Let us break the screw motion to a twist φ about uˆ and a translation h along u. ˆ A body point P moves from its first position at p to its second position P  at p by a rotation about u. ˆ Then it moves to P  at p by a translation h parallel to u. ˆ The angle–axis rotation formula (3.5) relates r and r, which are position vectors of P after and before rotation φ about uˆ when s = 0, h = 0.     r = r cos φ + (1 − cos φ) uˆ · r uˆ + uˆ × r sin φ (4.214) However, when the screw axis does not pass through the origin of G(OXY Z), then r and r must accordingly be substituted with the following equations: r = p−s

(4.215)

r = p − s − huˆ

(4.216)

where r is a vector after rotation and hence in G coordinate frame, and r is a vector before rotation and hence in B coordinate frame.

4.6  Screw Coordinates

185

Therefore, the relationship between the new and old positions of the body point P after a screw motion is   p = (p − s) cos φ + (1 − cos φ) uˆ · (p − s) uˆ   + uˆ × (p − s) sin φ + (s + hu) ˆ

(4.217)

Equation (4.217) is the Rodriguez formula for the most general rigid body motion. Defining new notations G p = p and p = p and also noting that s indicates a point on the rotation axis and therefore rotation does not affect s, we may factor out B p and write the Rodriguez formula in the following form:

B

G

  p = I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ B p   − I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ G s +

G

s + huˆ

(4.218)

which can be rearranged to show that a screw can be represented by a homogenous transformation G

G

p=

G

=

G

RB B p +

G

s−

G

RB G s + huˆ =

G

RB B p +

G

d

B

TB p

(4.219)

TB =

G sˇB (h, φ, u, ˆ s) # "G # "G G RB s − G RB G s + huˆ RB G d = = 0 1 0 1

(4.220)

RB = I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ    G d = I − uˆ uˆ T (1 − cos φ) − u˜ sin φ G s + huˆ

(4.221)

where G

(4.222)

Direct substitution shows that ⎡

⎤ u21 vers φ + cφ u1 u2 vers φ − u3 sφ u1 u3 vers φ + u2 sφ G RB = ⎣ u1 u2 vers φ + u3 sφ u22 vers φ + cφ u2 u3 vers φ − u1 sφ ⎦ u1 u3 vers φ − u2 sφ u2 u3 vers φ + u1 sφ u23 vers φ + cφ

(4.223)

⎤ ⎡d = hu1 − u1 (s3 u3 + s2 u2 + s1 u1 ) vers φ + (s2 u3 − s3 u2 ) sφ + s1 vers φ ⎣ hu2 − u2 (s3 u3 + s2 u2 + s1 u1 ) vers φ + (s3 u1 − s1 u3 ) sφ + s2 vers φ ⎦ hu3 − u3 (s3 u3 + s2 u2 + s1 u1 ) vers φ + (s1 u2 − s2 u1 ) sφ + s3 vers φ

(4.224)

G

This representation of a rigid motion requires six independent parameters, namely one for rotation angle φ, one for translation h, two for screw axis u, ˆ and two for location vector G s. It is because three components of uˆ are related to each other according to uˆ T uˆ = 1 (4.225) and the location vector G s can locate any arbitrary point on the screw axis. It is convenient to choose the point where it has the minimum distance from O to make G s perpendicular to u. ˆ Let us indicate the shortest location vector by G s0 , then there is a constraint among the components of the location vector. G T s0

uˆ = 0

(4.226)

If s = 0, then the screw axis passes through the origin of G and (4.220) reduces to (4.205). The screw parameters φ and h, together with the screw axis uˆ and location vector G s, completely define a rigid motion of B(oxyz) in G(OXY Z). It means, given the screw parameters and screw axis, we can find the elements of the transformation

186

4 Motion Kinematics

matrix by Eqs. (4.223) and (4.224). On the other hand, given the transformation matrix G TB , we can find the screw angle and axis by  1  G   1  G  tr RB − 1 = tr TB − 2 2 2 1 = (r11 + r22 + r33 − 1) 2

cos φ =

u˜ = hence,

1 G RB − 2 sin φ

G

RBT



⎡ ⎤ r − r23 1 ⎣ 32 uˆ = r13 − r31 ⎦ 2 sin φ r21 − r12

(4.227)

(4.228)

(4.229)

To find all the required screw parameters, we must also find h and coordinates of one point on the screw axis. Since the points on the screw axis are invariant under the rotation, we must have ⎡

r11 ⎢ r21 ⎢ ⎣ r31 0

r12 r22 r32 0

r13 r23 r33 0

⎤⎡ ⎤ ⎡ ⎤⎡ ⎤ 1 0 0 hu1 r14 X X ⎢ Y ⎥ ⎢ 0 1 0 hu2 ⎥ ⎢ Y ⎥ r24 ⎥ ⎥⎢ ⎥ = ⎢ ⎥⎢ ⎥ r34 ⎦ ⎣ Z ⎦ ⎣ 0 0 1 hu3 ⎦ ⎣ Z ⎦ 1 1 000 1 1

(4.230)

where (X, Y, Z) are coordinates of points on the screw axis. As a sample point, we may find the intersection point of the screw line with (Y, Z)-plane, by setting Xs = 0 and searching  T for s = 0 Ys Zs . Therefore, ⎡

⎤⎡ ⎤ ⎡ ⎤ 0 r11 − 1 r12 r13 r14 − hu1 0 ⎢ r21 r22 − 1 r23 r24 − hu2 ⎥ ⎢ Ys ⎥ ⎢ 0 ⎥ ⎥⎢ ⎥ = ⎢ ⎥ ⎢ ⎣ r31 r32 r33 − 1 r34 − hu3 ⎦ ⎣ Zs ⎦ ⎣ 0 ⎦ 0 0 0 0 1 0

(4.231)

which generates three equations to be solved for Ys , Zs , and h. ⎡

⎤ ⎡ ⎤−1 ⎡ ⎤ h u1 −r12 −r13 r14 ⎣ Ys ⎦ = ⎣ u2 1 − r22 −r23 ⎦ ⎣ r24 ⎦ Zs u3 −r32 1 − r33 r34

(4.232)

Now we can find the shortest location vector G s0 by G

s0 = s − (s · u) ˆ uˆ

(4.233)

A screw motion sˇ (h, φ, u, ˆ s) has a line of action uˆ at G s, a twist φ, and a translation h. The screw motion was first used by the Italian mathematician, Giulio Mozzi (1730–1813) in 1763 although the French mathematician Michel Chasles (1793–1880) is credited for this discovery because of his publication in 1830. There are three principal central screws, namely the X-screw, Y -screw, Z-screw, respectively, having pitches pX , pY , pZ , and twist angles γ , β, α, which are ⎡ ⎤ cos α − sin α 0 0 ⎢ ⎥ ˆ = ⎢ sin α cos α 0 0 ⎥ sˇ (hZ , α, K) (4.234) ⎣ 0 0 1 pZ α ⎦ 0 0 0 1

4.6  Screw Coordinates

187

⎡

cos β ⎢ 0 sˇ (hY , β, Jˆ) = ⎢ ⎣ − sin β 0 ⎡ 1 0 ⎢ 0 cos γ sˇ (hX , γ , Iˆ) = ⎢ ⎣ 0 sin γ 0 0

⎤ 0 sin β 0 1 0 pY β ⎥ ⎥ 0 cos β 0 ⎦ 0 0 1 ⎤ 0 pX γ − sin γ 0 ⎥ ⎥ cos γ 0 ⎦ 0 1

(4.235)

(4.236) 

Example 123  Euler–Chasles theorem. Definition and proof of Euler-Chasles theorem about rigid body motions. Rigid body motion is well expressed by Euler and Michel Chasles (1793–1880) in a theorem. Theorem 124 A rigid body can be displaced from an initial position to another position by means of one translation and one rotation. Proof Let P1 be a point of the rigid body B in its initial position B1 , and P2 the corresponding point in the final position B2 . Let us first translate the body to an intermediate position B  so that P1 falls on P2 . If the position B  is identical with B2 , then we have displaced the body by means of one translation, conformably to the requirements of the theorem. Let us assume that the position B  is different than B2 . Because the positions B  and B2 of the rigid body have a common point P2 , then we can displace B  to B2 by a rotation about an axis passing through P2 . If we choose a different P1 , then in general we will have a different translation and a different rotation about a different axis.  Expression of rigid body motion by screw motion may have any of the following expressions: Every rigid body motion can be realized by a rotation about an axis combined with a translation parallel to the same axis. The most general rigid body displacement can be produced by a translation along a line followed by a rotation about that line. The displacement in the Euclidean space of a rigid body can be represented by a rotation along a unique axis and a translation parallel to the axis. Example 125  Alternative proof of Euler-Chasles theorem. Another proof for Euler-Chasles theorem on rigid body motions, using homogenous transformation matrix. Let [T ] be an arbitrary spatial displacement, and decompose it into a rotation R about uˆ and a translation D. [T ] = [D][R]

(4.237)

We may also decompose the translation [D] into two components [D ] and [D⊥ ], parallel and perpendicular to u, ˆ respectively. [T ] = [D ][D⊥ ][R]

(4.238)

Now [D⊥ ][R] is a planar motion and is therefore equivalent to some rotation [R  ] = [D⊥ ][R] about an axis parallel to the rotation axis u. ˆ This yields the decomposition [T ] = [D ][R  ]. This decomposition completes the proof, since the axis of [D ] can be taken equal to u. ˆ Example 126  Poinsot theorem. Definition of Poinsot theorem about rigid body motion to be equivalent to screw motion. Similar to the screw motion of kinematics of rigid body motions, there is a theorem about dynamics of rigid bodies, called Poinsot theorem. Theorem 127 Rigid body action is equivalent to a wrench on a screw, that is, a force along a unique line and a couple parallel to the line.

188

4 Motion Kinematics

Example 128  Central screw transformation of a base unit vector. Numerical example for a central screw motion of a unit vector. Consider two initially coincident frames G(OXY Z) and B(oxyz). The body performs a screw motion along the Y -axis  T for h = 2 cm and φ = 90 deg. The position of a body point at 1 0 0 1 can be found by applying the central screw transformation. π ˆ π , J ) = D(2Jˆ) R(Jˆ, ) 2 2 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 1000 0 010 0 010 ⎢0 1 0 2⎥⎢ 0 1 0 0⎥ ⎢ 0 1 0 2⎥ ⎥⎢ ⎥ ⎢ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ ⎣ −1 0 0 0 ⎦ = ⎣ −1 0 0 0 ⎦ 0001 0 001 0 001

sˇ (h, φ, u) ˆ = sˇ (2,

(4.239)

Therefore, G

π ˆ B , J ) ıˆ 2 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 010 1 0 ⎢ 0 1 0 2⎥⎢0⎥ ⎢ 2 ⎥ ⎥⎢ ⎥ ⎢ ⎥ =⎢ ⎣ −1 0 0 0 ⎦ ⎣ 0 ⎦ = ⎣ −1 ⎦ 0 001 1 1

ıˆ = sˇ (2,

(4.240)

The pitch of this screw is p=

h 2 4 = = = 1.2732 cm/rad φ π /2 π

(4.241)

Example 129  Screw transformation of a point. Numerical example for a screw motion of a point of a rigid body. Consider two initially parallel frames G(OXY Z) and B(oxyz). The body performs a screw motion along X = 2 and  T parallel to the Y -axis for h = 2 and φ = 90 deg. Therefore, the body coordinate frame is at location s = 2 0 0 . The  T position of a body point at B r = 3 0 0 1 can be found by applying the screw transformation, which is

G

because

TB =

"G

⎤ 0 012 ⎢ 0 1 0 2⎥ RB s − G RB s + huˆ ⎥ =⎢ ⎣ −1 0 0 2 ⎦ 0 1 0 001 #

⎡

⎤ 0 01 G RB = ⎣ 0 1 0 ⎦ −1 0 0

⎡ ⎤ 2 s = ⎣0⎦ 0

⎡

⎡ ⎤ 0 uˆ = ⎣ 1 ⎦ 0

(4.242)

(4.243)

Therefore, the position vector of G r would be G

r=

G

⎡

TB B r

⎤⎡ ⎤ ⎡ ⎤ 0 012 3 2 ⎢ 0 1 0 2⎥⎢0⎥ ⎢ 2 ⎥ ⎥⎢ ⎥ ⎢ ⎥ =⎢ ⎣ −1 0 0 2 ⎦ ⎣ 0 ⎦ = ⎣ −1 ⎦ 0 001 1 1

(4.244)

Example 130  The screw motion of a cube. Numerical example for a screw motion of a rigid body and calculation coordinates of important points. Consider the cubic rigid body of Fig. 4.21 that has a unit length and is at the corner of the first quadrant. If we turn the cube 45 deg about u and translating it by G d = hu, ˆ

4.6  Screw Coordinates

189

Fig. 4.21 A cubic rigid body at the corner of the first quadrant

 T u= 111  T huˆ = 1 1 1

(4.245) (4.246)

then the associated central screw motion would be G

TB =

sˇB (h, φ, u) ˆ "G # "G # RB huˆ RB G d = = 0 1 0 1 G

(4.247)

where G RB is G

RB = I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ ⎡ ⎤ 0.804 74 −0.310 62 0.505 88 = ⎣ 0.505 88 0.804 74 −0.310 62 ⎦ −0.310 62 0.505 88 0.804 74 π φ= 4

⎡ ⎤ 0.577 35 u uˆ = √ = ⎣ 0.577 35 ⎦ 3 0.577 35

(4.248)

(4.249)

Therefore, the central screw homogenous transformation is G

TB =

G

⎡

sˇB (h, φ, u) ˆ =

⎤ 0.804 74 −0.310 62 0.505 88 1 ⎢ 0.505 88 0.804 74 −0.310 62 1 ⎥ ⎥ =⎢ ⎣ −0.310 62 0.505 88 0.804 74 1 ⎦ 0 0 0 1

(4.250)

Figure 4.22 depicts the cube after the central screw G sˇB (h, φ, u). ˆ Now suppose we would like to turn the cube about u and translate it by huˆ = u where u is at: G

 T s= 100

The screw of this motion has the same rotation matrix G RB of (4.248) with a new translation vector G d.

(4.251)

190

4 Motion Kinematics

Fig. 4.22 A unit cube after a central screw G sˇB (h, φ, u), ˆ where h = 1, φ =

G

d=

π , uˆ = 4

s − G RB G s + huˆ    = I − uˆ uˆ T (1 − cos φ) − u˜ sin φ G s + huˆ ⎡ ⎤ 1.1953 = ⎣ 0.4941 ⎦ 1.3106

√1 3



111

T

G

(4.252)

It provides the following central screw transformation. G

"G

RB G s + huˆ TB = sˇB (h, φ, u, ˆ s) = 1 ⎤ ⎡ 0.8047 −0.3106 0.5059 1.1953 ⎢ 0.5059 0.8047 −0.3106 0.4941 ⎥ ⎥ =⎢ ⎣ −0.3106 0.5059 0.8047 1.3106 ⎦ 0 0 0 1 G

RB 0

G

s−

G

#

(4.253)

The local coordinates of the corners of the upper face are B

rA x 0 y 0 z 0 which after the screw motion will be at G r = B

rA X 1.195 Y 0.494 Z 1.311

B

G

B

rB B rC 1 1 0 1 0 0

B

rD 0 1 0

B

rE 0 0 1

B

rF 1 0 1

B

rG 1 1 1

B

rH 0 1 1

(4.254)

sˇB (h, φ, u, ˆ s) B r:

rB B rC B rD G rE G rF G rG G rH 2 1.689 0.885 1.701 2.506 2.195 1.390 1 1.805 1.299 0.183 0.689 1.494 0.988 1 1.506 1.816 2.115 1.804 2.311 2.62

(4.255)

4.6  Screw Coordinates

191

Fig. 4.23 A unit cube after a screw motion G sˇB (h, φ, u), ˆ where h = 1, φ =

π , uˆ = 4

√1 3



111

T

 T ,s= 1 0 0

because: ⎡

⎤ 0.804 74 −0.310 62 0.505 88 1.1953 ⎢ 0.505 88 0.804 74 −0.310 62 0.49412 ⎥ ⎥ ⎢ ⎣ −0.310 62 0.505 88 0.804 74 1.3106 ⎦ 0 0 0 1 ⎡ ⎤ 01100110 ⎢0 0 1 1 0 0 1 1⎥ ⎥ ×⎢ ⎣0 0 0 0 1 1 1 1⎦ 11111111 ⎤ ⎡ 1.195 2.0 1.689 0.885 1.701 2.506 2.195 1.391 ⎢ 0.494 1 1.805 1.299 0.183 0.689 1.494 0.988 ⎥ ⎥ =⎢ ⎣ 1.311 1 1.506 1.816 2.115 1.805 2.312 2.621 ⎦ 1 1 1 1 1 1 1 1

(4.256)

Figure 4.23 depicts the cube after the screw motion G sˇB (h, φ, u, ˆ s). Example 131  Rotation of a vector. Applying rigid body motion on two pints of a rigid body. Transformation equation G r = G RB B r and Rodriguez rotation formula (3.5) describe the rotation of any vector fixed in a rigid body. However, the vector can conveniently be described in terms of two points fixed in the body to derive screw equation. A reference point P1 with position vector r1 at the tail and a point P2 with position vector r2 at the head define a vector in the rigid body. Then the transformation equation between body and global frames can be written as: G

(r2 − r1 ) =

G

RB

B

(r2 − r1 )

(4.257)

Assume the original and final positions of the reference point P1 are along the rotation axis. Equation (4.257) can then be rearranged in a form suitable for calculating coordinates of the new position of point P2 in a transformation matrix form G

r2 =

G

RB

B

(r2 − r1 ) +

G

=

G

RB

B

r2 +

G

=

G

TB

B

r2

G

r1 −

(4.258)

r1

RB

B

r1

192

4 Motion Kinematics

Fig. 4.24 Motion in a plane

where G

TB =

"G

RB 0

G

r1 −

G

RB 1

B

r1

# (4.259)

It is compatible with screw motion (4.220) for h = 0. Example 132  Special cases for screw determination. Screw axis for special cases of φ = 0 and φ = π. There are two special cases for screws. The first one occurs when r11 = r22 = r33 = 1, then, φ = 0 and the motion is a pure translation h parallel to u, ˆ where uˆ =

r14 − s1 ˆ r24 − s2 ˆ r34 − s3 ˆ I+ J+ K h h h

(4.260)

As there is no unique screw axis in this case, we cannot locate any specific point on the screw axis. The second special case occurs when φ = 180 deg. In this case ⎡

1 ⎢2

(r11 + 1)

⎤

⎥ ⎢ ⎥ uˆ = ⎢ 21 (r22 + 1) ⎥ ⎣ ⎦ 1 + 1) (r 33 2

(4.261)

however, h and (X, Y, Z) can again be calculated from (4.232). Example 133  Pole of planar rotation and translation. Any planar rigid body motion may be interpreted by rotation about a point called pole. How to locate pole of a motion. Assume a flat planar shape in (X, Y )-plane is displaced from position 1 to position 2 according to Fig. 4.24. New coordinates of point Q2 are   rQ2 = 2 R1 rQ1 − rP1 + rP2 ⎡ ⎤ ⎛⎡ ⎤ ⎡ ⎤⎞ ⎡ ⎤ cos 58 − sin 58 0 3 1 4 = ⎣ sin 58 cos 58 0 ⎦ ⎝⎣ 1 ⎦ − ⎣ 1 ⎦⎠ + ⎣ 1.5 ⎦ 0 0 1 0 0 0 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1.06 4 5.06 = ⎣ 1.696 ⎦ + ⎣ 1.5 ⎦ = ⎣ 3.196 ⎦ 0 0 0.0

(4.262)

4.6  Screw Coordinates

193

or equivalently rQ2

# R1 rP2 − 2 R1 rP1 = T1 rQ1 = rQ1 0 1 ⎤ ⎡ ⎤⎡ ⎤ ⎡ cos 58 − sin 58 0 4.318 3 5.06 ⎢ sin 58 cos 58 0 0.122 ⎥ ⎢ 1 ⎥ ⎢ 3.196 ⎥ ⎥⎢ ⎥ = ⎢ ⎥ =⎢ ⎣ 0 0 1 0 ⎦⎣0⎦ ⎣ 0 ⎦ 0 0 0 1 1 1 "2

2

(4.263)

In the planar motion of a rigid body, going from position 1 to position 2, there is always one point in the plane of motion that does not change its location. Hence, the body can be considered as having rotated about that point. The point is known as the finite rotation pole. The transformation matrix can be used to locate the pole. Figure 4.24 depicts a planar motion of a triangle. To locate the pole of motion, P0 (X0 , Y0 ), we need the transformation matrix of the motion. Using the data given in Fig. 4.24, we have 2

T1 =

"2

R1 rP2 − 2 R1 rP1 0 1

⎡

cos α − sin α ⎢ sin α cos α =⎢ ⎣ 0 0 0 0

# (4.264)

⎤ 0 − cos α + sin α + 4 0 − cos α − sin α + 1.5 ⎥ ⎥ ⎦ 1 0 0 1

The location of the pole would be conserved under the transformation. Therefore, rP0 = 2 T1 rP0 ⎡ ⎤ ⎡ cos α − sin α X0 ⎢ Y0 ⎥ ⎢ sin α cos α ⎢ ⎥=⎢ ⎣ 0 ⎦ ⎣ 0 0 1 0 0

⎤⎡

⎤

(4.265)

0 − cos α + sin α + 4 X0 ⎥ ⎢ 0 − cos α − sin α + 1.5 ⎥ ⎢ Y0 ⎥ ⎥ ⎦⎣ 0 ⎦ 1 0 0 1 1

which for α = 58 deg provides X0 = −1.5 sin α + 1 − 4 cos α = 2.049

(4.266)

Y0 = 4 sin α + 1 − 1.5 cos α = 3.956

(4.267)

The location of the pole P0 (X0 , Y0 ) is indicated in Fig. 4.24. Example 134  Determination of screw parameters. How to determine screw parameters when motion of three points are given. We are able to determine screw parameters when we have the original and final positions of three non-colinear points of a rigid body. Assume p0 , q0 , r0 denote the position of points P , Q, R before the screw motion, and p1 , q1 , and r1 denote their positions after the screw motion. To determine screw parameters, φ, u, ˆ h, s, we should solve the following three simultaneous Rodriguez equations: φ uˆ × (p1 + p0 − 2s) + huˆ 2 φ q1 − q0 = tan uˆ × (q1 + q0 − 2s) + huˆ 2 φ r1 − r0 = tan uˆ × (r1 + r0 − 2s) + huˆ 2

p1 − p0 = tan

(4.268) (4.269) (4.270)

194

4 Motion Kinematics

Let begin by subtracting Eq. (4.270) from (4.268) and (4.269). φ uˆ × [(p1 + p0 ) − (r1 − r0 )] 2 φ (q1 − q0 ) − (r1 − r0 ) = tan uˆ × [(q1 + q0 ) − (r1 − r0 )] 2

(p1 − p0 ) − (r1 − r0 ) = tan

(4.271)

(4.272) Multiplying both sides of (4.271) by [(q1 − q0 ) − (r1 − r0 )], which is perpendicular to uˆ [(q1 − q0 ) − (r1 − r0 )] × [(p1 − p0 ) − (r1 − r0 )] ! φ = tan [(q1 − q0 ) − (r1 − r0 )] × uˆ × [(p1 + p0 ) − (r1 − r0 )] 2

(4.273)

[(q1 − q0 ) − (r1 − r0 )] × [(p1 + p0 ) − (r1 − r0 )] φ = tan [(q1 − q0 ) − (r1 − r0 )] · [(p1 + p0 ) − (r1 − r0 )] uˆ 2

(4.274)

gives us

and, therefore, the rotation angle can be found by equating tan φ2 and the norm of the right-hand side of the following equation: tan

φ [(q1 − q0 ) − (r1 − r0 )] × [(p1 + p0 ) − (r1 − r0 )] uˆ = 2 [(q1 − q0 ) − (r1 − r0 )] · [(p1 + p0 ) − (r1 − r0 )]

(4.275)

To find s, we may start with the cross product of uˆ with Eq. (4.268). # " φ uˆ × (p1 − p0 ) = uˆ × tan uˆ × (p1 + p0 − 2s) + huˆ 2  φ  = tan uˆ · (p1 + p0 ) uˆ − (p1 + p0 ) 2    ! +2 s − uˆ · s uˆ

(4.276)

  The term s − uˆ · s uˆ is the component of s perpendicular to u, ˆ where s is a vector from the origin of the global frame G(OXY Z) to an arbitrary point on the screw axis. This perpendicular component indicates a vector with the shortest distance between O and u. ˆ Let us assume s0 is the name of the shortest s. Therefore,   s0 = s − uˆ · s uˆ    1 uˆ × p1 − p0  = − uˆ · (p1 + p0 ) uˆ + p1 + p0 2 tan φ2

(4.277)

The last parameter of the screw is the pitch h, which can be found from any one of the Eqs. (4.268), (4.269), or (4.270). h = uˆ · (p1 − p0 ) = uˆ · (q1 − q0 ) = uˆ · (r1 − r0 )

(4.278)

Example 135  Alternative derivation of screw transformation. Another proof of screw motion homogenous transformation expression. Assume the screw axis does not pass through the origin of G. If G s is the position vector of some point on the axis u, ˆ then we can derive the matrix representation of screw sˇ (h, φ, u, ˆ s) by translating the screw axis back to the origin, performing the central screw motion, and translating the line back to its original position. This is how a general and a central screw are related.

4.6  Screw Coordinates

195

sˇ (h, φ, u, ˆ s) = D(G s) sˇ (h, φ, u) ˆ D(− G s) = D(G s) D(hu) ˆ R(u, ˆ φ) D(− G s) " G #"G #" # I s RB huˆ I − Gs = 0 1 0 1 0 1 # "G RB G s − G RB G s + huˆ = 0 1

(4.279)

Example 136  Rotation about an off-center axis. Screw transformation for rotation about a non-central axis. Rotation of a rigid body about an axis indicated by uˆ and passing through a point at G s, where G s×uˆ = 0, is a rotation about an off-center axis. The transformation matrix associated to an off-center rotation can be obtained from the screw transformation by setting h = 0. Therefore, an off-center rotation transformation is G

TB =

"G

RB 0

G

s−

G

RB G s

# (4.280)

1

Example 137  Every rigid motion is a screw. An illustrative proof of rigid body motion to be expressed by screw motion. To show that any proper rigid motion can be considered as a screw motion, we must show that a homogenous transformation matrix # "G RB G d G (4.281) TB = 0 1 can be written in the form of an screw motion transformation. "G RB (I − G TB = 0

G

RB ) s + huˆ 1

# (4.282)

This problem is then equivalent to the following equation to find h and u. ˆ G

d = (I −

G

RB ) s + huˆ

(4.283)

The matrix [I− G RB ] is singular because G RB always has 1 as an eigenvalue. This eigenvalue corresponds to uˆ as eigenvector. Therefore, [I − G RB ]uˆ = [I − G RBT ]uˆ = 0 (4.284) and an inner product shows that uˆ ·

G

  d = uˆ · I − G RB s + uˆ · huˆ   = I − G RB uˆ · s + uˆ · huˆ

(4.285)

which leads to: h = uˆ · Now we may use h to find s.

 s= I−

G

RB

G

−1

d

(4.286)

(G d − hu) ˆ

(4.287)

Example 138  Classification of motions of a rigid body. This is a summary of all possible type of rigid body motions and how to express them. Imagine a body coordinate frame B(oxyz) is moving with respect to a global frame G(OXY Z). Point P with position  T vector pB = p1 p2 p3 is an arbitrary point in B(oxyz). The possible motions of B(oxyz) and the required transformation between frames can be classified as:

196

4 Motion Kinematics

 T 1. Rotation φ about an axis passing through the origin and indicating by the unit vector uˆ = u1 u2 u3 G

p=

G

RB B p

(4.288)

where G RB comes from the Rodriguez rotation formula (3.5).  T 2. Rotation G RB plus a translation by G d = d1 d2 d3 . G

p=

G

RB

B

p+

G

(4.289)

d

T  passing through an arbitrary point indicated by G s = 3. Rotation φ about an axis on the unit vector uˆ = u1 u2 u3  T s1 s2 s3 . G p = G RB B p + G s − G RB G s (4.290)  T 4. Screw motion with angle φ and displacement h, about and along an axis directed by uˆ = u1 u2 u3 passing through T  an arbitrary point indicated by G s = s1 s2 s3 . G

p=

G

RB B p +

G

s−

G

5. Reflection (a) in the (x, y)-plane:

RB

G

s + huˆ

⎤ p1 p(−z) = ⎣ p2 ⎦ −p3

(4.291)

⎡

G

p=

G

RB B p(−z)

(b) in the (y, z)-plane:

⎡

G

p=

G

⎤ −p1 p(−z) = ⎣ p2 ⎦ p3

(4.292)

RB B p(−x)

(c) in the xz-plane:

(4.293)

⎡

G

p=

G

⎤ p1 p(−z) = ⎣ −p2 ⎦ p3

RB B p(−y)

(4.294)

(d) in a plane with equation u1 x + u2 y + u3 z + h = 0; G

p= =

u21 G

G B  1 RB p − 2huˆ 2 2 + u2 + u3

RB B p − 2huˆ

(4.295)

where ⎡

⎤ −u21 + u22 + u23 −2u1 u2 −2u3 u1 G RB = ⎣ −2u2 u1 u21 − u22 + u23 −2u2 u3 ⎦ −2u1 u3 −2u3 u2 u21 + u22 − u23

(4.296)

 T  T (e) in a plane going through the point s1 s2 s3 and normal to uˆ = u1 u2 u3 , G

where G RB is as in (4.296).

p=

G

RB B p +

G

s−

G

RB

G

s

(4.297)

4.7  Inverse Screw

4.7

197

 Inverse Screw

Inverse of the general screw motion sˇ (h, φ, u, ˆ s) is defined by G −1 sˇB (h, φ, u, ˆ s)

=

sˇG (h, φ, u, ˆ s) "G T G # RB s − G RBT G s − huˆ = 0 1 B

(4.298)

where uˆ is a unit vector indicating the axis of screw, s is the location vector of a point on the axis of screw, φ is the screw angle, and h is the screw translation. If the screw motion is central, the axis of screw passes through the origin and s = 0. Therefore, the inverse of a central screw motion is G −1 sˇB (h, φ, u) ˆ

"G

=

RBT −huˆ 0 1

# (4.299)

Proof The homogenous matrix expression of a screw motion sˇ (h, φ, u, ˆ s) is G

TB =

G sˇB (h, φ, u, ˆ s) "G # G RB s − G RB G s + huˆ = 0 1

A homogenous transformation matrix G TB G

TB =

"G

can be inverted according to: B

TG =

G

TB−1

RB G d 0 1

"G

=

(4.300)

# (4.301)

RBT − G RBT G d 0 1

# (4.302)

To show the correctness of Eq. (4.298), we need to calculate − G RBT G d: −

G

G

RBT G d = − G RBT

s−

G

RB G s + huˆ

= − G RBT G s +

G

RBT

G

= − G RBT G s +

G

s−

G



RB G s −

G

RBT huˆ

RBT huˆ

(4.303)

Because uˆ is an invariant vector in both coordinate frames B and G, we have uˆ =

G

RB uˆ =

RBT uˆ

(4.304)

RBT G s − huˆ

(4.305)

G

and, therefore, −

G

RBT G d =

G

s−

G

This completes the inversion of a general screw motion: G −1 sˇB (h, φ, u, ˆ s)

=

"G

RBT 0

G

s−

G

RBT G s − huˆ 1

# (4.306)

If the screw is central, the location vector s is zero and the inverse of the screw motion will be G −1 sˇB (h, φ, u) ˆ

=

"G

RBT −huˆ 0 1

# (4.307)

198

4 Motion Kinematics

As the inversion of a rotation matrix G RB = Ru,φ ˆ ˆ indicates a rotation −φ about u G

RB−1 =

G

RBT =

B

RG = Ru,−φ ˆ

(4.308)

the inversion of a screw motion can also be interpreted as a rotation −φ about u, ˆ plus a translation −h along u. ˆ G −1 sˇB (h, φ, u, ˆ s)

= sˇ (−h, −φ, u, ˆ s)

(4.309)

To examine the screw inversion formula, we must have sˇ (h, φ, u, ˆ s) G sˇB−1 (h, φ, u, ˆ s) = I4

(4.310)

where I4 is a 4 by 4 identity matrix. It can be examined by matrix multiplication. #"G T G # RB G s − G RB G s + huˆ RB s − G RBT G s − huˆ 0 1 0 1   # " G G I3 RB s − G RBT G s − huˆ + G s − G RB G s + huˆ = 0 1 " G G # I3 RB s − G s − h G RB uˆ + G s − G RB G s + huˆ = 0 1 # " I 0 = I4 = 3 0 1

"G

(4.311) 

Example 139  Inversion of a central screw. Numerical example of inversion of a central screw. Assume the unit cubic of Fig. 4.21 turns φ = 45 deg about u and translates by G d = hu. ˆ  T huˆ = 1 1 1

 T u= 111

(4.312)

The associated central screw motion is "

G

# "G # Ru,φ huˆ RB G d ˆ = 0 1 0 1 ⎤ ⎡ 0.8047 −0.3106 0.5059 1 ⎢ 0.5059 0.8047 −0.3106 1 ⎥ ⎥ =⎢ ⎣ −0.3106 0.5059 0.8047 1 ⎦ 0 0 0 1

TB =

G

sˇB (h, φ, u) ˆ =

(4.313)

The inverse of this screw motion is G −1 sˇB (h, φ, u) ˆ

=

"G

RBT −huˆ 0 1

#

⎡

⎤ 0.8047 0.5059 −0.3106 −1 ⎢ −0.3106 0.8047 0.5059 −1 ⎥ ⎥ =⎢ ⎣ 0.5059 −0.3106 0.8047 −1 ⎦ 0 0 0 1

(4.314)

We may check the inverse screw by a matrix multiplication. G

sˇB (h, φ, u) ˆ G sˇB−1 (h, φ, u) ˆ =

G −1 sˇB (h, φ, u) ˆ G sˇB (h, φ, u) ˆ

= I4

(4.315)

4.7  Inverse Screw

199

Example 140  Inversion of a general screw. Numerical example of inversion of a general screw. Figure 4.23 shows the unit cubic of Fig. 4.21 after a rotation φ = 45 deg about G u and a translation huˆ = u where u is at G s. ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 1 1 G G u = ⎣1⎦ huˆ = ⎣ 1 ⎦ s = ⎣0⎦ (4.316) 1 1 0 The screw matrix of this motion is G

TB =

G

⎡

sˇB (h, φ, u, ˆ s)

⎤ 0.8047 −0.3106 0.5059 1.1953 ⎢ 0.5059 0.8047 −0.3106 0.4941 ⎥ ⎥ =⎢ ⎣ −0.3106 0.5059 0.8047 1.3106 ⎦ 0 0 0 1

(4.317)

The inverse of this screw is "G

RBT G s − huˆ = 1 ⎡ ⎤ 0.8047 0.5059 −0.3106 −0.8047 ⎢ −0.3106 0.8047 0.5059 −0.6894 ⎥ ⎥ =⎢ ⎣ 0.5059 −0.3106 0.8047 −1.5059 ⎦ 0 0 0 1 G −1 sˇB (h, φ, u, ˆ s)

RBT 0

G

s−

G

#

(4.318)

We must also be able to turn the cube back to its original position by a rotation φ = −45 deg about G u and a translation huˆ = −u where u is at G s. Such a screw motion would be ⎡

⎤ 0.8047 0.5059 −0.3106 −0.8047 ⎢ −0.3106 0.8047 0.5059 −0.6894 ⎥ G ⎥ TB = ⎢ ⎣ 0.5059 −0.3106 0.8047 −1.5059 ⎦ 0 0 0 1

(4.319)

because: G

RB = I cos φ + uˆ uˆ T (1 − cos φ) + u˜ sin φ ⎡ ⎤ 0.8047 0.5059 −0.3106 = ⎣ −0.3106 0.8047 0.5059 ⎦ 0.5059 −0.3106 0.8047

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 1 1 −1 G s − G RB G s + huˆ = ⎣ 0 ⎦ − G RB ⎣ 0 ⎦ + ⎣ −1 ⎦ 0 0 −1 ⎡ ⎤ −0.80474 = ⎣ −0.68938 ⎦ −1.5059 The screw (4.319) is the inverse of (4.317), so their multiplication is equal to I4 .

(4.320)

(4.321)

200

4.8

4 Motion Kinematics

 Combined Screw Transformation

Assume 1 sˇ2 (h1 , φ 1 , uˆ 1 , s1 ) is a screw motion to move from coordinate frame B2 to B1 and G sˇ1 (h0 , φ 0 , uˆ 0 , s0 ) is a screw motion to move from coordinate frame B1 to G. Then, the combined screw motion G sˇ2 (h, φ, u, ˆ s) to move from B2 to G is sˇ2 (h, φ, u, ˆ s) = G sˇ1 (h0 , φ 0 , uˆ 0 , s0 ) 1 sˇ2 (h1 , φ 1 , uˆ 1 , s1 ) "G # R2 G R1 (I − 1 R2 ) s1 + (I − G R1 ) s0 + h1 G R1 uˆ 1 + h0 uˆ 0 = 0 1

G

(4.322)

Proof Direct substitution for 1 s2 (h1 , φ 1 , uˆ 1 ) and G s1 (h0 , φ 0 , uˆ 0 ) "G

R1 s0 + h0 uˆ 0 1 # "1 R2 s1 − 1 R2 s1 + h1 uˆ 1 1 sˇ2 (h1 , φ 1 , uˆ 1 , s1 ) = 0 1

G

sˇ1 (h0 , φ 0 , uˆ 0 , s0 ) =

R1 s0 − 0

G

# (4.323) (4.324)

shows that sˇ2 (h, φ, u, ˆ s) = G sˇ1 (h0 , φ 0 , uˆ 0 , s0 ) 1 sˇ2 (h1 , φ 1 , uˆ 1 , s1 ) "G #"1 # R1 s0 − G R1 s0 + h0 uˆ 0 R2 s1 − 1 R2 s1 + h1 uˆ 1 = 0 1 0 1   # "G R2 G R1 s1 − 1 R2 s1 + h1 uˆ 1 + s0 − G R1 s0 + h0 uˆ 0 = 0 1 # "G R2 G R1 (I − 1 R2 ) s1 + (I − G R1 ) s0 + h1 G R1 uˆ 1 + h0 uˆ 0 = 0 1

G

(4.325)

where G

R2 =

G

R1 1 R2

(4.326)

To find the screw parameters of the combined screw motion G sˇ2 (h, φ, u, ˆ s), we start by obtaining uˆ and φ from G R2 based on (4.229) and (4.227). Then, utilizing (4.286) and (4.287) we can find h and s h = uˆ ·  s= I−

G

R2

G

−1

d

(4.327)

(G d − hu) ˆ

(4.328)

where G

d=

R1 (I − 1 R2 ) s1 + (I − G R1 ) s0 + h1 G R1 uˆ 1 + h0 uˆ 0     = G R1 − G R2 s1 + G R1 h1 uˆ 1 − s0 + s0 + h0 uˆ 0 G

(4.329) 

Example 141  Exponential representation of a screw. Screw motion can be expressed by exponential function. This shows how. To compute a rigid body motion associated with a screw, consider the motion of point P in Fig. 4.19. The final position of the point can be given by p = s + eφ u˜ r + huˆ = s + eφ u˜ (p − s) + huˆ = [T ]p (4.330) where [T ] is the exponential representation of screw motion.

4.8  Combined Screw Transformation

201

" [T ] =

eφ u˜ (I − eφ u˜ ) s + huˆ 0 1

# (4.331)

The exponential screw transformation matrix (4.331) is based on the exponential form of the Rodriguez formula (3.187). eφ u˜ = I + u˜ sin φ + u˜ 2 (1 − cos φ)

(4.332)

Therefore, the combination of two screws can also be found by [T ] = T1 T2 # " φ u˜ # " φ u˜ e 1 1 (I − eφ 1 u˜ 1 ) s1 + h1 uˆ 1 e 2 2 (I − eφ 2 u˜ 2 ) s2 + h2 uˆ 2 = 0 1 0 1 " φ u˜ +φ u˜ G # e 1 1 2 2 d = 0 1 where G

   d = eφ 1 u˜ 1 − eφ 1 u˜ 1 +φ 2 u˜ 2 s2 + eφ 1 u˜ 1 h2 uˆ 2 − s1 + s1 + h1 uˆ 1

(4.333)

(4.334)

Example 142  Combination of two principal central screws. Combination of two simple principal screws. An illustration. Combination of every two principal central screws can be found by matrix multiplication. As an example, a screw motion about Y followed by another screw motion about X is sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) ⎤ ⎡ ⎤⎡ 1 0 0 γ pX cβ 0 sβ 0 ⎢ 0 cγ −sγ 0 ⎥ ⎢ 0 1 0 β pY ⎥ ⎥⎢ ⎥ =⎢ ⎣ 0 sγ cγ 0 ⎦ ⎣ −sβ 0 cβ 0 ⎦ 0 0 0 1 0 0 0 1 ⎡ cos β 0 sin β γ pX ⎢ sin β sin γ cos γ − cos β sin γ β pY cos γ =⎢ ⎣ − cos γ sin β sin γ cos β cos γ β pY sin γ 0 0 0 1

(4.335)

⎤ ⎥ ⎥ ⎦

Screw motion combination is not commutative and, therefore, sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) = sˇ (hY , β, Jˆ) sˇ (hX , γ , Iˆ)

(4.336)

Example 143  Decomposition of a screw. How to determine screw parameters from a given homogenous transformation matrix. Every general screw can be decomposed into three principal central screws, G

where

sˇB (h, φ, u, ˆ s) =

"G

RB s − 0

G

RB s + huˆ 1

#

ˆ = sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) sˇ (hZ , α, K)

(4.337)

⎤ cαcβ −cβsα sβ G RB = ⎣ cγ sα + cαsβsγ cαcγ − sαsβsγ −cβsγ ⎦ sαsγ − cαcγ sβ cαsγ + cγ sαsβ cβcγ

(4.338)

⎡

202

4 Motion Kinematics

and ⎡

⎤ ⎡ ⎤ dX γ pX + αpZ sin β s − G RB s + huˆ = ⎣ βpY cos γ − αpZ cos β sin γ ⎦ = ⎣ dY ⎦ βpY sin γ + αpZ cos β cos γ dZ

(4.339)

The twist angles α, β, γ can be found from G RB , then the pitches pX , pY , pZ can be found as follows. pZ =

dZ cos γ − dY sin γ α cos β

(4.340)

pY =

dZ sin γ + dY cos γ β

(4.341)

pX =

dX dZ cos γ − dY sin γ − sin β γ γ cos β

(4.342)

Example 144  Decomposition of a screw to principal central screws. Any given screw motion can be reproduced by combining three principal screw motions in 6 ways. In general, there are six different independent combinations of triple principal central screws and, therefore, there are six different methods to decompose a general screw into a combination of principal central screws. They are ˆ 1 − sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) sˇ (hZ , α, K) ˆ sˇ (hX , γ , Iˆ) 2 − sˇ (hY , β, Jˆ) sˇ (hZ , α, K) ˆ sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) 3 − sˇ (hZ , α, K) ˆ sˇ (hY , β, Jˆ) sˇ (hX , γ , Iˆ) 4 − sˇ (hZ , α, K) ˆ 5 − sˇ (hY , β, Jˆ) sˇ (hX , γ , Iˆ) sˇ (hZ , α, K) ˆ sˇ (hY , β, Jˆ) 6 − sˇ (hX , γ , Iˆ) sˇ (hZ , α, K)

(4.343)

The expanded form of the six combinations of principal central screws are presented in Appendix C. It indicates that every screw can be decomposed into three principal central screws.

4.9

 The Plücker Line Coordinate

Plücker coordinates are a set of six coordinates used to define and show a directed line in space. Analytical representation of a line in space can be found if we have the position of two different points of that line. Such a line definition is more natural than the traditional methods and simplifies kinematics. Assume P1 (X1 , Y1 , Z1 ) and P2 (X2 , Y2 , Z2 ) at r1 and r2 are two different points on the line l as shown in Fig. 4.25. Using the position vectors r1 and r2 , the equation of line l can be defined by six elements of two vectors ⎡

⎤ L ⎢ ⎥ " # ⎢M ⎥ ⎢N ⎥ uˆ ⎥ l= =⎢ ⎢P ⎥ ρ ⎢ ⎥ ⎣Q⎦ R

(4.344)

referred to as Plücker coordinates of the directed line l, where uˆ is a unit vector along the line l referred to as direction vector, uˆ =

r2 − r1 = LIˆ + M Jˆ + N Kˆ |r2 − r1 |

(4.345)

and ρ is the moment vector of uˆ about the origin. ρ = r1 × uˆ = P Iˆ + QJˆ + R Kˆ

(4.346)

4.9  The Plücker Line Coordinate

203

Fig. 4.25 A line indicated by two points

The Plücker method is a canonical representation of line definition  and therefore is more efficient than the other methods such as parametric form, l(t) = r1 + t u, ˆ point and direction form r1 , uˆ , or two-point representation form (r1 , r2 ). Proof The unit vector u, ˆ whose direction is along the line connecting P1 and P2 , is uˆ = =

r2 − r1 = LIˆ + M Jˆ + N Kˆ |r2 − r1 | X2 − X1 ˆ Y2 − Y1 ˆ Z2 − Z1 ˆ I+ J+ K d d d

(4.347)

where L2 + M 2 + N 2 = 1

(4.348)

and the distance between P1 and P2 is d=

(X2 − X1 )2 + (Y2 − Y1 )2 + (Z2 − Z1 )2

(4.349)

If r represents a vector from the origin O to a point on the line l, then the vector r − r1 is parallel to uˆ and therefore, the equation of the line can be written as (r − r1 ) × uˆ = 0 (4.350) or equivalently r × uˆ = ρ

(4.351)

ρ = r1 × uˆ

(4.352)

where ρ is the moment of the direction vector uˆ about O. Because vectors ρ and uˆ are perpendicular, there is a constraint among their components. uˆ · ρ = 0 (4.353) Expanding (4.346) yields



Iˆ Jˆ Kˆ

ρ =

X1 Y1 Z1

=P Iˆ + QJˆ + R Kˆ

L M N

(4.354)

where P = Y1 N − Z1 M Q = Z1 L − X1 N R = X1 M − Y1 L

(4.355)

204

4 Motion Kinematics

Fig. 4.26 A unit cube

and, therefore, the orthogonality condition (4.353) can be expressed as: LP + MQ + NR = 0

(4.356)

The Plücker coordinates of the line (4.344) have four independent coordinates because of the two constraints (4.348) and (4.356). Our arrangement of Plücker coordinates in the form of (4.344) is the line arrangement and is called ray coordinates;  T however, sometimes the reverse order in axis arrangement l = ρ uˆ is also used by some other textbooks. In either case,  T  T a vertical line uˆ ρ or a semi-colon uˆ ; ρ may be utilized to separate the first three elements from the last three. Both arrangements can be used in kinematics efficiently.  T  T The Plücker line coordinates uˆ ρ are homogenous because Eq. (4.346) shows that the coordinates are wuˆ wρ , where w ∈ R, determines the same line. Force–moment, angular velocity–translational velocity, and rigid motion act like a line vector and can be expressed by Plücker coordinates. Julius Plücker (1801–1868), a German mathematician and physicist, invented the line geometry in 1836.  Example 145  Plücker coordinates of a line connecting two points. How to determine Plücker coordinates when positions of two points are given. Plücker line coordinates of the line connecting points P1 (1, 0, 0) and P2 (0, 1, 1) are " #  T uˆ l= = −1 1 1 0 −1 1 ρ because

and

√ r2 − r1 3uˆ = = −Iˆ + Jˆ + Kˆ |r2 − r1 | √ √ 3ρ = r1 × 3uˆ = −Jˆ + Kˆ

(4.357)

(4.358)

(4.359)

Example 146  Plücker coordinates of diagonals of a cube. Numerical example of Plücker coordinates for diagonals of a cube. Figure 4.26 depicts a unit cube and two lines on diagonals of two adjacent faces. Line l1 connecting corners P1 (1, 0, 1) and P2 (0, 1, 1) is " #  √ √ √ √ √ T uˆ 1 l1 = = − 22 22 0 − 22 − 22 22 (4.360) ρ1

4.9  The Plücker Line Coordinate

205

because uˆ 1 =

−Iˆ + Jˆ p2 − p1 = √ |p2 − p1 | 2

ρ 1 = p1 × uˆ 1 =

(4.361)

−Iˆ − Jˆ + Kˆ √ 2

(4.362)

Line l2 connecting corners Q1 (1, 0, 0) and Q2 (1, 1, 1) is " l2 =

uˆ 2 ρ2

#

 = 0

√

2 2

√

2 2

0−

√ √ 2 2 2 2

T (4.363)

because uˆ 2 =

q2 − q1 Jˆ + Kˆ = √ |q2 − q1 | 2

(4.364)

−Jˆ + Kˆ √ 2

(4.365)

ρ 2 = q1 × uˆ 2 =

Example 147  Grassmannian matrix to show Plücker coordinates. Grassmannian matrix is another method to express lines. It is connected to Plücker coordinates. It can be verified that the Grassmannian matrix for coordinates of two points, "

w1 X1 Y1 Z1 w2 X2 Y2 Z2

#

is a short notation for the Plücker coordinates if we define Plücker coordinates as follows.



Y1 Z1

w1 X1



L= P = w2 X2 Y2 Z2



w 1 Y1

Z1 X1



M= Q= w 2 Y2 Z2 X2



w1 Z1

X1 Y1



N = R= w2 Z2 X2 Y2

(4.366)

(4.367) (4.368) (4.369)

A German mathematician, Hermann Günther Grassmann (1809-1877), is credited for the development of a general calculus of vectors exposed in his masterpiece “The Theory of Linear Extension: A New Branch of Mathematics.” In that contribution Grassmann proposed a new foundation for all of mathematics, showing that once geometry is put into the algebraic context he advocated, the number of possible dimensions in space is unbounded. Grassmann developed a ground breaking n-dimensional algebra of space. In his 1862, Grassmann reported a study about the reduction and decomposition of geometric structures into two components with applications to screw theory. Example 148  Ray–axis arrangement transformation. different expressions of Plücker coordinates. It can be verified that the ray arrangement of Plücker coordinates, lray can be transformed to the axis arrangement, laxis

" # uˆ = ρ " # ρ = uˆ

(4.370)

(4.371)

206

4 Motion Kinematics

Fig. 4.27 Three cases of Plücker coordinates; (a) general case, (b) line through origin, (c) line at infinity

and vice versa utilizing the following 6 × 6 transformation matrix . " # " # ρ uˆ = uˆ ρ ⎤ 000100 ⎢0 0 0 0 1 0⎥ ⎢ ⎥ " # ⎢0 0 0 0 0 1⎥ 0 I3 ⎢ ⎥ =⎢ ⎥ = I3 0 ⎢1 0 0 0 0 0⎥ ⎣0 1 0 0 0 0⎦

(4.372)

⎡

(4.373)

001000 The transformation matrix  is symmetric and satisfies the following equations: 2 =  = I

T = 

(4.374)

Example 149  Classification of Plücker coordinates. Graphical expression of special cases of Plücker coordinates. There are three cases of Plücker coordinates as shown in Fig. 4.27. They are: general case, line through origin, and line at infinity.  T The general case of l = uˆ ρ , illustrated in Fig. 4.27a, happens when both uˆ and ρ are nonzero. The direction vector uˆ is parallel to the line, ρ is normal to the plane including the origin and the line, and |ρ| gives the distance from the origin to the line l.  T Line through origin l = uˆ 0 , illustrated in Fig. 4.27b, happens when the line l passes through the origin and ρ is zero.  T Line at infinity l = 0 ρ , illustrated in Fig. 4.27c, happens when the distance of the line l from origin tends to infinity. In this case we may assume uˆ is zero. When the line l is at infinity, it is better to redefine the Plücker coordinates by normalizing the moment vector. # " uˆ ρ T l= (4.375) |ρ| |ρ| Therefore, the direction components of the line l tend to zero by increasing the distance, while the moment components remain finite.  T No line is defined by the zero Plücker coordinates 0 0 0 0 0 0 . Example 150  Transformation of a line vector. How to employ Plücker coordinates to show motion of a vector. Consider the line B l in Fig. 4.28 that is defined in a local frame B(oxyz) by B

"B # " # B uˆ uˆ l= B = B ρ rP × B uˆ

(4.376)

4.10  The Geometry of Plane and Line

207

Fig. 4.28 A line vector in B and G frames

where uˆ is a unit vector parallel to the line l, and P is any point on the line. The Plücker coordinates of the line in the global frame G(OXY Z) is expressed by "G # " # G uˆ uˆ G l= G = G (4.377) ρ rP × G uˆ where G

uˆ =

G

RB B uˆ

(4.378)

and ρ is the moment of uˆ about O. G

G

G

ρ=

G

rP ×

G

uˆ = (G do +

=

G

do ×

G

RB B uˆ +

G

RB (B rP × B u) ˆ

=

G

do ×

G

RB B uˆ +

G

RB B ρ

=

G˜ G do RB B uˆ

+

G

G

RB B rP ) ×

G

RB B uˆ

RB B ρ

(4.379)

 T The 6 × 1 Plücker coordinates uˆ ρ for a line vector can be transformed from a frame B to another frame G by a 6 × 6 transformation matrix G B defined as l= "G # uˆ = G ρ G

"

G

where

B B l "B # uˆ G B B ρ G

G

RB B = G ˜ G d o RB

⎡

⎤ r11 r12 r13 G RB = ⎣ r21 r22 r23 ⎦ r31 r32 r33

0 G RB

(4.380) (4.381) #

⎤ 0 −d3 d2 G˜ do = ⎣ d3 0 −d1 ⎦ −d2 d1 0

(4.382)

⎡

⎤ d2 r31 − d3 r21 d2 r32 − d3 r22 d2 r33 − d3 r23 G˜ G do RB = ⎣ −d1 r31 + d3 r11 −d1 r32 + d3 r12 −d1 r33 + d3 r13 ⎦ d1 r21 − d2 r11 d1 r22 − d2 r12 d1 r23 − d2 r13

(4.383)

⎡

4.10

(4.384)

 The Geometry of Plane and Line

Plücker coordinates introduce a suitable method to define the moment between two lines, shortest distance between two lines, and the angle between two lines.

208

4 Motion Kinematics

Fig. 4.29 Two skew lines

4.10.1  Moment  T  T Consider two arbitrary lines l1 = uˆ 1 ρ 1 and l2 = uˆ 2 ρ 2 as shown in Fig. 4.29. Points P1 and P2 on l1 and l2 are indicated by vectors r1 and r2 , respectively. Direction vectors of the lines are uˆ 1 and uˆ 2 . The moment of the line l2 about P1 is (r2 − r1 ) × uˆ 2 and we can define the moment of the line l2 about l1 by l2 × l1 , l2 × l1 = uˆ 1 · (r2 − r1 ) × uˆ 2

(4.385)

uˆ 1 · r1 × uˆ 2 = uˆ 2 · uˆ 1 × r1

(4.386)

l2 × l1 = uˆ 1 · ρ 2 + uˆ 2 · ρ 1

(4.387)

which, because of simplifies to: The reciprocal product or virtual product of two lines described by Plücker coordinates is defined as: " l2 × l1 =

# " # uˆ 2 uˆ ⊗ 1 = uˆ 2 · ρ 1 + uˆ 1 · ρ 2 ρ2 ρ1

(4.388)

The reciprocal product is commutative and gives the moment between two directed lines.

4.10.2  Angle and Distance  T  T If d is the shortest distance between two lines l1 = uˆ 1 ρ 1 and l2 = uˆ 2 ρ 2 , and α ∈ [0, π ] is the angle between l1 and l2 , then

sin α = uˆ 2 × uˆ 1 (4.389) and

" # " #

1

1

uˆ 2 uˆ

d= uˆ 2 · ρ 1 + uˆ 1 · ρ 2 = ⊗ 1

ρ1 sin α sin α ρ 2 =

1 |l2 × l1 | sin α

(4.390)

Therefore, the lines l1 and l2 intersect if and only if their reciprocal product is zero. Two parallel lines may be assumed to intersect at infinity. The distance expression does not work for parallel lines.

4.10  The Geometry of Plane and Line

209

4.10.3  Plane and Line The equation of a plane G π having a normal unit vector nˆ is n1 X + n 2 Y + n 3 Z = s

(4.391)

nˆ = n1 Iˆ + n2 Jˆ + n3 Kˆ

(4.392)

where s is the minimum distance of the plane to the origin O. We may indicate a plane using a homogenous representation, T  π = n1 n 2 n 3 s

(4.393)

 T and write the condition π T · r = 0 for a point r = X Y Z w to be in the plane by ⎡

⎤ X  ⎢Y ⎥ ⎥ π T · r = n1 n2 n3 s ⎢ ⎣Z⎦ = 0 w

(4.394)

Moreover, w = 0 indicates all points at infinity, and s = 0 indicates all planes containing the origin. The intersection of π-plane with the X-axis, or the X-intercept, is X = −s/n1 , the Y -intercept is Y = −s/n2 , and the Z-intercept is Z = −s/n3 . The plane is perpendicular to (X, Y )-plane if n3 = 0. It is perpendicular to the X-axis if n2 = n3 = 0. There are similar conditions for the other planes and axes. If (X0 , Y0 , Z0 ) is a point in the π-plane (4.393), then n1 (X − X0 ) + n2 (Y − Y0 ) + n3 (Z − Z0 ) = s (4.395)  T The distance of a point X Y Z w from the origin is ) d=

X2 + Y 2 + Z 2 w2

(4.396)

 T while the distance of a plane π = m1 m2 m3 s from the origin is * s=

m21

s2 + m22 + m23

(4.397)

The equation of a line connecting two points P1 (X1 , Y1 , Z1 ) and P2 (X2 , Y2 , Z2 ) at r1 and r2 can also be expressed by l = r1 + m(r2 − r1 )

(4.398)

and the distance of a point P (X, Y, Z) from any point on l is given by d 2 = (X1 + m(X2 − X1 ) − X)2 + (Y1 + m(Y2 − Y1 ) − Y )2 + (Z1 + m(Z2 − Z1 ) − Z)2

(4.399)

(X2 − X1 )(X1 − X) + (Y2 − Y1 )(Y1 − Y ) + (Z2 − Z1 )(Z1 − Z) (X2 − X1 )2 + (Y2 − Y1 )2 + (Z2 − Z1 )2

(4.400)

which is minimum for m=−

To find the minimum distance of the origin, we set X = Y = Z = 0.

210

4 Motion Kinematics

Example 151  Angle and distance between two diagonals of a cube. Numerical example of application of Plücker coordinates to determine the distance between two lines. The Plücker coordinates of the two diagonals of the unit cube shown in Fig. 4.26 are " l1 = " l2 =

uˆ 1 ρ1 uˆ 2 ρ2

# #

 √ = − 22  = 0

√

2 2

√

2 2

√

2 2

0−

√ 2 2

0−

√ √ 2 2 2 2

−

√

2 2

√

2 2

T (4.401)

T (4.402)

The angle between l1 and l2 is

α = arcsin uˆ 2 × uˆ 1 = arcsin

√

3 π = rad = 60 deg 2 3

(4.403)

and the distance between them is

" # " #

1

uˆ 1 1

uˆ d= ⊗ 2

= uˆ 1 · ρ 2 + uˆ 2 · ρ 1

ρ2 sin α ρ 1 sin α 1 2 = √ |−0.5| = √ 3 3

(4.404)

Example 152 Distance of a point from a line. Numerical example of application of Plücker coordinates to determine the distance between a point and a line. The equation of the line connecting two points r1 and r2  T r1 = −1 2 1 1

 T r2 = 1 −2 −1 1

is

⎤ −1 + 2m ⎢ 2 − 4m ⎥ ⎥ l = r1 + m(r2 − r1 ) = ⎢ ⎣ 1 − 2m ⎦ 1  T Now the distance between point r3 = 1 1 0 1 and l is given by

(4.405)

⎡

(4.406)

s 2 = (X1 + m(X2 − X1 ) − X3 )2 + (Y1 + m(Y2 − Y1 ) − Y3 )2 + (Z1 + m(Z2 − Z1 ) − Z3 )2 = 24m2 − 20m + 6 which is minimum for: m=

5 12

(4.407)

(4.408)

Thus, the point on the line at a minimum distance from r3 is  T r = −1/6 1/3 1/6 1

(4.409)

Example 153 Distance between two lines. Numerical example of application of Plücker coordinates to determine the distance between two lines. The line connecting r1 and r2  T  T r1 = −1 2 1 1 r2 = 1 −2 −1 1 (4.410)

4.10  The Geometry of Plane and Line

is

⎡

⎤ −1 + 2m ⎢ 2 − 4m ⎥ ⎥ l = r1 + m(r2 − r1 ) = ⎢ ⎣ 1 − 2m ⎦ 1

and the line connecting r3 and r4 is

211

 T r3 = 1 1 0 1

 T r4 = 0 −1 2 1

⎤ 1−n ⎢ 1 − 2n ⎥ ⎥ l = r1 + m(r2 − r1 ) = ⎢ ⎣ 2n ⎦ 1

(4.411)

(4.412)

⎡

(4.413)

The distance between two arbitrary points on the lines is   s 2 = −1 + 2m − 1 + n2 + (2 − 4m − 1 + 2n)2 + (1 − 2m − 2n)2

(4.414)

The minimum distance is found by minimizing s 2 with respect to m and n. m = 0.443

n = 0.321

(4.415)

Hence, the two points on the lines at a minimum distance apart are at: ⎤ −0.114 ⎢ 0.228 ⎥ ⎥ rm = ⎢ ⎣ 0.114 ⎦ 1 ⎡

⎤ 0.679 ⎢ 0.358 ⎥ ⎥ rn = ⎢ ⎣ 0.642 ⎦ 1 ⎡

(4.416)

Example 154  Intersection condition for two lines. Expression of intersection of two lines by Plücker coordinates.   T T If two lines l1 = uˆ 1 ρ 1 and l2 = uˆ 2 ρ 2 intersect, and the position of their common point is at r, then ρ 1 = r × uˆ 1

ρ 2 = r × uˆ 2

(4.417)

and, therefore,     ρ 1 · uˆ 2 = r × uˆ 1 · uˆ 2 = r· uˆ 1 × uˆ 2     ρ 2 · uˆ 1 = r × uˆ 2 · uˆ 1 = r· uˆ 2 × uˆ 1

(4.418)

uˆ 1 · ρ 2 + uˆ 2 · ρ 1 = 0

(4.420)

(4.419)

which implies or equivalently:

"

# " # uˆ 1 uˆ ⊗ 2 =0 ρ1 ρ2

(4.421)

Example 155  Plücker coordinates of the axis of rotation. Expression of rotations by Plücker coordinates. Consider a homogenous transformation matrix corresponding to a rotation α about Z, along with a translation in (X, Y )plane ⎡ ⎤ ⎡ ⎤ r11 r12 0 Xo cos α − sin α 0 Xo ⎢ r21 r22 0 Y0 ⎥ ⎢ sin α cos α 0 Yo ⎥ G ⎥ ⎢ ⎥ TB = ⎢ (4.422) ⎣ 0 0 1 0 ⎦=⎣ 0 0 1 0 ⎦ 0 0 0 1 0 0 0 1

212

4 Motion Kinematics

The angle of rotation can be obtained by comparison. α = arctan

r21 r11

(4.423)

The pole of rotation can be found by searching for a point that has the same coordinates in both frames, ⎡

r11 ⎢ r21 ⎢ ⎣ 0 0 that can be written as:

r12 r22 0 0

⎤⎡ ⎤ ⎡ ⎤ 0 Xo Xp Xp ⎢ ⎥ ⎢ ⎥ 0 Yo ⎥ ⎥ ⎢ Yp ⎥ = ⎢ Yp ⎥ ⎦ ⎣ ⎦ ⎣ 1 0 0 0 ⎦ 0 1 1 1

⎤⎡ ⎤ ⎡ ⎤ r11 − 1 r12 0 Xo Xp 0 ⎢ r21 r22 − 1 0 Yo ⎥ ⎢ Yp ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥=⎢ ⎥ ⎣ 0 0 1 0 ⎦⎣ 0 ⎦ ⎣0⎦ 0 0 0 1 1 1

(4.424)

⎡

(4.425)

The solutions of these equations are Xp =

1 1 r21 1 1 sin α Yo = Xo − Xo − Yo 2 2 1 − r11 2 2 vers α

(4.426)

Yp =

1 1 r21 1 1 sin α Yo + Xo Xo = Yo + 2 2 1 − r11 2 2 vers α

(4.427)

 T The Plücker line coordinates l = uˆ ρ of the pole axis is then equal to:  T l = 0 0 1 Yp −Xp 0

4.11

(4.428)

 Screw and Plücker Coordinate

 T Consider a screw motion sˇ (h, φ, u, ˆ s) whose line of action is given by Plücker coordinates uˆ ρ , and its pitch is p = φh . The screw motion can be defined by a set of Plücker coordinates. " # " # " # uˆ uˆ φ uˆ sˇ (h, φ, u, ˆ s) = = = ξ ρ + puˆ φρ + huˆ

(4.429)

If the pitch is infinite, p = ∞, then the screw motion reduces to a pure translation, or equivalently, a line at infinity. "

0 sˇ (h, 0, u, ˆ r) = huˆ

# (4.430)

A zero pitch screw p = 0 corresponds to a pure rotation, then the screw coordinates are identical to the Plücker coordinates of the screw line. " # " # φ uˆ uˆ sˇ (0, φ, u, ˆ s) = = (4.431) φρ ρ A central screw motion is defined by a line through origin. " sˇ (h, φ, u) ˆ = sˇ (h, φ, u, ˆ 0) = = D(hu) ˆ R(u, ˆ φ)

# " # uˆ φ uˆ = puˆ huˆ (4.432)

4.12 Summary

213

Screw coordinates for differential screw motion are useful in velocity analysis of robots. Consider a screw axis l, an angular velocity ω = ωuˆ = φ˙ uˆ about l, and a velocity v along l. If the location vector s is the position of a point on l, then the Plücker coordinates of the line l are " # " # uˆ uˆ l= = (4.433) ρ s × uˆ The pitch of screw is p=

|v| |ω|

(4.434)

uˆ =

ω |ω|

(4.435)

and the direction of screw is defined by

so the instantaneous screw coordinates v(p, ˇ ω, u, ˆ s) are "

# r × ω + |v| ω T v(p, ˇ ω, u, ˆ r) = ωuˆ |ω| " # " # ωuˆ ω = = s × uˆ + v s × uˆ + pω " # ω = ρ + pω

(4.436)

Example 156  Pitch of an instantaneous screw. Screw motion may be expressed by Plücker coordinates. This is showing a special situation. The pitch of an instantaneous screw motion, defined by Plücker coordinates, can be found by p = uˆ · ξ

(4.437)

because two Plücker vectors are orthogonal, uˆ · ρ = 0, and therefore:     φ uˆ · φξ = φ uˆ · ρ + φpuˆ = φ uˆ · ρ + φ 2 p = φ 2 p

(4.438)

Example 157  Nearest point on a screw axis to the origin. An application of Plücker coordinates to screw motions. The point on the instantaneous screw axis, nearest to the origin, is indicated by the following position vector: s0 = φ uˆ × φξ

4.12

(4.439)

Summary

Arbitrary motion of a body B with respect to another body G is called rigid body motion and can be expressed by a rotation G RB B r plus a translation G d G r = G RB B r + G d (4.440) where  T r= XY Z  T B r= xyz  T G d = Xo Yo Zo G

(4.441) (4.442) (4.443)

The vector G d is translation of B with respect to G, and G RB is the rotation transformation matrix to map B r to G r when d = 0.

G

214

4 Motion Kinematics

By introducing the homogenous coordinate representation for a point ⎡ ⎤ x ⎢y ⎥ ⎥ r=⎢ ⎣z⎦ 1

(4.444)

we may combine the rotation and translation of a rigid body motion by a 4 × 4 homogenous transformation matrix G TB and show the coordinate transformation by G

r=

where G

TB =

G

"G

TB B r RB G d 0 1

(4.445) # (4.446)

As the homogenous transformation matrix G TB is not orthogonal, its inverse obeys a specific rule: G

TB−1

=

B

TG =

"G

RBT − G RBT G d 0 1

# (4.447)

to be consistent with G

TB−1 G TB = I4

(4.448)

The rigid motion can also be expressed with screw motion sˇ (h, φ, u, ˆ s) and screw transformation G sˇB (h, φ, u, ˆ s) B r "G # RB G s − G RB G s + huˆ sˇ (h, φ, u, ˆ s) = 0 1 G

r=

(4.449) (4.450)

The screw motion sˇ (h, φ, u, ˆ s) is indicated by screw parameters: a unit vector on the axis of rotation u, ˆ a location vector s, a twist angle φ, and a translation h (or pitch p). The location vector s indicates the global position of a point on the screw axis. When s = 0, then uˆ passes through the origin of the coordinate frame and the screw motion is called a central screw sˇ (h, φ, u). ˆ Every screw motion can be decomposed into three principal central screws about the three axes of the coordinate frame G. A rigid body motion can be expressed more effectively by screw and Plücker coordinates of directed lines. " # " # " # uˆ uˆ φ uˆ sˇ (h, φ, u, ˆ s) = = = (4.451) ξ ρ + puˆ φρ + huˆ ρ = r × uˆ

(4.452)

4.13 Key Symbols

4.13

Key Symbols

B c d D e G h I = [I ] ıˆ, jˆ, kˆ ı˜, j˜, k˜ Iˆ, Jˆ, Kˆ l L, M, N nˆ p P P , Q, R p, q, r rij R s s sˇ T uˆ u˜ v w x, y, z X, Y, Z

Body coordinate frame, local coordinate frame cos Translation vector, displacement vector Displacement matrix Exponential Global coordinate frame, fixed coordinate frame Translation of a screw Identity matrix Local coordinate axes’ unit vectors Skew symmetric matrices of the unit vector ıˆ, jˆ, kˆ Global coordinate axes’ unit vectors Line, Plücker coordinates, Plücker line Components of uˆ Normal unit vector to a plane Pitch of a screw A body point, a fixed point in B, point matrix Components of ρ Position vectors, homogenous position vector The element of row i and column j of a matrix Rotation transformation matrix sin Location vector of a screw Screw homogenous transformation matrix A unit vector on axis of rotation Skew symmetric matrix of the vector uˆ Velocity vector Weight factor of a homogenous vector Local coordinate axes Global coordinate axes

Greek α, β, γ G B λ ξ π ρ φ ω ω˜

Rotation angles about global axes Transformation matrix for Plücker coordinates Eigenvalues of R Moment vector of a Plücker line Homogenous expression of a plane Moment vector of uˆ about origin Angle of rotation about u, ˆ rotation of a screw Angular velocity vector Skew symmetric matrix of the vector ω

Symbol tr vers [ ]−1 [ ]T 

Trace operator 1 − cos Inverse of the matrix [ ] Transpose of the matrix [ ] Transformation matrix of ray to axis arrangement

215

216

4 Motion Kinematics

Exercises 1. Notation and symbols. Describe the meaning of: b − G TB−1 c − B TG d − G DB e − G sˇB (h, φ, u) ˆ

a − 2 d1 f − Du,h ˆ k−p =

g − sˇ

h φ

−1

h − B G

" # uˆ ˆ i− j − G sˇB (2, π3 , K) ρ

m − sˇ −T n −

l − G sT0

2. Global position in a rigid body motion. We move the body coordinate frame B to G

" # uˆ o − sˇ (h, φ, u, ˆ s) ξ

 T d = 4 −3 7

(4.453)

 T rP = 7 3 2

(4.454)

Find G rP if the local position of a point P is B

The orientation of B with respect to the global frame G can be found by a rotation 45 deg about the X-axis, and then 45 deg about the Y -axis. 3. Global rotation and global translation. A body frame B turns 90 deg about Z-axis, then 90 deg about X-axis. (a) Determine the transformation matrix G TB .  T (b) Determine the global coordinates of B rP = 1 1 1 after the rotations. (c) Determine a unique rotation about a unique axis to turn B to its new position. (d) If the body is a cube at the positive corner of the global coordinate frame such that P is the furthest corner of the cube from the origin, then determine the required translation vector G d to move the cube back to the corner of the first quadrant. (e) What are the global coordinates of P after moving the cube back to the corner of the first quadrant. 4.  Rotation matrix compatibility. It is not possible to find G RB from the equation of rigid body motion G

rP =

G

RB B rP +

G

d

if G rP , B rP , and G d are given. Explain why and find the required conditions to be able to find G RB . 5. Global position with local rotation in a rigid body motion. Assume a body coordinate frame B is at G d.  T G d = 4 −3 7 Find G rP if the local position of a point is B

 T rP = 7 3 2

(4.455)

(4.456) (4.457)

and the orientation of B with respect to the global frame G can be found by a rotation 45 deg about the x-axis, and then 45 deg about the y-axis. 6. Local rotation and global translation. A body frame B turns 90 deg about z-axis, then 90 deg about x-axis. (a) Determine the transformation matrix G TB .  T (b) Determine the global coordinates of B rP = 1 1 1 after the rotations. (c) Determine a unique rotation about a unique axis to turn B to its new configuration. 7. Repeating global rotation. A body frame B turns 90 deg about Z-axis, then 90 deg about Y -axis. (a) Determine the transformation matrix G TB .  T (b) Determine the global coordinates of B rP = 1 1 1 after the rotations.

Exercises

217

Fig. 4.30 A cube at the positive corner of the global coordinate frame

(c) Determine a unique rotation about a unique axis to turn B to its new position. (d)  Assume that the body is a cube at the positive corner of the global coordinate frame as shown in Fig. 4.30. The point P is the furthest corner of the cube from the origin. Show that if we repeat the combined rotations of section (a) three times, the cube will be back to its initial position and orientation.  T (e)  After rotations 90 deg about Z-axis, then 90 deg about Y -axis, turn the cube φ about G u = 1 1 1 . How much should φ be to turn the cube back to its initial position and orientation? 8. Global and local rotation in a rigid body motion. A body coordinate frame B is translated to G d. G

 T d = 4 −3 7

(4.458)

 T rP = 7 3 2

(4.459)

Find G rP if the local position of a point is B

and the orientation of B with respect to the global frame G can be found by a rotation 45 deg about the X-axis, then 45 deg about the y-axis, and finally 45 deg about the z-axis. 9. Combination of rigid motions. The frame B1 is rotated 35 deg about the z1 -axis and translated to 2

 T d = −40 30 20

(4.460)

with respect to another frame B2 . The orientation of the frame B2 in the global frame G can be found by a rotation of 55 deg about  T u = 2 −3 4 (4.461) Calculate G d1 and G R1 . 10. Global rotation and translation. Determine the transformation matrix for the unit cube of Fig. 4.30 after: (a) A rotation 90 deg about Z-axis followed by a unit translation along x-axis. (b) Determine the global coordinates of point P after the motion in (a). (c)  Repeat the motion in (a) four times. The cube will be back to its initial position. Determine the coordinates of P after each motion. 11. Rotation submatrix in a homogenous transformation matrix. Find the missing elements in this homogenous transformation matrix [T ]. ⎡

⎤ ? 0?4 ⎢ 0.707 ? ? 3 ⎥ ⎥ [T ] = ⎢ ⎣ ? ? 0 1⎦ 0 001

(4.462)

218

4 Motion Kinematics

12. Angle and axis of rotation. Find the angle and axis of rotation for [T ] and T −1 . ⎡

⎤ 0.866 −0.5 0 4 ⎢ 0.5 0.866 0 3 ⎥ ⎥ [T ] = ⎢ ⎣ 0 0 1 1⎦ 0 0 01

(4.463)

13. Combination of homogenous transformations. Assume the origin of the frames B1 , and B2 are at:  T d1 = −10 20 −20  T G d2 = −15 −20 30 2

(4.464) (4.465)

The orientation of B1 in B2 can be found by a rotation of 60 deg about 2 u, 2

 T u = 1 −2 4

(4.466)

and the orientation of B2 in the global frame G can be found by a rotation of 30 deg about G u. G

 T u = 4 3 −4

(4.467)

Calculate the transformation matrix G T2 , G T2−1 , G T1 , and G T1−1 . 14. Rotation about an axis not going through origin. Find the global position of a body point at B rP B

 T rP = 7 3 2

(4.468)

after a rotation of 30 deg about an axis parallel to G u G

 T u = 4 3 −4

(4.469)

and passing through a point at (3, 3, 3). 15.  Box arrangement. Figure 4.31 illustrates two adjacent unit boxes. The vectors G u1 , G u2 , and G u3 are globally fixed. (a) Put the second box on top of the first box by a rotation about u3 . Determine the transformation matrix and the coordinates of the four top points. (b) Put the second box on top of the first box by a rotation about u2 , u3 , and u2 . Determine the transformation matrix and the coordinates of the four top points. (c) Put the second box on top of the first box by a rotation about Z, then u3 , and a translation along Z-axis. Determine the transformation matrix and the name of the top points. (d) Put the first box on top of the second box by a rotation abut u3 . Determine the transformation matrix and the coordinates of the four top points. (e) Put the first box on top of the second box by a rotation about Z, u1 , u3 , and u2 . Determine the transformation matrix and the coordinates of the four top points. (f) Determine one rotation and one translation to put the second box on top of the first box. (g) Determine one rotation and one translation to put the first box on top of the second box.

Exercises

219

Fig. 4.31 Two adjacent unit boxes

16. Inversion of a square matrix. Knowing that the inverse of a 2 × 2 matrix

" [A] =

ab cd

# (4.470)

⎡

is A−1

⎤ b d − ⎢ bc ⎥ = ⎣ −adc + bc −ad + ⎦ a − −ad + bc −ad + bc

(4.471)

use the inverse method of splitting a matrix [T ] into "

AB [T ] = CD

# (4.472)

and applying the inverse technique (4.107), calculate the inverse of [T ]. ⎡

⎤ 1 2 3 4 ⎢ 12 13 14 5 ⎥ ⎥ [T ] = ⎢ ⎣ 11 16 15 6 ⎦ 10 9 8 7

(4.473)

17.  Combination of rotations about non-central axes. Consider a rotation 30 deg about an axis at the global point (3, 0, 0) followed by another rotation 30 deg about an axis at the global point (0, 3, 0). Find the possible transformations such that the final global coordinates of B

 T rP = 1 1 0

to be G

rP =

√

203

T

(4.474)

(4.475)

18. Transformation matrix from body points. Figure 4.32a shows a cube at initial configurations. Label the corners of the cube, at the final configuration shown in Fig. 4.32b, and find the associated homogenous transformation matrix. The length of each side of the cube is 2.

220

4 Motion Kinematics

Fig. 4.32 A cube, before and after a rigid motion

19. Eulerian angles and translations. (a) Determine the homogenous transformation matrix G TB for a set of Eulerian rotations ϕ, θ, ψ, plus a unit translations along eˆϕ , eˆθ , eˆψ . (b) Determine the inverse of the transformation matrix in (a). (c) Simplify the G TB and G TB−1 for ϕ = 45 deg, θ = 45 deg, ψ = 45 deg. 20. Euler angles and global translations. (a) Let turn a rigid body 45 deg about G u and translate one unit along G u. G

 T u= 123

(4.476)

(b) Determine the homogenous transformation matrix G TB . (c) What are the Eulerian rotations ϕ, θ, ψ, and translations one unit along G u to have the same G TB ? (d) What are the translations one unit along the Eulerian axes eˆϕ , eˆθ , eˆψ to have the same G TB ? (e) What are the Eulerian rotations ϕ, θ, ψ, and translations along eˆϕ , eˆθ , eˆψ to have the same G TB ? 21. Wrong order of rotation and translation. When we split a homogenous transformation to a rotation and a translation, we must follow the rule (4.38) and apply the rotation first. G TB = G DB G RB = G RB G DB (4.477)  T (a) Apply a rotation G RZ,45 and translation G DX,2 and determine the global coordinates of a point at B r = 1 1 1 . (b) Apply the translation G DX,2 on B r, followed by the rotation G RZ,45 and determine the global coordinates of a point at B r. (c) Determine the required rigid body motion to move G rb to G ra . (d) Determine the required rigid body motion to move G ra to G rb . 22. Determination of homogenous transformation matrix. Figure 4.33 illustrates 4 body frames in a G-frame. Determine the homogenous transformation matrices G T1 , G T2 , G T3 ,G T4 , 2 T1 , 3 T1 , 4 T1 , 3 T2 , 1 T3 , 3 TG 4 T2 , 4 T3 , 4 TG . 23.  Change the order of rotation and translation. When we split a homogenous transformation to a rotation and a translation, we must follow the rule (4.38) and apply the rotation first. G TB = G DB G RB = G RB G DB (4.478)  T (a) Apply a general rotation G RB and translation G DB and determine the global coordinates of a point at B r = x y z . (b) Apply the translation G DB on B r, followed by the rotation G RB and determine the global coordinates of a point at B r. (c) Determine the required rotation and translation to move G rb to G ra . (d) Introduce a new equation for splitting a homogenous transformation matrix to:

Exercises

221

Fig. 4.33 4 body frames in a G-frame

G

? G

TB =

D2 G R2 G RB G DB

(4.479)

(e) Introduce a new equation for splitting a homogenous transformation matrix to: G

? G

TB =

R2 G D2 G RB G DB

(4.480)

(f) Prove G

TB =

G

DB +

G

RB − I =

G

RB +

G

DB − I

(4.481)

24.  Principal central screw. Find the principal central screw that moves the point B rP B

 T rP = 1 0 0

(4.482)

G

 T rP = 0 1 4

(4.483)

 T rP = 1 0 0 1

(4.484)

to G rP . 25.  Screw motion. Find the global position of B

after a screw motion G sˇB (h, φ, u, ˆ s) =

G

sˇB (4, 60 deg, u, ˆ s), where  T s= 300  T G u= 111 G

(4.485) (4.486)

26.  Pole of a planar motion. (a) Find the pole position of a planar motion if we have the coordinates of two body points, before and after the motion, as given below. P1 (1, 1, 1)

P2 (5, 2, 1)

Q1 (4, 1, 1)

Q2 (7, 2 +

(b) Show that the pole is at the intersection of lines P1 P2 and Q1 Q2 .

(4.487)

√

5, 1)

(4.488)

222

4 Motion Kinematics

27.  Screw parameters Find the global coordinates of the body points P1 (5, 0, 0)

Q1 (5, 5, 0)

R1 (0, 5, 0)

(4.489)

after a rotation of 30 deg about the x-axis followed by a rotation of 40 deg about an axis parallel to the vector G u G

 T u = 4 3 −4

(4.490)

and passing through a global point at (0, 3, 0). Use the coordinates of the points and calculate the screw parameters that are equivalent to the two rotations. 28.  Non-central rotation. Find the global coordinates of a point at B rP B

 T rP = 10 20 −10

(4.491)

 T u= 111

(4.492)

when the body frame rotates about G u G

which passes through a point at (−1, −4, 2). 29.  Equivalent screw. Calculate the transformation matrix G TB for a rotation of 30 deg about the x-axis followed by a translation parallel to G d  T G d = 3 2 −1 (4.493) and then a rotation of 40 deg about an axis parallel to the vector G u. G

 T u = 2 −1 1

(4.494)

Find the screw parameters of G TB . 30.  Central screw decomposition. Find a triple central screws for the case 1 in Appendix C, G

ˆ sˇB (h, φ, u, ˆ s) = sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) sˇ (hZ , α, K)

(4.495)

to get the same screw as G

where G

sˇB (h, φ, u, ˆ s) =

 T s= 300

31.  Central screw composition. What is the final position of a point at B

G

sˇB (4, 60, u, ˆ s)

(4.496)

 T u= 111

(4.497)

G

 T rP = 10 0 0

ˆ after the central screw sˇ (4, 30 deg, Jˆ) followed by sˇ (2, 30 deg, Iˆ) and sˇ (−6, 120 deg, K)? 32.  Screw composition. Find the final position of a point at  T B rP = 10 0 0

(4.498)

(4.499)

after a screw √ ⎤ ⎡ ⎤⎞ 1/ √3 2 1 sˇ2 (h1 , φ 1 , uˆ 1 , s1 ) = 1 sˇ2 ⎝1, 40 deg, ⎣ −1/√ 3 ⎦ , ⎣ 3 ⎦⎠ −1 1/ 3 ⎛

⎡

(4.500)

Exercises

223

Fig. 4.34 A pyramid

followed by ⎛

⎡

⎤ ⎡ ⎤⎞ 1/9 −3 G sˇ1 (h0 , φ 0 , uˆ 0 , s0 ) = G sˇ1 ⎝−1, 45 deg, ⎣ 4/9 ⎦ , ⎣ 1 ⎦⎠ 4/9 5

(4.501)

33.  Zero translation screw motion. If the translation h is zero, then the screw motion is a rotation about a non-central axis. Show that a zero translation screw is equivalent to Eq. (4.132) for rotation about a non-central axis. 34.  Plücker line coordinate. Find the missing numbers in l.  T l = 1/3 1/5 ? ? 2 −1 (4.502) 35.  Plücker lines. Find the Plücker lines for AE, BE, CE, DE in the local coordinate B, and calculate the angle between AE, and the Z-axis in the pyramid shown in Fig. 4.34. The local coordinate of the edges are A(1, 1, 0) B(−1, 1, 0) C(−1, −1, 0)

D(1, −1, 0) E(0, 0, 3)

(4.503)

Transform the Plücker lines AE, BE, CE, DE to the global coordinate G. The global position of o is at: o(1, 10, 2)

(4.504)

36.  Angle between two lines. Find the angle between OE, OD, of the pyramid shown in Fig. 4.34. The coordinates of the points are D(1, −1, 0)

E(0, 0, 3)

(4.505)

37.  Distance from the origin. The equation of a plane is given as 4X − 5Y − 12Z − 1 = 0 Determine the perpendicular distance of the plane from the origin.

(4.506)

224

4 Motion Kinematics

38. Project: Continuous transformation. Figure 4.33 illustrates 4 body frames in a G-frame. (a) Determine the homogenous transformation G T1 . Then define a continuous function of time that moves the origin of B1 from O to o1 on a straight line all in a unit of time, say one second. Determine the unique axis–angle of rotation that turns B1 from the coincident configuration with G to its final orientation. Applying the functions will move B1 from G to its final position and orientation. Divide the unit of time by 10 and determine G T1 at every 0.1 s and show B1 at those incremental times. Determine the directional cosines of the axes of B1 and plot them versus time. (b) Repeat part (a) for B2 -frame moving in G-frame. (c) Repeat part (a) for B2 -frame moving from B1 -frame. (d) Can you find the result of part (c) by combining the results of parts (a) and (b)? 39. Project: Rigid body motion and incremental screw motions. Consider a body frame B coincident with a global frame G. (a) The frame B undergoes a rotation of 45 deg about the X-axis followed by a rotation of 90 deg about the Z-axis and then frame B moves to point (1, 1, 1) in frame G. Determine the axis uˆ and angle φ and translation h to move B to the final position only by one central screw motion. (b) The frame B undergoes a rotation of 45/2 deg about the X-axis followed by a rotation of 90/2 deg about the Z-axis and then frame B moves to point (1/2, 1/2, 1/2) in frame G. Determine the axis uˆ 1 and angle φ 1 and translation h1 to move B to its position B1 by one central screw motion. Then B undergoes a rotation of 45/2 deg about the X-axis followed by a rotation of 90/2 deg about the Z-axis and then frame B moves another displacement (1/2, 1/2, 1/2) in addition to the previous displacement in frame G. Determine the axis uˆ 2 and angle φ 2 and translation h2 to move B1 to the position B2 only by one central screw motion. Also determine the axis uˆ and angle φ and translation h to move B to the final position only by one central screw motion. (c) The frame B undergoes a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis and then frame B moves to point (1/3, 1/3, 1/3) in frame G. Determine the axis uˆ 1 and angle φ 1 and translation h1 to move B to its position B1 by one central screw motion. Then B undergoes a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis and then frame B moves another displacement (1/3, 1/3, 1/3) in addition to the previous displacement in frame G. Determine the axis uˆ 2 and angle φ 2 and translation h2 to move B1 to the position B2 only by one central screw motion. Then B undergoes a rotation of 45/3 deg about the X-axis followed by a rotation of 90/3 deg about the Z-axis and then frame B moves another displacement (1/3, 1/3, 1/3) in addition to the previous displacement in frame G. Determine the axis uˆ 3 and angle φ 3 and translation h3 to move B2 to the position B3 only by one central screw motion. Also determine the axis uˆ and angle φ and translation h to move B to the final position only by one central screw motion. (d) Keep doing this process of turning frame B a rotation of 45/n deg about the X-axis followed by a rotation of 90/n deg about the Z-axis and then frame B moves another displacement (1/n, 1/n, 1/n) in addition to the previous displacement in frame G. Repeat the rotations and displacement n times, n = 1, 2, 3, · · · , 45. Determine the axis uˆ n and angle φ n and translation hn to move B to Bn by one screw motion for every n. How uˆ n and angle φ n and translation hn are related to each other? Plot φ n versus n. Plot hn versus n. Plot directional cosines of uˆ n versus n. Determine the axis uˆ and angle φ and translation h that moves B to the final position only by one central screw motion for n = 1, 2, 3, · · · , 45.

5

Forward Kinematics

Forward kinematics means having the joint variables of a robot, we are able to determine the position and orientation of every link of the robot, including the end-effector. We attach a coordinate frame to every link and determine its configuration in the neighbor frames using rigid motion method. The analysis of determination of position and orientation of all links of a robot relative to each other is called forward kinematics. However, the first job of forward kinematics is to determine the position and orientation of the end-effector in base coordinate frame.

Fig. 5.1 Link (i) and its beginning joint i − 1 and its end joint i

5.1

Denavit–Hartenberg Notation

A serial robot with n joints will have n + 1 links. Numbering of links starts from link (0) for the immobile grounded base link. The moveable link jointed to the base link would be link number (1). The numbering of links increases sequentially up to link (n) for the end-effector. Numbering of joints starts from 1, for the joint connecting the first movable link (1) to the base link (0), and increases sequentially up to joint n. Therefore, the link (i) is connected to its lower link (i − 1) at its proximal end by joint i and is connected to its upper link (i + 1) at its distal end by joint i + 1, as shown in Fig. 5.1. For clarification, we show link numbers in parenthesis, such as link (i), and joint numbers free, such as joint i. Figure 5.3 illustrates links (i − 1), (i), and (i + 1) of a serial robot, along with joints i − 1, i, and i + 1. Every link of a robot needs a coordinate frame to be kinematically distinguished from other links. We use the coordinate frame transformation calculus to determine the relative position and orientation of the links. Although we are free to attach a coordinate frame at any point of a link, a point on a joint axis provides some advantages. Every joint is indicated by a joint axis, which will be either translational or rotational. To determine the kinematic information of a robot component, we rigidly attach a local coordinate frame Bi to every link (i) at joint i + 1 based on the following standard method, known as Denavit–Hartenberg (DH ) method.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_5

225

226

5 Forward Kinematics

Fig. 5.2 A link with two revolute joints at its ends. The joint axes of link (a) are parallel, link (b) are orthogonal, and link (c) are perpendicular

Denavit–Hartenberg (DH ) principles to set up a link coordinate frame: 1. The zi -axis is aligned with the i + 1 joint axis. Every joint, without exception, is represented by a z-axis. To set up link coordinate frames, we always begin with identifying the zi -axes. The positive direction of the zi -axis is arbitrary. The joint axis for revolute joints is the pin’s centerline axis. The axis of a prismatic joint may be any axis parallel to the direction of translation. By assigning the zi -axes, the pairs of links on either side of every joint, and also the two joints on either side of every link, are identified. Although the zi -axes of two joints at the ends of a link may be two skew lines, we usually build industrial robots in such a way that the zi -axes are either parallel, perpendicular, or orthogonal. Two parallel joint axes are indicated by a parallel sign, (). Two joint axes are orthogonal if their axes are intersecting at a right angle. Orthogonal joint axes are indicated by an orthogonal sign, (). Two joints are perpendicular if their axes are not intersecting but their perpendicular planes are at right angle. Perpendicular joint axes are indicated by a perpendicular sign, (⊥). Figure 5.2a–c illustrates two revolute joints at two ends of a link at parallel, orthogonal, and perpendicular configurations, respectively. Every link of a robot, except the base and the final links, has two joints, one at each end. They are either revolute (R) or prismatic (P). The axes of the joints can be parallel (), perpendicular (⊥), or orthogonal (). Therefore, we may indicate a link by the type and configuration of the end joints. A PR indicates a link where its lower joint is prismatic, its upper joint is revolute, and the joint axes are parallel. 2. The xi -axis is defined along the common normal between the zi−1 and zi axes, pointing from the zi−1 to the zi -axis. In general, the z axes may be skew lines; however, there is always one line mutually perpendicular to any two skew lines, called the common normal. The common normal has the shortest distance between the two skew lines. When the two z axes are parallel, there are an infinite number of common normals. In that case, we pick the common normal that is collinear with the common normal of the previous joint axes. When the two z axes are intersecting, there is no common normal between them. In that case, we assign the xi -axis perpendicular to the plane formed by the two z axes in the direction of zi−1 × zi . In case the two z axes are collinear, the only nontrivial arrangement of joints is either PR or RP. We assign the xi -axis for these arrangements in such a way that we have the joint variable equal to θ i = 0 in the rest position of the robot. 3. The yi -axis is determined by the right-hand rule, yi = zi × xi . We assign coordinate frames to every link such that one of the three coordinate axes xi , yi , or zi (usually xi ) is along the axis of the distal joint. By applying the DH method, the origin oi of the frame Bi (oi , xi , yi , zi ), attached to the link (i), is placed at the intersection of the joint axis i + 1 with the common normal between the zi−1 and zi axes. A DH coordinate frame is identified by four parameters: ai , α i , θ i , and di .

5.1 Denavit–Hartenberg Notation

227

Fig. 5.3 Links (i − 1), (i), and (i + 1) along with coordinate frames Bi and Bi+1

Fig. 5.4 DH parameters ai , α i , di , θ i defined for joint i and link (i)

1. Link length ai is the distance between zi−1 and zi axes along the xi -axis. ai is the kinematic length of the link (i). Hence, ai is along the xi -axis, from zi−1 to zi axes. 2. Link twist α i is the required rotation of the zi−1 -axis about the xi -axis to become parallel to the zi -axis. Hence, α i is about the xi -axis, from zi−1 to zi axes. 3. Joint distance di is the distance between xi−1 and xi axes along the zi−1 -axis. Joint distance is also called link offset. Hence, di is along the zi−1 -axis, from xi−1 to xi axes. 4. Joint angle θ i is the required rotation of xi−1 -axis about the zi−1 -axis to become parallel to the xi -axis. Hence, θ i is about the zi−1 -axis, from xi−1 to xi axes. Figure 5.4 illustrates the DH frame parameters (ai , α i , θ i , di ) of the links shown in Fig. 5.3. The parameters θ i and di are called joint parameters, because they define the relative position of two adjacent links connected at joint i. In a given design for a robot, every joint is revolute or prismatic. Thus, for every joint, it will always be the case that either θ i or di is fixed and the other is the joint variable. For a revolute joint (R) at joint i, the θ i is the unique joint variable, and the value of di is

228

5 Forward Kinematics

fixed. For a prismatic joint (P), the di is the only joint variable, while the value of θ i is fixed. The variable parameter, either θ i or di , is called the joint variable. The joint parameters θ i and di define a screw motion because θ i is a rotation about the zi−1 -axis and di is a translation along the zi−1 -axis. The parameters α i and ai are called link parameters, because they define relative positions of joints i and i + 1 at two ends of link (i). The link twist α i is the angle of rotation zi−1 -axis about xi to become parallel with the zi -axis. The other link kinematic length parameter, ai , is the translation along the xi -axis to bring the zi−1 -axis on the zi -axis. The link parameters α i and ai define a screw motion because α i is a rotation about the xi -axis and ai is a translation along the xi -axis. Therefore, we can move the zi−1 -axis to the zi -axis by a central screw sˇ (ai , α i , ıˆ) and move the xi−1 -axis to the xi -axis by a central screw sˇ (di , θ i , kˆi−1 ). Example 158 Simplification tricks in DH method. This is reference to remind some tricks and simplified cases in applying DH method. There are some comments to simplify the application of the DH frame method: 1. Showing only z and x axes is sufficient to identify a coordinate frame. Drawing is made clearer by not showing y axes. The DH coordinate frames are not unique because the direction of zi axes are arbitrary. Every link, except the base and the last, is a binary link and is connected to two other links. 2. If the first and last joints are R, then a0 = 0

an = 0

(5.1)

α0 = 0

αn = 0

(5.2)

In these cases, the zero position for θ 1 and θ n can be chosen arbitrarily, and link offsets can be set to zero. d1 = 0

dn = 0

(5.3)

θ1 = 0

θn = 0

(5.4)

3. If the first and last joints are P, then

4.

5. 6. 7.

8.

9.

and the zero position for d1 and dn can be chosen arbitrarily. We usually choose them to make as many parameters as possible to be zero. A general applied trick is to set the coordinate frames such that as many parameters as possible are zero at the rest position of the system. If the final joint n is R, we may choose xn to be along with xn−1 when θ n = 0 and the origin of Bn is such that dn = 0. If the final joint n is P, we may choose xn such that θ n = 0 and the origin of Bn is at the intersection of xn−1 and the axis of joint n to make dn = 0. The parameters ai and α i are determined by the geometric design of a robot and are always constant. The distance di is the offset of the frame Bi with respect to Bi−1 along the zi−1 -axis. Because ai is a length, ai ≥ 0. The angles α i and θ i are directional. Their positive directions are determined by the right-hand rule according to the directions of xi and zi−1 , respectively. For industrial robots, the link twist angle, α i , is usually a multiple of π /2 radians. The base frame B0 (x0 , y0 , z0 ) = G(X, Y, Z) is the global frame for an immobile robot. It is convenient to choose the Z-axis along the axis of joint 1 and set the origin O where the axes of the G frame are colinear or parallel with the axes of the frame B1 at rest position. The configuration of a robot at which all the joint variables are zero is called the home configuration or rest position, which is the reference configuration for all motions of a robot. The best rest position is where it makes as many axes parallel to each other and coplanar as possible. For convenience, we can relax the strict DH definition for the direction of xi , so that it points from zi to zi−1 and still obtains a valid DH parameterization. A flip direction of the xi -axis is to set a more convenient reference frame when most of the joint parameters are zero.

5.1 Denavit–Hartenberg Notation

229

Fig. 5.5 Illustration of a 3R planar manipulator robot and DH frames of each link Table 5.1 DH parameter table for establishment of link frames Frame no. 1 2 ······ j ······ n

ai a1 a2 ······ aj ······ an

αi α1 α2 ······ αj ······ αn

di d1 d2 ······ dj ······ dn

θi θ1 θ2 ······ θj ······ θn

Table 5.2 DH table for the 3R planar manipulator robot of Fig. 5.5 Frame no. 1 2 3

ai l1 l2 l3

αi 0 0 0

di 0 0 0

θi θ1 θ2 θ3

10. A DH parameter table helps to establish a systematic link frame. As shown in Table 5.1, a DH table has five columns for frame index and four DH parameters. The rows of the four DH parameters for each frame will be filled by constant parameters and the joint variable. The joint variable can be found by considering what frames and links will move with each varying active joint.

Example 159 DH table and coordinate frames for 3R planar manipulator. An application of DH method on a planar multiDOF robot. An RRR manipulator is a planar robot with three parallel revolute joints. Figure 5.5 illustrates a 3R planar manipulator robot. The DH table can be filled as shown in Table 5.2, and the link coordinate frames can be set up as shown in Fig. 5.5. Example 160 Coordinate frames for a 3R P U MA robot. Application of DH method on a manipulator robot. This is a very important robot in industry. A P U MA manipulator is shown in Fig. 5.6. It has RRR main structure, without considering the end-effector of the robot. Coordinate frames attached to the links of the robot are indicated in the figure and tabulated in Table 5.3. The joint axes of an R⊥RR manipulator are called waist z0 , shoulder z1 , and elbow z2 . Typically, the joint axes z1 and z2 are parallel, and z0 is perpendicular to z1 .

230

5 Forward Kinematics

Fig. 5.6 3R P U MA manipulator and links coordinate frame

Fig. 5.7 Stanford arm RRP:RRR Table 5.3 DH table for the 3R P U MA manipulator shown in Fig. 5.6 Frame no. 1 2 3

ai 0 l2 0

αi 90 deg 0 −90 deg

di 0 l1 0

θi θ1 θ2 θ3

Example 161 Stanford arm. Application of DH method on Stanford robot. This is one of the very first robots in robotic research. Stanford arm is an RRP robot. A Stanford arm with a spherical wrist RRR is illustrated in Fig. 5.7.

5.1 Denavit–Hartenberg Notation

231

Fig. 5.8 Base and tools frames Table 5.4 DH table for Stanford arm shown in Fig. 5.7 Frame no. 1 2 3 4 5 6

ai 0 0 0 0 0 0

αi −90 deg 90 deg 0 −90 deg 90 deg 0

di l1 l2 d3 0 0 l6

θi θ1 θ2 0 θ4 θ5 θ6

The DH parameters of the Stanford arm are tabulated in Table 5.4. This robot has 6 DOF s indicated by joint variables θ 1 , θ 2 , d3 , θ 4 , θ 5 , θ 6 . Example 162 Special coordinate frames. The base and end-effector links have special coordinate frame because they connected to another link only from one side. In a robotic manipulator, some frames have special names. The base frame B0 or G is the grounded link in which the robot is installed. It is in the base frame that every kinematic information must be calculated. The departure point, path of motion, and the arrival point of the end- effector are all defined in this frame. The base frame is illustrated in Fig. 5.8. The station frame S, also called world frame or universe frame, is the frame in which the kinematics of the robot are calculated. Station frame is important when several robots are installed in a workshop, or the robot is mobile. The wrist frames W are installed at the wrist point where the hand is attached to the last arm of a robot. The hands of industrial robots are usually attached to a wrist mechanism with three orthogonal revolute axes. The tools frame T is attached to the end of any tools the robot is holding. It may also be called the end-effector frame or final frame. When the hand is empty, the tools frame is chosen with its origin between the fingertips of the robot. The tools frame is illustrated in Fig. 5.8. The goal frame F is the location where the robot is to move a tool. The goal frame is specified relative to the station frame. Example 163 Transforming the frame Bi−1 into Bi . The steps to construct DH transformation matrix between two neighbor coordinate frames. Two neighbor coordinate frames can be brought into coincidence by several sequences of translations and rotations. However, to make a transformation matrix i−1 Ti we should start from the moment that coordinate frame Bi is coincident with Bi−1 , and then move Bi to come to its present position. Therefore, we must follow the following sequence of motions: 1. 2. 3. 4.

Rotate frame Bi through α i about the xi−1 -axis. Translate frame Bi along the xi−1 -axis by distance ai . Rotate frame Bi through θ i about the zi−1 -axis. Translate frame Bi along the zi−1 -axis by distance di .

232

5 Forward Kinematics

During these maneuvers, Bi−1 acts as the global coordinate frame for the local coordinate frame Bi , and these motions are about and along the global axes. The following prescribed set of two rotations and two translations is also a straightforward method to move the frame Bi−1 to coincide with the frame Bi . This is a method to make a transformation matrix i Ti−1 : 1. 2. 3. 4.

Translate frame Bi−1 along the zi−1 -axis by distance di . Rotate frame Bi−1 through θ i around the zi−1 -axis. Translate frame Bi−1 along the xi -axis by distance ai . Rotate frame Bi−1 through α i about the xi -axis.

Example 164 Shortcomings of the Denavit–Hartenberg method. DH method has some problems. Here are two important ones. The Denavit–Hartenberg method for describing link coordinate frames is neither unique nor the best method. The drawbacks of the DH method are: 1. The successive coordinate axes are defined in such a way that the origin oi and axis xi are defined on the common perpendicular to adjacent link axes. This may be a difficult task depending on the geometry of the links and may produce singularity. 2. The DH notation cannot be extended to ternary and compound links.

5.2

Transformation Between Adjacent Coordinate Frames

The coordinate frame Bi is fixed to the link (i) and the coordinate frame Bi−1 is fixed to the link (i − 1). Based on the Denavit–Hartenberg convention, the transformation matrix i−1 Ti to transform coordinate frames Bi into Bi−1 is represented as a product of four basic transformations using the parameters of link (i) and joint i. i−1

Ti = Dzi−1 ,di Rzi−1 ,θ i Dxi−1 ,ai Rxi−1 ,αi ⎡ ⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ ⎥ =⎢ ⎣ 0 ⎦ sin α i cos α i di 0 0 0 1 ⎤ 1 0 0 0 ⎢ 0 cos α i − sin α i 0 ⎥ ⎥ Rxi−1 ,αi = ⎢ ⎣ 0 sin α i cos α i 0 ⎦ 0 0 0 1 ⎡ ⎤ 1 0 0 ai ⎢0 1 0 0 ⎥ ⎥ Dxi−1 ,ai = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎡ ⎤ cos θ i − sin θ i 0 0 ⎢ sin θ i cos θ i 0 0 ⎥ ⎥ Rzi−1 ,θ i = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ Dzi−1 ,di = ⎢ ⎣ 0 0 1 di ⎦ 000 1

(5.5)

⎡

(5.6)

(5.7)

(5.8)

(5.9)

5.2 Transformation Between Adjacent Coordinate Frames

233

Therefore, the transformation equation from coordinate frame Bi (xi , yi , zi ), to its previous coordinate frame Bi−1 (xi−1 , yi−1 , zi−1 ), is ⎡ ⎡ ⎤ ⎤ xi xi−1 ⎢ yi−1 ⎥ ⎢ yi ⎥ i−1 ⎢ ⎥ ⎥ Ti ⎢ (5.10) ⎣ zi ⎦ ⎣ zi−1 ⎦ = 1 1 where

⎡

⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1

(5.11)

This 4 × 4 homogenous transformation matrix may be partitioned into two submatrices, which represent a unique rotation combined with a unique translation to produce the same rigid motion required to move from Bi to Bi−1 . i−1

Ti =

" i−1

Ri 0

i−1

di 1

# (5.12)

⎤ cos θ i − sin θ i cos α i sin θ i sin α i i−1 Ri = ⎣ sin θ i cos θ i cos α i − cos θ i sin α i ⎦ 0 sin α i cos α i ⎡ ⎤ ai cos θ i i−1 di = ⎣ ai sin θ i ⎦ di ⎡

(5.13)

(5.14)

The inverse of the homogenous transformation matrix (5.11) is i

Ti−1 =

i−1

⎡

Ti−1

⎤

(5.15)

cos θ i sin θ i 0 −ai ⎢ − sin θ i cos α i cos θ i cos α i sin α i −di sin α i ⎥ ⎥ =⎢ ⎣ sin θ i sin α i − cos θ i sin α i cos α i −di cos α i ⎦ 0 0 0 1 Proof 1 Equation (5.5) indicates the direct method to prove the DH homogenous transformation matrix (5.11). However, we may provide alternative proofs to look at this matrix from different viewpoints. Assume the coordinate frames B2 (x2 , y2 , z2 ) and B1 (x1 , y1 , z1 ) in Fig. 5.9 are set up based on Denavit–Hartenberg rules. The position vector of point P can be expressed in frame B1 (x1 , y1 , z1 ) using 2 rP and 1 s2 , 1

rP = 1 R2 2 rP + 1 s2

(5.16)

which, in homogenous coordinate representation, is equal to ⎡

⎤ ⎡ ⎤⎡ ⎤ cos(ˆı2 , ıˆ1 ) cos(jˆ2 , ıˆ1 ) cos(kˆ2 , ıˆ1 ) s2x x1 x2 ⎢ y1 ⎥ ⎢ cos(ˆı2 , jˆ1 ) cos(jˆ2 , jˆ1 ) cos(kˆ2 , jˆ1 ) s2 ⎥ ⎢ y2 ⎥ y ⎥⎢ ⎢ ⎥=⎢ ⎥ ⎣ z1 ⎦ ⎣ cos(ˆı , kˆ ) cos(jˆ , kˆ ) cos(kˆ , kˆ ) s ⎦ ⎣ z2 ⎦ 2 1 2 1 2 1 2z 1 1 0 0 0 1

(5.17)

234

5 Forward Kinematics

Fig. 5.9 Two coordinate frames based on Denavit–Hartenberg rules

Using the parameters introduced in Fig. 5.9, Eq. (5.17) becomes ⎤ ⎡ ⎤⎡ ⎤ cθ 2 −sθ 2 cα 2 sθ 2 sα 2 a2 cθ 2 x2 x1 ⎢ y1 ⎥ ⎢ sθ 2 cθ 2 cα 2 −cθ 2 sα 2 a2 sθ 2 ⎥ ⎢ y2 ⎥ ⎥⎢ ⎥ ⎢ ⎥=⎢ ⎣ z1 ⎦ ⎣ 0 sα 2 cα 2 d 2 ⎦ ⎣ z2 ⎦ 1 0 0 0 1 1 ⎡

(5.18)

If we substitute the coordinate frame B1 by Bi−1 (xi−1 , yi−1 , zi−1 ), and B2 by Bi (xi , yi , zi ), we may rewrite the above equation in the required form: ⎤ ⎡ ⎤⎡ ⎤ ⎡ cθ i −sθ i cα i sθ i sα i ai cθ i xi xi−1 ⎢ yi−1 ⎥ ⎢ sθ i cθ i cα i −cθ i sα i ai sθ i ⎥ ⎢ yi ⎥ ⎥ ⎢ ⎥⎢ ⎥ ⎢ (5.19) ⎣ zi−1 ⎦ = ⎣ 0 sα i cα i d i ⎦ ⎣ zi ⎦ 1 0 0 0 1 1 Following the inversion rule of homogenous transformation matrix, i−1

i−1

Ti =

Ti−1

=

"G "G

RB G s 0 1

#

RBT − G RBT G s 0 1

(5.20) # (5.21)

we also find the required inverse transformation (5.15). ⎡

⎤ ⎡ xi cθ i sθ i ⎢ yi ⎥ ⎢ −sθ i cα i cθ i cα i ⎢ ⎥=⎢ ⎣ zi ⎦ ⎣ sθ i sα i −cθ i sα i 1 0 0

⎤⎡ ⎤ xi−1 0 −ai ⎢ ⎥ sα i −di sα i ⎥ ⎥ ⎢ yi−1 ⎥ ⎦ ⎣ cα i −di cα i zi−1 ⎦ 0 1 1

(5.22)

 Proof 2 An alternative method to find i Ti−1 is to follow the sequence of translations and rotations that brings the frame Bi−1 to the present configuration starting from a coincident position with the frame Bi . Working with two frames can also be equivalently described by B ≡ Bi and G ≡ Bi−1 , so all the following rotations and translations are about and along the local coordinate frame axes. Inspecting Fig. 5.9 shows that:

5.2 Transformation Between Adjacent Coordinate Frames

1. 2. 3. 4.

235

Frame Bi−1 is translated along the local zi -axis by distance −di . The displaced frame Bi−1 is rotated through −θ i about the local zi -axis. The displaced frame Bi−1 is translated along the local xi -axis by distance −ai . The displaced frame Bi−1 is rotated through −α i about the local xi -axis. Following these displacement sequences, the transformation matrix i Ti−1 would be i

Ti−1 = Rxi ,−αi Dxi ,−ai Rzi ,−θ i Dzi ,−di ⎡ ⎤ cos θ i sin θ i 0 −ai ⎢ − sin θ i cos α i cos θ i cos α i sin α i −di sin α i ⎥ ⎥ =⎢ ⎣ sin θ i sin α i − cos θ i sin α i cos α i −di cos α i ⎦ 0 0 0 1

where

⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ Dzi ,−di = ⎢ ⎣ 0 0 1 −di ⎦ 000 1 ⎡ ⎤ cos θ i − sin θ i 0 0 ⎢ sin θ i cos θ i 0 0 ⎥ ⎥ Rzi ,−θ i = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 1 0 0 −ai ⎢0 1 0 0 ⎥ ⎥ Dxi ,−ai = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎤ ⎡ 1 0 0 0 ⎢ 0 cos α i − sin α i 0 ⎥ ⎥ Rxi ,−αi = ⎢ ⎣ 0 sin α i cos α i 0 ⎦ 0 0 0 1

(5.23)

⎡

(5.24)

(5.25)

(5.26)

(5.27)

−1 Using i−1 Ti = i Ti−1 we find: i−1

−1 Ti = i Ti−1 ⎤ ⎡ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ ⎥ =⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1

(5.28)

 Example 165 DH transformation matrices for a 2R planar manipulator. Application of DH method and forward kinematics of a planar two DOF , 2R planar robot. Figure 5.10 illustrates an RR planar manipulator and its DH link coordinate frames. Based on DH Table 5.5 we can find the transformation matrices from frame Bi to frame Bi−1 by direct substitution of DH parameters in Eq. (5.11). Therefore, ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(5.29)

236

5 Forward Kinematics

Fig. 5.10 Illustration of a 2R planar manipulator robot and DH frames of every link Table 5.5 DH table for 2R planar manipulator shown in Fig. 5.10 Frame no. 1 2

⎡

ai l1 l2

αi 0 0

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0

di 0 0

θi θ1 θ2

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(5.30)

and consequently the forward kinematics of the 2R planar robot will be 0

T2 = 0 T1 1 T2 ⎡ ⎤ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l1 sθ 1 + l2 s (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(5.31)

Example 166 Classification of industrial robot links. Most of industrial robots are built by assembling the links that are classified here. This example is important to be used as a reference to model all industrial robots easily. A robot link is identified and classified by its joints at both ends. Such classification determines the DH homogenous transformation matrix to go from the distal joint coordinate frame Bi to the proximal joint coordinate frame Bi−1 . There are 12 types of links to make an industrial robot. The transformation matrix for each type depends solely on the proximal joint and angle between the z axes. The 12 types of transformation matrices are summarized in Table 5.6. Their geometric illustration and their associated DH matrices are presented in Appendix D of the book as a reference to be used to solve forward kinematics of industrial robots.

5.2 Transformation Between Adjacent Coordinate Frames

237

Table 5.6 DH classification of industrial robot links. The angle in parenthesis is α i , the angle from zi−1 to zi about xi 1 2 3 4 5 6 7 8 9 10 11 12

RR(0) RR(180) R⊥R(90) R⊥R(−90) RR(90) RR(−90) PR(0) PR(180) P⊥R(90) P⊥R(−90) PR(90) PR(−90)

or or or or or or or or or or or or

RP(0) RP(180) R⊥P(90) R⊥P(−90) RP(90) RP(−90) PP(0) PP(180) P⊥P(90) P⊥P(−90) PP(90) PP(−90)

1. Link with RR(0) or RP(0) joints: a = const, α i = 0, di = const, θ i is the variable. ⎡

cos θ i − sin θ i ⎢ sin θ i cos θ i i−1 Ti = ⎢ ⎣ 0 0 0 0

⎤ 0 ai cos θ i 0 ai sin θ i ⎥ ⎥ 1 di ⎦ 0 1

(5.32)

2. Link with RR(180) or RP(180): a = const, α i = π , di = const, θ i is the variable. ⎡

⎤ cos θ i sin θ i 0 ai cos θ i ⎢ sin θ i − cos θ i 0 ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 −1 di ⎦ 0 0 0 1

(5.33)

3. Link with R⊥R(90) or R⊥P(90) joints: a = const, α i = π /2, di = const, θ i is the variable. ⎡

cos θ i ⎢ sin θ i i−1 Ti = ⎢ ⎣ 0 0

⎤ 0 sin θ i ai cos θ i 0 − cos θ i ai sin θ i ⎥ ⎥ 1 0 di ⎦ 0 0 1

(5.34)

4. Link with R⊥R(−90) or R⊥P(−90) joints: a = const, α i = −π/2, di = const, θ i is the variable. ⎡

cos θ i ⎢ sin θ i i−1 Ti = ⎢ ⎣ 0 0

⎤ 0 − sin θ i ai cos θ i 0 cos θ i ai sin θ i ⎥ ⎥ −1 0 di ⎦ 0 0 1

(5.35)

5. Link with RR(90) or RP(90) joints: a = 0, α i = π /2, di = const, θ i is the variable. ⎡

cos θ i ⎢ sin θ i i−1 Ti = ⎢ ⎣ 0 0

0 sin θ i 0 − cos θ i 1 0 0 0

⎤ 0 0⎥ ⎥ di ⎦ 1

(5.36)

238

5 Forward Kinematics

6. Link with RR(−90) or RP(−90) joints: a = 0, α i = −π/2, di = const, θ i is the variable. ⎡

cos θ i ⎢ sin θ i i−1 Ti = ⎢ ⎣ 0 0

0 − sin θ i 0 cos θ i −1 0 0 0

⎤ 0 0⎥ ⎥ di ⎦ 1

(5.37)

7. Link with PR(0) or PP(0) joints. a = const, α i = 0, di is the variable, θ i = 0. ⎡

⎤ 1 0 0 ai ⎢0 1 0 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 1 di ⎦ 000 1

(5.38)

8. Link with PR(180) or PP(180) joints. a = const, α i = π , di is the variable, θ i = 0. ⎤ 1 0 0 ai ⎢ 0 −1 0 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 −1 di ⎦ 0 0 0 1 ⎡

(5.39)

9. Link with P⊥R(90) or P⊥P(90) joints. a = const, α i = π/2, di is the variable, θ i = 0. ⎡

⎤ 1 0 0 ai ⎢ 0 0 −1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 00 0 1

(5.40)

10. Link with P⊥R(−90) or P⊥P(−90) joints. a = const, α i = −π/2, di is the variable, θ i = 0. ⎤ 1 0 0 ai ⎢0 0 1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1 ⎡

(5.41)

11. Link with PR(90) or PP(90) joints. a = 0, α i = π /2, di is the variable, θ i = 0. ⎡

⎤ 10 0 0 ⎢ 0 0 −1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 00 0 1

(5.42)

12. Link with PR(−90) or PP(−90) joints. a = 0, α i = −π/2, di is the variable, θ i = 0. ⎡

⎤ 1 0 0 0 ⎢0 0 1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1

(5.43)

Example 167 Assembling industrial links to make a manipulator. Illustration of making a robot by assembling classified links. Industrial manipulators are usually made by connecting the introduced industrial links in Example 166. A manipulator is a combination of three links that provide three DOF s to the tip point in a Cartesian space. The articulated and spherical manipulators are two common and practical manipulators. Figure 5.11a and b illustrates how we make these manipulators by connecting the proper industrial links.

5.2 Transformation Between Adjacent Coordinate Frames

239

Fig. 5.11 The articulated and spherical manipulators are two common and practical manipulators

Fig. 5.12 Alternative method to derive the Denavit–Hartenberg coordinate transformation

Example 168 DH coordinate transformation based on vector addition. Alternative method to construct DH transformation matrix using vector addition. The DH transformation from a coordinate frame to another may be described by a vector addition. The coordinates of a point P in frame B1 , as shown in Fig. 5.12, are given by a vector equation, −→ B1 − o1 P

=

−→ B2 − o2 P

+

→ B1 − o− 1 o2

(5.44)

where  T = s1 s2 s3 T −→  B1 − o 1 P = x 1 y 1 z1 T −→  B2 − o 2 P = x 2 y 2 z2

→ B1 − o− 1 o2

(5.45) (5.46) (5.47)

They must be expressed in the same coordinate frame. Employing cosines of the angles between axes of the two coordinate frames, we have

240

5 Forward Kinematics

x1 = x2 cos(x2 , x1 ) + y2 cos(y2 , x1 ) + z2 cos(z2 , x1 ) + s1 y1 = x2 cos(x2 , y1 ) + y2 cos(y2 , y1 ) + z2 cos(z2 , y1 ) + s2 z1 = x2 cos(x2 , z1 ) + y2 cos(y2 , z1 ) + z2 cos(z2 , z1 ) + s3 1 = x2 (0) + y2 (0) + z2 (0) + 1

(5.48)

The transformation (5.48) can be rearranged to be described with the homogenous matrix transformation. ⎤ ⎡ ⎤⎡ ⎤ cos(x2 , x1 ) cos(y2 , x1 ) cos(z2 , x1 ) s1 x2 x1 ⎢ y1 ⎥ ⎢ cos(x2 , y1 ) cos(y2 , y1 ) cos(z2 , y1 ) s2 ⎥ ⎢ y2 ⎥ ⎥⎢ ⎥ ⎢ ⎥=⎢ ⎣ z1 ⎦ ⎣ cos(x2 , z1 ) cos(y2 , z1 ) cos(z2 , z1 ) s3 ⎦ ⎣ z2 ⎦ 1 0 0 0 1 1 ⎡

(5.49)

In Fig. 5.12 the axis x2 has been selected such that it lies along the shortest common perpendicular between axes z1 and z2 . The axis y2 completes a right-handed set of coordinate axes. Other parameters are defined as follows: 1. 2. 3. 4.

a is the distance between axes z1 and z2 . α is the twist angle that screws the z1 -axis into the z2 -axis along a. d is the distance from the x1 -axis to the x2 -axis. θ is the angle that screws the x1 -axis into the x2 -axis along d. Using these definitions, the homogenous transformation matrix becomes ⎡

⎤ ⎡ ⎤⎡ ⎤ cos θ − sin θ cos α − sin θ sin α a cos θ x2 x1 ⎢ y1 ⎥ ⎢ sin θ cos θ cos α cos θ sin α a sin θ ⎥ ⎢ y2 ⎥ ⎢ ⎥=⎢ ⎥⎢ ⎥ ⎣ z1 ⎦ ⎣ 0 − sin α cos α d ⎦ ⎣ z2 ⎦ 1 0 0 0 1 1

(5.50)

rP = 1 T2 2 rP

(5.51)

T2 = (a, α, d, θ )

(5.52)

or 1

where 1

The DH parameters a, α, θ, d belong to B2 and define the configuration of B2 in B1 . In general, the DH parameters ai , α i , θ i , di belong to Bi and define the configuration of Bi with respect to Bi−1 : i−1

Ti = (ai , α i , di , θ i )

(5.53)

Example 169 Trebuchet kinematics as a multibody. How to apply DH on multibody systems. Introducing Sina coordinate frames. The trebuchet, shown in Fig. 5.13, is a shooting weapon of war powered by a falling massive counterweight m1 . The beam AB is pivoted to the chassis with two unequal sections a and b. The counterweight m1 is hinged at the shorter arm of the beam at a distance c from the end A. The mass of the projectile is m2 , and it is at the end of a rope with a length l attached to the end B of the longer arm of the beam. To analyze the trebuchet as a robotic system, we attach a global coordinate frame G at the fixed pivot M and three body frames B, B1 , B2 to the three moving bodies as shown in Fig. 5.13. The three independent variables α, θ, and γ define the relative angular positions of the bodies. Let us assume the parameters a, b, c, l, m1 , m2 to be constant and find the transformation matrices to determine the coordinates in the relative coordinate frames. We also attach two Sina coordinate frames B3 and B4 to the beam at joints A and B to simplify the coordinate transformations. A Sina coordinate frame is an extra frame that will be installed to simplify calculations of the main coordinate frames transformations. We may find B TG by using the transformation matrix of a zero length link RR(0) or equivalently by turning B a rotation −θ about the z-axis: ⎡ ⎤ cos −θ sin −θ 0 0 ⎢ − sin −θ cos −θ 0 0 ⎥ B ⎥ TG = Rz,−θ = ⎢ (5.54) ⎣ 0 0 1 0⎦ 0 0 01

5.2 Transformation Between Adjacent Coordinate Frames

241

Fig. 5.13 The kinematic model of a trebuchet

The Sina frame B3 is connected to the main frame B by a translation b along x3 -axis. Similarly, the Sina frame B4 is connected to B by a translation −a along x4 -axis. ⎤ ⎡ 1 0 0 −b ⎢0 1 0 0 ⎥ 3 ⎥ TB = Dx3 ,b = ⎢ (5.55) ⎣0 0 1 0 ⎦ 000 1 ⎤ ⎡ 100a ⎢0 1 0 0⎥ 4 ⎥ TB = Dx4 ,−a = ⎢ (5.56) ⎣0 0 1 0⎦ 0001 We find 1 T3 by turning B1 a rotation −γ about the z1 -axis: ⎡

1

T3 = Rz1 ,−γ

⎤ cos −γ sin −γ 0 0 ⎢ − sin −γ cos −γ 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0⎦ 0 0 01

(5.57)

Using the same method, we find 2 T4 by turning B2 a rotation −α about the z2 -axis: ⎡

2

T4 = Rz2 ,−α

⎤ cos −α sin −α 0 0 ⎢ − sin −α cos −α 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0⎦ 0 0 01

(5.58)

Therefore, 1 TB and 2 TB are ⎡

cos γ ⎢ sin γ 1 TB = 1 T3 3 TB = ⎢ ⎣ 0 0 ⎡ cos α ⎢ sin α 2 TB = 2 T4 4 TB = ⎢ ⎣ 0 0

⎤ 0 −b cos γ 0 −b sin γ ⎥ ⎥ ⎦ 1 0 0 1 ⎤ − sin α 0 a cos α cos α 0 a sin α ⎥ ⎥ 0 1 0 ⎦ 0 0 1 − sin γ cos γ 0 0

(5.59)

(5.60)

242

5 Forward Kinematics

By matrix multiplication and a homogenous matrix inverse calculation, we determine the required transformation matrices to go to the G frame: ⎡ ⎤ cos θ sin θ 0 0 ⎢ − sin θ cos θ 0 0 ⎥ G ⎥ TB = B TG−1 = ⎢ (5.61) ⎣ 0 0 1 0⎦ 0 0 01 G

T1 =

G

B

TG−1 1 TB−1 =

1

TB

B

TG

−1

⎤ cos (θ + γ ) sin (θ + γ ) 0 b cos θ ⎢ − sin (θ + γ ) cos (θ + γ ) 0 −b sin θ ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

G

T2 =

⎡

TB B T1 =

G

⎡

TB B T2 =

B

TG−1 2 TB−1 =

2

TB

B

TG

(5.62)

−1

⎤ cos (θ + α) sin (θ + α) 0 −a cos θ ⎢ − sin (θ + α) cos (θ + α) 0 a sin θ ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(5.63)

 T  T Therefore, the global coordinates of m1 at 1 rm1 = c 0 0 and m2 at 2 rm2 = l 0 0 are ⎡

G

rm1 =

G

T1 1 rm1

⎤ b cos θ + c cos (θ + γ ) ⎢ −b sin θ − c sin (θ + γ ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 1 ⎡

G

rm2 =

G

T2 2 rm2

⎤ l cos (θ + α) − a cos θ ⎢ a sin θ − l sin (θ + α) ⎥ ⎥ =⎢ ⎣ ⎦ 0 1

(5.64)

(5.65)

Example 170 The same DH transformation matrix. Decomposition of DH matrix in different methods, all equivalent. In the DH method of setting coordinate frames, because a translation D and a rotation R are along and about the same axis, it is immaterial if we first apply the translation D and then the rotation R or vice versa. Therefore, we can interchange the order of D and R along and about the same axis and obtain the same DH transformation matrix 5.11. i−1

Ti = Dzi ,di Rzi ,θ i Dxi ,ai Rxi ,αi = Rzi ,θ i Dzi ,di Dxi ,ai Rxi ,αi = Dzi ,di Rzi ,θ i Rxi ,αi Dxi ,ai = Rzi ,θ i Dzi ,di Rxi ,αi Dxi ,ai

(5.66)

Example 171 Non-DH transformation. DH is not the only way to construct transformation matrices between links of a robot, and it is sometimes not possible as well. Here is an illustration how to deal with such cases. The robot kinematic problems are always related to a correct expression of the transformation matrix between two coordinate frames. The classical method is to set up the coordinate frames based on DH rules and use the DH transformation matrix. However, when the DH rules cannot be applied, we recommend determining the homogenous transformation matrix by a proper combination of principal rotations and translations. Consider two arbitrary coordinate frames B and G. The

5.2 Transformation Between Adjacent Coordinate Frames

243

Fig. 5.14 A 2 DOF polar manipulator with non-DH coordinate frames

homogenous transformation matrix B TG can be found by moving B to its present position from the coincident configuration with G. The rotations are positive about the local axes of B and the translations are negative along the local axes of B. Figure 5.14 depicts a polar manipulator with two DOF s and a set of coordinate frames that are not set up exactly according to the DH rules. To determine the link’s transformation matrices 1 T2 , 2 T3 , and 1 T3 in such cases, we may always follow the method of homogenous transformations. The frame B1 can go to B2 by a rotation Rz2 ,θ 2 followed by Ry2 ,90 . There is no translation between B1 and B2 . Therefore, 1

T  R2 = Ry2 ,90 Rz2 ,θ 2 = RzT2 ,θ 2 RyT2 ,90 ⎡ ⎡ ⎤T cos π 0 − sin π cos θ 2 sin θ 2 0 2 2 ⎢ = ⎣ − sin θ 2 cos θ 2 0 ⎦ ⎢ 0 ⎣ 0π 1 π 0 0 1 sin 0 cos 2 2 ⎡ ⎤ 0 − sin θ 2 cos θ 2 = ⎣ 0 cos θ 2 sin θ 2 ⎦ −1 0 0

and hence,

⎤T ⎥ ⎥ ⎦

⎤ 0 − sin θ 2 cos θ 2 0 ⎢ 0 cos θ 2 sin θ 2 0 ⎥ 1 ⎥ T2 = ⎢ ⎣ −1 0 0 0⎦ 0 0 0 1

(5.67)

⎡

(5.68)

It is also possible to determine 1 R2 directly from the definition of the directional cosines: ⎡

⎤ ⎡ ⎤ Iˆ · ıˆ Iˆ · jˆ Iˆ · kˆ ıˆ1 · ıˆ2 ıˆ1 · jˆ2 ıˆ1 · kˆ2 ⎢ ⎥ ⎢ ⎥ 1 R2 = ⎣ Jˆ · ıˆ Jˆ · jˆ Jˆ · kˆ ⎦ = ⎣ jˆ1 · ıˆ2 jˆ1 · jˆ2 jˆ1 · kˆ2 ⎦ Kˆ · ıˆ Kˆ · jˆ Kˆ · kˆ kˆ1 · ıˆ2 kˆ1 · jˆ2 kˆ1 · kˆ2 π ⎡ ⎤ π + θ2 cos θ 2 cos cos ⎢ ⎥ 2 2 π ⎥ ⎢ π ⎢ = ⎢ cos cos θ 2 cos − θ2 ⎥ ⎥ 2 2 ⎣ ⎦ π π cos π cos cos 2 2 ⎡ ⎤ 0 − sin θ 2 cos θ 2 = ⎣ 0 cos θ 2 sin θ 2 ⎦ −1 0 0

(5.69)

244

5 Forward Kinematics

Fig. 5.15 A spherical robot made by a spherical manipulator equipped with a spherical wrist Table 5.7 DH parameter table for Stanford arm Frame no. 1 2 3 4 5 6

ai 0 0 0 0 0 0

αi −90 deg 90 deg 0 −90 deg 90 deg 0

di l1 l2 d3 0 0 0

θi θ1 θ2 0 θ4 θ5 θ6

The final transformation matrix is only a translation along z3 , ⎡

⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 d3 ⎦ 000 1 which provides

⎤ 0 − sin θ 2 cos θ 2 d3 cos θ 2 ⎢ 0 cos θ 2 sin θ 2 d3 sin θ 2 ⎥ 1 ⎥ T3 = 1 T2 2 T3 = ⎢ ⎣ −1 ⎦ 0 0 0 0 0 0 1

(5.70)

⎡

(5.71)

Example 172  DH application for spherical robot. Applying DH method on a 6 DOF spherical robot. Figure 5.15 illustrates a spherical manipulator equipped with a spherical wrist. A spherical manipulator is an RRP arm. The associated DH table of the robot is given in Table 5.7. The link-joint classifications of the robot are tabulated in Table 5.8. Using Appendix D, we determine the homogenous transformation matrices for the link-joint classification in Table 5.8 to move from Bi to Bi−1 : ⎡ ⎤ cos θ 1 0 − sin θ 1 0 ⎢ sin θ 1 0 cos θ 1 0 ⎥ 0 ⎥ T1 = ⎢ (5.72) ⎣ 0 −1 0 l1 ⎦ 0 0 0 1

5.2 Transformation Between Adjacent Coordinate Frames

245

Table 5.8 Link-joint classifications for Stanford arm Link no. 1 2 3 4 5 6

Type RR(−90) RP(90) PR(0) RR(−90) RR(90) RR(0)

⎡

⎤ cos θ 2 0 sin θ 2 0 ⎢ sin θ 2 0 − cos θ 2 0 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 1 0 l2 ⎦ 0 0 0 1 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 d3 ⎦ 000 1 ⎡ ⎤ cos θ 4 0 − sin θ 4 0 ⎢ sin θ 4 0 cos θ 4 0 ⎥ 3 ⎥ T4 = ⎢ ⎣ 0 −1 0 0⎦ 0 0 0 1 ⎡ ⎤ cos θ 5 0 sin θ 5 0 ⎢ sin θ 5 0 − cos θ 5 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1 ⎤ ⎡ cos θ 6 − sin θ 6 0 0 ⎢ sin θ 6 cos θ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01

(5.73)

(5.74)

(5.75)

(5.76)

(5.77)

Example 173  DH application for a slider-crank planar linkage. How to apply DH method on closed-loop mechanisms. For a closed-loop robot or a mechanism there would also be a connection between the first and last links. The DH convention will not be satisfied by this connection. Figure 5.16 depicts a planar slider-crank linkage R⊥PRRR and DH coordinate frames installed on each link. Table 5.9 summarizes the DH parameters of all links of the mechanism. A closed-loop robot provides a constraint on transformation matrices, (5.78) [T ] = 1 T2 2 T3 3 T4 4 T1 = I4 where the transformation matrix [T ] contains elements that are functions of a2 , d, a3 , θ 3 , a4 , θ 4 , θ 1 . The parameters a2 , a3 , and a4 are constant, while d, θ 3 , θ 4 , θ 1 are variables. Assuming θ 1 is input and specified, we may solve for other unknown variables θ 3 , θ 4 , d by equating the corresponding elements of [T ] and I. Example 174  Non-standard Denavit–Hartenberg notation. Instead of installing coordinate frame at the end of links, they could be set at the beginning. Such installation was also presented in the beginning of robotic application, but they are non-standard today. This is how DH matrix would be if we use the non-standard way to set coordinate frames.

246

5 Forward Kinematics

Fig. 5.16 A planar slider-crank linkage, making a closed-loop or parallel mechanism Table 5.9 DH table for the slider-crank planar linkage shown in Fig. 5.16 Frame no. 1 2 3 4

ai a2 a3 a4 0

αi −90 deg 0 0 −90 deg

di d 0 0 0

θi 180 deg θ3 θ4 θ1

The Denavit–Hartenberg notation presented in this section is the standard DH method. However, we may adopt a different set of DH parameters, by setting the link coordinate frame Bi at proximal joint i instead of the distal joint i + 1 as shown in Fig. 5.17. The zi -axis is along the axis of joint i and the xi -axis is along the common normal of the zi and zi+1 axes, directed from zi to zi+1 axes. The yi -axis makes the Bi frame a right-handed coordinate frame. The parameterization of this shift of coordinate frames is: 1. 2. 3. 4.

ai is the distance between the zi and zi+1 axes along the xi -axis. α i is the angle from zi to zi+1 axes about the xi -axis. di is the distance between the xi−1 and xi axes along the zi -axis. θ i is the angle from the xi−1 and xi axes about the zi -axis.

The transformation matrix from Bi−1 to Bi utilizing the non-standard DH method is made of two rotations and two translations about and along the local coordinate axes of Bi−1 . 1−Rotate α i−1 about xi−1 . 2−Translate ai−1 along xi−1 . 3−Translate di along zi−1 . 4−Rotate θ i about zi−1 . i

Ti−1 = Rzi−1 ,θ i Dzi−1 ,−di Dxi−1 ,ai−1 Rxi−1 ,−αi−1 ⎤ ⎡ cos θ i sin θ i cos α i−1 sin θ i sin α i−1 −ai−1 cos θ i ⎢ − sin θ i cos θ i cos α i−1 cos θ i sin α i−1 ai−1 sin θ i ⎥ ⎥ =⎢ ⎣ 0 ⎦ − sin α i−1 cos α i−1 −di 0 0 0 1

(5.79)

5.3 Forward Position Kinematics of Robots

247

Fig. 5.17 Non-standard definition of DH parameters ai , α i , di , θ i defined for joint i and link (i)

Therefore, the transformation matrix from the Bi to Bi−1 for the non-standard DH method is ⎤ cos θ i − sin θ i 0 ai−1 ⎢ sin θ i cos α i−1 cos θ i cos α i−1 − sin α i−1 −di sin α i−1 ⎥ i−1 ⎥ Ti = ⎢ ⎣ sin θ i sin α i−1 cos θ i sin α i−1 cos α i−1 di cos α i−1 ⎦ 0 0 0 1 ⎡

(5.80)

An advantage of the non-standard DH method is that the rotation θ i is around the zi -axis and the joint number is the same as the coordinate number. Actuation force, which is exerted at joint i, is also at the same place as the coordinate frame Bi . Addressing the link’s geometrical characteristics, such as center of gravity, is more natural in this non-standard DH system. A disadvantage of the non-standard DH method is that the transformation matrix is a mix of i − 1 and i indices. Applying the standard or non-standard notation is a personal preference, as both methods can be applied effectively.

5.3

Forward Position Kinematics of Robots

Determining the end-effector position and orientation for a given set of joint variables is the main problem in forward kinematics. The forward or direct kinematics is the transformation of kinematic information from the robot joint variable space to the Cartesian coordinate space. This problem can be solved by determining transformation matrices 0 Ti to express the kinematic information of link (i) in the base link coordinate frame. The traditional way of producing forward kinematic equations for robotic manipulators is to proceed link by link using the Denavit–Hartenberg transformation matrices. For an n-DOF robot, at least n transformation matrices, one for every link, are required to determine the global coordinate of any point in any frame. The last frame attached to the final frame is usually set at the center of the gripper as is shown in Fig. 5.18. For a given set of joint variables and a set of link coordinate frames, the transformation matrices j Ti are uniquely determined. Therefore, the transformation matrices j Ti = j Ti (qk ) are functions of n joint variables qk , k = 1, 2, 3, · · · , n. Therefore, the position and orientation of the end-effector are also a unique function of the joint variables. The configuration of the multibody when all the joint variables are zero is called the rest position. Determination of the transformation matrices at the rest position is an applied checking procedure. If the links of a robot are arranged such that every link (i) has only one coordinate frame Bi and the frames are arranged sequentially, then: 0 Ti = 0 T1 1 T2 2 T3 3 T4 · · · i−1 Ti i = 1, 2, 3, · · · , n (5.81) Having 0 Ti , we can determine the coordinates of any point P of link (i) in the base frame when its coordinates are given in frame Bi : 0 rP = 0 Ti i rP i = 1, 2, 3, · · · , n (5.82)

248

5 Forward Kinematics

Fig. 5.18 The position of the final frame in the base frame

Fig. 5.19 A 2R or RR planar manipulator

Generally speaking, the number and labels of coordinate frames do not need to be consequential or increasing. The designer is free to number the frames in any order. However, assigning them sequentially provides simpler and more meaningful transformations. Example 175 A 2R planar manipulator. The 2R robot is the simplest practical robot that is used to demonstrate classical methods of analysis and control. The number of links and transformation matrices are minimum, so the principle of forward kinematics will be clear. This example will be reference for forward kinematics of 2R planar robots. Figure 5.19 illustrates a 2R or RR planar manipulator with two parallel revolute joints. Links (1) and (2) are both RR(0), and therefore, the transformation matrices 0 T1 , 1 T2 are ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(5.83)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(5.84)

5.3 Forward Position Kinematics of Robots

249

Fig. 5.20 A 3D RR planar manipulator

The forward kinematics of the manipulator is to determine the transformation matrices 0 Ti , i = 1, 2. The matrix 0 T1 is given in (5.83) and 0 T2 can be found by matrix multiplication: 0

T2 = 0 T1 1 T2 ⎡ ⎤ cos (θ 1 + θ 2 ) − sin (θ 1 + θ 2 ) 0 l2 cos (θ 1 + θ 2 ) + l1 cos θ 1 ⎢ sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 l2 sin (θ 1 + θ 2 ) + l1 sin θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(5.85)

If the robot had depth and nonzero offsets as is shown in Fig. 5.20 , the robot becomes a 3D RR planar manipulator. Links (1) and (2) are both RR(0) and move in parallel planes. Their transformation matrices 0 T1 , 1 T2 are ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ 1 d1 ⎦ 0 1

(5.86)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ 1 d2 ⎦ 0 1

(5.87)

The forward kinematics of the manipulator is to determine the transformation matrices 0 Ti , i = 1, 2. The matrix 0 T1 is given in (5.83) and 0 T2 can be found by matrix multiplication: 0

T2 = 0 T1 1 T2 ⎡ ⎤ cos (θ 1 + θ 2 ) − sin (θ 1 + θ 2 ) 0 l2 cos (θ 1 + θ 2 ) + l1 cos θ 1 ⎢ sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 l2 sin (θ 1 + θ 2 ) + l1 sin θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 d1 + d2 0 0 0 1

(5.88)

250

5 Forward Kinematics

Fig. 5.21 An RRR planar manipulator

Example 176 3R, RRR, planar manipulator forward kinematics. Application of DH matrices in forward kinematic analysis of a planar 3 DOF robot. Figure 5.21 illustrates an RRR planar manipulator. Utilizing the DH parameters indicated in Example 159 and applying Eq. (5.11), we can find the transformation matrices i−1 Ti for i = 3, 2, 1. It is also possible to use the transformation matrix (5.32) of Example 166, because links (1) and (2) are both RR(0). ⎡

cos θ 3 − sin θ 3 ⎢ sin θ 3 cos θ 3 2 T3 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 ⎢ sin θ 2 1 T2 = ⎢ ⎣ 0 0 ⎡ cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

⎤ 0 l3 cos θ 3 0 l3 sin θ 3 ⎥ ⎥ ⎦ 1 0 0 1

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1 ⎤ − sin θ 1 0 l1 cos θ 1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 0 1 0 0 0 1 − sin θ 2 cos θ 2 0 0

(5.89)

(5.90)

(5.91)

Therefore, the transformation matrix to relate the end-effector frame to the base frame is 0

T3 = 0 T1 1 T2 2 T3 ⎡ ⎤ cos (θ 1 + θ 2 + θ 3 ) − sin (θ 1 + θ 2 + θ 3 ) 0 r14 ⎢ sin (θ 1 + θ 2 + θ 3 ) cos (θ 1 + θ 2 + θ 3 ) 0 r24 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

r14 = l1 cos θ 1 + l2 cos (θ 1 + θ 2 ) + l3 cos (θ 1 + θ 2 + θ 3 ) r24 = l1 sin θ 1 + l2 sin (θ 1 + θ 2 ) + l3 sin (θ 1 + θ 2 + θ 3 )

(5.92)

5.3 Forward Position Kinematics of Robots

251

The origin of the frame B3 is the tip point of the robot. Its position is at ⎤ ⎡ ⎤ ⎡ 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) + l3 c (θ 1 + θ 2 + θ 3 ) ⎢ 0 ⎥ ⎢ l1 sθ 1 + l2 s (θ 1 + θ 2 ) + l3 s (θ 1 + θ 2 + θ 3 ) ⎥ 0 ⎥ ⎥ ⎢ T3 ⎢ ⎣0⎦ = ⎣ ⎦ 0 1 1

(5.93)

It means we can find the coordinate of the tip point in the base Cartesian coordinate frame if we have the geometry of the robot and all joint variables. X = l1 cos θ 1 + l2 cos (θ 1 + θ 2 ) + l3 cos (θ 1 + θ 2 + θ 3 )

(5.94)

Y = l1 sin θ 1 + l2 sin (θ 1 + θ 2 ) + l3 sin (θ 1 + θ 2 + θ 3 )

(5.95)

The rest position of the manipulator is lying on the x0 -axis where θ 1 = 0, θ 2 = 0, θ 3 = 0 because 0 T3 becomes ⎡

⎤ 1 0 0 l1 + l2 + l3 ⎢0 1 0 ⎥ 0 0 ⎥ T3 = ⎢ ⎣0 0 1 ⎦ 0 000 1

(5.96)

Having the transformation matrices i−1 Ti are enough to determine the configuration of every link in other links’ frames. The configuration of the link (2) in (0) is 0

T2 = 0 T1 1 T2 ⎡ ⎤ cos (θ 1 + θ 2 ) − sin (θ 1 + θ 2 ) 0 f14 ⎢ sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 f24 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(5.97)

f14 = l1 cos θ 1 + l2 cos (θ 1 + θ 2 ) f24 = l1 sin θ 1 + l2 sin (θ 1 + θ 2 ) and the configuration of the link (3) in (1) is 1

T3 = 1 T2 2 T3 ⎡ ⎤ cos (θ 2 + θ 3 ) − sin (θ 2 + θ 3 ) 0 g14 ⎢ sin (θ 2 + θ 3 ) cos (θ 2 + θ 3 ) 0 g24 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(5.98)

g14 = l3 cos (θ 2 + θ 3 ) + l2 cos θ 2 g24 = l3 sin (θ 2 + θ 3 ) + l2 sin θ 2 Example 177 Redundancy. If a robot has degrees of freedom more than required, it has redundant freedom. A manipulator with n joints has n DOF s. It is redundant if it can perform a task that requires less than the n available degrees of freedom. For example, ignoring motions of fingers, a human hand has 7 DOF s, namely, 3 rotational DOF s at shoulder joint, 1 rotational DOF at elbow joint, 3 rotational DOF s at wrist joint. Therefore, a frozen palm may be moved to any location and any orientation with 7 DOF s which is one DOF more than need. Including fingers, a hand has 27 DOF s. In robotics, any extra DOF provides us with ability to add a constraint or a condition to share the motions between the joints such that a particular path or a particular condition such as minimum energy to be satisfied.

252

5 Forward Kinematics

Fig. 5.22 An RRR articulated arm Table 5.10 DH parameter table for setting up the link frames Frame no. 1 2 3

ai 0 l2 0

αi −90 deg 0 90 deg

di d1 d2 l3

θi θ1 θ2 θ3

Table 5.11 Link classification for setup of the link frames Link no. 1 2 3

Type RR(−90) RR(0) RR(90)

Example 178 3R, RRR, articulated arm forward kinematics. Articulated robot is very applied in industry. This is how to determine forward kinematics of the robot. Consider an RRR arm as shown schematically in Fig. 5.22. To develop forward kinematics of the robot, the DH parameter table of the robot at rest position is set up as indicated in Table 5.10. The rest position of the robot could be set at any configuration where the joint axes z1 , z2 , z3 are coplanar; however, two rest positions are mostly being used. The first rest position is where x1 is coplanar with z1 , z2 , z3 , and the second rest position is where x0 is coplanar with z1 , z2 , z3 . We apply the link-joints classification in Examples 166. Therefore, we must be able to determine the type of link-joints combination as shown in Table 5.11. Therefore, the successive transformation matrices have the following expressions: ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0 ⎡

0 − sin θ 1 0 cos θ 1 −1 0 0 0

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 0⎥ ⎥ d1 ⎦ 1

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ 1 d2 ⎦ 0 1

(5.99)

(5.100)

5.3 Forward Position Kinematics of Robots

253

⎡

cos θ 3 ⎢ sin θ 3 2 T3 = ⎢ ⎣ 0 0

0 sin θ 3 0 − cos θ 3 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(5.101)

To express the complete forward kinematics transformation 0

T3 = 0 T1 1 T2 2 T3

(5.102)

we need to determine the result of a matrix multiplication. 0

T3 = 0 T1 1 T2 2 T3 ⎡ r11 r12 r13 ⎢ r21 r22 r23 =⎢ ⎣ r31 r32 r33 0 0 0

⎤ r14 r24 ⎥ ⎥ r34 ⎦ 1

(5.103)

r11 = cos θ 1 cos(θ 2 + θ 3 )

(5.104)

r21 = sin θ 1 cos(θ 2 + θ 3 )

(5.105)

r31 = − sin(θ 2 + θ 3 )

(5.106)

r12 = − sin θ 1

r22 = cos θ 1

r32 = 0

(5.107)

r13 = cos θ 1 sin(θ 2 + θ 3 )

(5.108)

r23 = sin θ 1 sin(θ 2 + θ 3 )

(5.109)

r33 = cos(θ 2 + θ 3 )

(5.110)

r14 = l2 cos θ 1 cos θ 2 − d2 sin θ 1

(5.111)

r24 = l2 cos θ 2 sin θ 1 + d2 cos θ 1

(5.112)

r34 = d1 − l2 sin θ 2

(5.113)

 T The tip point P of the third arm is at 3 rP = 0 0 l3 in B3 . So, its position in the base frame would be at 0

rP = 0 T3 3 rP ⎡ ⎤ ⎡ ⎤ 0 −d2 sθ 1 + l2 cθ 1 cθ 2 + l3 cθ 1 s (θ 2 + θ 3 ) ⎢ 0 ⎥ ⎢ d2 cθ 1 + l2 cθ 2 sθ 1 + l3 sθ 1 s (θ 2 + θ 3 ) ⎥ ⎥ ⎢ ⎥ = 0 T3 ⎢ ⎣ l3 ⎦ = ⎣ ⎦ d1 − l2 sθ 2 + l3 c (θ 2 + θ 3 ) 1 1

(5.114)

The transformation matrix at rest position, where θ 1 = 0, θ 2 = 0, θ 3 = 0, is ⎡

⎤ 1 0 0 l2 ⎢ 0 1 0 d2 ⎥ 0 ⎥ T3 = ⎢ ⎣ 0 0 1 d1 ⎦ 000 1

(5.115)

254

5 Forward Kinematics

This setup of coordinate frames shows that at the rest position, x1 , x2 , x3 are colinear and parallel to x0 ; furthermore, z1 , z3 are colinear, and z1 , z2 are parallel. Example 179 An articulated arm kinematics. Numerical example for forward kinematics of an articulated arm. Consider the RRR arm of Fig. 5.22 with the following dimensions: l2 = 0.75 m

l3 = 0.65 m

d1 = 0.48 m

d2 = 0.174 m

(5.116)

Using the link-joints combination of Table 5.11, we have ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 − sin θ 1 0 cos θ 1 −1 0 0 0

⎤ 0 0 ⎥ ⎥ 0.48 ⎦ 1

⎤ cos θ 2 − sin θ 2 0 0.75 cos θ 2 ⎢ sin θ 2 cos θ 2 0 0.75 sin θ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 0 1 0.174 ⎦ 0 0 0 1 ⎡ ⎤ cos θ 3 0 sin θ 3 0 ⎢ sin θ 3 0 − cos θ 3 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1

(5.117)

⎡

(5.118)

(5.119)

Therefore, the transformation matrix of B3 to B0 is 0

T3 = 0 T1 1 T2 2 T3 ⎡ ⎤ cθ 1 c (θ 2 + θ 3 ) −sθ 1 cθ 1 s (θ 2 + θ 3 ) r11 ⎢ sθ 1 c (θ 2 + θ 3 ) cθ 1 sθ 1 s (θ 2 + θ 3 ) r12 ⎥ ⎥ =⎢ ⎣ −s (θ 2 + θ 3 ) 0 c (θ 2 + θ 3 ) r13 ⎦ 0 0 0 1

(5.120)

r11 = 0.75 cos θ 1 cos θ 2 − 0.174 sin θ 1 r12 = 0.174 cos θ 1 + 0.75 cos θ 2 sin θ 1 r13 = 0.48 − 0.75 sin θ 2 The tip point P of the third link is at ⎡

⎤ ⎡ ⎤ 0 r1 ⎢ 0 ⎥ ⎢ r1 ⎥ 3 0 0 0 ⎥ ⎢ ⎥ rP = T3 rP = T3 ⎢ ⎣ 0.65 ⎦ = ⎣ r1 ⎦ 1 1 r1 = 0.75 cos θ 1 cos θ 2 − 0.174 sin θ 1 + 0.65 cos θ 1 sin (θ 2 + θ 3 ) r2 = 0.174 cos θ 1 + 0.75 cos θ 2 sin θ 1 + 0.65 sin θ 1 sin (θ 2 + θ 3 ) r3 = 0.65 cos (θ 2 + θ 3 ) − 0.75 sin θ 2 + 0.48 At the rest position, where θ 1 = 0, θ 2 = 0, θ 3 = 0, we have

(5.121)

5.3 Forward Position Kinematics of Robots

255

⎡

⎤ 1 0 0 0.75 ⎢ 0 1 0 0.174 ⎥ 0 ⎥ T3 = ⎢ ⎣ 0 0 1 0.48 ⎦ 000 1 that shows the tip point is at

⎤ ⎡ ⎤ 0 0.75 ⎢ 0 ⎥ ⎢ 0.174 ⎥ 0 ⎥ ⎢ ⎥ rP = 0 T3 3 rP = 0 T3 ⎢ ⎣ 0.65 ⎦ = ⎣ 1.13 ⎦ 1 1

(5.122)

⎡

(5.123)

Example 180 Working space of an articulated arm. How to determine the space volume that a robot can reach. It is the result of a kinematic analysis. Consider the RRR arm of Fig. 5.22 with the following dimensions: l2 = 0.75 m

l3 = 0.65 m

d1 = 0.48 m

d2 = 0.174 m

(5.124)

Link transformation matrices are given in (5.117)–(5.119). The manipulator’s transformation matrix 0 T3 at the rest position is (5.122) when point P is at (5.123). Assume that every joint can turn 360 deg. Theoretically, point P must be able to reach any point in the sphere S1 , 

r − 0 d1 − d2 0 kˆ1

2

= (l2 + l3 )2

x 2 + (y − 0.174 )2 + (z − 0.48)2 = 1.96 ⎤ ⎡ ⎤ 0 0 0 d1 = ⎣ 0 ⎦ = ⎣ 0 ⎦ d1 0.48

(5.125) (5.126)

⎡

d2 0 kˆ1 = 0 R1 d2 1 kˆ1 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 1 0 0 0 0 = ⎣ 0 0 1 ⎦ ⎣ 0 ⎦ = ⎣ 0.174 ⎦ 0 −1 0 0.174 0

(5.127)

(5.128)

and out of the sphere S2 . 

r − 0 d1 − d2 0 kˆ1

2

= (l2 − l3 )2

x 2 + (y − 0.174 )2 + (z − 0.48)2 = 0.01

(5.129) (5.130)

The reachable space between S1 and S2 is called working space of the manipulator. Example 181 SCARA robot (RRRP). Forward kinematics. Consider the RRRP robot shown in Fig. 5.23. The forward kinematics of the robot can be solved by obtaining individual transformation matrices i−1 Ti . The first link is an RR(0), which has the following transformation matrix: ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(5.131)

256

5 Forward Kinematics

Fig. 5.23 An RRRP SCARA manipulator robot

The second link is also an RR(0).

⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(5.132)

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.133)

The third link is an RR(0) with zero length, ⎡

cos θ 3 − sin θ 3 ⎢ sin θ 3 cos θ 3 2 T3 = ⎢ ⎣ 0 0 0 0

and the fourth link is an RP(180). From a coincident position with B3 , the frame B4 must turn π about the x3 -axis and translate −d along the z3 -axis to move to the current position. ⎡

1 0 0 ⎢ 0 cos π − sin π 3 T4 = ⎢ ⎣ 0 sin π cos π 0 0 0

⎤ ⎡ ⎤ 0 1 0 0 0 ⎢ ⎥ 0 ⎥ ⎥ = ⎢ 0 −1 0 0 ⎥ −d ⎦ ⎣ 0 0 −1 −d ⎦ 1 0 0 0 1

(5.134)

Therefore, the configuration of the end-effector in the base coordinate frame is 0

T4 = 0 T1 1 T2 2 T3 3 T4 ⎡ ⎤ c(θ 1 + θ 2 + θ 3 ) s(θ 1 + θ 2 + θ 3 ) 0 l1 cθ 1 + l2 c(θ 1 + θ 2 ) ⎢ s(θ 1 + θ 2 + θ 3 ) −c(θ 1 + θ 2 + θ 3 ) 0 l1 sθ 1 + l2 s(θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 −1 −d 0 0 0 1

(5.135)

It shows the rest position of the robot θ 1 = θ 2 = θ 3 = d = 0 is at ⎡

⎤ 1 0 0 l1 + l2 ⎢ 0 −1 0 0 ⎥ 0 ⎥ T4 = ⎢ ⎣ 0 0 −1 0 ⎦ 0 0 0 1

(5.136)

5.3 Forward Position Kinematics of Robots

257

Example 182 Space station remote manipulator system (SSRMS). Forward kinematics of the 7 DOF Space station arm. The Space Shuttle Remote Manipulator system (SSRMS), also known as the Shuttle Remote Manipulator System (SRMS), is a robotic arm to act as the hand of the Shuttle or a space station. It is utilized for several purposes such as: satellite deployment, construction of a space station, transporting a crew member at the end of the arm, and surveying and inspecting the outside of the station. A simplified model of the SSRMS, schematically shown in Fig. 5.24, and the approximate values of its kinematic characteristics are given in Table 5.12. Table 5.13 indicates the DH parameters of SSRMS, and Table 5.14 names the link-joints classifications of SSRMS. Utilizing these values and indicating the type of each link enable us to determine the required transformation matrices to move from Bi to Bi−1 and solve the forward kinematics problem. Links (1) and (2) are RR(−90). ⎡ ⎤ cos θ 1 0 − sin θ 1 0 ⎢ sin θ 1 0 cos θ 1 0 ⎥ 0 ⎥ T1 = ⎢ (5.137) ⎣ 0 −1 0 d1 ⎦ 0 0 0 1 ⎤ ⎡ cos θ 2 0 − sin θ 2 0 ⎢ sin θ 2 0 cos θ 2 0 ⎥ 1 ⎥ (5.138) T2 = ⎢ ⎣ 0 −1 0 d2 ⎦ 0 0 0 1 Links (3) and (4) are RR(0).

⎡

cos θ 3 ⎢ sin θ 3 2 T3 = ⎢ ⎣ 0 0 ⎡ cos θ 4 ⎢ sin θ 4 3 T4 = ⎢ ⎣ 0 0

⎤ 0 a3 cos θ 3 0 a3 sin θ 3 ⎥ ⎥ ⎦ 1 d3 0 1 ⎤ − sin θ 4 0 a4 cos θ 4 cos θ 4 0 a4 sin θ 4 ⎥ ⎥ ⎦ 0 1 d4 0 0 1 − sin θ 3 cos θ 3 0 0

(5.139)

(5.140)

Link (5) is RR(90), and link (6) is RR(−90). ⎡

cos θ 5 ⎢ sin θ 5 4 T5 = ⎢ ⎣ 0 0 ⎡

cos θ 6 ⎢ sin θ 6 5 T6 = ⎢ ⎣ 0 0

0 sin θ 5 0 − cos θ 5 1 0 0 0 0 − sin θ 6 0 cos θ 6 −1 0 0 0

Finally, link (7) is RR(0) and the coordinate frame attached to the coordinate frame B6 . ⎡ cos θ 7 − sin θ 7 ⎢ sin θ 7 cos θ 7 6 T7 = ⎢ ⎣ 0 0 0 0

⎤ 0 0⎥ ⎥ d5 ⎦ 1 ⎤ 0 0⎥ ⎥ d6 ⎦ 1

(5.141)

(5.142)

end-effector has a translation d7 with respect to the ⎤ 0 0 0 0⎥ ⎥ 1 d7 ⎦ 0 1

(5.143)

The forward kinematics of SSRMS can be found by direct multiplication of i−1 Ti (i = 1, 2, · · · , 7). 0

T7 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7

The forward kinematics of the first three frames are given as

(5.144)

258

5 Forward Kinematics

Fig. 5.24 Illustration of the space station remote manipulator system (not in scale)

Table 5.12 Space station’s robot arm characteristics Length Diameter Weight Number of joints Handling capacity Maximum velocity of end of arm Maximum rotational speed

14.22 m 38.1 cm 1336 kg Seven 116000 kg (in space) Carrying nothing: 37 cm/s Full capacity: 1.2 cm/s Approximately: 4 deg /s

Table 5.13 DH parameters for SSRMS Frame no. 1 2 3 4 5 6 7

ai 0 0 7110 mm 7110 mm 0 0 0

αi −90 deg −90 deg 0 0 90 deg −90 deg 0

di 380 mm 1360 mm 570 mm 475 mm 570 mm 635 mm d7

Table 5.14 Link-joints classifications for SSRMS Link no. 1 2 3 4 5 6 7

Type RR(−90) RR(−90) PR(0) PR(0) RR(90) RR(−90) RR(0)

θi θ1 θ2 θ3 θ4 θ5 θ6 0

5.3 Forward Position Kinematics of Robots

259

⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 − sin θ 1 0 cos θ 1 −1 0 0 0

⎤ 0 0⎥ ⎥ d1 ⎦ 1

(5.145)

⎡

⎤ cθ 1 cθ 2 sθ 1 −cθ 1 sθ 2 −d2 sθ 1 ⎢ cθ 2 sθ 1 −cθ 1 −sθ 1 sθ 2 d2 cθ 1 ⎥ 0 ⎥ T2 = 0 T1 1 T2 = ⎢ ⎣ −sθ 2 0 −cθ 2 d1 ⎦ 0 0 0 1 0

where

T3 = 0 T1 1 T2 2 T3 ⎡ sθ 1 sθ 3 + cθ 1 cθ 2 cθ 3 cθ 3 sθ 1 − cθ 1 cθ 2 sθ 3 −cθ 1 sθ 2 ⎢ cθ 2 cθ 3 sθ 1 − cθ 1 sθ 3 −cθ 1 cθ 3 − cθ 2 sθ 1 sθ 3 −sθ 1 sθ 2 =⎢ ⎣ −cθ 3 sθ 2 sθ 2 sθ 3 −cθ 2 0 0 0

(5.146)

0

⎤

(5.147)

d3x d3y ⎥ ⎥ 0 d3z ⎦ 1

0

⎤ a3 sθ 1 sθ 3 − d3 cθ 1 sθ 2 − d2 sθ 1 + a3 cθ 1 cθ 2 cθ 3 ⎢ d2 cθ 1 − a3 cθ 1 sθ 3 − d3 sθ 1 sθ 2 + a3 cθ 2 cθ 3 sθ 1 ⎥ 0 ⎥ d3 = ⎢ ⎣ ⎦ d1 − d3 cθ 2 − a3 cθ 3 sθ 2 1 ⎡

(5.148)

and the forward kinematics of the last three frames as ⎡

cos θ 7 − sin θ 7 ⎢ sin θ 7 cos θ 7 6 T7 = ⎢ ⎣ 0 0 0 0

⎤ 0 0 0 0⎥ ⎥ 1 d7 ⎦ 0 1

(5.149)

⎡

⎤ cθ 6 cθ 7 −cθ 6 sθ 7 −sθ 6 −d7 sθ 6 ⎢ cθ 7 sθ 6 −sθ 6 sθ 7 cθ 6 d7 cθ 6 ⎥ 5 ⎥ T7 = 5 T6 6 T7 = ⎢ ⎣ −sθ 7 −cθ 7 0 d6 ⎦ 0 0 0 1 4

where

T7 = 4 T5 5 T6 6 T7 ⎡ cθ 5 cθ 6 cθ 7 − sθ 5 sθ 7 −cθ 7 sθ 5 − cθ 5 cθ 6 sθ 7 −cθ 5 sθ 6 ⎢ cθ 5 sθ 7 + cθ 6 cθ 7 sθ 5 cθ 5 cθ 7 − cθ 6 sθ 5 sθ 7 −sθ 5 sθ 6 =⎢ ⎣ cθ 7 sθ 6 −sθ 6 sθ 7 cθ 6 0 0 0 ⎤ d6 sθ 5 − d7 cθ 5 sθ 6 ⎢ −d6 cθ 5 − d7 sθ 5 sθ 6 ⎥ 4 ⎥ d7 = ⎢ ⎣ ⎦ d5 + d7 cθ 6 1

(5.150)

4

⎤

(5.151)

d7x d7y ⎥ ⎥ 4 d7z ⎦ 1

4

⎡

(5.152)

The transformation matrices 0 T4 , 0 T5 , 0 T6 are determined by matrix multiplication, 0

T4 = 0 T3 3 T4

(5.153)

0

T5 = 0 T4 4 T5

(5.154)

0

T6 = T5 T6

(5.155)

0

5

260

5 Forward Kinematics

Fig. 5.25 The space shuttle remote manipulator system (SSRMS) with a camera attached to link (4)

and finally, we can find the coordinates of a point in the gripper frame B7 in the base frame by using 0 T3 3 T4 4 T7 : ⎡

cos θ 4 − sin θ 4 ⎢ sin θ 4 cos θ 4 0 0 T7 = T3 ⎢ ⎣ 0 0 0 0

⎤ 0 a4 cos θ 4 0 a4 sin θ 4 ⎥ ⎥ 4 T7 ⎦ 1 d4 0 1

(5.156)

Example 183 Camera on an arm of SSRMS. We may attach several accessories as different points of links of a robot. Every accessory needs a coordinate frame to be located kinematically. A good example is an inspecting camera. This example shows how an accessory will be defined kinematically on a robot. The SSRMS is shown in Fig. 5.25, with the link transformation matrices given in Eqs. (5.137)–(5.150). Using the numerical values of Table 5.12 and assuming the following values for the joint variables and gripper length, π π π θ 2 = − rad θ 3 = − rad θ 4 = − rad 2 4 2 3π π θ5 = − θ 6 = rad θ 7 = 0 d7 = 500 mm 4 2 θ1 = 0

(5.157)

we find the following link transformation matrices: ⎡

⎤ 1 0 0 0 ⎢0 0 1 0 ⎥ 0 ⎥ T1 = ⎢ ⎣ 0 −1 0 380 ⎦ 0 0 0 1 ⎤ 0 0 1 0 ⎢ −1 0 0 0 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 −1 0 1360 ⎦ 0 0 0 1 ⎡ ⎤ 0.707 11 0.707 11 0 5027.5 ⎢ −0.707 11 0.707 11 0 −5027.5 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 570 ⎦ 0 0 0 1

(5.158)

⎡

(5.159)

(5.160)

5.3 Forward Position Kinematics of Robots

261

⎡

⎤ 0 10 0 ⎢ −1 0 0 −7110 ⎥ 3 ⎥ T4 = ⎢ ⎣ 0 0 1 475 ⎦ 0 00 1 ⎡ ⎤ −0.707 11 0 −0.707 11 0 ⎢ −0.707 11 0 0.707 11 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 1 0 570 ⎦ 0 0 0 1 ⎡ ⎤ 0 0 −1 0 ⎢ 1 0 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 −1 0 635 ⎦ 0 0 0 1 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 6 ⎥ T7 = ⎢ ⎣ 0 0 1 500 ⎦ 000 1

(5.161)

(5.162)

(5.163)

(5.164)

Assume there is a camera attached to link (4) to inspect the gripper’s operation. The camera is at a point P in B4 with position vector 4 rP and is aiming the origin of the gripper’s frame B7 : 4

 T rP = −a b 0 1 a = 696 mm

(5.165) b = 154 mm

(5.166)

To determine the position of the camera in the base frame, 0 rP , we need to calculate 0 T4 , 0

and find 0 rP ,

T4 = 0 T1 1 T2 2 T3 3 T4 ⎤ ⎡ 0 0 1 1045 ⎢ 0.707 11 0.707 11 0 11415 ⎥ ⎥ =⎢ ⎣ −0.707 11 0.707 11 0 379.95 ⎦ 0 0 0 1

⎤ ⎡ ⎤ −696 1045 ⎢ 154 ⎥ ⎢ 11032 ⎥ 0 ⎥ ⎢ ⎥ rP = 0 T4 4 rP = 0 T4 ⎢ ⎣ 0 ⎦ = ⎣ 980.99 ⎦ 1 1

(5.167)

⎡

(5.168)

At the configuration (5.157) the transformation matrix 0 T7 is given as 0

T7 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7 ⎡ ⎤ 1 0 0 1615 ⎢ 0 0 1 11915 ⎥ ⎥ =⎢ ⎣ 0 −1 0 1015 ⎦ 0 0 0 1

(5.169)

where the position of the gripper point in the base frame is at ⎡

⎤ 1615 ⎢ 11915 ⎥ 0 ⎥ d7 = ⎢ ⎣ 1015 ⎦ 1

(5.170)

262

5 Forward Kinematics

Therefore, the position of the camera in the gripper frame B7 would be at 7

rP = 7 T4 4 rP

4

= 7 T6 6 T5 5 T4 4 rP = 6 T7−1 5 T6−1 4 T5−1 4 rP = T5 5 T6 6 T7 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 0 1 −570 −696 −570 ⎢ 0.707 −0.707 0 635 ⎥ ⎢ 154 ⎥ ⎢ 33.965 ⎥ ⎥ ⎥⎢ ⎥ ⎢ =⎢ ⎣ 0.707 0.707 0 −500 ⎦ ⎣ 0 ⎦ = ⎣ −883.25 ⎦ 0 0 0 1 1 1

−1

(5.171) 4

rP

To use the camera kinematically, we attach a coordinate frame B8 to the camera at P and determine the configuration of the gripper frame B7 in the camera frame. The position vectors of the camera in the base and gripper frames are calculated in (5.168) and (5.171) as ⎡ ⎤ ⎡ ⎤ 1045 −570 ⎢ 11032 ⎥ ⎢ 33.965 ⎥ 0 7 ⎥ ⎥ rP = ⎢ rP = ⎢ (5.172) ⎣ 980.99 ⎦ ⎣ −883.25 ⎦ 1 1 Assume the camera is equipped with two electric motors that give it two rotational degrees of freedom. To determine the coordinate frame B8 at the present configuration, we begin with calculating the direction of the x8 -axis in the gripper frame B7 , ⎡ ⎤ 0.54193 7 − r P 7 ıˆ8 = 7 = ⎣ −3.2292 × 10−2 ⎦ (5.173)

rP 0.83975 and determine the x8 -axis in frame B4 .

⎤ 0.57096 ⎢ 0.61663 ⎥ 4 ⎥ ıˆ8 = 4 R7 7 ıˆ8 = ⎢ ⎣ 0.54193 ⎦ 0 ⎡

(5.174)

Let us assume the first motor of the camera turns the camera about z8 and the second motor turns it about the displaced y8 . The B4 frame may act as a global frame to the camera. So, we can find the matrix 8 R4 by a rotation α about z8 followed by a rotation β about y8 : 8

R4 = Ry,β Rz,α ⎡ ⎤ cos α cos β cos β sin α − sin β = ⎣ − sin α cos α 0 ⎦ cos α sin β sin α sin β cos β

(5.175)

  Using 8 ıˆ8 = 1 0 0 , we have ıˆ8 = 4 R8 8 ıˆ8 = 8 R4T 8 ıˆ8 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0.57096 1 cos α cos β ⎣ 0.61663 ⎦ = 8 R4T ⎣ 0 ⎦ = ⎣ cos β sin α ⎦ 0.54193 0 − sin β 4

where

⎤ cos α cos β − sin α cos α sin β 4 R8 = 8 R4T = ⎣ cos β sin α cos α sin α sin β ⎦ − sin β 0 cos β

(5.176) (5.177)

⎡

(5.178)

The third component of (5.177) provides β, β = − arcsin 0.54193 = −0.5727 rad ≈ −32.8 deg

(5.179)

5.3 Forward Position Kinematics of Robots

263

and the first and second components provide α. α = arctan

0.61663 = 0.8238 rad ≈ 47.2 deg 0.57096

(5.180)

 T The angles α and β determine the orientation of B8 at 4 d8 = −a b 0 1 . Therefore, ⎡

⎤ 0.57103 −0.73372 −0.36821 −696 ⎢ 0.61664 0.67945 −0.39762 154 ⎥ 4 ⎥ T8 = ⎢ ⎣ 0.54193 0 0.84042 0 ⎦ 0 0 0 1 ⎤ 0.57103 0.61664 0.54192 302.47 ⎢ −0.73372 0.67945 −6.58 × 10−8 −615.31 ⎥ 8 ⎥ T4 = ⎢ ⎣ −0.36822 −0.39763 0.84043 −195.04 ⎦ 0 0 0 1

(5.181)

⎡

(5.182)

Now, we can calculate the position and orientation of the gripper frame in the eye of camera as well as the camera in the base coordinate frame: 8

T7 = 8 T4 4 T5 5 T6 6 T7 ⎡ ⎤ 0.54192 −3.225 × 10−2 0.83981 1051.8 ⎢ 0 −0.99927 −3.837 × 10−2 0 ⎥ ⎥ ≈⎢ ⎣ 0.84043 2.08 × 10−2 −0.54154 0 ⎦ 0 0 0 1

(5.183)

T8 = 0 T7 7 T8 = 0 T7 8 T7−1 ⎡ ⎤ 0.54192 −0.03225 0.83981 2666.8 ⎢ 0.84043 0.0208 −0.54154 11915 ⎥ ⎥ =⎢ ⎣ 0 0.99927 0.03837 1015 ⎦ 0 0 0 1

(5.184)

0

Example 184  Directional control system. A detailed robotic analysis of kinematic and control of a motorized camera. Such cameras are always applied in robotics for vision and inspection. A radar, satellite antenna, or inspection camera are samples of direction control and detection systems for which we need to control the angles of a disc and direct an axis normal vector to a desired direction. Such a directional control system is usually equipped with two motors that provide two rotational motions, as shown in Fig. 5.26. The axes of the rotations intersect at a wrist point. Let attach a coordinate frame B8 to the camera at the wrist point and a Sina coordinate frame B9 to the base link at the same point. The fixed frame B9 (x9 , y9 , z9 ) ≡ B9 (X, Y, Z) acts as the global frame for the local frame B8 (x8 , y8 , z8 ). Sina frames are auxiliary coordinate frames we attach to any point of a robot to simplify transformations. Sina frames usually have no variable parameter. The Sina frame B9 is a fixed frame on the base link on which the camera is installed. The first motor turns the control plate by α about the fixed axis z9 that is initially coincident with z8 . The second motor turns the plate by β about the y8 -axis. The transformation matrix 8 R9 between B8 and B9 from a coincident configuration may be found by a rotation α about z9 followed by a rotation β about y8 : ⎡

8

R9 = Ry,β Rz,α

⎤ cos α cos β cos β sin α − sin β = ⎣ − sin α cos α 0 ⎦ cos α sin β sin α sin β cos β

(5.185)

264

5 Forward Kinematics

Fig. 5.26 A directional control system that is equipped with two motors

To clarify the transformation matrix 8 R9 let us determine 8 R9 by rotations about the actual rotation axes z9 and y8 . The first rotation RZ,α is about the global Z-axis, ⎡

9

R8 = RZ,α

⎤ cos α − sin α 0 = ⎣ sin α cos α 0 ⎦ 0 0 1

(5.186)

and the second rotation Ry,β is about the local y-axis, ⎡

8

R9 = Ry,β

⎤ cos β 0 − sin β =⎣ 0 1 0 ⎦ sin β 0 cos β

(5.187)

T T To determine the combined rotation matrix, we may find 9 R8 = Ry,β RZ,α or 8 R9 = Ry,β RZ,α . 8

T R9 = Ry,β RZ,α ⎡ ⎤⎡ cos β 0 − sin β cos α − sin α ⎣ ⎦ ⎣ = 0 1 0 sin α cos α sin β 0 cos β 0 0 ⎡ ⎤ cos α cos β cos β sin α − sin β = ⎣ − sin α cos α 0 ⎦ cos α sin β sin α sin β cos β

(5.188) ⎤T 0 0⎦ 1

The transformation matrix (5.188) is the same as (5.185) and works only when the initial configuration of the camera frame B8 coincides with the Sina frame B9 . Furthermore, the rotations must be in order. Assume the camera aims the desired direction when motor 1 turns α and motor 2 turns β. Because the two motors can act independently, the order of action of motors is immaterial in determination of the final aiming direction as well as the transformation matrix between B8 and B9 . The two motors may also act together and turn their associated angles together. In this case, the motor will perform an axis–angle of rotation to the camera. Having the rotation matrix as (5.188), we are able to determine the equivalent angle-axis of rotation by Eqs. (3.10) and (3.11).

5.3 Forward Position Kinematics of Robots

265

 1  G  tr RB − 1 2 1 = (cos α + cos β + cos α cos β − 1) 2

cos φ =

  1 8 T 1 G RB − G RBT = R9 − 8 R9 2 sin φ 2 sin φ ⎡ ⎤ 0 −sα − cβsα sβ + cαsβ ⎣ sα + cβsα 0 sαsβ ⎦ −sβ − cαsβ −sαsβ 0 = *  2 1 2 1− (cos α + cos β + cos α cos β − 1) 2

(5.189)

u˜ =

(5.190)

Let us assume that α = 0.823 78 rad ≈ 47.199 deg

(5.191)

β = −0.57273 rad ≈ −32.815 deg

(5.192)

therefore, the rotation matrix (5.188) is ⎡

8

R9 = Ry,β Rz,α

⎤ 0.57103 0.61664 0.54193 ⎦ = ⎣ −0.73372 0.67945 0 −0.36821 −0.39762 0.84042

(5.193)

and the associated axis and angle are cos φ =

 1  8 T  tr R9 − 1 = 0.54545 rad ≈ 31.252 deg 2  1 8 T R9 − 8 R9 2 sin φ ⎡ ⎤ 0 −1.301 5 −0.877 15 = ⎣ 1.301 5 0 −0.383 21 ⎦ 0.87715 0.383 21 0

(5.194)

u˜ =

(5.195)

To activate both motors together, we may command motors 1 and 2 to act according to the following suggested functions: α(t) = 2.4713

t2 t3 − 1.6476 tf2 tf3

β(t) = −1.7182

t2 t3 + 1.1455 3 2 tf tf

(5.196)

(5.197)

where t is the time and tf is the total time it takes for the motors to finish their rotations. These equations guarantee that both motors start from zero angular velocity at t = 0 and reach their final angular rotations at t = tf and stop there. Let us assume tf = 10 s

(5.198)

266

5 Forward Kinematics

Fig. 5.27 A 3D slider-crank mechanism

Fig. 5.28 Side view of the 3D slider-crank mechanism of Fig. 5.27

to have α(t) = 2.471 3 × 10−2 t 2 − 1.6476 × 10−3 t 3

(5.199)

β(t) = −1.718 2 × 10−2 t 2 + 1.1455 × 10−3 t 3

(5.200)

Example 185  A three dimensional slider-crank mechanism. We may treat a mechanism as a robot to determine its kinematics similar to a robot forward kinematic analysis. This is a good sample of three dimensional motion of a floating bar with one end sliding on a fixed axis, while the other end is turning on a spatial circle. This example shows how auxiliary Sina coordinate frames simplify kinematic analysis of a complicated robotic system. Figure 5.27 illustrates a slider that is moving back and forth on the global X-axis by the connecting rod AB. The rod is attached to a spinning disc that its angular rotation is measured by an input angle θ. The plane of the disc is at an angle 45 deg about the Z-axis. A side view of the mechanism along the Z-axis is shown in Fig. 5.28. The global coordinate frame G is set at the fixed center of the disc. The displacement of the slider is measured by the distance b. We have four relatively moving bodies, including the ground. So, we technically need four coordinate frames. However, it might be clearer to set up some auxiliary Sina coordinate frames to simplify the kinematic of the system with the penalty of extra matrix multiplications. The frame B1 is another globally fixed frame at an angle 45 deg about the Z-axis such that the x1 -axis is perpendicular to the disc and indicates its spin axis. The body frame B2 is attached to the disc such that x1 and x2 are coincident and y2 points the joint A. The angle between y1 and y2 is θ . The connecting rod AB has a constant length l and is attached to the disc at point A at a distance R from the disc center: 2

 T rA = 0 R 0

(5.201)

5.3 Forward Position Kinematics of Robots

267

A Sina frame B3 parallel to B2 is attached to the disc at point A. The frame B4 is also at A such that y4 is coincident with y3 and x4 is seen coincident with AB from the Z viewpoint. The angle between x3 and x4 is ϕ. There is another frame B5 at A such that z4 and z5 are coincident and x5 is along AB. The angle between x4 and x5 is ψ. At point B of the bar AB, we attach a coordinate frame B6 parallel to B5 . The slider frame B7 , which is parallel to G, is attached to the box at point B: G

 T rB = b 0 0

(5.202)

The transformation between G and B1 is a constant 45 deg rotation about the Z-axis: " G

T1 =

RZ,π/4 0

⎡

cos π4 − sin π4 ⎢ sin π cos π 0 4 4 =⎢ ⎣ 0 1 0 0 0 #

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.203)

The transformation between B1 and B2 is a variable rotation θ about the x1 -axis: " 1

T2 =

Rx1 ,θ 0

⎡

1 0 0 ⎢ 0 cos θ − sin θ 0 =⎢ ⎣ 0 sin θ cos θ 1 0 0 0 #

⎤ 0 0⎥ ⎥ 0⎦ 1

(5.204)

The transformation between B2 and B3 is a constant translation R along the x2 -axis. ⎤ 100R ⎢0 1 1 0 ⎥ 2 ⎥ T3 = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎡

The transformation between B3 and B4 is a variable rotation ϕ about the y3 -axis: ⎡ ⎤ cos ϕ 0 sin ϕ 0 " # ⎢ 0 1 0 0⎥ Ry3 ,ϕ 0 3 ⎥ T4 = =⎢ ⎣ − sin ϕ 0 cos ϕ 0 ⎦ 0 1 0 0 0 1 The transformation between B4 and B5 is a variable rotation ψ about the z4 -axis: ⎡ ⎤ cos ψ − sin ψ 0 0 " # ⎢ sin ψ cos ψ 0 0 ⎥ Rz4 ,ψ 0 4 ⎥ T5 = =⎢ ⎣ 0 0 1 0 1 0⎦ 0 0 01 The transformation between B5 and B6 is a constant translation l along the x5 -axis: ⎤ ⎡ 100l ⎢0 1 1 0⎥ 5 ⎥ T6 = ⎢ ⎣0 0 1 0⎦ 0001 The transformation between B7 and G is a variable translation b along the X-axis: ⎡ ⎤ 100b ⎢0 1 1 0⎥ G ⎥ T7 = ⎢ ⎣0 0 1 0⎦ 0001

(5.205)

(5.206)

(5.207)

(5.208)

(5.209)

268

5 Forward Kinematics

To find the global position of joint B, we go through B1 , B2 , · · · , B6 and determine G T6 : G

T6 =

T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ r11 r12 r13 d6X ⎢ r21 r22 r23 d6Y ⎥ ⎥ =⎢ ⎣ r31 r32 r33 d6Z ⎦ 0 0 0 1 G

(5.210)

where √

2 (cos θ cos ψ − cos ψ sin θ ) sin ϕ 2 √ 2 + (cos ψ cos ϕ − cos θ sin ψ) 2 √ 2 = (− cos θ cos ψ + cos ψ sin θ ) sin ϕ 2 √ 2 + (cos ψ cos ϕ + cos θ sin ψ) 2 1 = (cos (θ − ψ) − cos (θ + ψ)) 2 − (cos θ cos ψ + cos ψ sin θ ) sin ϕ √ 2 = (sin θ sin ψ − cos θ sin ψ) sin ϕ 2 √ 2 − (cos ϕ sin ψ + cos θ cos ψ) 2 √ 2 = (cos θ sin ψ − sin θ sin ψ) sin ϕ 2 √ 2 + (cos θ cos ψ − cos ϕ sin ψ) 2 1 1 1 = sin (θ + ψ − ϕ) + sin (θ − ψ + ϕ) − sin (θ − ψ − ϕ) 4 4 4 1 − sin (θ + ψ + ϕ) + cos ψ sin θ + cos θ sin ψ sin ϕ 4 √ √ 2 2 =− cos θ cos ψ + (sin θ sin ψ − cos θ sin ψ) sin ϕ 2 2 √ √ 2 2 + sin ϕ (sin θ − cos θ − sin ψ) cos ϕ + 2 2 √ √ 2 2 = sin ϕ + (cos θ sin ψ − sin θ sin ψ) sin ϕ 2 2 √ √ 2 2 + (cos ψ + cos ϕ) cos θ − (sin θ + sin ψ) cos ϕ 2 2 1 = (sin (θ + ψ − ϕ) + sin (θ − ψ + ϕ) − sin (θ − ψ − ϕ)) 4 1 − sin (θ + ψ + ϕ) + cos θ (cos ϕ + sin ψ sin ϕ) 4 + (cos ψ + cos ϕ) sin θ

r11 =

r21

r31

r12

r22

r32

r13

r23

r33

(5.211)

(5.212)

(5.213)

(5.214)

(5.215)

(5.216)

(5.217)

(5.218)

(5.219)

5.4 Spherical Wrist

269

√ √ 2 2 2 = R+ l cos ψ cos ϕ − l cos θ sin ψ 2 2 2 √ 2 + l (cos θ cos ψ − cos ψ sin θ ) sin ϕ 2 √ √ √ 2 2 2 = R+ l cos ψ cos ϕ + l cos θ sin ψ 2 2 2 √ 2 − l (cos θ cos ψ + cos ψ sin θ ) sin ϕ 2 = l sin θ sin ψ − l (cos θ cos ψ + cos ψ sin θ ) sin ϕ √

d6X

d6Y

d6Z

(5.220)

(5.221)

(5.222) The X-component of G d6 must be equal to b and the Y - and Z-components of G d6 must be zero: d6X = b

d6Y = 0

d6Z = 0

(5.223)

We should be able to solve these three equations to calculate b, ϕ, ψ for a given θ. In a different design, we may assume that the angle between G and B1 is a controllable angle α. In this case the transformation between G and B1 is based on a given angle α rotation about the Z-axis: " G

T1 =

RZ,α 0

⎡

cos α − sin α ⎢ sin α cos α 0 =⎢ ⎣ 0 1 0 0 0 #

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.224)

Re-evaluating both sides of Eq. (5.210) provides us with three new equations instead of (5.220)–(5.222) that must be solved for b, ϕ, ψ, for given θ and α. When α = 90 deg, the mechanism reduces to the planar slider-crank mechanism, and when α = 0, the slider will not move and we will have b = const.

5.4

Spherical Wrist

Figure 5.29 illustrates a spherical wrist with a spherical joint. A spherical wrist is a combination of links and joints that provides three rotations about three orthogonal axes intersecting at the wrist point. The wrist is a practical device for an industrial robot to hold, adjust, and control the orientation of a tool. The spherical joint connects two links: the forearm link and hand link. The axis of the forearm and hand are colinear at the rest position of the hand. The axis of the hand is called the gripper axis. An industrial spherical wrist is to simulate a spherical joint and provide three rotational DOF for the gripper link. At the wrist point, we define two coordinate frames. The first one is the wrist dead frame, attached to the forearm link, and the second frame is the wrist live frame, attached to the hand link. We also introduce a tool or gripper frame. The gripper ˆ s ≡ ıˆ, n ≡ jˆ. It is set at a symmetric point between the fingers of an frame of the wrist is denoted by three vectors, a ≡ k, empty hand or at the tip of the tools held by the fingers. The vector n is called tilt and is the normal vector perpendicular to the fingers or jaws. The vector s is called twist and the vector a is called turn. Figure 5.29 illustrates the ideal case of a spherical joint; however, a spherical joint is not mechanically controllable. We need to replace the ideal spherical joint with three joints, every one with only one rotational DOF . It is made by three RR links with zero lengths and zero offset where their joint axes are mutually orthogonal and intersecting at the wrist point. The wrist point is invariant in a robot structure and will not move by wrist angular motions. Figure 5.30 illustrates a schematic of a practical spherical wrist configuration. It is made of an RR(−90) link, attached to another RR(90) link, that finally is attached to a spinning gripper link RR(0). The gripper coordinate frame B7 is always parallel to B6 and is attached at a distance d7 from the wrist point. To classify spherical wrists, let us decompose the rotations of the spherical wrist into three rotations about three orthogonal axes. We call the rotations, Roll, Pitch, and Yaw as are shown in Fig. 5.29. The Roll is any rotation that turns the gripper about its axis when the wrist is at the rest position. The gripper axis a defines a perpendicular plane s × n to the gripper axis that is

270

5 Forward Kinematics

Fig. 5.29 A spherical joint provides roll, pitch, and yaw rotations

Fig. 5.30 Spherical wrist kinematics Table 5.15 Spherical classification Type 1 2 3

wrists

Rotation order Roll–Pitch–Roll Roll–Pitch–Yaw Pitch–Yaw–Roll

called the gripper wall. The Pitch and Yaw are rotations about two perpendicular axes in the gripper wall at the wrist point. We can consider the first rotation about an axis in the gripper wall as pitch and the rotation about the axis perpendicular to the first, as yaw. The Roll, Pitch, Yaw rotations are defined at the rest position of the wrist. There are three types of practical spherical wrists that are classified in Table 5.15. Figure 5.30 illustrates a Roll–Pitch–Roll spherical wrist with the following transformation matrix. 3

T6 = 3 T4 4 T5 5 T6 ⎡ ⎤ cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 0 ⎢ cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 0 ⎥ ⎥ =⎢ ⎣ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 0 ⎦ 0 0 0 1

(5.225)

Proof We provide the roll, pitch, and yaw rotations by introducing two links and three frames between the dead and live frames. The links will be connected by three revolute joints. The joint axes must always intersect at the wrist point and be

5.4 Spherical Wrist

271

orthogonal when the wrist is at the rest position. The kinematic analysis of such wrists should be conducted by attaching a proper DH coordinate frame to every link. Then the associated DH transformation matrices can be combined to develop the overall wrist transformation matrix. Utilizing the transformation matrix (5.37) for link (4), (5.36) for link (5), RZ,θ 6 for link (6), and a DZ,d6 for frame B7 , we find the following transformation matrices: ⎡

cos θ 4 ⎢ sin θ 4 3 T4 = ⎢ ⎣ 0 0

0 − sin θ 4 0 cos θ 4 −1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎤ cos θ 5 0 sin θ 5 0 ⎢ sin θ 5 0 − cos θ 5 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1 ⎡ ⎤ cos θ 6 − sin θ 6 0 0 ⎢ sin θ 6 cos θ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 100 0 ⎢ 010 0⎥ 6 ⎥ T7 = ⎢ ⎣ 0 0 1 d7 ⎦ 000 1

(5.226)

⎡

(5.227)

(5.228)

(5.229)

The matrix 3 T6 = 3 T4 4 T5 5 T6 provides the wrist’s orientation in the forearm coordinate frame B3 , 3

T6 = 3 T4 4 T5 5 T6 ⎤ ⎡ cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 0 ⎢ cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 0 ⎥ ⎥ =⎢ ⎣ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 0 ⎦ 0 0 0 1

(5.230)

and the following transformation matrix provides the configuration of the tool frame B7 in the forearm coordinate frame B3 . T7 = 3 T4 4 T5 5 T6 6 T7 ⎡ ⎤ cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 d7 cθ 4 sθ 5 ⎢ cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 d7 sθ 4 sθ 5 ⎥ ⎥ =⎢ ⎣ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 d7 cθ 5 ⎦ 0 0 0 1 3

(5.231)

It is also possible to define a compact 5 T6 to include rotation θ 6 and translation d7 . ⎡

cos θ 6 − sin θ 6 ⎢ sin θ 6 cos θ 6 5 T6 = ⎢ ⎣ 0 0 0 0

⎤ 0 0 0 0⎥ ⎥ 1 d7 ⎦ 0 1

(5.232)

Employing the compact matrix 5 T6 reduces the number of matrices, and therefore the number of numerical calculations.

272

5 Forward Kinematics

Fig. 5.31 Hand of a robot at rest position

The transformation matrix at rest position, where θ 4 = 0, θ 5 = 0, θ 6 = 0, is ⎡

⎤ 100 0 ⎢0 1 0 0 ⎥ 3 ⎥ T7 = ⎢ ⎣ 0 0 1 d7 ⎦ 000 1

(5.233)

To show there are only three types of spherical wrists, we start with the first rotation of the wrist that is always about a fixed axis on the forearm link. If the first joint axis is along the forearm axis, then the first motion is a 1−Roll. This axis would also be along the gripper axis at the rest position. If the first axis of rotation is perpendicular to the forearm axis, then we consider the first rotation as 1−Pitch. If the first rotation is a 1−Roll, then the second rotation is perpendicular to the forearm axis and will be a 2−Pitch. There are two possible situations for the third rotation. It is a 3−Roll, if it is about the gripper axis, or a 3−Yaw, if it is orthogonal to the axis of the first two rotations. In case the first rotation is 1−Pitch, then the second one practically can only be a 2−Yaw, and the third one a 3−Roll. Other combinations are only possible mathematically, not practically. If the first rotation is a Pitch, the second rotation can be a Roll or a Yaw. If it is a Yaw, then the third rotation must be a Roll to have independent rotations. If it is a Roll, then the third rotation must be a Yaw. The Pitch–Yaw–Roll and Pitch–Roll–Yaw are not distinguishable, and we may pick Pitch–Yaw–Roll as the only possible spherical wrist with the first rotation as a Pitch. Therefore, we may have three types of wrists: (1−Roll, 2−Pitch, 3−Roll), (1−Roll, 2−Pitch, 3−Yaw), and (1−Pitch, 2−Yaw, 3−Roll), as are shown in (5.234). wrist types =

⎧ ⎨

Roll P itch

⎩

P itch Y aw



Roll Y aw Roll

(5.234)

Figure 5.31 illustrates a Roll–Pitch–Roll wrist at the rest position, and Fig. 5.32 illustrates it in motion. This type of wrist is also called Eulerian wrist just because Roll–Pitch–Roll reminds Z − x − z rotation axes. We model the Roll, Pitch, and Yaw rotations by introducing two links and three frames between the dead and live frames of a wrist. The links will be connected by three revolute joints. The joints’ axes intersect at the wrist point and are orthogonal when the wrist is at the rest position. It is simpler if we kinematically analyze a spherical wrist by defining three non-DH coordinate frames at the wrist point and determine their relative transformations. Figure 5.33 shows a Roll–Pitch–Roll wrist with three coordinate frames. The first orthogonal frame B0 (x0 , y0 , z0 ) is fixed to the forearm and acts as the wrist dead frame such that z0 is the joint axis of the forearm and a rotating link. The rotating link is the first wrist link and the joint is the first wrist joint. The direction of the axes x0 and y0 is arbitrary. The second frame B1 (x1 , y1 , z1 ) is defined such that z1 is along the gripper axis and x1 is the axis of the second joint. B1 always turns ϕ about z0 and θ about x1 relative to B0 . The third frame B2 (x2 , y2 , z2 ) is the wrist live frame and is defined such that z2 is always along the gripper axis. If the third joint provides a Roll, then z2 is the joint axis; otherwise, the third joint provides Yaw and x2 is the joint axis. Therefore, B2 always turns ψ about z2 or x2 , relative to B1 . Introducing the coordinate frames B1 and B2 simplifies the spherical wrist kinematics by not seeing the interior links

5.4 Spherical Wrist

273

Fig. 5.32 Hand of a robot in motion

Fig. 5.33 Spherical wrist of the Roll–Pitch–Roll or Eulerian type

Fig. 5.34 Spherical wrist of the Roll–Pitch–Yaw type

of the wrist. Considering the definition and rotations of B2 relative to B1 , and B1 relative to B0 , there are only three types of practical spherical wrists as are classified in Table 5.15. These three wrists are shown in Figs. 5.33, 5.34, and 5.35.  Example 186 Roll–Pitch–Roll or Eulerian wrist and DH frames of spherical wrist. This example shows the forward kinematics of the Roll–Pitch–Roll wrist between live and dead frames in detail and step by step. The transformation matrices between frames need special attention to be used as classical method for other types of wrists.

274

5 Forward Kinematics

Figures 5.31 and 5.32 depict illustrations of a spherical wrist of type 1. The common origin of frames B4 , B5 , and B6 is at the wrist point. The final frame, which is called the tool or end-effector frame, is denoted by three vectors, a, s, n, and is set at a symmetric point between the fingers of an empty gripper or at the tip of the tools hold by the hand. The vector n is called tilt and is the normal vector perpendicular to the fingers or jaws. The vector s is called twist and is the slide vector showing the direction of fingers opening. The vector a is called turn and is the approach vector perpendicular to the palm of the hand. The placement of internal links’ coordinate frames is predetermined by the DH method; however, for the end link the placement of the tool’s frame Bn is arbitrary. This arbitrariness may be resolved through simplifying choices or by placement at a distinguished location in the gripper. It is easier to work with the coordinate system Bn if zn is made coincident with zn−1 . This choice sets an = 0 and α n = 0. Figure 5.33 illustrates another view of the Roll–Pitch–Roll spherical wrist. B0 indicates its dead and B2 indicates its live coordinate frames. The transformation matrix 0 R1 is a rotation ϕ about the dead z0 -axis followed by a rotation θ about the x1 -axis. 0

 T  T T R1 = 1 R0T = Rx1 ,θ RzT0 ,ϕ = Rx,θ RZ,ϕ ⎡⎡ ⎤⎡ ⎤T ⎤T 1 0 0 cos ϕ − sin ϕ 0 ⎢ ⎥ = ⎣⎣ 0 cos θ sin θ ⎦ ⎣ sin ϕ cos ϕ 0 ⎦ ⎦ 0 − sin θ cos θ 0 0 1 ⎡ ⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(5.235)

The transformation matrix 1 R2 is a rotation ψ about the local axis z2 . ⎡

1

T R2 = 2 R1T = RzT2 ,ψ = Rz,ψ

⎤ cos ψ − sin ψ 0 = ⎣ sin ψ cos ψ 0 ⎦ 0 0 1

(5.236)

Therefore, the transformation matrix between the live and dead wrist frames is 0

T  R2 = 0 R1 1 R2 = Rx1 ,θ RzT0 ,ϕ RzT2 ,ψ = Rz0 ,ϕ RxT1 ,θ RzT2 ,ψ T T = RZ,ϕ Rx,θ Rz,ψ ⎡ ⎤ cψcϕ − cθsψsϕ −cϕsψ − cθ cψsϕ sθsϕ = ⎣ cψsϕ + cθ cϕsψ cθ cψcϕ − sψsϕ −cϕsθ ⎦ sθsψ cψsθ cθ

(5.237)

Example 187 Roll–Pitch–Yaw spherical wrist. Forward kinematics of the second type of spherical wrist. Figure 5.34 illustrates a spherical wrist of type 2, Roll–Pitch–Yaw. B0 indicates the wrist dead coordinate frame. The main kinematic disadvantage of this type of spherical wrist is that z1 is not fixed to the gripper. However, we attach a coordinate frame B2 to the gripper as the wrist live frame such that z2 to be on the gripper axis and x2 to be the third joint axis. The transformation between B2 and B1 is only a rotation ψ about the x2 -axis. ⎡

1

T R2 = RxT2 ,ψ = Rx,ψ

⎤T 1 0 0 = ⎣ 0 cos ψ sin ψ ⎦ 0 − sin ψ cos ψ

(5.238)

5.4 Spherical Wrist

275

Fig. 5.35 Spherical wrist of the Pitch–Yaw–Roll type

To determine the transformation matrix 0 R1 , we turn B1 first ϕ about the z0 -axis and then θ about the x1 -axis. 0

 T  T T R1 = 1 R0T = Rx1 ,θ RzT0 ,ϕ = Rx,θ RZ,ϕ ⎡⎡ ⎤⎡ ⎤T ⎤T 1 0 0 cϕ −sϕ 0 ⎢ ⎥ = ⎣⎣ 0 cθ sθ ⎦ ⎣ sϕ cϕ 0 ⎦ ⎦ 0 −sθ cθ 0 0 1 ⎡ ⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(5.239)

Therefore, the transformation matrix between the live and dead wrist’s frames is 0

 T R2 = 0 R1 1 R2 = Rx1 ,θ RzT0 ,ϕ RxT2 ,ψ = Rz0 ,ϕ RxT1 ,θ RxT2 ,ψ T T = RZ,ϕ Rx,θ Rx,ψ ⎡ ⎤ cϕ sθsψsϕ − cθ cψsϕ cθ sψsϕ + cψsθsϕ = ⎣ sϕ cθ cψcϕ − cϕsθsψ −cθ cϕsψ − cψcϕsθ ⎦ 0 cθ sψ + cψsθ cθ cψ − sθsψ

(5.240)

Example 188 Pitch–Yaw–Roll spherical wrist. Forward kinematics of the third type of spherical wrist. Figure 5.35 illustrates a spherical wrist of the type 3, Pitch–Yaw–Roll. B0 indicates its dead and B2 indicates its live coordinate frames. The transformation matrix 1 R2 is a rotation ψ about the local z2 -axis. ⎡

1

T R2 = 2 R1T = RzT2 ,ψ = Rz,ψ

⎤ cos ψ − sin ψ 0 = ⎣ sin ψ cos ψ 0 ⎦ 0 0 1

(5.241)

276

5 Forward Kinematics

Fig. 5.36 A practical spherical wrist

To determine the transformation matrix 0 R1 , we turn B1 first ϕ about the z0 -axis and then θ about the x1 -axis. 0

T T   T R1 = 1 R0T = Rx1 ,θ RzT0 ,ϕ = Rx,θ RZ,ϕ ⎡⎡ ⎤⎡ ⎤T ⎤T 1 0 0 cϕ −sϕ 0 ⎥ ⎢ = ⎣⎣ 0 cθ sθ ⎦ ⎣ sϕ cϕ 0 ⎦ ⎦ 0 −sθ cθ 0 0 1 ⎡ ⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(5.242)

Therefore, the transformation matrix between the live and dead wrist’s frames is 0

T  R2 = 0 R1 1 R2 = Rx1 ,θ RzT0 ,ϕ RzT2 ,ψ = Rz0 ,ϕ RxT1 ,θ RzT2 ,ψ T T = RZ,ϕ Rx,θ Rz,ψ ⎡ ⎤ cψcϕ − cθsψsϕ −cϕsψ − cθ cψsϕ sθsϕ = ⎣ cψsϕ + cθ cϕsψ cθ cψcϕ − sψsϕ −cϕsθ ⎦ sθsψ cψsθ cθ

(5.243)

Example 189 Practical design of a spherical wrist. An illustration of how a spherical wrist is designed and works. The three rotational motions are all controlled by three coaxes shafts through gear arrangements. Figure 5.36 illustrates a practical Eulerian spherical wrist. The three rotations of Roll–Pitch–Roll are controlled by three coaxes shafts. The first rotation is a Roll of B4 about z4 . The second rotation is a Pitch of B5 about z5 . The third rotation is a roll of B6 about z6 . The figure shows the wrist at rest position.

5.5

Assembling Kinematics

Most modern industrial robots have a main manipulator and a series of changeable wrists. The manipulator is multibody so that holds the main power units and provides a powerful motion for the wrist point. Figure 5.37 illustrates an example of an articulated manipulator with three DOF s. This manipulator can rotate θ 1 about the z0 -axis relative to the global frame by the base motor M1 . It carries the other motors M2 and M3 . Motor M2 is the base motor of a 2R manipulator with arms l1 and l8 . The motor M2 turns the arms about the z1 -axis by angle θ 2 and carries motor M3 . The motor M3 controls the motion of the last arm by turning θ 3 about the z2 -axis. The tip point of the last arm of the manipulator will be the point at which a wrist will be attached.

5.5 Assembling Kinematics

277

Fig. 5.37 An articulator manipulator with three DOF s

Fig. 5.38 A spherical wrist and its kinematics

Changeable wrists are complex multibodies that are made to provide three rotational DOF s about the wrist point. The base of the wrist will be attached to the tip point of the manipulator. The wrist is the actual operator of the robot that is also called the end-effector, gripper, hand, or tool. Figure 5.38 illustrates a sample of a spherical wrist that is supposed to be attached to the manipulator in Fig. 5.37. To solve the kinematics of a modular robot, we consider the manipulator and the wrist as individual multibodies. We attach a temporary coordinate frame at the tip point of the manipulator and another temporary frame at the base point of the wrist. The temporary coordinate frame at the tip point is called the takht frame, and the coordinate frame at the base of the wrist is called the neshin frame. Mating the neshin and takht frames assembles the robot kinematically. The kinematic mating of the wrist and arm is called assembling. The coordinate frame B8 in Fig. 5.37 is the takht frame of the manipulator, and the coordinate frame B9 in Fig. 5.38 is the neshin frame of the wrist. In the assembling process, the neshin frame B9 sits on the takht frame B8 such that z8 be coincident with z9 , and x8 be coincident with x9 . The articulated robot that is made by assembling the spherical wrist and articulated manipulator is shown in Fig. 5.39. The assembled multibody will always have some additional coordinate frames. The extra frames require extra transformation matrices that may increase the number of mathematical calculations. It is possible to make a recommendation to eliminate the neshin coordinate frame and keep the takht frame at the connection point. However, as long as the transformation matrices between the frames are known, having extra coordinate frames is not a significant disadvantage. In Fig. 5.39, we may ignore B8 and directly go from B3 to B4 and substitute l8 and l9 with l3 = l8 + l9 .

278

5 Forward Kinematics

Fig. 5.39 An articulated robot that is made by assembling a spherical wrist to an articulated manipulator

Fig. 5.40 A single DOF arm as the base for a RR planar manipulator

Every link needs at least one coordinate frame, called the main frame of the link. However, we may also attach some other frames to the link for better expression of coordinates or a simpler transformation. An extra coordinate frame on a link is called the Sina frame. A Sina coordinate frame is a spare frame that may be used instead of the main frame. Usually Sina frames have no variables and are related to the main frame of the same link by a constant translation and constant rotation. In fact, whenever we need to install a sensor, a camera, or any equipment to an arm of a robot, we must attach a Sina coordinate frame to determine the position and orientation of the equipment. Such auxiliary coordinate frame is a Sina coordinate frame. The Sina frame may also be called the spare, extra, dummy, temporary, intermediate, or jump frame. The frames B3 and B4 are the Sina frames for the link AB, while B is its main frame. The word “takht” means “chair,” and the word “neshin” means “sit,” both from Persian. Sina (Avicenna or Abu Ali Sina) (980−1037), a Persian polymath, also known as the “Prince of physicians,” and “Father of early modern medicine,” is known as one of the foremost philosophers and scientists of the world in the tenth century. Example 190 A planar 2R manipulator assembling. A first example to show the principle of assembling process. Figure 5.40 illustrates an example of a one DOF arm as the base for an RR planar manipulator. This arm can rotate relative to the global frame by a motor at M1 and carries another motor at M2 . Figure 5.41 illustrates a sample of a planar wrist that is supposed to be attached to the arm in Fig. 5.40. The coordinate frame B2 in Fig. 5.40 is the takht frame of the arm, and the coordinate frame B3 in Fig. 5.41 is the neshin frame of the wrist.

5.5 Assembling Kinematics

279

Fig. 5.41 A planar wrist

In Fig. 5.40, transformation between B1 and B0 is a rotation θ 1 about z0 . ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.244)

Transformation between the takht frame B2 and B1 is a displacement l1 along x1 . ⎤ 1 0 0 l1 ⎢0 1 0 0 ⎥ 1 ⎥ T2 = ⎢ ⎣0 0 1 0 ⎦ 0001 ⎡

(5.245)

In Fig. 5.41, transformation between B4 and the neshin frame B3 is a displacement l1 along x1 . ⎤ 1 0 0 l2 ⎢0 1 0 0 ⎥ 3 ⎥ T4 = ⎢ ⎣0 0 1 0 ⎦ 0001 ⎡

(5.246)

The RR planar manipulator that is made by assembling the wrist and arm is shown in Fig. 5.42. In Fig. 5.42, transformation between the takht frame B2 and the neshin frame B3 is a rotation θ 2 about x2 . ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 2 T3 = ⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.247)

The forward kinematics of the end-effector of the assembled 2R robot will be 0

T4 = 0 T1 1 T2 2 T3 3 T4

(5.248)

280

5 Forward Kinematics

Fig. 5.42 The RR planar manipulator that is made by assembling a wrist and an arm

Fig. 5.43 A spherical wrist

We may eliminate B1 and connect the takht frame B2 to the ground frame B0 , directly. ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T2 = 0 T1 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(5.249)

We may also eliminate the neshin frame B3 and connect the end-effector frame B4 to B2 . ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 3 2 2 T4 = T3 T4 = ⎢ ⎣ 0 0 0 0

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(5.250)

These matrices match with Eqs. (5.83) and (5.84) after a rename of the remaining coordinate frames. Example 191 Spherical wrist. In this example, a spherical wrist will be decomposed into its component to see how kinematically, we assemble the components to make the wrist working. A spherical wrist is a combined multibody that simulates a spherical joint. Such a combination gives three rotational DOF s to a final link. The link may be a gripper if the wrist is being used in an assembling and production line. Figure 5.43 illustrates a sample of spherical wrists. The axes z5 , z6 , z7 are the rotation axes of the wrists.

5.5 Assembling Kinematics

281

Fig. 5.44 The rotating bodies of a spherical wrist

Fig. 5.45 The neshin, dead, living, and gripper frames of a spherical wrist

The coordinate frame B4 is the neshin frame of the wrist, and B9 is the tool frame of the wrist. The coordinate frame B5 is fixed with respect to B4 and may be considered as the wrist dead frame. B6 is the frame of the first link in shape  that rotates about z5 . B7 is the frame of the middle cylindrical link  that rotates about z6 . B8 is the wrist live frame and is the fixed frame to the shaft that supports the gripper. B8 rotates about z7 . Therefore, B6 , B7 , and B8 are the frames of the three rotating links. The joint axes of the wrist are intersecting at the wrist point. These three links and their associated frames are illustrated in Fig. 5.44. The spherical wrist of Fig. 5.43 is made of a link RR(−90) attached to another link RR(90) that finally is attached to a spinning gripper link RR(0). The final frame B9 is always parallel to B8 and is attached to the gripper at a distance d9 from the wrist point. The final frame B9 is the gripper or tool frame. It is set at a symmetric point between the fingers of an empty gripper. The wrist will be attached to the final link of a manipulator arm, which usually provides three DOF s for positioning the wrist point at a desired coordinate in space. The kinematic analysis of the spherical wrist begins by deriving the required transformation matrices 5 T6 , 6 T7 , and 7 T8 . The matrix 4 T5 determines how the wrist dead frame B5 relates to the neshin frame B4 , and the matrix 8 T9 determines how the tool frame B9 connects to the wrist live frame B8 , as are shown in Fig. 5.45.

282

5 Forward Kinematics

Link (6) is an RR(−90), link (7) is an RR(90), and link (8) is an RR(0); therefore, ⎡

cos θ 6 ⎢ sin θ 6 5 T6 = ⎢ ⎣ 0 0

0 − sin θ 6 0 cos θ 6 −1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎤ cos θ 7 0 sin θ 7 0 ⎢ sin θ 7 0 − cos θ 7 0 ⎥ 6 ⎥ T7 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1 ⎤ ⎡ cos θ 8 − sin θ 8 0 0 ⎢ sin θ 8 cos θ 8 0 0 ⎥ 7 ⎥ T8 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01

(5.251)

⎡

5

T8 = 5 T6 6 T7 7 T8 ⎤ ⎡ cθ 6 cθ 7 cθ 8 − sθ 6 sθ 8 −cθ 8 sθ 6 − cθ 6 cθ 7 sθ 8 cθ 6 sθ 7 0 ⎢ cθ 6 sθ 8 + cθ 7 cθ 8 sθ 6 cθ 6 cθ 8 − cθ 7 sθ 6 sθ 8 sθ 6 sθ 7 0 ⎥ ⎥ =⎢ ⎣ −cθ 8 sθ 7 sθ 7 sθ 8 cθ 7 0 ⎦ 0 0 0 1

(5.252)

(5.253)

(5.254)

The matrix 5 T8 = 5 T6 6 T7 7 T8 provides the spherical wrist’s transformation. 5 T8 must be reduced to an identity matrix when the wrist is at rest position and all angular variables are zero. The transformation of the wrist dead frame B5 into the neshin frame B4 and the transformation of the wrist live frame B8 into the tool frame B9 are only translations d5 and d9 , respectively. ⎤ 100 0 ⎢0 1 0 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 0 1 d5 ⎦ 000 1

(5.255)

⎤ 100 0 ⎢0 1 0 0 ⎥ 8 ⎥ T9 = ⎢ ⎣ 0 0 1 d9 ⎦ 000 1

(5.256)

⎡

⎡

Example 192 Assembling a wrist mechanism to a manipulator. Forward kinematics of a wrist and a manipulator have been calculated independently and in this example they are attached to each other to make a full robot. Consider a robot made by mounting the hand shown in Fig. 5.31, to the tip point of the articulated arm shown in Fig. 5.22. The resulting robot would have six DOF s to reach any point within the working space in a desired orientation. The robot’s forward kinematics can be found by combining the wrist transformation matrix (5.225) and the manipulator transformation matrix (5.103). 0 T7 = Tarm Twrist 6 T7 = 0 T3 3 T6 6 T7 (5.257) The wrist transformation matrix 3 T7 has been given in Eq. (5.225), and the arm transformation matrix 0 T3 has been found in Example 178. However, because we are attaching the wrist at point P of the frame B3 , the transformation matrix 3 T4 in (5.226) must include this joint distance. So, we substitute matrix (5.226) with

5.5 Assembling Kinematics

283

Fig. 5.46 A spherical manipulator

⎡

cos θ 4 ⎢ sin θ 4 3 T4 = ⎢ ⎣ 0 0

0 − sin θ 4 0 cos θ 4 −1 0 0 0

⎤ 0 0⎥ ⎥ l3 ⎦ 1

(5.258)

to find Twrist = 3 T6 ⎡ ⎤ −sθ 4 sθ 6 + cθ 4 cθ 5 cθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 0 ⎢ cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 0 ⎥ ⎥ =⎢ ⎣ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 l3 ⎦ 0 0 0 1

(5.259)

The rest position of the robot can be checked to be at ⎡

⎤ 1 0 0 l2 ⎢ 0 1 0 d 2 + l3 ⎥ 0 ⎥ T7 = ⎢ ⎣ 0 0 1 d1 + d7 ⎦ 000 1 because

⎤ 100 0 ⎢0 1 0 0 ⎥ 6 ⎥ T7 = ⎢ ⎣ 0 0 1 d7 ⎦ 000 1

(5.260)

⎡

(5.261)

Example 193 A spherical manipulator. A spherical manipulator simulates the spherical coordinate for positioning a point in a 3D space to move the tip point of the final link to a desired point in space by two rotations and a translation. Figure 5.46 illustrates a spherical manipulator. The coordinate frame B0 is the global or base frame of the manipulator. The link (1) can turn about z0 and the link (2) can turn about z1 that is perpendicular to z0 . These two rotations simulate the two angular motions of spherical coordinates. The radial coordinate is simulated by link (3) that has a prismatic joint with link (2). There is also a takht coordinate frame at the tip point of link (3) at which a wrist can be attached. The link (1) in Fig. 5.46 is an RR(90), link (2) is also an RP(90), and link (3) is a PR(0); therefore,

284

5 Forward Kinematics

⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 sin θ 1 0 − cos θ 1 1 0 0 0

⎡

cos θ 2 0 sin θ 2 ⎢ sin θ 2 0 − cos θ 2 1 T2 = ⎢ ⎣ 0 1 0 0 0 0 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 d3 ⎦ 000 1

⎤ 0 0⎥ ⎥ d1 ⎦ 1

(5.262)

⎤ 0 0⎥ ⎥ 0⎦ 1

(5.263)

(5.264)

The transformation matrix from B3 to the takht frame B4 is only a translation d4 . ⎡

⎤ 100 0 ⎢0 1 0 0 ⎥ 3 ⎥ T4 = ⎢ ⎣ 0 0 1 d4 ⎦ 000 1

(5.265)

The transformation matrix of the takht frame B4 to the base frame B0 is 0

0

T4 at the rest position reduces to

T4 = 0 T1 1 T2 2 T3 3 T4 ⎡ ⎤ cθ 1 cθ 2 sθ 1 cθ 1 sθ 2 (d3 + d4 ) (cθ 1 sθ 2 ) ⎢ cθ 2 sθ 1 −cθ 1 sθ 1 sθ 2 (d3 + d4 ) (sθ 2 sθ 1 ) ⎥ ⎥ =⎢ ⎣ sθ 2 0 −cθ 2 d1 − d3 cθ 2 − d4 cθ 2 ⎦ 0 0 0 1 ⎤ 1 0 0 0 ⎢ 0 −1 0 0 ⎥ 0 ⎥ T4 = ⎢ ⎣ 0 0 −1 d1 − d4 ⎦ 0 0 0 1

(5.266)

⎡

(5.267)

As a general recommendation, the setup of the DH coordinate frames is such that the overall transformation matrix at the rest position becomes an identity matrix. If we rearrange the coordinate frame of the link (1) to make it an RR(−90), then 0 T1 becomes ⎡ ⎤ cos θ 1 0 − sin θ 1 0 ⎢ sin θ 1 0 cos θ 1 0 ⎥ 0 ⎥ T1 = ⎢ (5.268) ⎣ 0 −1 0 d1 ⎦ 0 0 0 1 and the overall transformation matrix at the rest position becomes an identity matrix. To make link (1) to be RR(−90), we may reverse the direction of z1 - or x1 -axis. Figure 5.47 illustrates the new arrangement of the coordinate frames. Therefore, the transformation matrix of the takht frame B4 to the base frame B0 at the rest position reduces to ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 0 ⎥ T4 = ⎢ (5.269) ⎣ 0 0 1 d1 + d4 ⎦ 000 1

5.5 Assembling Kinematics

285

Fig. 5.47 A spherical arm with the arrangement of coordinate frames such that the overall transformation matrix reduces to an identity at rest position

Fig. 5.48 Assembling of a spherical hand and arm

Example 194  Assembling of a spherical wrist to a spherical manipulator. The spherical wrist will be attached to the spherical manipulator by a kinematic surgery to make a complete robot. This example indicates how easier will be such attachment by introducing auxiliary Sina frame whenever needed. This example will be a reference for similar operations. To transform the manipulator of Fig. 5.47 into a robot, we need to attach a hand to it. Let kinematically assemble the Eulerian spherical wrist of Example 191 or 186 to the spherical manipulator. The wrist, manipulator, and their associated DH coordinate frames are shown in Figs. 5.43 and 5.47 , respectively. Assembling a hand to a manipulator is a kinematic surgery in which during a kinematic operation we attach a multibody to the other. In this example we attach a spherical hand to a spherical manipulator to make a spherical arm-hand robot. The takht coordinate frame B4 of the manipulator and the neshin coordinate frame B4 of the wrist are set up to be exactly the same. Therefore, we may assemble the manipulator and wrist by matching these two frames and make a combined manipulator-wrist robot as is shown in Fig. 5.48. However, in general case the takht and neshin coordinate frames may have different labels and there be a constant transformation matrix between them. The forward kinematics of the robot for tool frame B9 can be found by a matrix multiplication. 0

T9 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7 7 T8 8 T9

(5.270)

286

5 Forward Kinematics

Fig. 5.49 Simplification of the coordinate frames for an assembled of a spherical hand and arm

The matrices i−1 Ti are given in Examples 191 and 193. We can eliminate the coordinate frames B3 and B4 to reduce the total number of frames, and simplify the matrix calculations. However, we may prefer to keep them and simplify the assembling process of changing the wrist with a new one. If this assembled robot is supposed to work for a while, we may do the elimination and simplify the robot to the one in Fig. 5.49. We should mathematically substitute the eliminated frames B3 and B4 by a transformation matrix 2 T5 . ⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T5 = 2 T3 3 T4 4 T5 = ⎢ ⎣ 0 0 1 d6 ⎦ 000 1 ⎡

d6 = d3 + d4 + d5

(5.271)

(5.272)

Now the forward kinematics of the tool frame B9 becomes 0

T9 = 0 T1 1 T2 2 T5 5 T6 6 T7 7 T8 8 T9

(5.273)

Example 195 Spherical robot forward kinematics. Technically there is nothing new in this example that readers cannot extract from previous example. However, it is assumed here that a new spherical robot with different designs is given and we are supposed to determine its forward kinematics. Here is a detailed analysis. Figure 5.50 illustrates a spherical manipulator equipped with a spherical wrist to make an RRP robot. The associated DH parameters are shown in Table 5.16. However, we recommend applying the link-joints classification of Example 166. The link-joint combinations are shown in Table 5.17. Employing the associated transformation matrices for moving from Bi to Bi−1 provides the following matrices: ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0 ⎡

cos θ 2 ⎢ sin θ 2 1 T2 = ⎢ ⎣ 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(5.274)

⎤ 0 0⎥ ⎥ l2 ⎦ 1

(5.275)

0 − sin θ 1 0 cos θ 1 −1 0 0 0 0 sin θ 2 0 − cos θ 2 1 0 0 0

5.5 Assembling Kinematics

287

Fig. 5.50 A spherical robot made by a spherical manipulator equipped with a spherical wrist Table 5.16 DH parameter table for Stanford arm Frame no. 1 2 3 4 5 6

ai 0 0 0 0 0 0

αi −90 deg 90 deg 0 −90 deg 90 deg 0

di 0 l2 d3 0 0 0

θi θ1 θ2 0 θ4 θ5 θ6

Table 5.17 DH parameter table for setting up the link frames Link no. 1 2 3 4 5 6

Type RR(−90) RP(90) PR(0) RR(−90) RR(90) RR(0)

⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 d3 ⎦ 000 1 ⎡ cos θ 4 0 − sin θ 4 ⎢ sin θ 4 0 cos θ 4 3 T4 = ⎢ ⎣ 0 −1 0 0 0 0 ⎡

(5.276) ⎤ 0 0⎥ ⎥ 0⎦ 1

(5.277)

288

5 Forward Kinematics

⎡

⎤ cos θ 5 0 sin θ 5 0 ⎢ sin θ 5 0 − cos θ 5 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1 ⎡ ⎤ cos θ 6 − sin θ 6 0 0 ⎢ sin θ 6 cos θ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01

(5.278)

(5.279)

Therefore, the configuration of the wrist final coordinate frame B6 in the global coordinate frame is 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ r11 r12 r13 r14 ⎢ r21 r22 r23 r24 ⎥ ⎥ =⎢ ⎣ r31 r32 r33 r34 ⎦ 0 0 0 1

(5.280)

where r11 = sθ 6 (−cθ 4 sθ 1 − cθ 1 cθ 2 sθ 4 ) +cθ 6 (−cθ 1 sθ 2 sθ 5 + cθ 5 (−sθ 1 sθ 4 + cθ 1 cθ 2 cθ 4 ))

(5.281)

r21 = sθ 6 (cθ 1 cθ 4 − cθ 2 sθ 1 sθ 4 ) +cθ 6 (−sθ 1 sθ 2 sθ 5 + cθ 5 (cθ 1 sθ 4 + cθ 2 cθ 4 sθ 1 )) r31 = sθ 2 sθ 4 sθ 6 + cθ 6 (−cθ 2 sθ 5 − cθ 4 cθ 5 sθ 2 )

(5.282) (5.283)

r12 = cθ 6 (−cθ 4 sθ 1 − cθ 1 cθ 2 sθ 4 ) −sθ 6 (−cθ 1 sθ 2 sθ 5 + cθ 5 (−sθ 1 sθ 4 + cθ 1 cθ 2 cθ 4 ))

(5.284)

r22 = cθ 6 (cθ 1 cθ 4 − cθ 2 sθ 1 sθ 4 ) −sθ 6 (−sθ 1 sθ 2 sθ 5 + cθ 5 (cθ 1 sθ 4 + cθ 2 cθ 4 sθ 1 )) r32 = cθ 6 sθ 2 sθ 4 − sθ 6 (−cθ 2 sθ 5 − cθ 4 cθ 5 sθ 2 )

(5.285) (5.286)

r13 = cθ 1 cθ 5 sθ 2 + sθ 5 (−sθ 1 sθ 4 + cθ 1 cθ 2 cθ 4 )

(5.287)

r23 = cθ 5 sθ 1 sθ 2 + sθ 5 (cθ 1 sθ 4 + cθ 2 cθ 4 sθ 1 )

(5.288)

r33 = cθ 2 cθ 5 − cθ 4 sθ 2 sθ 5

(5.289)

r14 = −l2 sθ 1 + d3 cθ 1 sθ 2

(5.290)

r24 = l2 cθ 1 + d3 sθ 1 sθ 2

(5.291)

r34 = d3 cθ 2

(5.292)

The end-effector kinematics can be solved by multiplying the position of the tool frame B7 with respect to the wrist point, by 0 T6 , 0 T7 = 0 T6 6 T7 (5.293)

5.6  Coordinate Transformation Using Screws

289

where

⎡

⎤ 100 0 ⎢0 1 0 0 ⎥ 6 ⎥ T7 = ⎢ ⎣ 0 0 1 d7 ⎦ 000 1

(5.294)

To check the correctness of the final transformation matrix to map the coordinates in tool frame into the base frame, we may set the joint variables at a specific rest position. Let us substitute the joint rotational angles of the spherical robot equal to zero. (5.295) θ 1 = 0, θ 2 = 0, θ 3 = 0, θ 4 = 0, θ 5 = 0 Therefore, the transformation matrix (5.280) would be 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ 100 0 ⎢ 0 1 0 l2 ⎥ ⎥ =⎢ ⎣ 0 0 1 d3 ⎦ 000 1

(5.296)

that correctly indicates the origin of the tool frame in robot’s stretched-up configuration, at ⎤ 0 G ro6 = ⎣ l2 ⎦ d3 ⎡

5.6

(5.297)

 Coordinate Transformation Using Screws

It is possible to use screws to describe a transformation matrix between two adjacent coordinate frames Bi and Bi−1 . We can move Bi to Bi−1 by a central screw sˇ (ai , α i , ıˆi−1 ) followed by another central screw sˇ (di , θ i , kˆi−1 ). i−1

Ti = sˇ (di , θ i , kˆi−1 ) sˇ (ai , α i , ıˆi−1 ) ⎡ ⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ ⎥ =⎢ ⎣ 0 ⎦ sin α i cos α i di 0 0 0 1

(5.298)

Proof A Denavit–Hartenberg (DH ) frame is based on four parameters (ai , α i , θ i , di ) of the link (i). The parameters θ i and di are joint parameters, because they define the relative position of two adjacent links connected at joint i. Assuming every joint to be revolute or prismatic, it will always be the case that either θ i or di is fixed and the other is the joint variable. For a revolute joint (R) at joint i, the θ i is the unique joint variable, and the value of di is fixed. For a prismatic joint (P), the di is the only joint variable, while the value of θ i is fixed. The variable parameter, either θ i or di , is the joint variable. The parameters α i and ai are link parameters, because they define relative positions of joints i and i + 1 at two ends of link (i). The link twist α i is the angle of rotation zi−1 -axis about xi to become parallel with the zi -axis. The kinematic length parameter, ai , is the translation along the xi -axis to bring the zi−1 -axis on the zi -axis. The joint parameters θ i and di define a screw motion because θ i is a rotation about the zi−1 -axis, and di is a translation along the zi−1 -axis. The link parameters α i and ai also define a screw motion because α i is a rotation about the xi -axis, and ai is a translation along the xi -axis. Therefore, we can move the zi−1 -axis to the zi -axis by a central screw sˇ (ai , α i , ıˆi ), and move the xi−1 -axis to the xi -axis by a central screw sˇ (di , θ i , kˆi−1 ).

290

5 Forward Kinematics

The central screw sˇ (ai , α i , ıˆi ) is sˇ (ai , α i , ıˆi ) = D(ai , ıˆi )R(ˆıi , α i ) = Dxi ,ai Rxi ,αi ⎤⎡ ⎤ ⎡ 1 0 0 ai 1 0 0 0 ⎢ 0 1 0 0 ⎥ ⎢ 0 cos α i − sin α i 0 ⎥ ⎥ ⎥⎢ =⎢ ⎣ 0 0 1 0 ⎦ ⎣ 0 sin α i cos α i 0 ⎦ 000 1 0 0 0 1 ⎡ ⎤ 1 0 0 ai ⎢ 0 cos α i − sin α i 0 ⎥ ⎥ =⎢ ⎣ 0 sin α i cos α i 0 ⎦ 0 0 0 1

(5.299)

and the central screw sˇ (di , θ i , kˆi−1 ) is sˇ (di , θ i , kˆi−1 ) = D(di , kˆi−1 )R(kˆi−1 , θ i ) = Dzi−1 ,di Rzi−1 ,θ i ⎡ ⎤⎡ ⎤ 100 0 cos θ i − sin θ i 0 0 ⎢ 0 1 0 0 ⎥ ⎢ sin θ i cos θ i 0 0 ⎥ ⎥⎢ ⎥ =⎢ ⎣ 0 0 1 di ⎦ ⎣ 0 0 1 0⎦ 000 1 0 0 01 ⎡ ⎤ cos θ i − sin θ i 0 0 ⎢ sin θ i cos θ i 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 di ⎦ 0 0 0 1

(5.300)

Therefore, the transformation matrix i−1 Ti made by two screw motions would be i−1

Ti = sˇ (di , θ i , kˆi−1 ) sˇ (ai , α i , ıˆi ) ⎤⎡ ⎤ ⎡ cos θ i − sin θ i 0 0 1 0 0 ai ⎢ sin θ i cos θ i 0 0 ⎥ ⎢ 0 cos α i − sin α i 0 ⎥ ⎥⎢ ⎥ =⎢ ⎣ 0 0 1 di ⎦ ⎣ 0 sin α i cos α i 0 ⎦ 0 0 0 1 0 0 0 1 ⎡ ⎤ cos θ i − cos α i sin θ i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ ⎥ =⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1

(5.301)

The resultant transformation matrix i−1 Ti is equivalent to a general screw whose parameters can be found based on Eqs. (4.223) and (4.224). The twist of screw, φ, can be computed based on Eq. (4.227),  1  G  tr RB − 1 2 1 = (cos θ i + cos θ i cos α i + cos α i − 1) 2

cos φ =

(5.302)

5.6  Coordinate Transformation Using Screws

291

and the axis of screw, u, ˆ can be found by using Eq. (4.229)  1 G RB − G RBT 2 sin φ ⎡ ⎤ ⎡ ⎤ sθ i 0 cθ −cα i sθ i sθ i sα i cθ i 1 ⎣ i sθ i cθ i cα i −cθ i sα i ⎦ − ⎣ −cα i sθ i cθ i cα i sα i ⎦ = 2sφ 0 sα i cα i sθ i sα i −cθ i sα i cα i ⎡ ⎤ 0 −sθ i − cα i sθ i sθ i sα i 1 ⎣ sθ i + cα i sθ i 0 −sα i − cθ i sα i ⎦ = 2sφ −sθ i sα i sα i + cθ i sα i 0

u˜ =

and therefore,

⎡ ⎤ sin α i + cos θ i sin α i 1 ⎣ ⎦ uˆ = sin θ i sin α i 2sφ sin θ i + cos α i sin θ i

(5.303)

(5.304)

  The translation parameter, h, and the position vector of a point on the screw axis, for instance 0 yi−1 zi−1 , can be found based on Eq. (4.232). ⎡

⎤−1 ⎡ ⎤ u1 −r12 −r13 r14 ⎣ yi−1 ⎦ = ⎣ u2 1 − r22 −r23 ⎦ ⎣ r24 ⎦ yi−1 u3 −r32 1 − r33 r34 ⎡ ⎤−1 ⎡ ⎤ sα + cθ i sα i −sθ i cα i sθ i sα i ai cθ i 1 ⎣ i = sθ i sα i 1 − cθ i cα i −cθ i sα i ⎦ ⎣ ai sθ i ⎦ 2sφ sθ i + cα i sθ i sα i cα i di h

⎤

⎡

(5.305)

 Example 196  Classification of industrial robot links by screws. This example indicates how we may determine DH transformation matrices of the 12 types of industrial links, using screw method. There are 12 different link configurations that are mostly used for industrial robots. Every type has its own class of geometrical configuration and transformation. Each class is identified by its joints at both ends and has its own transformation matrix to go from the distal joint coordinate frame Bi to the proximal joint coordinate frame Bi−1 . The transformation matrix of each class depends solely on the proximal joint and the angle between z axes. The screw expression for two arbitrary coordinate frames is i−1 Ti = sˇ (di , θ i , kˆi−1 ) sˇ (ai , α i , ıˆi ) (5.306) where sˇ (di , θ i , kˆi−1 ) = D(di , kˆi−1 )R(kˆi−1 , θ i ) sˇ (ai , α i , ıˆi ) = D(ai , ıˆi )R(ˆıi , α i ) The screw expression of the frame transformation can be simplified for each class according to Table 5.18.

(5.307) (5.308)

292

5 Forward Kinematics Table 5.18 Classification of industrial robot link by screws No. 1 2 3 4 5 6 7 8 9 10 11 12

Type RR(0) RR(180) R⊥R(90) R⊥R(−90) RR(90) RR(−90) PR(0) PR(180) P⊥R(90) P⊥R(−90) PR(90) PR(−90)

of or or or or or or or or or or or or

Link RP(0) RP(180) R⊥P(90) R⊥P(−90) RP(90) RP(−90) PP(0) PP(180) P⊥P(90) P⊥P(−90) PP(90) PP(−90)

i−1 T

i

sˇ (0, θ i , kˆi−1 ) sˇ (ai , 0, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (ai , 2π, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (ai , π, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (ai , −π, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (0, π, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (0, −π, ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (ai , 0, ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (ai , 2π, ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (ai , π, ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (ai , −π, ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (0, π , ıˆi ) sˇ (di , 0, kˆi−1 ) sˇ (0, −π, ıˆi )

Table 5.19 Screw transformation for the spherical robot shown in Fig. 5.50 Link no. 1 2 3 4 5 6

Class RR(−90) RP(90) PR(0) RR(−90) RR(90) RR(0)

Screw transformation 1 = sˇ (0, θ i , kˆi−1 ) sˇ (0, −π, ıˆi ) 1 T = sˇ (0, θ , kˆ 2 i i−1 ) sˇ (0, π, ıˆi ) 2 T = sˇ (d , 0, kˆ 3 i i−1 ) sˇ (ai , 0, ıˆi ) 3 T = sˇ (0, θ , kˆ 4 i i−1 ) sˇ (0, −π, ıˆi ) 4 T = sˇ (0, θ , kˆ 5 i i−1 ) sˇ (0, π, ıˆi ) 5 T = sˇ (0, θ , kˆ 6 i i−1 ) sˇ (ai , 0, ıˆi ) 0T

As an example, we may examine the first class, i−1

Ti = sˇ (0, θ i , kˆi−1 ) sˇ (ai , 0, ıˆi ) = D(0, kˆi−1 )R(kˆi−1 , θ i )D(ai , ıˆi )R(ˆıi , 0) ⎡ ⎤⎡ ⎤ cos θ i − sin θ i 0 0 1 0 0 ai ⎢ sin θ i cos θ i 0 0 ⎥ ⎢ 0 1 0 0 ⎥ ⎥⎢ ⎥ =⎢ ⎣ 0 0 1 0⎦⎣0 0 1 0 ⎦ 0 0 01 000 1 ⎡ ⎤ cos θ i − sin θ i 0 ai cos θ i ⎢ sin θ i cos θ i 0 ai sin θ i ⎥ ⎥ =⎢ ⎣ 0 ⎦ 0 1 0 0 0 0 1

(5.309)

and find the same result as Eq. (5.32). Example 197  Spherical robot forward kinematics based on screws. An example to show how we are able to determine forward kinematics of robots by screw method easier. Application of screws in forward kinematics can be done by determining the class of each link of a robot and applying the associated screws. The class of links for the spherical robot shown in Fig. 5.50 is indicated in Table 5.19. Therefore, the configuration of the end-effector frame of the spherical robot in the base frame is 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 = sˇ (0, θ i , kˆi−1 ) sˇ (0, −π, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (0, π , ıˆi ) סs (di , 0, kˆi−1 ) sˇ (ai , 0, ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (0, −π , ıˆi ) סs (0, θ i , kˆi−1 ) sˇ (0, π , ıˆi ) sˇ (0, θ i , kˆi−1 ) sˇ (ai , 0, ıˆi )

(5.310)

5.7  Non-Denavit–Hartenberg Methods

293

Example 198  Plücker coordinate of a central screw. Plücker coordinate is an alternative to work with screws. In general, whenever in science we are working with lines and rotations, Plücker coordinate is considered a smart alternative method of expression of operations. Utilizing Plücker coordinates we can define a central screw as " sˇ (h, φ, u) ˆ = which is equal to

"

φ uˆ huˆ

#

# φ uˆ = D(hu) ˆ R(u, ˆ φ) huˆ

(5.311)

(5.312)

The central screw sˇ (ai , α i , ıˆi ) can also be expressed by a proper Plücker coordinate. " sˇ (ai , α i , ıˆi ) =

α i ıˆi ai ıˆi

# = D(ai , ıˆi )R(ˆıi , α i )

(5.313)

Similarly, the central screw sˇ (di , θ i , kˆi−1 ) can also be expressed by a proper Plücker coordinate.  sˇ (di , θ i , kˆi−1 ) =

θ i kˆi−1 di kˆi−1

 = D(di , kˆi−1 )R(kˆi−1 , θ i )

(5.314)

Example 199  Intersecting two central screws. The condition of intersecting screw axes by Plücker coordinate is shown here. Two lines (and therefore two screws) are intersecting if their reciprocal product is zero. We can check that the reciprocal product of the screws sˇ (ai , α i , ıˆi ) and sˇ (di , θ i , kˆi−1 ) is zero.   " # ˆi−1 θ k α i ıˆi i ˆ sˇ (di , θ i , ki−1 ) × sˇ (ai , α i , ıˆi ) = ⊗ (5.315) ai ıˆi di kˆi−1 = θ i kˆi−1 · ai ıˆi + α i ıˆi · θ i kˆi−1 = 0

5.7

 Non-Denavit–Hartenberg Methods

The Denavit–Hartenberg (DH ) method of assigning relative coordinate frames of links of a robot is the most common method. However, the DH method is not the only applied method, nor necessarily the best. There are other methods with advantages and disadvantages when compared to the DH method. The Sheth method is an alternative method that can overcome the limitations of the DH method for higher order links, by introducing a number of frames equal to the number of joints on the link. It also provides more flexibility to specify the link geometry. In the Sheth method, we define a coordinate frame at every joint of a link. Hence, an n joint robot would have 2n frames. Figure 5.51 shows the case of a binary link (i) where a first frame (xi , yi , zi ) is attached at the origin of the link and a second frame (ui , vi , wi ) to the end of the link. The assignment of the origin joint and the end joint is arbitrary; however, it is easier if they are in the direction of base-to-tool frames. To describe the geometry, first we locate the joint axes by zi and wi and then determine the common perpendicular to both joint axes zi and wi . The common normal is indicated by a unit vector nˆ i . Specifying the link geometry requires six parameters that are determined as follows: 1. 2. 3. 4. 5. 6.

ai is the distance from zi to wi , measured along nˆ i . It is the kinematic distance between zi and wi . bi is the distance from nˆ i to ui , measured along wi . It is the elevation of the wi -axis. ci is the distance from xi to nˆ i , measured along zi . It is the elevation of the zi -axis. α i is the angle made by axes zi and wi , measured positively from zi to wi about nˆ i . β i is the angle made by axes nˆ i and ui , measured positively from nˆ i to ui about wi . γ i is the angle made by axes xi and nˆ i , measured positively from xi to nˆ i about zi .

294

5 Forward Kinematics

Fig. 5.51 Sheth method for defining the origin and end coordinate frames on a binary link

The Sheth parameters generate a homogenous transformation matrix to map the end coordinate frame Be to the origin coordinate frame Bo ,   o (5.316) Te = o Te ai , bi , ci , α i , β i , γ i ⎡ ⎤ ⎡ ⎤ xi ui ⎢ yi ⎥ ⎢ vi ⎥ o ⎢ ⎥ = Te = ⎢ ⎥ (5.317) ⎣ zi ⎦ ⎣ wi ⎦ 1 1 where o Te denotes the Sheth transformation matrix ⎡

r11 ⎢ r21 o Te = ⎢ ⎣ r31 0

r12 r22 r32 0

r13 r23 r33 0

⎤ r14 r24 ⎥ ⎥ r34 ⎦ 1

(5.318)

r11 = cos β i cos γ i − cos α i sin β i sin γ i

(5.319)

r21 = cos β i sin γ i + cos α i cos γ i sin β i

(5.320)

r31 = sin α i sin β i

(5.321)

r12 = − cos γ i sin β i − cos α i cos β i sin γ i

(5.322)

r22 = − sin β i sin γ i + cos α i cos β i cos γ i

(5.323)

r32 = cos β i sin α i

(5.324)

r13 = sin α i sin γ i

(5.325)

r23 = − cos γ i sin α i

(5.326)

r33 = cos α i

(5.327)

r14 = ai cos γ i + bi sin α i sin γ i

(5.328)

5.7  Non-Denavit–Hartenberg Methods

295

r24 = ai sin γ i − bi cos γ i sin α i

(5.329)

r34 = ci + bi cos α i

(5.330)

The Sheth transformation matrix for two coordinate frames at a joint is simplified to a translation for a prismatic joint and a rotation about the Z-axis for a revolute joint. Proof The homogenous transformation matrix to provide the coordinates in Bi , when the coordinates in Bj are given, is o

Te = Dzi ,ci Rzi ,γ i Dxi ,ai Rxi ,αi Dzi ,bi Rzi ,β i

(5.331)

Employing the associated transformation matrices ⎡

Rzi ,β i

cos β i − sin β i ⎢ sin β i cos β i =⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

⎡

Dzi ,bi

⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 bi ⎦ 000 1

(5.332)

⎤ 1 0 0 0 ⎢ 0 cos α i − sin α i 0 ⎥ ⎥ Rxi ,αi = ⎢ ⎣ 0 sin α i cos α i 0 ⎦ 0 0 0 1 ⎡ ⎤ 1 0 0 ai ⎢0 1 0 0 ⎥ ⎥ Dxi ,ai = ⎢ ⎣0 0 1 0 ⎦ 000 1 ⎡ ⎤ cos γ i − sin γ i 0 0 ⎢ sin γ i cos γ i 0 0 ⎥ ⎥ Rzi ,γ i = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ Dzi ,ci = ⎢ ⎣ 0 0 1 ci ⎦ 000 1

(5.333)

⎡

(5.334)

(5.335)

(5.336)

(5.337)

we can verify that the Sheth transformation matrix is ⎡

cβ i cγ i ⎢ −cα i sβ i sγ i ⎢ ⎢ ⎢ ⎢ cβ i sγ i o Te = ⎢ ⎢ +cα i cγ sβ i i ⎢ ⎢ ⎢ ⎣ sα i sβ i

0

⎤ ai cγ i +bi sα i sγ i ⎥ ⎥ ⎥ ⎥ ⎥ −sβ i sγ i ai sγ i ⎥ +cα i cβ i cγ i −cγ i sα i −bi cγ i sα i ⎥ ⎥ ⎥ ⎥ cβ i sα i cα i ci + bi cα i ⎦ 0 0 1 −cγ i sβ i −cα i cβ i sγ i sα i sγ i

(5.338)



296

5 Forward Kinematics

Fig. 5.52 Illustration of (a) a revolute joint and (b) a prismatic joint to define Sheth coordinate transformation

Example 200  Sheth transformation matrix at revolute and prismatic joints. Two links connected by a revolute joint are shown in Fig. 5.52a. The coordinate frames of the two links at the common joint are set such that the axes zi and wj coincide with the rotation axis, and both frames have the same origin. The Sheth parameters are a = 0, b = 0, c = 0, α = 0, β = 0, γ = θ, and therefore, the transformation matrix will be ⎡

cos θ − sin θ ⎢ sin θ cos θ i Tj = ⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 0⎦ 01

(5.339)

Two links connected by a prismatic joint are illustrated in Fig. 5.52b. The Sheth variable at this joint is d along the joint axis. The coordinate frames of the two links at the common joint are set such that the axes zi and wj coincide with the translational axis, and axes xi and uj are chosen parallel in the same direction. The Sheth parameters are a = 0, b = 0, c = d, α = 0, β = 0, γ = 0, and therefore, the transformation matrix will be ⎤ 1000 ⎢0 1 0 0⎥ i ⎥ Tj = ⎢ ⎣0 0 1 d ⎦ 0001 ⎡

(5.340)

Example 201  Sheth transformation matrix at a cylindrical joint. A cylindrical joint provides two DOF s, a rotational and a translational about the same axis. Two links connected by a cylindrical joint are shown in Fig. 5.53. The transformation matrix for a cylindrical joint can be described by combining a revolute and a prismatic joint. Therefore, the Sheth parameters are a = 0, b = 0, c = d, α = 0, β = 0, γ = θ. ⎡

cos θ − sin θ ⎢ sin θ cos θ i Tj = ⎢ ⎣ 0 0 0 0

⎤ 00 0 0⎥ ⎥ 1 d⎦ 01

(5.341)

Example 202  Sheth transformation matrix at a screw joint. A screw joint, as is shown in Fig. 5.54, provides us with a proportional rotation and translation motion, which has one DOF . The relationship between translation h and rotation θ is called pitch of screw and is defined by p=

h θ

(5.342)

5.7  Non-Denavit–Hartenberg Methods

297

Fig. 5.53 Illustration of a cylindrical joint to define Sheth coordinate transformation

Fig. 5.54 Illustration of a screw joint to define Sheth coordinate transformation

Fig. 5.55 Illustration of a gear joint to define Sheth coordinate transformation

The transformation for a screw joint may be expressed in terms of the relative rotation θ, ⎡

cos θ − sin θ ⎢ sin θ cos θ i Tj = ⎢ ⎣ 0 0 0 0 or displacement h.

⎡

⎤ 0 0 0 0 ⎥ ⎥ 1 pθ ⎦ 0 1

(5.343)

⎤ 00 0 0⎥ ⎥ 1 h⎦ 01

(5.344)

cos ph − sin ph ⎢ sin h cos h i p p Tj = ⎢ ⎣ 0 0 0 0

The coordinate frames are installed on the two connected links at the screw joint such that the axes wj and zi are aligned along the screw axis, and the axes uj and xi coincide at rest position. Example 203  Sheth transformation matrix at a gear joint. The Sheth method can also be utilized to describe the relative motion of two links connected by a gear joint. A gear joint, as is shown in Fig. 5.55, provides us with a proportional rotation, which has 1 DOF . The axes of rotations indicate the axes wj and zi , and their common perpendicular shown by the nˆ vector. Then, the Sheth parameters are defined as a = Ri + Rj ,

298

5 Forward Kinematics

Fig. 5.56 Hayati–Roberts (H R) notation to avoid the singularity in the DH method

b = 0, c = 0, α = 0, β = θ j , γ = θ i , to have ⎡

cos (1 + ε) θ i − sin (1 + ε) θ i ⎢ sin (1 + ε) θ i cos (1 + ε) θ i i Tj = ⎢ ⎣ 0 0 0 0 where ε=

⎤ 0 Rj (1 + ε) cos θ i 0 Rj (1 + ε) sin θ i ⎥ ⎥ ⎦ 1 0 0 1

Ri Rj

(5.345)

(5.346)

Example 204  Hayati–Roberts method and singularity of DH notation. In DH notation, the common normal is not well defined when the two joint axes are parallel. In this condition, the DH notation has a singularity, because a small change in the spatial positions of the parallel joint axes can cause a large change in the DH coordinate representation of their relative position. The Hayati–Roberts (H R) notation is another convention to represent subsequent links. H R avoids the coordinate singularity in the DH method for the case of parallel lines. In the H R method, the direction of the zi -axis is defined in the Bi−1 frame using roll and pitch angles α i and β i as are shown in Fig. 5.56. The origin of the Bi frame is chosen to lie in the (xi−1 , yi−1 )-plane where the distance di is measured between oi−1 and oi . Similar to DH convention, there is no unique H R convention concerning the freedom in choosing the angle of rotations. Furthermore, although the H R method can eliminate the parallel joint axes’ singularity, it has its own singularities when the zi -axis is parallel to either the xi−1 or yi−1 axes, or when zi intersects the origin of the Bi−1 frame. Example 205  Parametrically Continuous Convention method. There exists another method for coordinate transformation called parametrically continuous convention (P C). The P C method defines the zi -axis by two steps: 1. Direction of the zi -axis is defined by two direction cosines, α i and β i , with respect to the axes xi−1 and yi−1 . 2. Position of the zi -axis is defined by two distance parameters, li and mi , to indicate the xi−1 and yi−1 coordinates of the intersection of zi in the xi−1 yi−1 -plane from the origin and perpendicular to the zi -axis. The P C homogenous matrix to transform Bi−1 coordinates into Bi is ⎡ i−1

⎢ ⎢ Ti = ⎢ ⎣

α 2i 1+γ i αi β i − 1+γ i

1−

−α i 0

αi β i − 1+γ

α i li

⎤

i ⎥ β 2i 1 − 1+γ β i mi ⎥ ⎥ i −β i γ i 0 ⎦ 0 0 1

(5.347)

5.8 Summary

299

where γi =

 1 − α 2i − β 2i

(5.348)

The P C notation uses four parameters to indicate the position and orientation of the Bi−1 frame in the Bi frame. Since we need six parameters in general, there must be two conditions: 1. The origin of the Bi frame must lie on the common normal and be along the joint axis. 2. The xi -axis must lie along the common normal.

5.8

Summary

Forward kinematics is determination of the configuration of every link, specially the end-effector, coordinate frame in the base coordinate frame of a robot when the joint variables are given. 0

rP = 0 Tn n rP

(5.349)

For an n-link serial robot, it is equivalent to finding the transformation matrix 0 Tn as a function of joint variables qi . 0

Tn = 0 T1 (q1 ) 1 T2 (q2 ) 2 T3 (q3 ) 3 T4 (q4 ) · · ·

n−1

Tn (qn )

(5.350)

There is a special rule for installing the coordinate frames attached to each robot’s link called the standard Denavit–Hartenberg convention. Based on the DH rule, each transformation matrix i−1 Ti from the coordinate frame Bi to Bi−1 can be expressed by four parameters: link length ai , link twist α i , joint distance di , and joint angle θ i . ⎡

⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1

(5.351)

However, for most industrial robots, the link transformation matrix i−1 Ti can be classified into 12 simple types. 1 2 3 4 5 6 7 8 9 10 11 12

R  R(0) R  R(180) R ⊥ R(90) R ⊥ R(−90) R  R(90) R  R(−90) P  R(0) P  R(180) P ⊥ R(90) P ⊥ R(−90) P  R(90) P  R(−90)

or or or or or or or or or or or or

R  P(0) R  P(180) R ⊥ P(90) R ⊥ P(−90) R  P(90) R  P(−90) P  P(0) P  P(180) P ⊥ P(90) P ⊥ P(−90) P  P(90) P  P(−90)

Most industrial robots are made of a 3 DOF manipulator equipped with a 3 DOF spherical wrist. The transformation matrix 0 T7 can be decomposed into three submatrices 0 T3 , 3 T6 and 6 T7 . 0

T6 = 0 T3 3 T6 6 T7

(5.352)

The matrix 0 T3 positions the wrist point and depends only on the manipulator joints’ variables. The matrix 3 T6 is the wrist transformation and depends only on the manipulator wrist’s variables. The constant matrix 6 T7 is the tools transformation matrix. Decomposing 0 T7 into submatrices enables us to make the forward kinematics modular.

300

5.9

5 Forward Kinematics

Key Symbols

a a ai , α i , d i , θ i b B HR c d d D DH DOF F G, B0 h I = [I ] ıˆ, jˆ, kˆ l n n nˆ p r q ri rij R s s sˇ S SSRMS T uˆ u˜ W x, y, z X, Y, Z z and w

Kinematic link length, kinematic distance between z and w Turn vector of end-effector frame DH parameters of link (i) Elevation of w-axis Body coordinate frame Hayati–Roberts cos, elevation of z-axis Joint distance Translation vector, displacement vector Displacement transformation matrix Denavit–Hartenberg Degree of freedom Goal frame Global coordinate frame, base coordinate frame Translation of a screw Identity matrix Coordinate axes unit vectors Length The number of links of a robot, the number of joints of a robot Tilt vector of end-effector frame Common normal of joint axes in Sheth method Pitch of a screw, a body point, a fixed point in B Position vectors, homogenous position vector Joint variable The element i of r The element of row i and column j of a matrix Rotation transformation matrix sin Location vector of a screw, twist vector of end-effector frame Screw Station frame Space station remote manipulator system Homogenous transformation matrix, tool frame Unit vector on axis of rotation Skew symmetric matrix of the vector uˆ Wrist frames Local coordinate axes Global coordinate axes Joint axes of Sheth method

Greek α β γ θ ξ ρ φ

Link twist, roll angle of H R frame, angle from z to w about nˆ Pitch angle of H R frame, angle from nˆ i to ui about wi Angle from xi to nˆ i about zi Joint angle Moment vector of a Plücker line Moment vector of uˆ about origin Angle of rotation about u, ˆ rotation of a screw

5.9 Key Symbols

Symbol vers [ ]−1 [ ]T ≡  (i)  ⊥ ×

301

1 − cos Inverse of the matrix [ ] Transpose of the matrix [ ] Equivalent Orthogonal Link number i Parallel sign Perpendicular Vector cross product

302

5

Forward Kinematics

Fig. 5.57 A 4R planar manipulator

Exercises 1. Notation and symbols. Describe the meaning of a − θi

b − α i c- R  R(180)

d- P ⊥ R(−90) e- R  R(−90)

ˆ f −i−1 Ti g − ⊗ h − sˇ (ai , α i , ıˆi ) i − sˇ (h, φ, u) "

k − di

l − ai

α i ıˆi m− ai ıˆi

#

"

φ uˆ n− huˆ

#

j − sˇ (di , θ i , kˆi−1 ) 

θ i kˆi−1 o− di kˆi−1



2. A 4R planar manipulator kinematics. For the 4R planar manipulator, shown in Fig. 5.57, find the followings: (a) DH table. (b) Link-type table. (c) Individual frame transformation matrices i−1 Ti , i = 1, 2, 3, 4. (d) Global coordinates of the end-effector. (e) Orientation of the end-effector ϕ. 3. A one-link RR(−90) arm. Forward kinematics for different frame installments. A one-link RR(−90) manipulator is shown in Fig. 5.58a and b, with different frame installments. (a) Find the transformation matrices 0 T1 , 1 T2 , 0 T2 . (b) Compare the transformation matrix 1 T2 for both frame installations. 4. A 2R planar manipulator. Forward kinematics for different frame arrangements. Determine the link’s homogenous transformation matrices 1 T2 , 2 T3 , 1 T3 for the 2R planar manipulator shown in: (a) Figure 5.59. (b) Figure 5.60. (c) Determine the transformation matrices 3 T2 , 2 T1 , 3 T1 for the manipulator of Fig. 5.59 by moving from a frame to the other, not by matrix inversion. 5. DH coordinate frame setup. It is on engineers to set up coordinate frame the way suit them. (a) Set up the required link coordinate frames for the manipulators in Fig. 5.11a and b. (b) Set up the required link coordinate frames for the manipulators in Fig. 5.61a and b using l1 , l2 , l3 for the length of the links.

Exercises

303

Fig. 5.58 A one-link RR(−90) manipulator

Fig. 5.59 A 2R planar manipulator with DH coordinate frames

Fig. 5.60 A 2R planar manipulator with arbitrary coordinate frames

(c) Determine the forward kinematics transformation matrix of the manipulator in Fig. 5.61a and b and find their rest positions. (d) Determine the global coordinates of the tip point of the manipulator in Fig. 5.61a and b at the position shown. 6. Frame at center. A practice on link coordinate frames not installed at either end. Let us attach the link’s coordinate frame at the geometric center of the link, ai /2. Using the rigid motion and homogenous matrices, develop the transformation matrices 0 T1 , 1 T2 , 0 T2 for the manipulator of Fig. 5.62.

304

5

Forward Kinematics

Fig. 5.61 Two manipulators that are made by connecting industrial links

Fig. 5.62 A 2R planar manipulator with a coordinate frame at the geometric center of each link

Fig. 5.63 A 2 DOF polar manipulator

7. A polar manipulator. A planar two degrees of freedom robot working based on polar coordinate system. (a) Determine the link’s transformation matrices 1 T2 , 2 T3 , 1 T3 for the polar manipulator shown in Fig. 5.63. (b) Determine the transformation matrices 3 T2 , 2 T1 , 3 T1 for the manipulator of Fig. 5.63 by moving from a frame to the other, not by matrix inversion.

Exercises

305

Fig. 5.64 A 2 DOF Cartesian manipulator

Fig. 5.65 A planar manipulator with 4 DOF s

8. A planar Cartesian manipulator. The simplest and strongest planar robot is the Cartesian manipulator that is working based on Cartesian coordinate system. Determine the link’s transformation matrices 1 T2 , 2 T3 , 1 T3 for the planar Cartesian manipulator shown in Fig. 5.64. Hint: The coordinate frames are not based on DH rules. 9.  Manipulator designing. Industrial robots are designed modular by assembling the 12 standard links. Here is a practice. Use the industrial robot links to make a manipulator to have: (a) Three prismatic joints and reach every point in a three dimensional Cartesian space. (b) Two prismatic and one revolute joints and reach every point in a three dimensional Cartesian space. (c) One prismatic and two revolute joints and reach every point in a three dimensional Cartesian space. (d) Three revolute joints and reach every point in a three dimensional Cartesian space. 10.  Special manipulator design. In case special paths are supposed to be traced by the end-effector repeatedly, it is better to design the robot such that the path can be traced with the minimum number of actuators, ideally one. Use the industrial robot links to make a manipulator with three DOF s such that: (a) The tip point of the manipulator traces a circular path about a center point when two joints are lucked. (b) The tip point of the manipulator traces a circular path about the origin of global frame when two joints are lucked. (c) The tip point of the manipulator traces a straight path when two joints are lucked. (d) The tip point of the manipulator traces a straight path passing through the origin of global frame. 11. Coordinate frame assigning. Figure 5.65 depicts a planar manipulator with 4 DOF s. (a) Follow the DH rules and assign the link coordinate frames. (b) Determine the link-joint table for the manipulator. (c) Determine the DH transformation matrices. (d) Determine the coordinates of point P as functions of the joint coordinates. (e) Attach a tool coordinate frame at P and solve the forward kinematics to determine the orientation of the frame. (d) Determine the rest configuration and transformation matrix of the manipulator.

306

5

Forward Kinematics

Fig. 5.66 A 3 degree of freedom manipulator

Fig. 5.67 A design of an articulated manipulator

12. Modular articulated manipulators. How to build a 3D manipulator by attaching a two DOF planar robot to a rotating base link. Most industrial robots are modular. Some are manufactured by attaching a 2 DOF manipulator to a one-link RR(−90) arm. Articulated manipulators are made by attaching a 2R planar manipulator, such as the one shown in Fig. 5.59, to a one-link RR(−90) manipulator shown in Fig. 5.58a. Attach the 2R manipulator to the one-link RR(−90) arm and make an articulated manipulator. Make the required changes into the coordinate frames of Exercises 3 and 4 to find the link’s transformation matrices of the articulated manipulator. Examine the rest position of the manipulator. 13. Coordinate frame completing. A practice of coordinate frame setup. Figure 5.66 shows a 3 degree of freedom manipulator. (a) Follow the DH rules and complete the link coordinate frames. (b) Determine the link-joint table for the manipulator. (c) Determine the DH transformation matrices. (d) Determine the coordinates of point P as functions of the joint coordinates. (e) Attach a tool coordinate frame at P and solve the forward kinematics to determine the orientation of the frame. (f) Determine the rest configuration and transformation matrix of the manipulator. 14. Articulated manipulator. Modular robot with strange shape. Figure 5.67 illustrates an articulated manipulator. (a) Follow the DH rules and complete the link coordinate frames. (b) Determine the link-joint table for the manipulator. (c) Determine the DH transformation matrices. (d) Determine the coordinates of point P as functions of the joint coordinates. (e) Attach a tool coordinate frame at P and solve the forward kinematics to determine the orientation of the frame. (f) Determine the rest configuration and transformation matrix of the manipulator.

Exercises

307

Fig. 5.68 A set of non-industrial connected links

15. Modular spherical manipulators. How to build a 3D manipulator by attaching a two DOF polar robot to a rotating base link. Spherical manipulators are made by attaching a polar manipulator shown in Fig. 5.63 to a one-link RR(−90) manipulator shown in Fig. 5.58b. Attach the polar manipulator to the one-link RR(−90) arm to make a spherical manipulator. Make the required changes to the coordinate frames of figures in Exercises 3 and 7 to find the link’s transformation matrices of the spherical manipulator. Examine the rest position of the manipulator. 16.  Non-industrial links and DH parameters. In special design we may use non-standard industrial links. However, DH method works for all binary links with one joint at each end. This is an example of three general links joined to each other. Figure 5.68 illustrates a set of non-industrial connected links. Complete the DH coordinate frames and assign the DH parameters and transformation matrices. 17. Modular cylindrical manipulators. A three DOF manipulator that simulates cylindrical coordinate system is designed in this example. Cylindrical manipulators are made by attaching a 2 DOF Cartesian manipulator, shown in Fig. 5.64, to a one-link RR(−90) manipulator shown in Fig. 5.58a. Attach the 2 DOF Cartesian manipulator to the one-link RR(−90) arm and make a cylindrical manipulator. Make the required changes into the coordinate frames of Exercises 3 and 8 to find the link’s transformation matrices of the cylindrical manipulator. Examine the rest position of the manipulator. 18. Disassembled spherical wrist. Practice on defining coordinate frame on a spherical wrist as three links. A spherical wrist has three revolute joints in such a way that their joint axes intersect at a common wrist point. Every revolute joint of the wrist attaches two links. Disassembled links of a spherical wrist are shown in Fig. 5.69. Define the required DH coordinate frames to the links in (a), (b), and (c) consistently. Find the transformation matrices 3 T4 for (a), 4 T5 for (b), and 5 T6 for (c). 19.  Assembled spherical wrist. There are three links in a spherical wrist, as shown in previous exercise. Here, the three links are supposed to be kinematically assembled to make a wrist. Label the coordinate frames attached to the spherical wrist in Fig. 5.70 according to the frames that you installed in Exercise 18. Determine the transformation matrices 3 T6 and 3 T7 for the wrist. 20.  A 5 DOF robot. Attachment of a 3 DOF spherical wrist to a 2 DOF manipulator. Figure 5.71 illustrates a five DOF robot having a spherical wrist. (a) Follow the DH rules and complete the link coordinate frames such that the hand of the robot at the rest position stays straight with the forearm. (b) Determine the link-joint table for the manipulator. (c) Determine the DH transformation matrices. (d) Determine the forward kinematics final transformation matrix. (e) Determine the rest configuration and transformation matrix of the manipulator.

308

5

Forward Kinematics

Fig. 5.69 Disassembled links of a spherical wrist

Fig. 5.70 Assembled spherical wrist

21. Articulated robots. Assembling a wrist to an articulated manipulator to make a robot. Attach the spherical wrist of Exercise 19 to the articulated manipulator of Exercise 12 and make a 6 DOF articulated robot. Change your DH coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot. 22. Spherical robots. Assembling a wrist to a spherical manipulator to make a robot. Attach the spherical wrist of Exercise 19 to the spherical manipulator of Exercise 15 and make a 6 DOF spherical robot. Change your DH coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot. 23. Cylindrical robots. Assembling a wrist to a cylindrical manipulator to make a robot. Attach the spherical wrist of Exercise 19 to the cylindrical manipulator of Exercise 17 and make a 6 DOF cylindrical robot. Change your DH coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot. 24. An RP manipulator. Kinematic analysis of the end part of a SCARA manipulator. Figure 5.72 depicts a 2 DOF RP manipulator. The end-effector of the manipulator can slide on a line and rotate about the same line. Label the installed coordinate frames on the links of the manipulator and determine the transformation matrix of the end-effector to the base link.

Exercises

309

Fig. 5.71 A five DOF robot having a spherical wrist

Fig. 5.72 A 2 DOF RP manipulator

Fig. 5.73 A 2R manipulator, acting in a horizontal plane

25. Horizontal 2R manipulator. Kinematic analysis of the beginning part of a SCARA manipulator. Figure 5.73 illustrates a 2R planar manipulator that acts in a horizontal plane. Label the coordinate frames and determine the transformation matrix of the end-effector in the base frame.

310

5

Forward Kinematics

Fig. 5.74 The space shuttle remote manipulator system (SSRMS) with a camera attached to the link (3)

26. SCARA manipulator. Attachment of the two parts of a SCARA manipulator studied in the previous exercises. A SCARA robot can be made by attaching a 2 DOF RP manipulator to a 2R planar manipulator. Attach the 2 DOF RP manipulator of Exercise 24 to the 2R horizontal manipulator of Exercise 25 and make a SCARA manipulator. Solve the forward kinematics problem for the manipulator. 27.  Roll–Pitch–Yaw spherical wrist kinematics. Attach the required DH coordinate frames to the Roll–Pitch–Yaw spherical wrist of Fig. 5.34, similar to 5.32, and determine the forward kinematics of the wrist. 28.  Pitch–Yaw–Roll spherical wrist kinematics. Attach the required coordinate DH frames to the Pitch–Yaw–Roll spherical wrist of Fig. 5.35, similar to 5.32, and determine the forward kinematics of the wrist. 29.  Assembling a Roll–Pitch–Yaw wrist to a spherical arm. Assemble the Roll–Pitch–Yaw spherical wrist of Fig. 5.34 to the spherical manipulator of Fig. 5.47 and determine the forward kinematics of the robot. 30.  Assembling a Pitch–Yaw–Roll wrist to a spherical arm. Assemble the Pitch–Yaw–Roll spherical wrist of Fig. 5.50 to the spherical manipulator of Fig. 5.47 and determine the forward kinematics of the robot. 31.  SCARA robot with a spherical wrist. Attach the spherical wrist of Exercise 19 to the SCARA manipulator of Exercise 26 and make a 7 DOF robot. Change your DH coordinate frames in the exercises accordingly and solve the forward kinematics problem of the robot. 32.  Modular articulated manipulators by screws. Solve Exercise 12 by screws. 33.  Modular spherical manipulators by screws. Solve Exercise 15 by screws. 34.  Modular cylindrical manipulators by screws. Solve Exercise 17 by screws. 35.  Spherical wrist kinematics by screws. Solve Exercise 19 by screws. 36.  Modular SCARA manipulator by screws. Solve Exercises 24, 25, and 26 by screws. 37.  Space station remote manipulator system. Attach a spherical wrist to the SSRMS and make a 10 DOF robot. Solve the forward kinematics of the robot by matrix and screw methods.

Exercises

311

Fig. 5.75 A planar 2R robot with arbitrary coordinate frame setup

38.  Camera on a space station remote manipulator. Assume that we attach an inspection camera to link (3) of the space station remote manipulator system, as shown in Fig. 5.74. Determine the matrix 8 T7 such that x8 points the origin of the gripper frame B7 and z8 be in (x3 , x4 )-plane and perpendicular to x4 . Then, determine the matrices 0 T8 , 4 T8 , 3 T8 , 6 T8 , 1 T8 . 39. Forward kinematics of non-DH frames. Figure 5.75 illustrates a planar 2R robot with arbitrary coordinate frame setup. Determine its forward kinematics. 40. Project: Joint angles of a 2R robot for a given path. Consider the 2R robot of Fig. 5.75 with the following dimensions. l1 = 1 m

l2 = 1 m

(5.353)

The tip point P is moving from (X1 , Y1 ) to (X2 , Y2 ) on a straight line. (X1 , Y1 ) = (1, 1.5)

(X2 , Y2 ) = (−1, 1.5)

(5.354)

(a) Assume an elbow up configuration. Determine the location of joint A when P is at (X1 , Y1 ) by intersecting a circle with radius l1 and center M, and another circle with radius l2 and center P . Then determine the location of joint A when P is at (X2 , Y2 ) by intersecting a circle with radius l1 and center M and another circle with radius l2 and center P . Calculate θ 1 at the beginning and end of the motion. Divide the sweep angle of θ 1 into 10 equal angles. Position A at the 11 angular positions of θ 1 . Calculate θ 2 to locate P at a point on the path of motion by trial and error and apply forward kinematics collations. (b) Assume an elbow down configuration. Determine the location of joint A when P is at (X1 , Y1 ) by intersecting a circle with radius l1 and center M, and another circle with radius l2 and center P . Then determine the location of joint A when P is at (X2 , Y2 ) by intersecting a circle with radius l1 and center M, and another circle with radius l2 and center P . Calculate θ 1 at the beginning and end of the motion. Divide the sweep angle of θ 1 into 10 equal angles. Position A at the 11 angular positions of θ 1 . Calculate θ 2 to locate P at a point on the path of motion by trial and error and apply forward kinematics collations. (c) Which configuration, elbow up or elbows down, will provide minmax deviation of θ 2 from zero.

6

Inverse Kinematics

What are the joint variables for a given configuration of a robot? This is the problem to be answered by inverse kinematic analysis. In this chapter, you will see that determination of the joint variables reduces to solving a set of nonlinear coupled algebraic equations. Although there is no standard and generally applicable method to solve the inverse kinematic problem, there are a few analytic and numerical methods to solve the problem. The main difficulty of inverse kinematic is the multiple solutions such as the one that is shown in Fig. 6.1 for a planar 2R manipulator.

Fig. 6.1 Multiple solution for inverse kinematic problem of a planar 2R manipulator

6.1

Decoupling Technique

Determination of joint variables in terms of the end-effector position and orientation is called inverse kinematics. Mathematically, inverse kinematics is searching for the elements of joint variable vector q,  T q = q1 q2 q3 · · · qn

(6.1)

when a transformation 0 Tn is given as a function of the joint variables q1 , q2 , q3 , · · · , qn . 0

Tn = 0 T1 (q1 ) 1 T2 (q2 ) 2 T3 (q3 ) 3 T4 (q4 ) · · ·

n−1

Tn (qn )

(6.2)

Computer-controlled robots are usually actuated in the joint variable space; however, objects to be manipulated are usually expressed in the global Cartesian coordinate frame. Therefore, carrying kinematic information, back and forth, between joint space and Cartesian space, is a need in robotics. To control the configuration of the end-effector to reach an object, the inverse kinematics problem must be solved. Hence, we need to know what the required values of joint variables are to reach a desired point in a desired orientation. © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_6

313

314

6 Inverse Kinematics

Consider a robot with n prismatic or revolute joints. Such a robot has n degrees-of-freedom (DOF ). To have a robot capable of reaching a particular point at a particular orientation, the robot needs six DOF , three to position the point and three to adjust the orientation of the end-effector. The result of forward kinematics of such a six DOF multibody is a 4 × 4 transformation matrix. 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ r11 r12 "0 0 # ⎢ r21 r22 R6 d 6 = =⎢ ⎣ r31 r32 0 1 0 0

r13 r23 r33 0

⎤ r14 r24 ⎥ ⎥ r34 ⎦ 1

(6.3)

The 12 elements of 0 T6 are trigonometric functions of 6 unknown joint variables. However, because the upper left 3 × 3 submatrix of (6.3) is a rotation matrix, only 3 elements of them are independent. This is because of the orthogonality condition (2.285). Hence, only six equations out of the 12 equations of (6.3) are independent. Theoretically, we should be able to use the 6 independent equations and determine the 6 joint variables. The inverse kinematic analysis leads to a set of trigonometric equations. Trigonometric functions inherently provide multiple solutions. Therefore, multiple configurations of the robot are expected when the 6 equations are solved for the unknown joint variables. It is possible to decouple the inverse kinematics problem into two subproblems, known as inverse position and inverse orientation kinematics. The practical consequence of such decoupling is to break the problem into two independent problems, each with only three unknown variables. Following the decoupling principle, the overall transformation matrix of a robot can be decomposed to a translation and a rotation. 0

T6 = 0 D6 0 R6 "0 0 # " 0 #"0 # R6 d 6 I d6 R6 0 = = 0 1 0 1 0 1

(6.4)

The translation matrix 0 D6 indicates the position of the end-effector in the base frame B0 and involves only the three joint variables of the manipulator. We will solve 0 d6 for the variables that control the wrist position. The rotation matrix 0 R6 indicates the orientation of the end-effector in B0 and involves only the three joint variables of the wrist. We will solve 0 R6 for the variables that control the wrist orientation. Proof Most robots have a wrist made of three revolute joints with three intersecting orthogonal axes at the wrist point. Taking advantage of having such wrists, we may decouple the kinematics of the wrist and manipulator by decomposing the overall forward kinematics transformation matrix 0 T6 into the wrist orientation and wrist position: 0

T6 = T3 T6 = 0

3

"0

R3 0 d 3 0 1

#"3

R6 0 0 1

# (6.5)

where the wrist orientation matrix is: ⎡

⎤ r11 r12 r13 3 R6 = 0 R3T 0 R6 = 0 R3T ⎣ r21 r22 r23 ⎦ r31 r32 r33 and the wrist position vector is:

⎤ r14 0 d6 = ⎣ r24 ⎦ r34

(6.6)

⎡

(6.7)

The wrist position vector 0 d6 ≡ 0 d3 includes the manipulator joint variables only. Hence, to solve the inverse kinematics of such a robot, we must solve 0 d3 for position of the wrist point and then solve 3 R6 for orientation of the wrist. The components of the wrist position vector 0 d6 = 0 dwrist provide three equations for the three unknown manipulator joint variables. Solving

6.1 Decoupling Technique

315

Fig. 6.2 Illustration of a 2R planar manipulator in two possible configurations: (a) elbow up and (b) elbow down

0

d6 for the manipulator joint variables and substituting them in 3 R6 makes 3 R6 of (6.6) to be only a function of wrist joint variables. Then, the wrist orientation matrix 3 R6 will be solved for the wrist joint variables. In case we include the tool coordinate frame in forward kinematics, the decomposition must be done according to the following equation to exclude the effect of tool distance d7 from the robot’s kinematics. 0

T7 = 0 T3 3 T7 = 0 T3 3 T6 6 T7 ⎡ #"3 "0 # R3 d w R6 0 ⎢ ⎢I = 0 1 0 1 ⎣ 0

⎤ 0 0 ⎥ ⎥ d7 ⎦ 1

(6.8)

In this case, inverse kinematics starts from determination of 0 T6 , which can be found as: 0

T6 = 0 T7 6 T7−1 ⎡ ⎤−1 ⎡ ⎤ 100 0 100 0 ⎢0 1 0 0 ⎥ ⎢0 1 0 0 ⎥ 0 ⎥ ⎥ ⎢ = 0 T7 ⎢ ⎣ 0 0 1 d7 ⎦ = T7 ⎣ 0 0 1 −d7 ⎦ 000 1 000 1

(6.9)

 Example 207 Inverse kinematics for 2R planar manipulator. This is the simplest multi DOF robotic system and this example will show the inverse kinematic problems to have multiple solutions. Although, all solutions are possible, only one of the solutions will be the practical one. It is also the first time students are introduced to the trigonometric equations and the function atan2(y, x). Figure 6.2 illustrates a 2R planar manipulator with two RR links. Employing the transformation matrix of two links RR(0), we find the forward kinematics of the manipulator as: 0

T2 = 0 T1 1 T2 ⎡ ⎤ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l1 sθ 1 + l2 s (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(6.10)

316

6 Inverse Kinematics

The global position of the tip point of the manipulator is at: "

X Y

#

"

l cos θ 1 + l2 cos (θ 1 + θ 2 ) = 1 l1 sin θ 1 + l2 sin (θ 1 + θ 2 )

# (6.11)

We must use these two equations to determine the required angles θ 1 , θ 2 for a given value of X and Y . To find θ 2 , we use X2 + Y 2 = l12 + l22 + 2l1 l2 cos θ 2

(6.12)

and cos θ 2 =

X2 + Y 2 − l12 − l22 2l1 l2

θ 2 = cos−1

X2 + Y 2 − l12 − l22 2l1 l2

(6.13) (6.14)

The first joint variable θ 1 of an elbow up configuration can geometrically be found from: θ 1 = arctan

l2 sin θ 2 Y + arctan X l1 + l2 cos θ 2

(6.15)

Y l2 sin θ 2 − arctan X l1 + l2 cos θ 2

(6.16)

and for an elbow down configuration from: θ 1 = arctan

However, we usually avoid using functions arcsin and arccos because of their inaccuracy. It is more exact if we are able to calculate angles by function arctan. Let us employ the half angle formula, tan2 to find θ 2 using an atan2 function.

θ 1 − cos θ = 2 1 + cos θ *

θ 2 = ±2 atan2

(6.17)

  (l1 + l2 ) 2 − X2 + Y 2   X2 + Y 2 − (l1 − l2 )2

(6.18)

The meaning of the function atan2(y, x) is explained in Example 208. At the moment assume: atan2(y, x) ≡ arctan

y x

(6.19)

The ± in Eq. (6.18) is because of the square root, which generates two solutions for θ 2 . These two solutions are called elbow up and elbow down configurations, as shown in Fig. 6.2a,b respectively. The angle θ 1 can also be found from the following alternative equation. θ 1 = atan2

−Xl2 sin θ 2 + Y (l1 + l2 cos θ 2 ) Y l2 sin θ 2 + X (l1 + l2 cos θ 2 )

(6.20)

Most of the time, the value of θ 1 should be corrected by adding or subtracting π depending on the sign of X. The two different sets of solutions for θ 1 and θ 2 correspond to the elbow up and elbow down configurations. It is also possible to combine Equations of (6.11) and determine a trigonometric equation for θ 1 . 2Xl1 cos θ 1 + 2Y l1 sin θ 1 = X2 + Y 2 + l12 − l22

(6.21)

6.1 Decoupling Technique

317

Fig. 6.3 A 2R planar manipulator with l1 = 1 m, l1 = 1 m moving from P1 to P2 on a straight line

Fig. 6.4 The variation of the angles θ 1 and θ 2 of the 2R planar manipulator

The trigonometric equation appears in inverse kinematic problems frequently. The method of trigonometric equations will be explained in Example 209. As an example, let us consider the motion of a 2R planar manipulator with l1 = 1 m, l1 = 1 m that its tip point is moving from P1 (1.2, 1.5) to P2 (−1.2, 1.5) on a straight line. Employing the inverse kinematic equations (6.14) and (6.15), we determine the configuration of the manipulator at any point of the path. Figure 6.3 illustrates the manipulator at n = 22 equally spaced points between P1 and P2 . The variation of the angles θ 1 and θ 2 is as shown in Fig. 6.4. Example 208  Meaning of the function atan2(y, x) ≡ arctan yx . Inverse tangent is the most exact way to determine the value of an angle. It provides more exact solution than other inverse trigonometric functions. The argument of inverse tangent y = arctan −x . To overcome this problem, usually appears as a fraction y/x. The problem of inverse tangent is that arctan −y x a modified inverse tangent function will be introduced in this example. In robotic kinematics calculation, especially in solving inverse kinematic problems, we need to find angles based on the sin and cos functions of the angles. However, arctan cannot show the effect of the individual sign for the numerator and denominator. It always represents an angle in the first or fourth quadrant. To overcome this problem and determine the joint angles in the correct quadrant, the atan2 function is introduced as:

y ⎧

⎪ sgn y arctan

⎪ ⎪ x ⎪ ⎪ π ⎨ sgn y atan2(y, x) = 2

y  ⎪

⎪ sgn y π − arctan ⎪

⎪ ⎪ x ⎩ π − π sgn x

if x > 0, y = 0 if x = 0, y = 0 if x < 0, y = 0 if x = 0, y = 0

(6.22)

318

6 Inverse Kinematics

The sgn represents the signum function.

⎧ ⎨ 1 if x > 0 sgn(x) = 0 if x = 0 ⎩ −1 if x < 0

(6.23)

As an example, let us compare the arctan and atan2 for four points in four quadrants. x x x x

= 1, y = 1 = −1, y = 1 = −1, y = −1 = 1, y = −1

then then then then

arctan 11 = 0.785 1 arctan −1 = −0.785 −1 arctan −1 = 0.785 arctan −1 = −0.785 1

atan2(1, 1) = 0.785 atan2(1, −1) = 2.356 atan2(−1, −1) = −2.356 atan2(−1, 1) = −0.785

In robotic kinematics calculations, wherever arctan yx is needed, it is recommended to be calculated based on atan2(y, x). Example 209  Solution of first type of trigonometric equation a cos θ + b sin θ = c. Trigonometric equations frequently appear in kinematic analysis of robots. The method of solution of the first type of trigonometric equation will be introduced in this example. The first type of trigonometric equation for the unknown angle θ is a linear combination of cos θ and sin θ. a cos θ + b sin θ = c

(6.24)

This equation can be solved by introducing two new variables r and φ such that: a = r sin φ

r = a 2 + b2

b = r cos φ

(6.25)

φ = atan2(a, b)

(6.26)

We may verify these identities by expanding and substituting the new variables. sin(φ + θ) =

c r

(6.27) )

cos(φ + θ) = ± 1 −

c2 r2

(6.28)

Hence, the unknown angle θ of the trigonometric equation (6.24) is: ) c2 c θ = atan2( , ± 1 − 2 ) − atan2(a, b) r r

2 = atan2(c, ± r − c2 ) − atan2(a, b) c a ≡ arctan √ − arctan b ± r 2 − c2

(6.29) (6.30) (6.31)

To solve the first type of trigonometric equation (6.24), we may employ new variables (6.25) to rewrite the equation in a new form, c = r sin φ cos θ + r cos φ sin θ = r sin(φ + θ) (6.32) and determine θ as: θ = arcsin |c| < |r|

c a − arctan r b

(6.33) (6.34)

Solutions (6.29) and (6.33) are convertible. Therefore, Eq. (6.24) has two solutions if c2 < r 2 , one solution if r 2 = c2 , and no solution if r 2 < c2 .

6.1 Decoupling Technique

319

As an example, let us solve the following equation. 1.5 cos θ + 2.5 sin θ = 2.549

(6.35)

a 2 + b2 = 2.915475947

(6.36)

Having a = 1.5 and b = 2.5, we find r and φ. r=

φ = atan2(a, b) = 0.5404195 rad

(6.37)

Therefore,

θ = atan2(c, ± r 2 − c2 ) − atan2(a, b) √ = atan2(2.549, ± 2) − φ = 0.5235718477 rad, 1.537181805 rad ≈ 30 deg, 88.07 deg

(6.38)

Example 210  Second method to solve the first type of trigonometric equation a cos θ + b sin θ = c. Converging a trigonometric equation using tangent function allows us to solve the equation in an alternative method, as exact and as applied as the first method of Example 209. An alternative method to solve the first type of trigonometric equation for the unknown angle θ a cos θ + b sin θ = c

(6.39)

is to convert the equation to tan θ/2. c − a + (a + c) tan2 12 θ − 2b tan 12 θ tan2 12 θ + 1

=0

(6.40)

If θ = π /2, then we have a quadratic equation for tan2 12 θ. 1 1 (a + c) tan2 θ − 2b tan θ + c − a = 0 2 2 √ b ± a 2 + b2 − c2 θ = 2 arctan a+c

(6.41) (6.42)

Example 211 An articulated manipulator. Articulated manipulator is one of the most important and practical industrial robots. This example will be a good reference for detailed analytic inverse kinematics of this manipulator. Consider an articulated manipulator as is shown in Fig. 6.5. The links of the manipulator are RR(90), RR(0), RR(90), and their associated transformation matrices between coordinate frames are: ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0 ⎡

0 sin θ 1 0 − cos θ 1 1 0 0 0

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 0⎥ ⎥ l1 ⎦ 1

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(6.43)

(6.44)

320

6 Inverse Kinematics

Fig. 6.5 An RRR articulated manipulator

⎡

cos θ 3 ⎢ sin θ 3 2 T3 = ⎢ ⎣ 0 0

0 sin θ 3 0 − cos θ 3 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(6.45)

The forward kinematics of the manipulator is: 0

T3 = 0 T1 1 T2 2 T3 ⎡ ⎤ cθ 1 c (θ 2 + θ 3 ) sθ 1 cθ 1 s (θ 2 + θ 3 ) l2 cθ 1 cθ 2 ⎢ sθ 1 c (θ 2 + θ 3 ) −cθ 1 sθ 1 s (θ 2 + θ 3 ) l2 cθ 2 sθ 1 ⎥ ⎥ =⎢ ⎣ s (θ 2 + θ 3 ) 0 −c (θ 2 + θ 3 ) l1 + l2 sθ 2 ⎦ 0 0 0 1

(6.46)

and therefore, the tip point P is at: ⎤ ⎡ ⎤ X 0 ⎢0⎥ ⎢Y ⎥ 0 0 ⎥ ⎢ ⎥ dP = ⎢ ⎣ Z ⎦ = T3 ⎣ l 3 ⎦ 1 1 ⎤ ⎡ l3 sin (θ 2 + θ 3 ) cos θ 1 + l2 cos θ 1 cos θ 2 ⎢ l3 sin (θ 2 + θ 3 ) sin θ 1 + l2 sin θ 1 cos θ 2 ⎥ ⎥ =⎢ ⎣ ⎦ l1 − l3 cos (θ 2 + θ 3 ) + l2 sin θ 2 1 ⎡

(6.47)

Point P is supposed to be the point at which we attach a spherical wrist. Therefore, 0 dP is the decoupled position vector of the wrist point that will not be affected by the wrist attachment. The vector 0 dP provides three equations for the three joint variables θ 1 , θ 2 , θ 3 of the manipulator. In a practical problem, the left-hand side of Eq. (6.47) is a known vector and hence, X, Y , Z are given numbers. The right-hand sides are three equations with all parameters given and including only three unknowns θ 1 , θ 2 , θ 3 . The first angle can be found from X sin θ 1 − Y cos θ 1 = 0

(6.48)

θ 1 = atan2 (Y, X)

(6.49)

that is:

6.1 Decoupling Technique

321

We may combine the first and second elements of 0 dP to find: X cos θ 1 + Y sin θ 1 = l3 sin (θ 2 + θ 3 ) + l2 cos θ 2

(6.50)

X cos θ 1 + Y sin θ 1 − l2 cos θ 2 = l3 sin (θ 2 + θ 3 )

(6.51)

Now, let us rewrite this equation as:

and rewrite the third component of (6.47) as: Z − l1 − l2 sin θ 2 = l3 cos (θ 2 + θ 3 )

(6.52)

Then, combining Eqs. (6.51) and (6.52) provides: (Z − l1 − l2 sin θ 2 )2 + (X cos θ 1 + Y sin θ 1 − l2 cos θ 2 )2 = l32

(6.53)

or − 2l2 (X cos θ 1 + Y sin θ 1 ) cos θ 2 + 2l2 (l1 − Z) sin θ 2 = l32   − (l1 − Z)2 − l22 − Y 2 − X2 − Y 2 cos2 θ 1 − XY sin 2θ 1

(6.54)

That is a trigonometric equation of the form (6.24). a cos θ 2 + b sin θ 2 = c

(6.55)

a = −2l2 (X cos θ 1 + Y sin θ 1 ) b = 2l2 (l1 − Z) c=

l32 − (l1 −Z)2

−l22 −Y 2 −



(6.56)

X −Y 2

2



(6.57) cos θ 1 −XY sin 2θ 1 2

(6.58) We solve this equation for θ 2 considering θ 1 has been calculated in (6.49). 



θ 2 = arcsin √

c a2

+

b2

− arctan

a b

(6.59)

Dividing (6.50) by the third element of 0 dP determines θ 3 . tan (θ 2 + θ 3 ) =  θ 3 = atan2

X cos θ 1 + Y sin θ 1 − l2 cos θ 2 l1 + l2 sin θ 2 − Z

X cos θ 1 + Y sin θ 1 − l2 cos θ 2 l1 + l2 sin θ 2 − Z

(6.60)

− θ2

(6.61)

Therefore, Eqs. (6.49), (6.59), and (6.61) are the solutions of the inverse kinematics of the articulated manipulator. There is no standard method to solve the three equations (6.47) for the three unknown angles θ 1 , θ 2 , θ 3 , and this is a weakness of inverse kinematics. However, when a set of solution is set, they will be applicable to all similar manipulators. The method described in this example is not the only method. The readers may manipulate the equations to come up with other ways to calculate the unknowns.

322

6 Inverse Kinematics

Example 212 A set of equations for inverse kinematic equations of articulated manipulator. This example introduces another set of equations for inverse kinematics of articulated manipulators, suitable for computerization. Although the final equations of Example 211 are convertible to the equations introduced here. Figure 6.5 illustrates an articulated manipulator. The forward kinematics of the manipulator ends up with three equations with three unknown joint variables θ 1 , θ 2 , θ 3 , indicating the position of the tip point of the robot in the base Cartesian coordinate frame. ⎤ ⎡ ⎤ ⎡ X l3 sin (θ 2 + θ 3 ) cos θ 1 + l2 cos θ 1 cos θ 2 0 (6.62) dP = ⎣ l3 sin (θ 2 + θ 3 ) sin θ 1 + l2 sin θ 1 cos θ 2 ⎦ = ⎣ Y ⎦ Z l1 − l3 cos (θ 2 + θ 3 ) + l2 sin θ 2 In forward kinematics, the joint variables θ 1 , θ 2 , θ 3 are given and the coordinates of the tip point X, Y , Z are requested. Forward kinematics is usually a simple and straightforward problem that will be solved by substitution numerical values. In inverse kinematics, the coordinates of the tip point X, Y , Z are given and the joint variables θ 1 , θ 2 , θ 3 are requested. Hence, in the following analysis we assume X, Y , Z are given numerical values. Interestingly, we have a good relationship between X and Y as Y/X = tan θ 1 . Therefore, calculating θ 1 is reduced to an inverse tangent calculation. ⎧ Y ⎪ ⎨ arctan X≥0 X θ1 = (6.63) Y ⎪ ⎩ arctan + π X < 0 X To combine the two possible solutions in a single equation, we may employ the H eaviside function H and rewrite Eqs. (6.63) in a new form. Y θ 1 = arctan + π H (−X) (6.64) X H eaviside is an on-off switching function. It was introduced by the English mathematician Oliver Heaviside (1850–1925) as:  1 x > x0 (6.65) H (x − x0 ) = 0 x < x0 A combination of X, Y , Z eliminates θ 3 and provides us with the following identity. (Z − l1 − l2 sin θ 2 )2 + (X cos θ 1 + Y sin θ 1 − l2 cos θ 2 )2 = l32

(6.66)

Considering θ 1 is a known value from (6.63), Eq. (6.66) will be a first kind of trigonometric equation for θ 2 . a cos θ 2 + b sin θ 2 = c Its solution from Example 210 is: θ 2 = 2 arctan

b±

√

(6.67)

b2 + a 2 − c2 a+c

(6.68)

where a = −2l2 (X cos θ 1 + Y sin θ 1 ) b = 2l2 (l1 − Z)

(6.69) 

c = l32 − (l1 − Z)2 − l22 − Y 2 − X2 − Y

 2

(6.70) cos2 θ 1

−XY sin 2θ 1

(6.71)

Introducing new parameters, C1 = − (a + c)

C2 = −b

we have: θ 2 = 2 arctan

−C2 ±

C3 = a − c

 C22 − C1 C3 C1

(6.72)

(6.73)

6.1 Decoupling Technique

323

where C1 = l12 − 2l1 Z + l22 +

2l2 X X2 − l32 + + Z2 cos θ 1 cos2 θ 1

C2 = 2l1 l2 − 2l2 Z

(6.74) (6.75)

C3 = l12 − 2l1 Z + l22 −

2

2l2 X X − l32 + + Z2 cos θ 1 cos2 θ 1

(6.76)

Finally the third component of 0 dP is enough to find θ 3 .  θ 3 = arccos

l1 − Z + l2 sin θ 2 l3

− θ2

(6.77)

Therefore, Eqs. (6.63), (6.73), and (6.77) are the solutions of the inverse kinematics of the articulated manipulator. Example 213 Numerical case of an articulated manipulator. Analytic solutions must eventually provide numerical solutions to be practical. Here we use the analytic solutions of inverse kinematics of articulated manipulator to show how we recognize the correct solution out of multiple solutions. To check the inverse kinematic equations of Example 211, let us examine an articulated manipulator with the following dimensions l1 = 1 m l2 = 1.05 m l3 = 0.89 m (6.78) when its tip point is at: 0

 T  T dP = 1 1.1 1.2 ≡ X Y Z

(6.79)

Equation (6.49) provides θ 1 . 1.1 1 = 0.83298 rad ≈ 47.726 deg

θ 1 = atan2 (Y, X) = arctan

To determine θ 2 , we should solve Eq. (6.55)

(6.80)

a cos θ 2 + b sin θ 2 = c

(6.81)

where a = −2l2 (X cos θ 1 + Y sin θ 1 ) = −3.12187

(6.82)

b = 2l2 (l1 − Z) = −0.42   c = l32 − (X cos θ 1 + Y sin θ 1 ) 2 + l12 − 2l1 Z + l22 + Z 2

(6.83)

= −2.56

(6.84)

We find two values for θ 2 θ 2 = 0.7555 rad ≈ 43.289 deg

(6.85)

θ 2 = −0.488 rad ≈ −27.9648 deg

(6.86)

θ 3 comes from (6.61). If θ 2 = 0.7555 rad we have: 

X cos θ 1 + Y sin θ 1 − l2 cos θ 2 θ 3 = atan2 l1 + l2 sin θ 2 − Z = 0.19132 rad ≈ 10.9618 deg

− θ2 (6.87)

324

6 Inverse Kinematics

and if θ 2 = −0.488 rad, then we have:

θ 3 = −0.1913 rad ≈ −10.9618 deg

(6.88)

There are two sets of solutions that only one of them will match with the current position of the manipulator. θ 1 = 0.83298 θ 2 = 0.7555 θ 3 = 0.1913

(6.89)

θ 1 = 0.83298 θ 2 = −0.488 θ 3 = −0.1913

(6.90)

To determine the correct set, we need to use them in direct kinematic analysis and calculate the position of the end-effector of the manipulator. ⎡ ⎤ ⎡ ⎤ l3 sin (θ 2 + θ 3 ) cos θ 1 + l2 cos θ 1 cos θ 2 X 0 dP = ⎣ l3 sin (θ 2 + θ 3 ) sin θ 1 + l2 sin θ 1 cos θ 2 ⎦ ≡ ⎣ Y ⎦ (6.91) Z l1 − l3 cos (θ 2 + θ 3 ) + l2 sin θ 2 Substituting the first set (6.89) provides us with the correct position of the end-effector. ⎡

⎤ 1.000 0 dP = ⎣ 1. 100 ⎦ 1. 1999

(6.92)

The second set (6.90) provides us with the incorrect position of the end-effector. ⎡

⎤ 0.24774 0 dP = ⎣ 0.27252 ⎦ −0.18473

(6.93)

Therefore, the correct solutions of the inverse kinematic for the manipulator are (6.89). Let us also use the alternative Eqs. (6.63), (6.73), and (6.77) to solve this example to compare. Equation (6.63) gives θ 1 . θ 1 = arctan

Y 1.1 = arctan = 0.83298 rad X 1

(6.94)

2l2 X X2 − l32 + +Z 2 = 5.6822 cos θ 1 cos2 θ 1

(6.95)

Equation (6.73) provides θ 2 . C1 = l12 − 2l1 Z + l22 +

C2 = 2l1 l2 − 2l2 Z = −0.42 C3 = l12 − 2l1 Z + l22 −

θ 2 = 2 arctan

(6.96) 2

2l2 X X − l32 + + Z 2 = −0.56147 cos θ 1 cos2 θ 1

−C2 ±

 C22 − C1 C3 C1

 =

0.7555 rad −0.4881 rad

(6.97)

(6.98)

Equation (6.77) provides θ 3 . It will give two values, one for every θ 2 . 

X cos θ 1 + Y sin θ 1 − l2 cos θ 2 θ 3 = atan2 l1 + l2 sin θ 2 − Z  0.19132 rad = 2.95027 rad

− θ2 (6.99)

The alternative method also provides two sets of solutions, where the first set matches with the correct solutions (6.89). θ 1 = 0.83298 θ 2 = 0.7555 θ 3 = 0.1913

(6.100)

6.1 Decoupling Technique

325

Fig. 6.6 A 6 DOF articulated manipulator

θ 1 = 0.83298 θ 2 = −0.488 θ 3 = 2.95027

(6.101)

Example 214 Inverse kinematics of an articulated robot. The decoupling method will be reviewed in this example for a 6 DOF reboot to determine manipulator joint variables and wrist orientation joint variables separately. The forward kinematics of the articulated robot, illustrated in Fig. 6.6, was solved in Example 192, where the overall transformation matrix of the end-effector was found, based on the wrist and arm transformation matrices. 0

T7 = Tarm Twrist = 0 T3 3 T7

(6.102)

The wrist transformation matrix Twrist is described in (5.231) and the manipulator transformation matrix, Tarm is found in (5.103). However, according to a new setup coordinate frame, as shown in Fig. 6.6, we have a 6R robot with a six links configurations and a displacement TZ,d7 . 1 R  R(90) 2 R  R(0) 3 R  R(90) 4 R  R(−90) 5 R  R(90) 6 R  R(0) Therefore, the individual links’ transformation matrices are: ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 sin θ 1 0 − cos θ 1 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎤ cos θ 2 − sin θ 2 0 l2 cos θ 2 ⎢ sin θ 2 cos θ 2 0 l2 sin θ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 0 1 d2 ⎦ 0 0 0 1 ⎤ ⎡ cos θ 3 0 sin θ 3 0 ⎢ sin θ 3 0 − cos θ 3 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1

(6.103)

⎡

(6.104)

(6.105)

326

6 Inverse Kinematics

⎡

⎤ cos θ 4 0 − sin θ 4 0 ⎢ sin θ 4 0 cos θ 4 0 ⎥ 3 ⎥ T4 = ⎢ ⎣ 0 −1 0 l3 ⎦ 0 0 0 1 ⎡ ⎤ cos θ 5 0 sin θ 5 0 ⎢ sin θ 5 0 − cos θ 5 0 ⎥ 4 ⎥ T5 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1 ⎡ ⎤ cos θ 6 − sin θ 6 0 0 ⎢ sin θ 6 cos θ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 6 ⎥ T7 = ⎢ ⎣ 0 0 1 d6 ⎦ 000 1

(6.106)

(6.107)

(6.108)

(6.109)

Hence, the tool transformation matrix in the base coordinate frame is: 0

T7 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7 = 0 T3 3 T6 6 T7 ⎡ t11 t12 t13 ⎢ t21 t22 t23 =⎢ ⎣ t31 t32 t33 0 0 0

where

⎡

cθ 1 c(θ 2 + θ 3 ) sθ 1 ⎢ sθ 1 c(θ 2 + θ 3 ) −cθ 1 0 T3 = ⎢ ⎣ s(θ 2 + θ 3 ) 0 0 0 ⎡ cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 ⎢ cθ 5 cθ 6 sθ 4 + cθ 4 sθ 6 3 T6 = ⎢ ⎣ −cθ 6 sθ 5 0

(6.110)

⎤ t14 t24 ⎥ ⎥ t34 ⎦ 1

⎤ cθ 1 s(θ 2 + θ 3 ) l2 cθ 1 cθ 2 + d2 sθ 1 sθ 1 s(θ 2 + θ 3 ) l2 cθ 2 sθ 1 − d2 cθ 1 ⎥ ⎥ ⎦ −c(θ 2 + θ 3 ) l2 sθ 2 0 1 ⎤ −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 0 cθ 4 ccθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 0 ⎥ ⎥ sθ 5 sθ 6 cθ 5 l3 ⎦ 0 0 1

(6.111)

(6.112)

and t11 = cθ 1 (c (θ 2 + θ 3 ) (cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 ) − cθ 6 sθ 5 s (θ 2 + θ 3 )) +sθ 1 (cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 )

(6.113)

t21 = sθ 1 (c (θ 2 + θ 3 ) (−sθ 4 sθ 6 + cθ 4 cθ 5 cθ 6 ) − cθ 6 sθ 5 s (θ 2 + θ 3 )) −cθ 1 (cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 ) t31 = s (θ 2 + θ 3 ) (cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 ) + cθ 6 sθ 5 c (θ 2 + θ 3 )

(6.114) (6.115)

t12 = cθ 1 (sθ 5 sθ 6 s (θ 2 + θ 3 ) − c (θ 2 + θ 3 ) (cθ 6 sθ 4 + cθ 4 cθ 5 sθ 6 )) +sθ 1 (cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 )

(6.116)

t22 = sθ 1 (sθ 5 sθ 6 s (θ 2 + θ 3 ) − c (θ 2 + θ 3 ) (cθ 6 sθ 4 + cθ 4 cθ 5 sθ 6 )) +cθ 1 (−cθ 4 cθ 6 + cθ 5 sθ 4 sθ 6 )

(6.117)

6.1 Decoupling Technique

327

t32 = −sθ 5 sθ 6 c (θ 2 + θ 3 ) − s (θ 2 + θ 3 ) (cθ 6 sθ 4 + cθ 4 cθ 5 sθ 6 )

(6.118)

t13 = sθ 1 sθ 4 sθ 5 + cθ 1 (cθ 5 s (θ 2 + θ 3 ) + cθ 4 sθ 5 c (θ 2 + θ 3 ))

(6.119)

t23 = −cθ 1 sθ 4 sθ 5 + sθ 1 (cθ 5 s (θ 2 + θ 3 ) + cθ 4 sθ 5 c (θ 2 + θ 3 ))

(6.120)

t33 = cθ 4 sθ 5 s (θ 2 + θ 3 ) − cθ 5 c (θ 2 + θ 3 )

(6.121)

t14 = d6 (sθ 1 sθ 4 sθ 5 + cθ 1 (cθ 4 sθ 5 c (θ 2 + θ 3 ) + cθ 5 s (θ 2 + θ 3 ))) +l3 cθ 1 s (θ 2 + θ 3 ) + d2 sθ 1 + l2 cθ 1 cθ 2

(6.122)

t24 = d6 (−cθ 1 sθ 4 sθ 5 + sθ 1 (cθ 4 sθ 5 c (θ 2 + θ 3 ) + cθ 5 s (θ 2 + θ 3 ))) +sθ 1 s (θ 2 + θ 3 ) l3 − d2 cθ 1 + l2 cθ 2 sθ 1

(6.123)

t34 = d6 (cθ 4 sθ 5 s (θ 2 + θ 3 ) − cθ 5 c (θ 2 + θ 3 )) +l2 sθ 2 + l3 c (θ 2 + θ 3 )

(6.124)

 T  T Solution of the inverse kinematics problem begins with the wrist position vector d = X Y Z , which is t14 t24 t34 of 0 T7 for d7 = 0, and (X, Y, Z) are coordinates of the position of the wrist point. ⎡

⎤ ⎡ ⎤ X (l3 sin (θ 2 + θ 3 ) + l2 cos θ 2 ) cos θ 1 + d2 sin θ 1 d = ⎣ (l3 sin (θ 2 + θ 3 ) + l2 cos θ 2 ) sin θ 1 − d2 cos θ 1 ⎦ = ⎣ Y ⎦ Z l3 cos (θ 2 + θ 3 ) + l2 sin θ 2

(6.125)

Theoretically, we must be able to solve Eq. (6.125) for the three joint variables θ 1 , θ 2 , θ 3 . It can be seen that

which provides:

X sin θ 1 − Y cos θ 1 = d2

(6.126)

 θ 1 = 2 atan2(X ± X2 + Y 2 − d22 , d2 − Y )

(6.127)

Equation (6.127) has two solutions for X2 + Y 2 > d22 , one solution for X2 + Y 2 = d22 , and no real solution for X2 + Y 2 < d22 . Combining the first two elements of d gives  l3 sin (θ 2 + θ 3 ) = ± X2 + Y 2 − d22 − l2 cos θ 2

(6.128)

then the third element of d may be utilized to find   2 l32 = ± X2 + Y 2 − d22 − l2 cos θ 2 + (Z − l2 sin θ 2 )2

(6.129)

which can be rearranged to the following form a cos θ 2 + b sin θ 2 = c  a = 2l2 X2 + Y 2 − d22

b = 2l2 Z

c = X2 + Y 2 + Z 2 − d22 + l22 − l32

(6.130)

(6.131) (6.132)

328

6 Inverse Kinematics

with two solutions: ) c c2 θ 2 = atan2( , ± 1 − 2 ) − atan2(a, b) r r r 2 = a 2 + b2

(6.133) (6.134)

Summing the squares of the elements of d gives X2 + Y 2 + Z 2 = d22 + l22 + l32 + 2l2 l3 sin (2θ 2 + θ 3 ) that provides:

 θ 3 = arcsin

X2 + Y 2 + Z 2 − d22 − l22 − l32 2l2 l3

(6.135)

− 2θ 2

(6.136)

Having θ 1 , θ 2 , θ 3 means we can find the wrist point in space. However, because the joint variables in 0 T3 and in 3 T6 are independent, we should find the orientation of the end-effector by solving 3 T6 or 3 R6 for θ 4 , θ 5 , θ 6 . ⎤ cθ 4 cθ 5 cθ 6 − sθ 4 sθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 3 R6 = ⎣ cθ 5 cθ 6 sθ 4 + cθ 4 sθ 6 cθ 4 ccθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 ⎦ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 ⎡ ⎤ s11 s12 s13 = ⎣ s21 s22 s23 ⎦ s31 s32 s33 ⎡

The angles θ 4 , θ 5 , θ 6 can be found by examining elements of 3 R6 . θ 4 = atan2 (s23 , s13 )  2 2 θ 5 = atan2 s13 + s23 , s33 θ 6 = atan2 (s32 , −s31 )

(6.137)

(6.138) (6.139) (6.140)

Example 215  General inverse kinematics formulas. Inverse trigonometric functions frequently appear in inverse kinematic analysis. To minimize numerical error, it is recommended to calculate angles by arctan. Here are some useful equations. There are some general trigonometric equations that regularly appear in inverse kinematics problems. The following indicates the most frequently equations and solutions. 1. If sin θ = a then we have two answers: θ and π − θ.

θ = atan2

√

a

± 1 − a2

(6.141) (6.142)

2. If cos θ = b then we have two answers: θ and −θ.

√ ± 1 − b2 θ = atan2 b

(6.143)

(6.144)

3. If sin θ = a

cos θ = b

then: θ = atan2

a b

(6.145) (6.146)

6.2 Inverse Transformation Technique

329

4. If a cos θ + b sin θ = 0

(6.147)

then we have two answers: θ and θ + π . θ = atan2

a b

θ = atan2

−a −b

(6.148)

5. If a cos θ + b sin θ = c

(6.149)

√ a ± a 2 + b2 − c2 θ = atan2 + atan2 b c

then:

(6.150)

6. If a cos θ + b sin θ = c

(6.151)

a cos θ − b sin θ = d

(6.152)

then: a 2 + b2 = c2 + d 2 θ = atan2

(6.153)

ac − bd ad + bc

(6.154)

7. If sin θ sin ϕ = a

cos θ sin ϕ = b

(6.155)

then we have two answers: θ and θ + π . θ = atan2

a b

θ = atan2

−a −b

(6.156)

8. If sin θ sin ϕ = a

cos θ sin ϕ = b

cos ϕ = c

(6.157)

then we have two answers for θ and ϕ: θ corresponds to ϕ, and θ + π corresponds to −ϕ. θ = atan2 √ ϕ = atan2

6.2

a b

a 2 + b2 c

θ = atan2

−a −b

√ − a 2 + b2 ϕ = atan2 c

(6.158)

(6.159)

Inverse Transformation Technique

Assume we have the 4×4 transformation matrix 0 T6 from forward kinematics expressed by numbers. The matrix 0 T6 includes the global position and the orientation of the end-effector of a 6 DOF robot in the base frame B0 . Also, assume the individual transformation matrices 0 T1 (q1 ), 1 T2 (q2 ), 2 T3 (q3 ), 3 T4 (q4 ), 4 T5 (q5 ), and 5 T6 (q6 ) are known as functions of joint variables analytically.

330

6 Inverse Kinematics

According to forward kinematics we have: 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ r11 r12 r13 r14 ⎢ r21 r22 r23 r24 ⎥ ⎥ =⎢ ⎣ r31 r32 r33 r34 ⎦ 0 0 0 1

(6.160)

We can solve the inverse kinematics problem by solving the following equations for the unknown joint variable. 1

T6 = 0 T1−1 0 T6

(6.161)

2

T6 = 1 T2−1 0 T1−1 0 T6

(6.162)

3

T6 = 2 T3−1 1 T2−1 0 T1−1 0 T6

(6.163)

4

T6 =

T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6

(6.164)

5

T6 = 4 T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6

(6.165)

I=

3

5

T6−1 4 T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6

(6.166)

Proof The left-hand side of Eq. (6.160) is a numerical matrix. The right-hand side of Eq. (6.160) is multiplication of several individual transformation matrices, everyone with one unknown variable. If we multiply both sides of Eq. (6.160) by 0 T1−1 , we obtain an equation which on the left-hand side there is a matrix with only one unknown variable q1 , and on the right-hand side there is a matrix including all other variables q2 , q3 , q4 , q5 , q6 . 0

T1−1 0 T6 = 0 T1−1

0

 T1 1 T2 2 T3 3 T4 4 T5 5 T6 = 1 T6

(6.167)

Note that 0 T1−1 is the mathematical inverse of the 4 × 4 matrix 0 T1 , and not an inverse transformation. So, 0 T1−1 must be calculated by a mathematical matrix inversion. The elements of the matrix 1 T6 on the right-hand side are either zero, constant, or functions of q2 , q3 , q4 , q5 , q6 . A zero or constant element of the right-hand side provides the required algebraic equation to be solved for q1 . Then, we multiply both sides of (6.167) by 1 T2−1 to obtain a new equation. 1

T2−1 0 T1−1 0 T6 = 1 T2−1 0 T1−1

0

T1 1 T2 2 T3 3 T4 4 T5 5 T6

= 2 T6

 (6.168)

Assuming q1 has been calculated from (6.167), the left-hand side of this equation is a function of q2 , while the elements of the matrix 2 T6 , on the right-hand side, are either zero, constant, or functions of q3 , q4 , q5 , q6 . Equating the associated element, with constant or zero element on the right-hand side, provides the required algebraic equation to be solved for q2 . Following this procedure, we can find the joint variables q3 , q4 , q5 , q6 by using the following equalities, respectively. T3−1 1 T2−1 0 T1−1 0 T6   = 2 T3−1 1 T2−1 0 T1−1 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 2

= 3 T6

(6.169)

T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6   = 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 3

= 4 T6

(6.170)

6.2 Inverse Transformation Technique

331

t Fig. 6.7 An articulated manipulator

T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6   = 4 T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 4

= 5 T6

(6.171)

T6−1 4 T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6   = 5 T6−1 4 T5−1 3 T4−1 2 T3−1 1 T2−1 0 T1−1 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 5

= I

(6.172)

The inverse transformation technique may sometimes be called Pieper technique.



Example 216 Articulated manipulator and numerical case. Articulated manipulator is a very practical industrial robot and an important educational example. Here is the use of inverse transformation technique to solve its inverse kinematics. Consider the articulated manipulator shown in Fig. 6.7. The transformation matrices between its coordinate frames are: ⎡

cos θ 1 ⎢ sin θ 1 0 T1 = ⎢ ⎣ 0 0

0 sin θ 1 0 − cos θ 1 1 0 0 0

⎤ 0 0⎥ ⎥ l1 ⎦ 1

⎤ cos θ 2 − sin θ 2 0 l2 cos θ 2 ⎢ sin θ 2 cos θ 2 0 l2 sin θ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 ⎦ 0 1 0 0 0 0 1 ⎡ ⎤ cos θ 3 0 sin θ 3 0 ⎢ sin θ 3 0 − cos θ 3 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 1 0 0⎦ 0 0 0 1

(6.173)

⎡

(6.174)

(6.175)

332

6 Inverse Kinematics

The forward kinematics of the manipulator is: 0

T3 = 0 T1 1 T2 2 T3 ⎡ ⎤ cθ 1 c (θ 2 + θ 3 ) sθ 1 cθ 1 s (θ 2 + θ 3 ) l2 cθ 1 cθ 2 ⎢ sθ 1 c (θ 2 + θ 3 ) −cθ 1 sθ 1 s (θ 2 + θ 3 ) l2 cθ 2 sθ 1 ⎥ ⎥ =⎢ ⎣ s (θ 2 + θ 3 ) 0 −c (θ 2 + θ 3 ) l1 + l2 sθ 2 ⎦ 0 0 0 1

(6.176)

Point P is the point at which we attach a spherical wrist. Therefore, we attach a takht coordinate frame B4 at P that is at a constant distance l3 from B3 . ⎡ ⎤ 1000 ⎢0 1 0 0 ⎥ 3 ⎥ T4 = ⎢ (6.177) ⎣ 0 0 1 l3 ⎦ 0001 So, the overall forward kinematics of the manipulator is: ⎡

⎤ c (θ 2 + θ 3 ) cθ 1 sθ 1 s (θ 2 + θ 3 ) cθ 1 l3 s (θ 2 + θ 3 ) cθ 1 + l2 cθ 1 cθ 2 ⎢ c (θ 2 + θ 3 ) sθ 1 −cθ 1 s (θ 2 + θ 3 ) sθ 1 l3 s (θ 2 + θ 3 ) sθ 1 + l2 cθ 2 sθ 1 ⎥ 0 ⎥ T4 = 0 T3 3 T4 = ⎢ ⎣ s (θ 2 + θ 3 ) 0 −c (θ 2 + θ 3 ) l1 − l3 c (θ 2 + θ 3 ) + l2 sθ 2 ⎦ 0 0 0 1

(6.178)

Using the following dimensions l1 = 1 m

l2 = 1.05 m

when its tip point is at: 0

l3 = 0.89 m

(6.179)

 T dP = 1 1.1 1.2

(6.180)

the forward kinematics reduces to: 0

T4 ⎡

cos (θ 2 + θ 3 ) cos θ 1 sin θ 1 sin (θ 2 + θ 3 ) cos θ 1 ⎢ cos (θ 2 + θ 3 ) sin θ 1 − cos θ 1 sin (θ 2 + θ 3 ) sin θ 1 =⎢ ⎣ sin (θ 2 + θ 3 ) 0 − cos (θ 2 + θ 3 ) 0 0 0

⎤ 1 1.1 ⎥ ⎥ 1.2 ⎦ 1

(6.181)

Let us multiply both sides of 0 T4 by 0 T1−1 to have: 0

T1−1 0 T4 = 0 T1−1

0

 T1 1 T2 2 T3 3 T4 = 1 T4

(6.182)

where 0

T1−1 0 T4 = 1 T4 ⎡ ⎤ cos θ 1 sin θ 1 0 0 ⎢ 0 0 1 −1 ⎥ ⎥0 =⎢ ⎣ sin θ 1 − cos θ 1 0 0 ⎦ T4 0 0 0 1 ⎤ ⎡ cos (θ 2 + θ 3 ) 0 sin (θ 2 + θ 3 ) cos θ 1 + 1.1 sin θ 1 ⎥ ⎢ sin (θ 2 + θ 3 ) 0 − cos (θ 2 + θ 3 ) 0.2 ⎥ =⎢ ⎣ 0 1 0 sin θ 1 − 1.1 cos θ 1 ⎦ 0 0 0 1

(6.183)

6.2 Inverse Transformation Technique

333

and T2 2 T3 3 T4 = ⎡ ⎤ cos (θ 2 + θ 3 ) 0 sin (θ 2 + θ 3 ) 0.89 sin (θ 2 + θ 3 ) + 1. 05 cos θ 2 ⎢ sin (θ 2 + θ 3 ) 0 − cos (θ 2 + θ 3 ) 1. 05 sin θ 2 − 0.89 cos (θ 2 + θ 3 ) ⎥ ⎢ ⎥ ⎣ ⎦ 0 1 0 0 0 0 0 1

1

(6.184)

The last column of the left-hand side of (6.182) is only a function of θ 1 while the right-hand side is a function of θ 2 and θ 3 . Equating the element r24 of both sides of (6.182) provides an equation to determine θ 1 . sin θ 1 − 1.1 cos θ 1 = 0 1.1 1 = 0.8329812667 rad ≈ 47.72631098 deg

(6.185)

θ 1 = atan2 (1.1, 1) = arctan

(6.186)

Substituting θ 1 = 0.83298 rad in (6.183) provides a matrix 1 T4 with a numerical values in the last column. ⎡

⎤ cos (θ 2 + θ 3 ) 0 sin (θ 2 + θ 3 ) 1.486 6 ⎢ sin (θ 2 + θ 3 ) 0 − cos (θ 2 + θ 3 ) 0.2 ⎥ 1 ⎥ T4 = ⎢ ⎣ 0 1 0 0 ⎦ 0 0 0 1

(6.187)

We multiply both sides of (6.187) by 1 T2−1 to have: 1

T2−1 1 T4 = 1 T2−1

1

 T2 2 T3 3 T4 = 2 T4

(6.188)

where ⎤ cos θ 2 sin θ 2 0 −1.05 ⎢ − sin θ 2 cos θ 2 0 0 ⎥ 1 1 −1 1 ⎥ T T2 T4 = 2 T4 = ⎢ ⎣ 0 0 1 0 ⎦ 4 0 0 0 1 ⎤ ⎡ cos θ 3 0 sin θ 3 1.4866 cos θ 2 + 0.2 sin θ 2 − 1.05 ⎢ sin θ 3 0 − cos θ 3 ⎥ 0.2 cos θ 2 − 1.4866 sin θ 2 ⎥ =⎢ ⎣ 0 1 ⎦ 0 0 0 0 0 1 ⎡

and

⎡

cos θ 3 ⎢ sin θ 3 2 T3 3 T4 = ⎢ ⎣ 0 0

⎤ 0 sin θ 3 0.89 sin θ 3 0 − cos θ 3 −0.89 cos θ 3 ⎥ ⎥ ⎦ 1 0 0 0 0 1

(6.189)

(6.190)

Squaring the elements r14 and r24 of the left-hand sides of (6.188) provides an equation to determine θ 2 . (1.4866 cos θ 2 + 0.2 sin θ 2 − 1.05)2 + (0.2 cos θ 2 − 1.4866 sin θ 2 )2 = (0.89 sin θ 3 )2 + (−0.89 cos θ 3 )2 3.1219 cos θ 2 + 0.42 sin θ 2 = 2.560 4

(6.191) (6.192)

334

6 Inverse Kinematics

Fig. 6.8 A spherical robot made of a spherical manipulator attached to a spherical wrist

This equation has the following solutions: θ 2 = 0.7555 rad ≈ 43.29 deg

(6.193)

θ 2 = −0.4881 rad ≈ −27.96 deg

(6.194)

Having θ 2 , we can calculate θ 3 from the last column of (6.190) and (6.189).  θ 3 = arctan If θ 2 = 0.7555 rad, then we have:arctan If θ 2 = −0.488 rad, then we have:

1.486 6 cos θ 2 + 0.2 sin θ 2 − 1.05 0.2 cos θ 2 − 1.486 6 sin θ 2

+π

(6.195)

θ 3 = 2.95 rad ≈ 169 deg

(6.196)

θ 3 = 0.19198 rad ≈ 11 deg

(6.197)

Example 217 Inverse kinematics for a spherical robot. This robot has a long history in education and research of robotic science. Its inverse kinematics will be solved in this example by inverse transformation method. Homogenous transformation matrices of the spherical robot shown in Fig. 6.8 are: ⎡

cθ 1 ⎢ sθ 1 0 T1 = ⎢ ⎣ 0 0

0 −sθ 1 0 cθ 1 −1 0 0 0

⎤ 100 0 ⎢0 1 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 d3 ⎦ 000 1 ⎡

⎤ 0 0⎥ ⎥ 0⎦ 1

⎡

cθ 2 ⎢ sθ 2 1 T2 = ⎢ ⎣ 0 0 ⎡

cθ 4 ⎢ sθ 4 3 T4 = ⎢ ⎣ 0 0

0 sθ 2 0 −cθ 2 1 0 0 0

0 −sθ 4 0 cθ 4 −1 0 0 0

⎤ 0 0⎥ ⎥ l2 ⎦ 1

⎤ 0 0⎥ ⎥ 0⎦ 1

(6.198)

(6.199)

6.2 Inverse Transformation Technique

335

⎡

cθ 5 ⎢ sθ 5 4 T5 = ⎢ ⎣ 0 0

0 sθ 5 0 −cθ 5 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎡

⎤ cθ 6 −sθ 6 0 0 ⎢ sθ 6 cθ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01

(6.200)

Therefore, the position and orientation of the end-effector for a set of joint variables can be found by matrix multiplication, 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ r11 r12 r13 r14 ⎢ r21 r22 r23 r24 ⎥ ⎥ =⎢ ⎣ r31 r32 r33 r34 ⎦ 0 0 0 1

(6.201)

where the elements of 0 T6 are the same as the elements of the matrix in Eq. (5.280). Multiplying both sides of the (6.201) by 0 T1−1 provides ⎡

⎤⎡ cos θ 1 sin θ 1 0 0 r11 ⎢ ⎥ ⎢ 0 0 −1 0 0 −1 0 ⎥ ⎢ r21 T1 T6 = ⎢ ⎣ − sin θ 1 cos θ 1 0 0 ⎦ ⎣ r31 0 0 0 1 0 ⎡ ⎤ f11 f12 f13 f14 ⎢ f21 f22 f23 f24 ⎥ ⎥ =⎢ ⎣ f31 f32 f33 f34 ⎦ 0 0 0 1

r12 r22 r32 0

r13 r23 r33 0

⎤ r14 r24 ⎥ ⎥ r34 ⎦ 1

(6.202)

where f1i = r1i cos θ 1 + r2i sin θ 1

(6.203)

f2i = −r3i

(6.204)

f3i = r2i cos θ 1 − r1i sin θ 1

(6.205)

i = 1, 2, 3, 4 Based on the given transformation matrices, we find: 1

T6 = 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ ⎤ f11 f12 f13 f14 ⎢ f21 f22 f23 f24 ⎥ ⎥ =⎢ ⎣ f31 f32 f33 f34 ⎦ 0 0 0 1

(6.206)

f11 = −cθ 2 sθ 4 sθ 6 + cθ 6 (−sθ 2 sθ 5 + cθ 2 cθ 4 cθ 5 )

(6.207)

f21 = −sθ 2 sθ 4 sθ 6 + cθ 6 (cθ 2 sθ 5 + cθ 4 cθ 5 sθ 2 )

(6.208)

f31 = cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4

(6.209)

f12 = −cθ 2 cθ 6 sθ 4 − sθ 6 (−sθ 2 sθ 5 + cθ 2 cθ 4 cθ 5 )

(6.210)

f22 = −cθ 6 sθ 2 sθ 4 − sθ 6 (cθ 2 sθ 5 + cθ 4 cθ 5 sθ 2 )

(6.211)

f32 = cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6

(6.212)

336

6 Inverse Kinematics

Fig. 6.9 Left shoulder configuration of a spherical robot

f13 = cθ 5 sθ 2 + cθ 2 cθ 4 sθ 5

(6.213)

f23 = −cθ 2 cθ 5 + cθ 4 sθ 2 sθ 5

(6.214)

f33 = sθ 4 sθ 5

(6.215)

f14 = d3 sθ 2

(6.216)

f24 = −d3 cθ 2

(6.217)

f34 = l2

(6.218)

The only constant element of the matrix (6.206) is f34 = l2 , therefore, r24 cos θ 1 − r14 sin θ 1 = l2

(6.219)

This is a first kind of trigonometric equations, solved in Example 209, which has a systematic method of solution. θ 1 = arctan

r24 l2 − arctan  r14 2 2 ± r24 + r14 − l22

(6.220)

The (−) sign corresponds to a left shoulder configuration of the robots as shown in Fig. 6.9, and the (+) sign corresponds to the right shoulder configuration. The elements f14 and f24 of matrix (6.206) are functions of θ 1 and θ 2 only. f14 = d3 sin θ 2 = r14 cos θ 1 + r24 sin θ 1

(6.221)

f24 = −d3 cos θ 2 = −r34

(6.222)

Hence, it is possible to use them and find θ 2 θ 2 = tan−1 where θ 1 must be substituted from (6.220).

r14 cos θ 1 + r24 sin θ 1 r34

(6.223)

6.2 Inverse Transformation Technique

337

In the next step, we find the third joint variable d3 from 1

where

(6.224)

⎡

1

and

T2−1 0 T1−1 0 T6 = 2 T6

T2−1

⎤ cos θ 2 sin θ 2 0 0 ⎢ 0 0 1 −l2 ⎥ ⎥ =⎢ ⎣ sin θ 2 − cos θ 2 0 0 ⎦ 0 0 0 1

(6.225)

⎡

⎤ −sθ 4 sθ 6 + cθ 4 cθ 5 cθ 6 −cθ 6 sθ 4 − cθ 4 cθ 5 sθ 6 cθ 4 sθ 5 0 ⎢ cθ 4 sθ 6 + cθ 5 cθ 6 sθ 4 cθ 4 cθ 6 − cθ 5 sθ 4 sθ 6 sθ 4 sθ 5 0 ⎥ 2 ⎥ T6 = ⎢ ⎣ −cθ 6 sθ 5 sθ 5 sθ 6 cθ 5 d3 ⎦ 0 0 0 1

(6.226)

Employing the elements of the matrices on both sides of Eq. (6.224) shows that the element (3, 4) can be utilized to find d3 . d3 = r34 cos θ 2 + r14 cos θ 1 sin θ 2 + r24 sin θ 1 sin θ 2

(6.227)

There is no other element in Eq. (6.224) to be a function of another single variable. The equation 2 T3−1 1 T2−1 0 T1−1 0 T6 = 3 T6 also provides no new equation. Hence, we move to the next step and evaluate θ 4 from 4 T6 . T4−1 2 T3−1 1 T2−1 0 T1−1 0 T6 = 4 T6

(6.228)

⎤ cos θ 5 cos θ 6 − cos θ 5 sin θ 6 sin θ 5 0 ⎢ cos θ 6 sin θ 5 − sin θ 5 sin θ 6 − cos θ 5 0 ⎥ 4 ⎥ T6 = ⎢ ⎣ sin θ 6 cos θ 6 0 0⎦ 0 0 0 1

(6.229)

3

Evaluating 4 T6

⎡

and the left-hand side of (6.228) utilizing

⎡

2

and

T3−1

⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 −d3 ⎦ 000 1

⎡

3

T4−1

cos θ 4 sin θ 4 ⎢ 0 0 =⎢ ⎣ − sin θ 4 cos θ 4 0 0

shows that

(6.230)

⎤ 0 0 −1 0 ⎥ ⎥ 0 0⎦ 0 1

⎡

g11 ⎢ g21 3 −1 2 −1 1 −1 0 −1 0 T 4 T 3 T 2 T 1 T6 = ⎢ ⎣ g31 0

g12 g22 g32 0

g13 g23 g33 0

(6.231)

⎤ g14 g24 ⎥ ⎥ g34 ⎦ 1

(6.232)

where g1i = −r3i cθ 4 sθ 2 + r2i (cθ 1 sθ 4 + cθ 2 cθ 4 sθ 1 ) +r1i (−sθ 1 sθ 4 + cθ 1 cθ 2 cθ 4 ) g2i = d3 δ 4i − r31 cθ 2 − r11 cθ 1 sθ 2 − r21 sθ 1 sθ 2

(6.233) (6.234)

g3i = r31 sθ 2 sθ 4 + r21 (cθ 1 cθ 4 − cθ 2 sθ 1 sθ 4 ) +r11 (−cθ 4 sθ 1 − cθ 1 cθ 2 sθ 4 ) i = 1, 2, 3, 4

(6.235)

338

6 Inverse Kinematics

The symbol δ 4i indicates the Kronecker delta and is:  δ 4i =

1 if i = 4 0 if i = 4

(6.236)

Therefore, we can find θ 4 by equating the element (3, 3), θ 5 by equating the elements (1, 3) or (2, 3), and θ 6 by equating the elements (3, 1) or (3, 2). Starting from element (3, 3) r13 (−cθ 4 sθ 1 − cθ 1 cθ 2 sθ 4 ) + r23 (cθ 1 cθ 4 − cθ 2 sθ 1 sθ 4 ) +r33 sθ 2 sθ 4 = 0 we find θ 4 θ 4 = tan−1

(6.237)

−r13 sθ 1 + r23 cθ 1 cθ 2 (r13 cθ 1 + r23 sθ 1 ) − r33 sθ 2

(6.238)

which, based on the second value of θ 1 , can also be equal to: θ4 =

π −r13 sθ 1 + r23 cθ 1 + tan−1 2 cθ 2 (r13 cθ 1 + r23 sθ 1 ) − r33 sθ 2

(6.239)

Now we use elements (1, 3) and (2, 3), to find θ 5 . sin θ 5 = r23 (cos θ 1 sin θ 4 + cos θ 2 cos θ 4 sin θ 1 ) − r33 cos θ 4 sin θ 2 +r13 (cos θ 1 cos θ 2 cos θ 4 − sin θ 1 sin θ 4 ) − cos θ 5 = −r33 cos θ 2 − r13 cos θ 1 sin θ 2 − r23 sin θ 1 sin θ 2 θ 5 = tan−1

sin θ 5 cos θ 5

(6.240) (6.241) (6.242)

Finally, θ 6 can be found from the elements (3, 1) and (3, 2). sin θ 6 = r31 sin θ 2 sin θ 4 + r21 (cos θ 1 cos θ 4 − cos θ 2 sin θ 1 sin θ 4 ) +r11 (− cos θ 4 sin θ 1 − cos θ 1 cos θ 2 sin θ 4 )

(6.243)

cos θ 6 = r32 sin θ 2 sin θ 4 + r22 (cos θ 1 cos θ 4 − cos θ 2 sin θ 1 sin θ 4 ) +r12 (− cos θ 4 sin θ 1 − cos θ 1 cos θ 2 sin θ 4 )

θ 6 = tan−1

sin θ 6 cos θ 6

(6.244)

(6.245)

Example 218 Inverse of parametric Euler angles transformation matrix. Any given numerical matrix that is equal to the multiplication of a finite number of matrices based on unknown variables may be solved by inverse transformation matrix method. As an example, we find Euler angles of the Euler transformation matrix. The global rotation matrix based on Euler angles has been found in Eq. (2.174). G

  RB = Az,ψ Ax,θ Az,ϕ T = RZ,ϕ RX,θ RZ,ψ ⎡ ⎤ cϕcψ − cθ sϕsψ −cϕsψ − cθcψsϕ sθsϕ = ⎣ cψsϕ + cθ cϕsψ −sϕsψ + cθ cϕcψ −cϕsθ ⎦ sθsψ sθcψ cθ ⎡ ⎤ r11 r12 r13 = ⎣ r21 r22 r23 ⎦ r31 r32 r33

(6.246)

6.2 Inverse Transformation Technique

339

Let us assume the left-hand side matrix G RB is given as a numerical matrix. The matrices on the right-hand side are also −1 known, each with an unknown Euler angle. Premultiplying G RB by RZ,ϕ , gives: ⎡

⎤ cos ϕ sin ϕ 0 ⎣ − sin ϕ cos ϕ 0 ⎦ G RB 0 0 1 ⎤ ⎡ r11 cϕ + r21 sϕ r12 cϕ + r22 sϕ r13 cϕ + r23 sϕ = ⎣ r21 cϕ − r11 sϕ r22 cϕ − r12 sϕ r23 cϕ − r13 sϕ ⎦ r31 r32 r33 ⎡ ⎤ cos ψ − sin ψ 0 ⎣ = cos θ sin ψ cos θ cos ψ − sin θ ⎦ sin θ sin ψ sin θ cos ψ cos θ

(6.247)

Equating the elements (1, 3) of both sides r13 cos ϕ + r23 sin ϕ = 0

(6.248)

ϕ = atan2 (r13 , −r23 )

(6.249)

gives ϕ. Having ϕ helps us to find ψ by using elements (1, 1) and (1, 2) cos ψ = r11 cos ϕ + r21 sin ϕ

(6.250)

− sin ψ = r12 cos ϕ + r22 sin ϕ

(6.251)

therefore, ψ = atan2

−r12 cos ϕ − r22 sin ϕ r11 cos ϕ + r21 sin ϕ

(6.252)

−1 In the next step, we may postmultiply G RB by RZ,ψ , to provide:

⎡

⎤ cos ψ sin ψ 0 G RB ⎣ − sin ψ cos ψ 0 ⎦ 0 0 1 ⎡ ⎤ r11 cψ − r12 sψ r12 cψ + r11 sψ r13 = ⎣ r21 cψ − r22 sψ r22 cψ + r21 sψ r23 ⎦ r31 cψ − r32 sψ r32 cψ + r31 sψ r33 ⎡ ⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(6.253)

The elements (3, 1) on both sides make an equation to find ψ. r31 cos ψ − r31 sin ψ = 0

(6.254)

Therefore, it is possible to find ψ from the following equation: ψ = atan2 (r31 , r31 ) .

(6.255)

340

6 Inverse Kinematics

Finally, θ can be found using elements (3, 2) and (3, 3) r32 cψ + r31 sψ = sin θ

(6.256)

r33 = cos θ

(6.257)

which give θ. θ = atan2

r32 cos ψ + r31 sin ψ r33

(6.258)

As an example, let us assume the global rotation matrix based on Euler angles is given as: G

  RB = Az,ψ Ax,θ Az,ϕ T = RZ,ϕ RX,θ RZ,ψ ⎡ ⎤ cϕ cψ − cθ sϕ sψ −cϕ sψ − cθ cψ sϕ sθ sϕ = ⎣ cψ sϕ + cθ cϕ sψ −sϕ sψ + cθ cϕ cψ −cϕ sθ ⎦ sθ sψ sθcψ cθ ⎡ ⎤ 0.126 83 −0.780 33 0.612 37 = ⎣ 0.926 78 −0.126 83 −0.353 55 ⎦ 0.353 55 0.612 37 0.707 11

(6.259)

−1 Premultiplying G RB by RZ,ϕ , gives

⎡

⎤ cos ϕ sin ϕ 0 ⎣ − sin ϕ cos ϕ 0 ⎦ G RB 0 0 1 ⎡ ⎤ 0.126cϕ + 0.926sϕ −0.780cϕ − 0.126sϕ 0.612cϕ − 0.353sϕ = ⎣ 0.926cϕ − 0.126sϕ 0.780sϕ − 0.126cϕ −0.353cϕ − 0.612sϕ ⎦ 0.353 55 0.612 37 0.707 11 ⎡ ⎤ cos ψ − sin ψ 0 = ⎣ cos θ sin ψ cos θ cos ψ − sin θ ⎦ . sin θ sin ψ sin θ cos ψ cos θ

(6.260)

Equating the elements (1, 3) of both sides 0.612 37 cos ϕ − 0.353 55 sin ϕ = 0 gives

 ϕ = atan2

0.612 37 0.353 55

(6.261)

= 1.0472 rad = 60 deg

(6.262)

Having ϕ helps us to find ψ by using elements (1, 1) and (1, 2) cos ψ = 0.126 cos ϕ + 0.926 sin ϕ

(6.263)

− sin ψ = −0.78 cos ϕ − 0.126 sin ϕ

(6.264)

therefore, ψ = atan2 = atan2

0.78 cos ϕ + 0.126 sin ϕ 0.126 cos ϕ + 0.926 sin ϕ 0.499 12 = 0.523 rad = 30 deg 0.864 94

(6.265)

6.2 Inverse Transformation Technique

341

−1 Although we can find θ from elements (2, 3) and (3, 3), let us postmultiply G RB by RZ,ψ , to follow the inverse transformation technique.

⎡

⎤ cos ψ sin ψ 0 G RB ⎣ − sin ψ cos ψ 0 ⎦ 0 0 1 ⎡ ⎤ 0.126cψ + 0.78sψ 0.126sψ − 0.78cψ 0.612 37 = ⎣ 0.926cψ + 0.126sψ 0.926sψ − 0.126cψ −0.353 55 ⎦ 0.353cψ − 0.612sψ 0.612cψ + 0.353sψ 0.707 11 ⎡ ⎤ cos ϕ − cos θ sin ϕ sin θ sin ϕ = ⎣ sin ϕ cos θ cos ϕ − cos ϕ sin θ ⎦ 0 sin θ cos θ

(6.266)

The elements (3, 1) on both sides make an equation to find ψ. 0.353 55 cos ψ − 0.612 37 sin ψ = 0

(6.267)

Therefore, it is also possible to find ψ from the following equation:  ψ = atan2

0.353 55 0.612 37

= 0.523 rad = 30 deg

(6.268)

Finally, θ can be found using elements (3, 2) and (3, 3) 0.612 37 cos ψ + 0.353 55 sin ψ = sin θ

(6.269)

0.707 11 = cos θ

(6.270)

which gives θ. θ = atan2

0.707 11 = 1 rad = 45 deg 0.707 11

(6.271)

Example 219  Inverse kinematics and non-standard DH frames. The inverse transformation method may equally applied on kinematics of robots, multibody kinematics, with any standard in their forward kinematics. Here the method will be applied on forward kinematics based on non-standard DH method. Consider a 3 DOF planar manipulator shown in Fig. 5.5. The non-standard DH transformation matrices of the manipulator are: ⎡ ⎤ cos θ 1 − sin θ 1 0 0 ⎢ sin θ 1 cos θ 1 0 0 ⎥ 0 ⎥ T1 = ⎢ (6.272) ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ cos θ 2 − sin θ 2 0 l1 ⎢ sin θ 2 cos θ 2 0 0 ⎥ 1 ⎥ T2 = ⎢ (6.273) ⎣ 0 0 1 0⎦ 0 0 01 ⎡ ⎤ cos θ 3 − sin θ 3 0 l2 ⎢ sin θ 3 cos θ 3 0 0 ⎥ 2 ⎥ T3 = ⎢ (6.274) ⎣ 0 0 1 0⎦ 0 0 01

342

6 Inverse Kinematics

⎡

⎤ 1 0 0 l3 ⎢0 1 0 0 ⎥ 3 ⎥ T4 = ⎢ ⎣0 0 1 0 ⎦ 0001

(6.275)

The solution of the inverse kinematics problem is a mathematical problem and none of the standard or non-standard DH methods for defining link frames provides any simplicity. To calculate the inverse kinematics, we begin with calculating the forward kinematics transformation matrix 0 T4 ,

0

T4 = 0 T1 1 T2 2 T3 3 T4 ⎡ cos θ 123 − sin θ 123 ⎢ sin θ 123 cos θ 123 =⎢ ⎣ 0 0 0 0 ⎡ ⎤ r11 r12 r13 r14 ⎢ r21 r22 r23 r24 ⎥ ⎥ =⎢ ⎣ r31 r32 r33 r34 ⎦ 0 0 0 1

⎤

(6.276)

0 l1 cos θ 1 + l2 cos θ 12 + l3 cos θ 123 0 l1 sin θ 1 + l2 sin θ 12 + l3 sin θ 123 ⎥ ⎥ ⎦ 1 0 0 1

where we used the following short notation to simplify the equation.

Examining the matrix 0 T4 indicates that:

θ ij k = θ i + θ j + θ k

(6.277)

θ 123 = atan2 (r21 , r11 )

(6.278)

The next equation T4 3 T4−1 = 0 T1 1 T2 2 T3 ⎡ ⎤ ⎡ r11 r12 0 r14 − l3 r11 cθ 123 −sθ 123 ⎢ r21 r22 0 r24 − l3 r21 ⎥ ⎢ sθ 123 cθ 123 ⎢ ⎥=⎢ ⎣ 0 0 1 ⎦ ⎣ 0 0 0 0 0 0 1 0 0 0

⎤

(6.279)

0 l1 cθ 1 + l2 cθ 12 0 l1 sθ 1 + l2 sθ 12 ⎥ ⎥ ⎦ 1 0 0 1

shows that: θ 2 = arccos

f12 + f22 − l12 − l22 2l1 l2

θ 1 = atan2 (f2 f3 − f1 f4 , f1 f3 + f2 f4 )

(6.280) (6.281)

where f1 = r14 − l3 r11 = cθ 1 (l2 cθ 2 + l1 ) − sθ 1 (l2 sθ 2 ) = f3 cθ 1 − f4 sθ 1

(6.282)

f2 = r24 − l3 r21 = sθ 1 (l2 cθ 2 + l1 ) + cθ 1 (l2 sθ 2 ) = f3 sθ 1 + f4 cθ 1 Finally, the angle θ 3 will be:

θ 3 = θ 123 − θ 1 − θ 2

(6.283)

(6.284)

6.3  Iterative Technique

6.3

343

 Iterative Technique

The inverse kinematics problem of robots can be interpreted as searching for the unknowns qk of a set of nonlinear algebraic equations 0

Tn = T(q)

(6.285)

= 0 T1 (q1 ) 1 T2 (q2 ) 2 T3 (q3 ) 3 T4 (q4 ) · · · ⎡ ⎤ r11 r12 r13 r14 ⎢ r21 r22 r23 r24 ⎥ ⎥ =⎢ ⎣ r31 r32 r33 r34 ⎦ 0 0 0 1

n−1

Tn (qn )

or rij = rij (qk )

k = 1, 2, · · · n

(6.286)

where n is the number of degree of freedom (DOF ) of the robot. Assuming n = 6, maximum 6 out of the 12 equations of (6.285) are independent and can be utilized to solve for joint variables qk . The functions T(q) are given explicitly based on forward kinematic analysis. Numerous methods are available to find the zeros of Eq. (6.285). However, the methods are, in general, iterative. The most common method is known as the Newton-Raphson method. The iteration technique can be set in an algorithm. Algorithm 6.1 Inverse kinematics iteration technique. 1. Set the initial counter i = 0. 2. Find or guess an initial estimate q(0) . 3. Calculate the residue δT(q(i) ) = J(q(i),) δq(i) . , , , If every element of T(q(i) ) or its norm ,T(q(i) ), is less than a tolerance, ,T(q(i) ), <  then terminate the iteration. The q(i) is the desired solution. 4. Calculate q(i+1) = q(i) + J−1 (q(i) ) δT(q(i) ). 5. Set i = i + 1 and return to step 3. The tolerance  can equivalently be set up on variables q(i+1) − q(i) < 

(6.287)

J−I <  " # ∂Ti J(q) = ∂qj

(6.288)

or on Jacobian J.

(6.289)

Proof To solve the kinematic equations for variables q by iterative technique, T(q) = 0

(6.290)

we begin with an initial guess q for the joint variables q. q = q + δq

(6.291)

Using the forward kinematics, we determine the configuration of the end-effector frame for the guessed joint variables. T = T(q )

(6.292)

344

6 Inverse Kinematics

The difference between the calculated configuration with the forward kinematics T and the desired configuration T represents an error, called residue δT, which must be minimized. δT = T − T

(6.293)

A first order Taylor expansion of the set of equations is: T = T(q + δq) = T(q ) +

∂T δq + O(δq2 ) ∂q

(6.294)

Assuming δq n This is the redundant case for which an infinity of solutions are available. Selection of an appropriate solution can be made under the condition that it is optimal in some sense. For example, let us find a solution for (8.448) that minimizes the deviation from a given reference configuration q (0) . The problem may then be formulated as of finding the minimum of a constrained function     1 (0) T (0) min F = q−q W q−q (8.466) 2 subject to a constraint equation. y − f(q) = 0

(8.467)

Using the technique of Lagrangian multipliers, problem (8.466) and (8.467) may be replaced by an equivalent problem ∂G =0 ∂q

∂G =0 ∂λ

(8.468)

with the definition of the functional. G(q, λ) =

   1 q − q(0) T W q − q(0) + λT [y − f(q)] 2

(8.469)

It leads to a system of m + n equations with m + n unknowns.   W q − q(0) − JT λ = 0

(8.470)

y − f(q) = 0

(8.471)

Linearization of Eqs. (8.470) provides the system of equations for the displacement corrections and variations of Lagrangian multipliers W δ − JT λ = 0

(8.472)

Wδ = r

(8.473)

where λ is the increment of λ. Substitution of the solution δ obtained from the first equation of (8.470) into the second one yields

or, in terms of the displacement correction.

JW−1 JT δλ = r

(8.474)

  δq = W−1 JT JW−1 JT

(8.475)

+

The matrix J has the meaning of a pseudo-inverse to the singular Jacobian matrix J.

It verifies the identity and, whenever J is invertible, we have

 −1 J+ = W−1 JT JW−1 JT

(8.476)

JJ+ = I

(8.477)

J+ = J−1

(8.478)

8.8  Jacobian Matrix From Link Transformation Matrices

8.8

469

 Jacobian Matrix From Link Transformation Matrices

In robot motion, we need to calculate the Jacobian matrix in a very short time for every configuration of the robot. The Jacobian matrix of a robot can be found easier in an algorithmic way by evaluating columns of the Jacobian J   J = c1 c2 · · · cn "0 0 0 0 # 0 k˜0 0 dn k˜1 1 dn · · · 0 k˜n−1 n−1 dn = 0ˆ 0ˆ 0ˆ k0 k1 · · · kn−1   0 0ˆ dn k0 × 00 dn 0 kˆ1 × 01 dn · · · 0 kˆn−1 × n−1 = 0ˆ 0ˆ 0ˆ k0 k1 ··· kn−1

(8.479)

(8.480)

where ci is called the Jacobian generating vector and 0 kˆi−1 is the vector associated to the skew matrix 0 k˜i−1 . ci =

"0

0 k˜i−1 i−1 dn 0ˆ ki−1



# =

0ˆ

0 ki−1 × i−1 dn 0ˆ ki−1

 (8.481)

This method is solely based on link transformation matrices found in forward kinematics and does not involve differentiation. The matrix 0 k˜i−1 is 0˜ T ki−1 = 0 Ri−1 i−1 k˜i−1 0 Ri−1 (8.482) which means 0 kˆi−1 to be a unit vector in the direction of joint axis i in the global coordinate frame. For a revolute joint, we have "0 # 0 k˜i−1 i−1 dn ci = (8.483) 0ˆ ki−1 and for a prismatic joint we have ci =

"0

kˆi−1 0

# (8.484)

Proof Transformation between two coordinate frames is based on a transformation matrix that is a combination of rotation matrix R and the position vector d. G r = G TB B r (8.485) " # Rd T = (8.486) 0 1 Introducing the infinitesimal transformation matrix "

δR δd δT = 0 0 leads to δT T −1 = where T

−1

( is the infinitesimal rotations matrix, and, therefore, δθ

"

"

#

( δv δθ 0 0

(8.487) #

R T −R T d = 0 1

(8.488) # (8.489)

470

8 Velocity Kinematics

⎡

⎤ 0 −δθ z δθ y ( = δR R T = ⎣ δθ z δθ 0 −δθ x ⎦ −δθ y δθ x 0

(8.490)

and δv is a vector related to infinitesimal displacements. (d δv = δd − δθ

(8.491)

Let us define a 6 × 1 coordinate vector describing the rotational and translational coordinates of the end-effector " # d X= θ and its variation is δX.

" δX =

δd δθ

(8.492)

# (8.493)

The Jacobian matrix J is then a matrix that maps differential joint variables to differential end-effector motion. δX =

∂T(q) δq = J δq ∂q

(8.494)

The transformation matrix T , generated in forward kinematics, is a function of joint coordinates 0

Tn = T(q)

(8.495)

= 0 T1 (q1 ) 1 T2 (q2 ) 2 T3 (q3 ) 3 T4 (q4 ) · · ·

n−1

Tn (qn )

therefore, the infinitesimal transformation matrix is δT =

n 

0

T1 (q1 ) T2 (q2 ) · · · 1

δ

i=1

i−1  Ti ··· δqi

n−1

Tn (qn ) · δqi

(8.496)

Interestingly, the partial derivative of the transformation matrix can be arranged in the form δ

i−1  Ti = δqi

i−1

i−1 i−1 Ti

(8.497)

where according to DH transformation matrix i−1 Ti is ⎡

⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1 " i−1 i−1 # Ri di = 0 1

(8.498)

The velocity coefficient matrices i for a revolute joint is

i−1

i−1 = R =

" i−1

k˜i−1 0

⎤ 0 −1 0 0 ⎢1 0 0 0⎥ 0 ⎥ =⎢ ⎣0 0 0 0⎦ 0 0 0 00 #

⎡

(8.499)

8.8  Jacobian Matrix From Link Transformation Matrices

471

and for a prismatic joint is " i−1

i−1 = P =

0 i−1 kˆi−1 0 0

#

⎡

⎤ 0000 ⎢0 0 0 0⎥ ⎥ =⎢ ⎣0 0 0 1⎦ 0000

(8.500)

We may now express each term of (8.496) in the following form. 0

δ

T1 (q1 ) T2 (q2 ) · · · 1

i−1  Ti ··· δqi

n−1

Tn (qn ) = Ci T

(8.501)

i−1  −1 Ti 0 1 Ci = T1 T2 · · · Ti−1 T1 T2 · · · i−1 Ti δqi     = 0 T1 1 T2 · · · i−2 Ti−1 i−1 i−1 i−1 Ti 0 T1 1 T2 · · · i−1 Ti −1    −1 = 0 T1 1 T2 · · · i−2 Ti−1 i−1 i−1 0 T1 1 T2 · · · i−2 Ti−1 0

1

 δ

i−2

= 0 T1 1 T2 · · ·

i−2

Ti−1

i−1

i−1

i−2

−1 Ti−1 · · · 1 T2−1 0 T1−1

(8.502)

The matrix Ci can be rearranged for a revolute joint, Ci =

"0

kˆi−1 0 k˜i−1 0 dn − 0 k˜ i−1 0 di−1 0 0

and for a prismatic joint. Ci =

"0

kˆi−1 0 0 0

# (8.503)

# (8.504)

Ci has six independent terms that can be combined in a 6 × 1 vector. This is the generating vector ci that makes the ith column of the Jacobian matrix. The Jacobian generating vector for a revolute joint is ci = and for a prismatic joint is

"0

0 k˜i−1 i−1 dn 0ˆ ki−1

"

0 ci = 0 ˆ ki−1

# (8.505)

# (8.506)

0 dn indicated the origin of the The position vector 0 di indicated the origin of the Bi -frame in the base B0 -frame. Hence, i−1 end-effector coordinate Bn -frame with respect to coordinate Bi−1 -frame and expressed in the base frame B0 . Therefore, the Jacobian matrix describing the instantaneous kinematics of the robot can be obtained.

J=

"0

0 k˜0 00 dn 0 k˜1 01 dn · · · 0 k˜n−1 n−1 dn 0ˆ 0ˆ 0ˆ k0 k1 · · · kn−1

# (8.507) 

472

8 Velocity Kinematics

Example 294 Jacobian matrix for articulated robots. The forward and inverse kinematics of the articulated robot has been analyzed in Example 214 with the following individual transformation matrices: ⎡

cθ 1 ⎢ sθ 1 0 T1 = ⎢ ⎣ 0 0

0 sθ 1 0 −cθ 1 1 0 0 0

⎤ ⎡ ⎤ 0 cθ 2 −sθ 2 0 l2 cθ 2 ⎢ ⎥ 0⎥ ⎥ 1 T = ⎢ sθ 2 cθ 2 0 l2 sθ 2 ⎥ 0⎦ 2 ⎣ 0 0 1 d2 ⎦ 1 0 0 0 1

⎡

⎡ ⎤ ⎤ cθ 3 0 sθ 3 0 cθ 4 0 −sθ 4 0 ⎢ sθ 3 0 −cθ 3 0 ⎥ 3 ⎢ ⎥ 2 ⎥ T4 = ⎢ sθ 4 0 cθ 4 0 ⎥ T3 = ⎢ ⎣ 0 1 0 0⎦ ⎣ 0 −1 0 l3 ⎦ 0 0 0 1 0 0 0 1 ⎡ ⎤ ⎡ ⎤ cθ 5 0 sθ 5 0 cθ 6 −sθ 6 0 0 ⎢ sθ 5 0 −cθ 5 0 ⎥ 5 ⎢ sθ 6 cθ 6 0 0 ⎥ 4 ⎥ ⎢ ⎥ T5 = ⎢ ⎣ 0 1 0 0 ⎦ T6 = ⎣ 0 0 1 0⎦ 0 0 0 1 0 0 01

(8.508)

The articulated robot has 6 DOF and, therefore, its Jacobian matrix is a 6 × 6 matrix   J(q) = c1 (q) c2 (q) · · · c6 (q)

(8.509)

˙ that relates the translational and angular velocities of the end-effector to the joints’ velocities q. " # v = J(q) q˙ ω

(8.510)

The ith column vector ci (q) for a revolute joint is given by  ci (q) =

0ˆ

0 ki−1 × i−1 d6 0ˆ ki−1

and for a prismatic joint is given as: ci (q) =

"0

kˆi−1 0

 (8.511)

# (8.512)

Column 1 The first column of the Jacobian matrix has the simplest calculation, because it is based on the contribution of the z0 -axis and the position of the end-effector frame 0 d6 and these are the result of forward kinematics. The direction of the z0 -axis in the base coordinate frame always is ⎡ ⎤ 0 0ˆ ⎣ k0 = 0 ⎦ (8.513) 1 and the position vector of the end-effector frame B6 is given by 0 d6 directly determined from 0 T6 , 0

T6 = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 ⎡ t11 t12 "0 0 # ⎢ t21 t22 R6 d 6 = =⎢ ⎣ t31 t32 0 1 0 0 ⎤ t14 0 d6 = ⎣ t24 ⎦ t34

t13 t23 t33 0

⎤ t14 t24 ⎥ ⎥ t34 ⎦ 1

(8.514)

⎡

(8.515)

8.8  Jacobian Matrix From Link Transformation Matrices

473

where t14 = d6 (sθ 1 sθ 4 sθ 5 + cθ 1 (cθ 4 sθ 5 c (θ 2 + θ 3 ) +cθ 5 s (θ 2 + θ 3 ))) +l3 cθ 1 s (θ 2 + θ 3 ) + d2 sθ 1 + l2 cθ 1 cθ 2

(8.516)

t24 = d6 (−cθ 1 sθ 4 sθ 5 + sθ 1 (cθ 4 sθ 5 c (θ 2 + θ 3 ) +cθ 5 s (θ 2 + θ 3 ))) +sθ 1 s (θ 2 + θ 3 ) l3 − d2 cθ 1 + l2 cθ 2 sθ 1

(8.517)

t34 = d6 (cθ 4 sθ 5 s (θ 2 + θ 3 ) − cθ 5 c (θ 2 + θ 3 )) +l2 sθ 2 + l3 c (θ 2 + θ 3 ) .

(8.518)

Therefore, 0ˆ

k0 × 0 d6 = 0 k˜0 0 d6 ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 −1 0 t14 −t24 = ⎣ 1 0 0 ⎦ ⎣ t24 ⎦ = ⎣ t14 ⎦ 0 0 0 t34 0

and the first Jacobian generating vector is

⎤ −t24 ⎢ t14 ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ ⎥ c1 = ⎢ ⎥ ⎢ 0 ⎥ ⎣ 0 ⎦ 1

(8.519)

⎡

(8.520)

Column 2 The z1 -axis in the base frame can be found by transformation. ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 cθ 1 0 sθ 1 0 sin θ 1 k1 = R1 ⎣ 0 ⎦ = ⎣ sθ 1 0 −cθ 1 ⎦ ⎣ 0 ⎦ = ⎣ − cos θ 1 ⎦ 1 0 1 0 1 0

0ˆ

0

(8.521)

The second half of c2 needs the cross product of 0 kˆ 1 and position vector 1 d6 . The vector 1 d6 is the position of the end-effector in the coordinate frame B1 ; however, it must be described in the base frame to be able to perform the cross product. An easier method is to find 1 kˆ1 × 1 d6 and transform the resultant into the base frame. 0ˆ

k 1 × 1 d6 ⎛⎡ ⎤ ⎡ ⎤⎞ 0 l2 cos θ 2 + l3 sin (θ 2 + θ 3 ) = 0 R1 ⎝⎣ 0 ⎦ × ⎣ l2 sin θ 2 − l3 cos (θ 2 + θ 3 ) ⎦⎠ d2 1 ⎡ ⎤ cos θ 1 (−l2 sin θ 2 + l3 cos (θ 2 + θ 3 )) = ⎣ sin θ 1 (−l2 sin θ 2 + l3 cos (θ 2 + θ 3 )) ⎦ l2 cos θ 2 + l3 sin (θ 2 + θ 3 )

k 1 × 1 d 6 = 0 R1

Therefore, c2 is found as a 6 × 1 vector.



1ˆ

(8.522)

474

8 Velocity Kinematics

⎡

⎤ cos θ 1 (−l2 sin θ 2 + l3 cos (θ 2 + θ 3 )) ⎢ sin θ 1 (−l2 sin θ 2 + l3 cos (θ 2 + θ 3 )) ⎥ ⎢ ⎥ ⎢ ⎥ l2 cos θ 2 + l3 sin (θ 2 + θ 3 ) ⎥ c2 = ⎢ ⎢ ⎥ sin θ 1 ⎢ ⎥ ⎣ ⎦ − cos θ 1 0

(8.523)

Column 3 The z2 -axis in the base frame can be found using the same method. ⎡ ⎤ ⎡ ⎤ 0 sin θ 1 0ˆ k2 = 0 R2 2 kˆ2 = 0 R1 1 R2 ⎣ 0 ⎦ = ⎣ − cos θ 1 ⎦ 1 0

(8.524)

The second half of c3 can be found by finding 2 kˆ 2 × 2 d6 and transforming the resultant into the base coordinate frame. ⎡

⎤ l3 cos θ 3 k2 × d6 = ⎣ l3 sin θ 3 ⎦ 0

2ˆ

0

 R2

Therefore, c3 is

2

2ˆ

k 2 × 2 d6



⎤ l3 cos θ 1 sin (θ 2 + θ 3 ) = ⎣ l3 sin θ 1 sin (θ 2 + θ 3 ) ⎦ −l3 cos (θ 2 + θ 3 )

(8.525)

⎡

⎤ l3 cos θ 1 sin (θ 2 + θ 3 ) ⎢ l3 sin θ 1 sin (θ 2 + θ 3 ) ⎥ ⎢ ⎥ ⎢ −l3 cos (θ 2 + θ 3 ) ⎥ ⎥ c3 = ⎢ ⎢ ⎥ sin θ 1 ⎢ ⎥ ⎣ ⎦ − cos θ 1 0

(8.526)

⎡

(8.527)

Column 4 The z3 -axis in the base frame is ⎡ ⎤ 0 0ˆ k 3 = 0 R1 1 R2 2 R3 ⎣ 0 ⎦ 1 ⎡ ⎤ cos θ 1 (cos θ 2 sin θ 3 + cos θ 3 sin θ 2 ) = ⎣ sin θ 1 (cos θ 2 sin θ 3 + sin θ 2 cos θ 3 ) ⎦ − cos (θ 2 + θ 3 )

(8.528)

and the second half of c4 can be found by finding 3 kˆ 3 × 3 d6 and transforming the resultant into the base coordinate frame. 0

Therefore, c4 is

 R3

3ˆ

k 3 × 3 d6



⎛⎡ ⎤ ⎡ ⎤⎞ ⎡ ⎤ 0 0 0 = 0 R3 ⎝⎣ 0 ⎦ × ⎣ 0 ⎦⎠ = ⎣ 0 ⎦ 1 l3 0

⎤ 0 ⎥ ⎢ 0 ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ c4 = ⎢ ⎥ ⎢ cos θ 1 (cos θ 2 sin θ 3 + cos θ 3 sin θ 2 ) ⎥ ⎣ sin θ 1 (cos θ 2 sin θ 3 + sin θ 2 cos θ 3 ) ⎦ − cos (θ 2 + θ 3 )

(8.529)

⎡

(8.530)

8.8  Jacobian Matrix From Link Transformation Matrices

475

Column 5 The z4 -axis in the base frame is ⎡ ⎤ ⎡ ⎤ 0 cθ 4 sθ 1 − cθ 1 sθ 4 c (θ 2 + θ 3 ) 0ˆ k4 = 0 R4 ⎣ 0 ⎦ = ⎣ −cθ 1 cθ 4 − sθ 1 sθ 4 c (θ 2 + θ 3 ) ⎦ 1 −sθ 4 s (θ 2 + θ 3 )

(8.531)

and the second half of c5 can be found by finding 4 kˆ 4 × 4 d6 and transforming the resultant into the base coordinate frame. 0

Therefore, c5 is

 R4

4ˆ

k 4 × 4 d6



⎛⎡ ⎤ ⎡ ⎤⎞ ⎡ ⎤ 0 0 0 = 0 R4 ⎝⎣ 0 ⎦ × ⎣ 0 ⎦⎠ = ⎣ 0 ⎦ 1 0 0

⎤ 0 ⎢ ⎥ 0 ⎢ ⎥ ⎢ ⎥ 0 ⎢ ⎥ c5 = ⎢ ⎥ ⎢ cos θ 4 sin θ 1 − cos θ 1 sin θ 4 cos (θ 2 + θ 3 ) ⎥ ⎣ − cos θ 1 cos θ 4 − sin θ 1 sin θ 4 cos (θ 2 + θ 3 ) ⎦ − sin θ 4 sin (θ 2 + θ 3 )

(8.532)

⎡

(8.533)

Column 6 The z5 -axis in the base frame is ⎡ ⎤ 0 0ˆ k 5 = 0 R5 ⎣ 0 ⎦ 1 ⎡ ⎤ −cθ 1 cθ 4 s (θ 2 + θ 3 ) − sθ 4 (sθ 1 sθ 4 + cθ 1 cθ 4 c (θ 2 + θ 3 )) = ⎣ −sθ 1 cθ 4 s (θ 2 + θ 3 ) − sθ 4 (−cθ 1 sθ 4 + sθ 1 cθ 4 c (θ 2 + θ 3 )) ⎦ cθ 4 c (θ 2 + θ 3 ) − 12 s (θ 2 + θ 3 ) s2θ 4

(8.534)

and the second half of c6 can be found by finding 5 kˆ 5 × 5 d6 and transforming the resultant into the base coordinate frame. 0

 R5

5ˆ

k 5 × 5 d6



⎛⎡ ⎤ ⎡ ⎤⎞ ⎡ ⎤ 0 0 0 = 0 R5 ⎝⎣ 0 ⎦ × ⎣ 0 ⎦⎠ = ⎣ 0 ⎦ 1 0 0

(8.535)

Therefore, c6 is ⎤ 0 ⎢ ⎥ 0 ⎥ ⎢ ⎥ ⎢ 0 ⎢ ⎥ c6 = ⎢ ⎥ −cθ cθ s + θ − sθ sθ + cθ cθ c + θ (θ ) (sθ (θ )) 1 4 2 3 4 1 4 1 4 2 3 ⎥ ⎢ ⎣ −sθ 1 cθ 4 s (θ 2 + θ 3 ) − sθ 4 (−cθ 1 sθ 4 + sθ 1 cθ 4 c (θ 2 + θ 3 )) ⎦ cθ 4 c (θ 2 + θ 3 ) − 12 s (θ 2 + θ 3 ) s2θ 4 ⎡

(8.536)

476

8.9

8 Velocity Kinematics

Summary

Each link of a serial robot has an angular and a translational velocity. The angular velocity of link (i) in the global coordinate frame can be found as a summation of the global angular velocities of its lower links 0 0 ωi

=

i 

0 j −1 ωj .

(8.537)

j =1

Using DH parameters, the angular velocity of link (j ) with respect to link (j − 1) is  0 j −1 ωj

=

θ˙ j 0 kˆj −1 if joint i is R 0 if joint i is P

(8.538)

The translational velocity of link (i) is the global velocity of the origin of coordinate frame Bi attached to link (i) 0 ˙ i−1 di

=

0

0 0 ωi × i−1 di 0 d˙i kˆi−1 + 00 ωi

×

0 i−1 di

if joint i is R if joint i is P

(8.539)

where θ and d are DH parameters and d is the frame’s origin position vector. The velocity kinematics of a robot is defined by the relationship between joint speeds q˙  T q˙ = q˙n q˙n q˙n · · · q˙n

(8.540)

  ˙ = X˙ n Y˙n Z˙ n ωXn ωY n ωZn T X

(8.541)

˙ and global speeds of the end-effector X.

Such a relationship introduces Jacobian matrix J.

˙ = J q˙ X

(8.542)

Having J, we are able to find the end-effector speeds for a given set of joint speeds and vice versa. Jacobian is a function of joint coordinates and is the main tool in velocity kinematics of robots. We practically calculate J by Jacobian generating vectors denoted by ci (q)  ci (q) =

0ˆ

0 ki−1 × i−1 dn 0ˆ ki−1

 (8.543)

where ci (q) makes the column i − 1 of the Jacobian matrix.   J = c0 c1 c2 · · · cn−1

(8.544)

There are some general numerical calculations needed in robot kinematics. Solutions to a set of linear and nonlinear algebraic equations are the most important ones for calculating a matrix inversion and a Jacobian matrix. An applied solution for a set of linear equations is LU factorization, and a practical method for a set of nonlinear equations is the Newton–Raphson method. Both of these methods are cast in applied algorithms.

8.10 Key Symbols

8.10

Key Symbols

a a A A, B, C, D aij b B c c con d dx , d y , d z d det D e f G, B0 H ıˆ, jˆ, kˆ ı˜, j˜, k˜ Iˆ, Jˆ, Kˆ I = [I] J JD JR Jφ JA l lij L m n P q q ri rij r R s sT , lT sgn tij T Tarm Twrist T

Kinematic length of a link Turn vector of end-effector frame Coefficient matrix Submatrices of J The element of row i and column j of A The vector of known values in a set of linear equations Body coordinate frame Dummy matrix with upper U and lower L cos Jacobian generating vector Condition number Differential, prismatic joint variable Elements of d Translation vector, displacement vector Determinant Displacement transformation matrix Lower right submatrix of B Rotation quaternion A set of nonlinear algebraic equations Global coordinate frame, base coordinate frame Dummy matrix to calculate D Local coordinate axes unit vectors Skew symmetric matrices of the unit vector ıˆ, jˆ, kˆ Global coordinate axes unit vectors Identity matrix Jacobian, geometric Jacobian Displacement Jacobian Rotational Jacobian Angular Jacobian Analytic Jacobian Length The element of row i and column j of L Lower triangle submatrix of A Number of independent equations Number of rows and columns of A Prismatic joint, point The vector of unknowns of f, vector of joint variables Joint coordinate Vector joint coordinates The element i of r The element of row i and column j of a matrix Position vectors, homogenous position vector Rotation transformation matrix, revolute joint sin Nondiagonal first column of D Signum function The element of row i and column j of T Homogenous transformation matrix Manipulator transformation matrix Wrist transformation matrix A set of nonlinear algebraic equations of q

477

478

8 Velocity Kinematics

uij uˆ u˜ u1 , u2 , u3 U uT , rT v V W x X, Y, Z y x, y, z X, Y, Z

The element of row i and column j of U Unit vector along the axis of ω Skew symmetric matrix of the vector uˆ Components of uˆ Upper triangle submatrix of A Nondiagonal first row of D Velocity vector Velocity transformation matrix Weight factor matrix Vector of unknowns Global coordinate axes Dummy vector of unknowns Local coordinate axes Global coordinate axes, coordinates of end-effector

Greek α, β, γ δ δ δ  θ θ ij k θ ϕ, θ, ψ φ ω ω˜ ω1 , ω2 , ω3

Angles of rotation about the axes of global frame Kronecker function, small increment of a parameter Small increment of a parameter Difference in q for in two steps of iteration Small test number to terminate a procedure Rotary joint angle θi + θj + θk Vector of θ i Angles of rotation about the axes of body frame Angle of rotation about uˆ Angular velocity vector Skew symmetric matrix of the vector ω Components of ω

Symbol DOF [ ]−1 [ ]T  (i)  ⊥ e∗   [ ]−1 [ ]T [ ]+ ≡  (i)  ⊥ × q 

Degree of freedom Inverse of the matrix [ ] Transpose of the matrix [ ] Orthogonal Link number i Parallel Perpendicular Conjugate of e Norm of the matrix [ ] Inverse of the matrix [ ] Transpose of the matrix [ ] Pseudo-inverse of the matrix [ ] Equivalent Orthogonal Link number i Parallel sign perpendicular Vector cross product A guess value for q Perturbation in a vector or a matrix

Exercises

479

Fig. 8.11 A 3R, RRR planar manipulator

Exercises 1. Notation and symbols. Describe the meaning of following notations.

i−1 a −i−1 di b −i−10 di c − i−1i di d −i−1i di−1 e − i−1 i−1 di f − 0 di i−1 ˙ ˙ g −i−1 d˙ i h −i−10 d˙ i i −i−1i d˙ i j −i−1i d˙ i−1 k −i−1 i−1 di l − 0 di

m −0 kˆi n −0 kˆi−1 o −i−10 kˆ i−2 p −i−10 vi q −i−1i vi r −i vi s − [L] t − [U ] u − [B] v − [Di ] w − uii x − dii y − con (A) z − A∞ A − A1 B − A2 C − ci D − X ˙ −i−1 i−1 H − T˙ I − V˙ J − θ E − JF − qG 2. 3R planar manipulator velocity kinematics. Figure 8.11 illustrates an RRR planar manipulator. The forward kinematics of the manipulator provides the following transformation matrices: ⎡

⎤ cθ 3 −sθ 3 0 l3 cθ 3 ⎢ sθ 3 cθ 3 0 l3 sθ 3 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

⎡

⎤ cθ 2 −sθ 2 0 l2 cθ 2 ⎢ sθ 2 cθ 2 0 l2 sθ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

⎡

⎤ cθ 1 −sθ 1 0 l1 cθ 1 ⎢ sθ 1 cθ 1 0 l1 sθ 1 ⎥ 0 ⎥ T1 = ⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(8.545)

Calculate the Jacobian matrix, J, using direct differentiating, and find the Cartesian velocity vector of the end point for numerical values. θ 1 = 56 deg

θ 2 = −28 deg

l1 = 100 cm

l2 = 55 cm

θ˙ 1 = 30 deg /s

θ 3 = −10 deg l3 = 30 cm

θ˙ 2 = 10 deg /s

θ˙ 3 = −10 deg /s

(8.546)

480

8 Velocity Kinematics

Fig. 8.12 Schematic of a spherical wrist

3. Spherical wrist velocity kinematics. Figure 8.12 illustrates a schematic of a spherical wrist. The associated transformation matrices are given below. Assume the frame B3 is the base frame. Find the angular velocity vector of the coordinate frame B6 . ⎡

cθ 4 ⎢ sθ 4 3 T4 = ⎢ ⎣ 0 0

0 −sθ 4 0 cθ 4 −1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎡

cθ 5 ⎢ sθ 5 4 T5 = ⎢ ⎣ 0 0

0 sθ 5 0 −cθ 5 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎤ cθ 6 −sθ 6 0 0 ⎢ sθ 6 cθ 6 0 0 ⎥ 5 ⎥ T6 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎡

(8.547)

4. Spherical wrist and tool’s frame velocity kinematics. Assume we attach a tool’s coordinate frame, with the following transformation matrix, to the last coordinate frame B6 of the spherical wrist of Fig. 8.12. ⎡ ⎤ 100 0 ⎢0 1 0 0 ⎥ 6 ⎥ T7 = ⎢ (8.548) ⎣ 0 0 1 d6 ⎦ 000 1 The wrist transformation matrices are given in Exercise 3. Assume that the frame B3 is the base frame and find the translational and angular velocities of the tool’s coordinate frame B7 . 5. SCARA manipulator velocity kinematics. An RRRP SCARA manipulator is shown in Fig. 8.17 with the following transformation matrices. Calculate the Jacobian matrix of the robot using (a) the Jacobian generating vector technique. (b) the direct differentiating method. ⎡

⎤ cθ 1 −sθ 1 0 l1 cθ 1 ⎢ sθ 1 cθ 1 0 l1 sθ 1 ⎥ 0 ⎥ T1 = ⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1 ⎡

⎤ cθ 2 −sθ 2 0 l2 cθ 2 ⎢ sθ 2 cθ 2 0 l2 sθ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

⎡

⎤ cθ 3 −sθ 3 0 0 ⎢ sθ 3 cθ 3 0 0 ⎥ 2 ⎥ T3 = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎤ 1000 ⎢0 1 0 0⎥ 3 ⎥ T4 = ⎢ ⎣0 0 1 d ⎦ 0001

(8.549)

⎡

(8.550)

Exercises

481

Fig. 8.13 An RRR articulated arm

Fig. 8.14 A RRP planar redundant manipulator

6. RRR articulated arm velocity kinematics. Figure 8.13 illustrates a 3 DOF RRR manipulator with the following transformation matrices. Find the Jacobian matrix using direct differentiating, and Jacobian-generating vector methods. ⎡

cθ 1 ⎢ sθ 1 0 T1 = ⎢ ⎣ 0 0 ⎡

0 −sθ 1 0 cθ 1 −1 0 0 0

⎤ 0 0⎥ ⎥ d1 ⎦ 1

⎤ cθ 2 −sθ 2 0 l2 cθ 2 ⎢ sθ 2 cθ 2 0 l2 sθ 2 ⎥ 1 ⎥ T2 = ⎢ ⎣ 0 0 1 d2 ⎦ 0 0 0 1

⎡

cθ 3 ⎢ sθ 3 2 T3 = ⎢ ⎣ 0 0

0 sθ 3 0 −cθ 3 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎡ ⎤ 0 ⎢ 0⎥ 0 ⎥ rP = 0 T3 ⎢ ⎣ l3 ⎦ 1

(8.551)

(8.552)

7. A RRP planar redundant manipulator. Redundant manipulator is the one that has more degrees of freedom than needed. A planar manipulator theoretically needs only two DOF . Here we have a planar manipulator with three DOF . Figure 8.14 illustrates a 3 DOF planar manipulator with joint variables θ 1 , θ 2 , d3 . (a) Determine the link transformation matrices and calculate 0 T3 . (b) Solve the inverse kinematics of the manipulator for a given values of X, Y , ϕ, where X, Y are global coordinates of the end-effector frame B3 , and ϕ is the angular coordinate of B3 .

482

8 Velocity Kinematics

y3

x2

x3

l3

y1

M

y2

y0

T2

x1

T1

x0

d1

Fig. 8.15 A RP R planar redundant manipulator

(c) Show that the following equation can be a set of solution for inverse kinematic problem. θ 2 = tan

−1

± 1 − β2

θ 1 = tan−1 d3 =

√

β

β=

Y − (θ 2 − α) X

X2 + Y 2 sin (θ 2 − α) l1

α = ϕ − tan−1

Y X



l12 + X2 + Y 2 − 2l1 X2 + Y 2 cos (θ 2 − α)

(8.553)

(8.554)

(8.555)

(d) Determine the Jacobian matrix of the manipulator and show that the following equation solves the forward velocity kinematics. ⎡ ⎤ ⎡ ⎤ θ˙ 1 X˙ ⎣ Y˙ ⎦ = J ⎣ θ˙ 2 ⎦ (8.556) d˙3 ϕ˙ ⎡

⎤ −l1 sθ 1 − d3 s (θ 1 + θ 2 ) −d3 s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) J = ⎣ −l1 cθ 1 + d3 c (θ 1 + θ 2 ) d3 c (θ 1 + θ 2 ) s (θ 1 + θ 2 ) ⎦ 1 1 0

(8.557)

(e) Determine J−1 and solve the inverse velocity kinematics. 8. A RP R planar redundant manipulator. (a) Figure 8.15 illustrates a 3 DOF planar manipulator with joint variables θ 1 , d2 , and θ 2 . (b) Determine the link transformation matrices and calculate 0 T3 . (c) Solve the inverse kinematics of the manipulator for a given values of X, Y , ϕ, where X, Y are global coordinates of the end-effector frame B3 , and ϕ is the angular coordinate of B3 . (d) Determine the Jacobian matrix of the manipulator to solve the forward velocity kinematics. ⎡

⎤ ⎡ ⎤ θ˙ 1 X˙ ⎣ Y˙ ⎦ = J ⎣ θ˙ 2 ⎦ d˙3 ϕ˙ (e) Determine J−1 and solve the inverse velocity kinematics.

(8.558)

Exercises

483

Fig. 8.16 An offset articulated manipulator

9. An offset articulated manipulator. Figure 8.16 illustrates an offset articulated manipulator. (a) Determine the forward kinematics of the manipulator. (b) Determine the global coordinates of the tip point P . (c) Determine the Jacobian of the manipulator, using direct differentiating. (d) Determine the Jacobian of the manipulator, using generating vectors. (e) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics. 10. Articulated robots. Attach the spherical wrist of Exercise 19 to the articulated manipulator of Fig. 8.16 and make a 6 DOF articulated robot. Determine the Jacobian of the robot, using generating vectors. 11. Spherical robots. Attach the spherical wrist of Exercise 19 to the spherical manipulator of Exercise 15 and make a 6 DOF spherical robot. Determine the Jacobian of the robot, using generating vectors. 12. Cylindrical robots. Attach the spherical wrist of Exercise 19 to the cylindrical manipulator of Exercise 17 and make a 6 DOF cylindrical robot. Determine the Jacobian of the robot, using generating vectors. 13. SCARA robot inverse velocity kinematics. Figure 8.17 illustrates a SCARA robot. Assume θ 3 = 0. (a) Determine the coordinates of the origin of B4 in G ≡ B0 . (b) Determine the Jacobian of the manipulator, using direct differentiating. (c) Determine the Jacobian of the manipulator, using generating vectors. (d) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics. 14.  SCARA robot with B0 on the ground. Figure 8.18 illustrates a SCARA robot. Assume θ 3 = 0. (a) Determine the coordinates of the origin of B4 in G ≡ B0 . (b) Determine the Jacobian of the manipulator, using direct differentiating. (c) Determine the Jacobian of the manipulator, using generating vectors. (d) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics. 15. Rigid link velocity. Figure 8.19 illustrates the coordinate frames and velocity vectors of a rigid link (i). Find (a) velocity 0 vi of the link at Ci in terms of d˙ i and d˙ i−1 . (b) angular velocity of the link 0 ωi in terms of d˙ i and d˙ i−1 . (c) velocity 0 vi of the link at Ci in terms of proximal joint i velocity. (d) velocity 0 vi of the link at Ci in terms of distal joint i + 1 velocity. (e) velocity of proximal joint i in terms of distal joint i + 1 velocity. (f) velocity of distal joint i + 1 in terms of proximal joint i velocity.

484

8 Velocity Kinematics

Fig. 8.17 A SCARA robot

Fig. 8.18 A SCARA robot with B0 on the ground

16.  Spherical robot velocity kinematics. A spherical manipulator RRP, equipped with a spherical wrist, is shown in Fig. 5.50. The transformation matrices of the robot are given in Example 195. Find the Jacobian matrix of the robot. 17.  Space station remote manipulator system velocity kinematics. The transformation matrices for the shuttle remote manipulator system (SRMS), shown in Fig. 5.24, are given in the Example 182. Solve the velocity kinematics of the SRMS by calculating the Jacobian matrix. 18. LU factorization method. Use the LU factorization method and find the associated [L] and [U ] for the following matrices. (a) ⎡ ⎤ 148 (8.559) [A] = ⎣ 5 2 7 ⎦ 963

Exercises

485

Fig. 8.19 Rigid link velocity vectors

(b)

⎡

⎤ 2 −1 3 −3 ⎢ 1 3 −1 −2 ⎥ ⎥ [B] = ⎢ ⎣0 2 2 4 ⎦ 3 1 5 −2

(c)

(8.560)

⎡

⎤ −2 −1 3 −3 6 ⎢ 1 3 −1 −2 0 ⎥ ⎢ ⎥ ⎥ [C] = ⎢ ⎢ 1 2 2 4 −2 ⎥ ⎣ 3 1 5 −2 −1 ⎦ 7 −5 2 1 1

(8.561)

19. LU inversion method. Use the LU inversion method and find the inverse of the matrices in Exercises 18. 20. LU calculations. Use the LU inversion method and calculate the inversion of the following matrices based on the matrices in Exercises 18. D = AB

E = AB −1

F = A−1 B

G = A−1 B −1

(8.562)

21. A set of liner equations. Use the LU factorization method and solve the following set of equations and show that the solutions are x1 = 4, x2 = 1, x3 = 2. − 3x1 + 8x2 + 5x3 = 6 2x1 − 7x2 + 4x3 = 9

(8.563)

x1 + 9x2 − 6x3 = 1 22. A set of six equations. Use the LU factorization method and solve the following set of equations and show that the solutions are x1 = 75, x2 = 52, x3 = 40, x4 = 31, x5 = 22, x6 = 10. 11x1 − 5x2 − x6 = 500 −20x1 + 41x2 − 15x3 − 6x5 = 0

486

8 Velocity Kinematics

−3x2 + 7x3 − 4x4 = 0 −x3 + 2x4 − x5 = 0

(8.564)

−2x1 − 15x5 + 47x6 = 0 −3x2 − 10x4 + 28x5 − 15x6 = 0 23. A set of nonlinear equations. Solve the following set of equations. x1 x2 − 2x1 − x2 = 0 x12 x2

− 2x1 x2 + x2 − 2x12 + 4x1 = 2

(8.565)

24.  Gaussian elimination method. There are two situations where the Gaussian elimination method fails: division by zero and round-off errors. Examine the LU factorization method for the possibility of division by zero. 25.  Number of subtractions as a source of round-off error. Round-off error is common in numerical techniques; however, it increases by increasing the number of subtractions. Apply the Gaussian elimination and LU factorization methods for solving a set of four equations ⎡

⎤⎡ ⎤ ⎡ ⎤ 2 1 3 −3 x1 1 ⎢ 1 0 −1 −2 ⎥ ⎢ x2 ⎥ ⎢ 2 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ 0 2 2 1 ⎦ ⎣ x3 ⎦ = ⎣ 0 ⎦ 3 1 0 −2 −2 x4

(8.566)

and count the number of subtractions in each method. 26.  Jacobian matrix from transformation matrices. Use the Jacobian matrix technique from links’ transformation matrices and find the Jacobian matrix of the RRR planar manipulator shown in Fig. 5.21. Choose a set of sample data for the dimensions and kinematics of the manipulator and find the inverse of the Jacobian matrix. 27.  Jacobian matrix for a spherical wrist. Use the Jacobian matrix technique from links’ transformation matrices and find the Jacobian matrix of the spherical wrist shown in Fig. 5.30. Assume that the frame B3 is the base frame. 28. Jacobian matrix for a SCARA manipulator. Use the Jacobian matrix technique from links’ transformation matrices and find the Jacobian matrix of the RRRP robot shown in Fig. 5.23. Assume θ 3 = 0. 29. Jacobian matrix for an RRR articulated manipulator. Figure 5.22 illustrates a 3 DOF RRR manipulator. Use the Jacobian matrix technique from links’ transformation matrices and find the Jacobian matrix for the manipulator. 30.  Partitioning inverse method. Calculate the matrix inversion for the matrices in Exercise 18 using the partitioning inverse method. 31.  Analytic matrix inversion. Use the analytic and LU factorization methods and find the inverse of [A] or an arbitrary 3 × 3 matrix. ⎡

⎤ 148 [A] = ⎣ 5 2 7 ⎦ 963 Count and compare the number of arithmetic operations.

(8.567)

Exercises

487

Fig. 8.20 A planar 3R manipulator with one DOF redundancy

32.  Cayley–Hamilton matrix inversion. Use the Cayley–Hamilton and LU factorization methods and find the inverse of [A] or an arbitrary 3 × 3 matrix. ⎡

⎤ 148 [A] = ⎣ 5 2 7 ⎦ 963

(8.568)

Count and compare the number of arithmetic operations. 33.  Norms of matrices. Calculate the following norms of the matrices in Exercise 18. A1 = Max

1≤j ≤n

n 



aij

  A2 = λMax AT A A∞ = Max

1≤i≤n

AF =

(8.569)

i=1

n 

(8.570)



aij

(8.571)

aij2

(8.572)

j =1

n n   i=1 j =1

34.  Project. Redundant manipulator and extra condition. Figure 8.20 illustrates a planar 3R manipulator with one DOF redundancy. Assume the length of the links is li = 2/3, and the end point is moving from (1.2, 1.5) to (−1.2, 1.5) on a straight lime according to the following time functions. X = 1 − 6t 2 + 4t 3

Y = 1.5

(a) Solve the inverse kinematics if always θ˙ 3 = 2θ˙ 2 . Plot θ˙ i for the whole trip. (b) Solve the inverse kinematics if always θ˙ 3 = θ˙ 1 − θ˙ 2 . Plot θ˙ i for the whole trip.

(8.573)

9

Acceleration Kinematics

Acceleration kinematics of robots is divided into forward and inverse acceleration kinematics. To study and develop the acceleration kinematics of robots, we need to introduce the concept of angular acceleration and rigid body acceleration. Links of a robot are considered rigid bodies in relative motion. They will have rotational and translational accelerations. Angular acceleration of a rigid body with respect to a global frame is the time derivative of instantaneous angular velocity of the body. In general, it is a vectorial quantity that is in a different direction than angular velocity. In this chapter we learn acceleration kinematics of robots.

Fig. 9.1 A rotating rigid body B(Oxyz) with a fixed point O in a reference frame G(OXY Z)

9.1

Angular Acceleration Vector and Matrix

Consider a rotating rigid body B(Oxyz) with a fixed point O in a reference frame G(OXY Z) as shown in Fig. 9.1. The velocity vector of a fixed point in the body frame is G

v(t) =

G

r˙ (t) =

˜B Gω

G

r(t) =

G ωB

×

G

r(t)

(9.1)

This equation can be utilized to find the acceleration vector of a body point at G r(t). G

a= =

G

v˙ =

GαB

G

dG r˙ (t) = dt

×

G

r+

G ωB

G

r¨ =

×



G SB

G ωB

G

×

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_9

(9.2)

r G

 r

(9.3)

489

490

9 Acceleration Kinematics



=

˜B Gα

+



˜ 2B Gω

 G

r=



·

˜B Gω

+

˜ 2B Gω

G

r

" # · 2 2 G ¨ ˙ ˙ = φ u˜ + φ u˜ + φ u˜ r   = φ¨ uˆ + φ˙ u˙ × = GαB

G

R¨ B

G

G

(9.4) (9.5)

 2 r + φ˙ uˆ × uˆ ×

G

 r

RBT G r

(9.6) (9.7)

is the angular acceleration vector of the body point with respect to and expressed in the G-frame. GαB

=

˜B Gα

=

G

d G ωB dt

(9.8)

·

˜B Gω

(9.9)

The acceleration G r¨ can also be defined using the angular acceleration matrix G α˜ B and rotational acceleration transformation G SB   G (9.10) r¨ = G SB G r = G α˜ B + G ω˜ 2B G r where G α˜ B is the angular acceleration matrix, ˜B Gα

=

·

˜B Gω

=

· d˙ T Ru,φ = φ¨ u˜ + φ˙ u˜ ˆ Ru,φ ˆ dt

G

(9.11)

and G SB is the rotational acceleration transformation. G SB

=

G

R¨ B G RBT =

˜B Gα

+

˜ 2B Gω

=

˜B Gα

−

˜ B G ω˜ TB Gω

·

2 = φ¨ u˜ + φ˙ u˜ + φ˙ u˜ u˜

(9.12)

The angular velocity vector G ωB and matrix G ω˜ B are ˜B Gω

=

R˙ B G RBT

(9.13)

G ωB

= φ˙ uˆ = φ˙ uˆ ω

(9.14)

G

The relative angular acceleration of two bodies B1 , B2 in the global frame G can be combined. Gα2

=

G S2

=

G

d G ω2 = dt

G S1

+

G 1 S2

Gα1

+

G 1 α2

(9.15)

+ 2 G ω˜ 1 G ˜2 1ω

(9.16)

B

(9.17)

The B-expression of G a and G SB are B Ga

=

B GαB

× Br +

B G ωB

×

B G SB

=

B

RG G R¨ B =

B ˜B Gα

+

G ωB

B 2 ˜B Gω

 × Br

(9.18)

9.1 Angular Acceleration Vector and Matrix

491

The global and body expressions of the rotational acceleration transformations G SB and BG SB can be transformed to each other by the following rules: G SB

=

G

RB

B G T G S B RB

(9.19)

B G SB

=

G

RBT G SB G RB

(9.20)

Proof The global position and velocity vectors of a body point are r=

G

RB B r

v=

G

r˙ =

G G

G

(9.21) R˙ B B r =

˜B Gω

G

r=

×

G ωB

G

r

(9.22)

Differentiating Equation (9.22) gives G

a= =

G

r¨ =

˙B Gω

×

GαB

G

×

r+

G

r+

G ωB

G ωB

×



×

G ωB

G

r˙

×

G

 r

(9.23)

Employing the axis–angle expression of angular velocity, ⎡

⎤ ⎡ ⎤ u1 ω1 ω = φ˙ uˆ = φ˙ ⎣ u2 ⎦ = ⎣ ω2 ⎦ u3 ω3 ⎡ ⎤ ⎡ ⎤ 0 −u3 u2 0 −ω3 ω2 ω˜ = φ˙ u˜ = φ˙ ⎣ u3 0 −u1 ⎦ = ⎣ ω3 0 −ω1 ⎦ −u2 u1 0 −ω2 ω1 0

(9.24)

(9.25) we find the angular acceleration vector α and matrix α˜ in terms of the instantaneous axis and angle of rotation: α = ω˙ = φ¨ uˆ + φ˙ u˙ ·

(9.26)

·

α˜ = ω˜ = φ¨ u˜ + φ˙ u˜

(9.27)

We may substitute the matrix expressions of angular velocity and acceleration in (9.23) to derive Eq. (9.6). G

r¨ =

GαB

×

  r + G ωB × G ωB × G r     2 = φ¨ uˆ + φ˙ u˙ × G r + φ˙ uˆ × uˆ × G r

(9.28)

=

˜B Gα

G

(9.29)

=



˜B Gα

G

r+

+

˜ B G ω˜ B Gω

˜ 2B Gω



G

r=

G

r "

·

˜B Gω

# +

˜ 2B Gω

" # · 2 2 G ¨ ˙ ˙ = φ u˜ + φ u˜ + φ u˜ r = G SB G r

G

r

(9.30) (9.31)

Recalling that ˜B Gω G

=

r˙ (t) =

G

R˙ B G RBT

˜B Gω

G

r(t)

(9.32) (9.33)

492

9 Acceleration Kinematics

we find Eqs. (9.7) and (9.11): G

d G ˙ G T G  RB RB r dt    = G R¨ B G RBT G r + G R˙ B G R˙ BT G r + G R˙ B G RBT G R˙ B G RBT G r   2  G r = G R¨ B G RBT + G R˙ B G R˙ BT + G R˙ B G RBT   2  2  G = G R¨ B G RBT − G R˙ B G RBT + G R˙ B G RBT r

r¨ =

=

G

G

R¨ B G RBT G r

˜B Gα

=

·

˜B Gω

(9.34) =

G

R¨ B

G

RBT +

=

G

R¨ B

G

RBT +

G

=

G

R¨ B

G

RBT +

˜ B G ω˜ TB Gω

R˙ B

G

R˙ BT

RBT G RB G R˙ BT  T  = G R¨ B G RBT + G R˙ B G RBT G R˙ B G RBT

which indicates that G

R¨ B G RBT =

R˙ B

G

G

˜B Gα

+

=

G

˜ 2B Gω

R¨ B =

G

RBT −

˜ 2B Gω

G SB

(9.35)

(9.36)

The expanded forms of the angular accelerations G α B , G α˜ B and rotational acceleration transformation G SB are ⎤ 0 −ω˙ 3 ω˙ 2 = φ¨ u˜ + φ˙ u˜ = ⎣ ω˙ 3 0 −ω˙ 1 ⎦ −ω˙ 2 ω˙ 1 0 ⎡

˜B Gα

·

·

= G ω˜ B

⎤ 0 −u˙ 3 φ˙ − u3 φ¨ u˙ 2 φ˙ + u2 φ¨ = ⎣ u˙ 3 φ˙ + u3 φ¨ 0 −u˙ 1 φ˙ − u1 φ¨ ⎦ 0 −u˙ 2 φ˙ − u2 φ¨ u˙ 1 φ˙ + u1 φ¨ ⎡

⎤ ⎡ ⎤ u˙ 1 φ˙ + u1 φ¨ ω˙ 1 ⎣ ω˙ 2 ⎦ = ⎣ u˙ 2 φ˙ + u2 φ¨ ⎦ GαB = ω˙ 3 u˙ 3 φ˙ + u3 φ¨

(9.37)

⎡

G SB

G SB

=

·

˜B Gω

⎡

+

˜ 2B Gω

=

˜B Gα

+

(9.38)

˜ 2B Gω

⎤ −ω22 − ω23 ω1 ω2 − ω˙ 3 ω˙ 2 + ω1 ω3 = ⎣ ω˙ 3 + ω1 ω2 −ω21 − ω23 ω2 ω3 − ω˙ 1 ⎦ ω1 ω3 − ω˙ 2 ω˙ 1 + ω2 ω3 −ω21 − ω22

(9.39)

·

2 = φ¨ u˜ + φ˙ u˜ + φ˙ u˜ 2 ⎡  2 −φ˙ u22 + u23 ⎢ = ⎣ u1 u2 φ˙ 2 + u˙ 3 φ˙ + u3 φ¨ 2 u1 u3 φ˙ − u˙ 2 φ˙ − u2 φ¨ ⎡ 2 (u21 − 1)φ˙ ⎢ 2 = ⎣ u1 u2 φ˙ + u˙ 3 φ˙ + u3 φ¨ 2 u1 u3 φ˙ − u˙ 2 φ˙ − u2 φ¨

⎤ 2 2 u1 u2 φ˙ − u˙ 3 φ˙ − u3 φ¨ u1 u3 φ˙ + u˙ 2 φ˙ + u2 φ¨  ⎥ 2 2 −φ˙ u21 + u23 u2 u3 φ˙ − u˙ 1 φ˙ − u1 φ¨ ⎦  2 2 2 u2 u3 φ˙ − u˙ 1 φ˙ + u1 φ¨ −φ˙ u1 + u22 ⎤ 2 2 u1 u2 φ˙ − u˙ 3 φ˙ − u3 φ¨ u1 u3 φ˙ + u˙ 2 φ˙ + u2 φ¨ ⎥ 2 2 (u22 − 1)φ˙ u2 u3 φ˙ − u˙ 1 φ˙ − u1 φ¨ ⎦ 2 2 u2 u3 φ˙ − u˙ 1 φ˙ + u1 φ¨ (u23 − 1)φ˙

(9.40)

9.1 Angular Acceleration Vector and Matrix

493

Therefore, the position, velocity, and acceleration vectors of a body point are rP = xˆı + y jˆ + zkˆ

B

G

G

vP =

aP =

G

G

(9.41)

G

dG rP = dt

r˙ P =

v˙ P =

G

r¨ P =

G ωB

×

G

(9.42)

r

G 2

d dt 2

G

rP r˙

=

GαB

×

G

r+

G ωB

×

=

GαB

×

G

r+

G ωB

× ( G ωB ×

G

G

r)

(9.43)

The angular acceleration expressed in the body frame is the body derivative of the angular velocity vector. To show this, we use the derivative transport formula (7.214). B GαB

= =

G

B dB dB ω = ωB + B G dt dt G

B G ωB

×

B G ωB

=

B

dB ωB dt G

B ˙B Gω

(9.44)

The angular acceleration of B in G can always be expressed in the form GαB

=

GαB

uˆ α

(9.45)

where uˆ α is a unit vector parallel to G α B . The angular velocity and angular acceleration vectors are not parallel in general, and therefore, uˆ α = uˆ ω

(9.46)

=

(9.47)

GαB

˙B Gω

However, the only special case is when the axis of rotation is fixed in both G and B frames, uˆ = uˆ α = uˆ ω . GαB

= α uˆ = ω˙ uˆ = φ¨ uˆ

(9.48)

The angular velocity of several bodies rotating relative to each other are related according to relative velocity equation: 0 ωn

= 0 ω1 + 01 ω2 + 02 ω3 + · · · +

0 n−1 ωn

(9.49)

The angular accelerations of several relatively rotating rigid bodies follow the same rule: 0αn

= 0 α 1 + 01 α 2 + 02 α 3 + · · · +

0 n−1 α n

(9.50)

To show this fact and develop the relative acceleration formula, we consider two relatively rotating rigid links, B1 , B2 in a base coordinate frame B0 with a fixed point at O. The angular velocities of the links are related by 0 ω2

= 0 ω1 + 01 ω2

(9.51)

Their angular accelerations are 0α1

=

0

d 0 ω1 dt

(9.52)

494

9 Acceleration Kinematics

0α2

=

0

d 0 0 ω2 = 0 α 1 + 1 α 2 dt

(9.53)

and therefore, 0 S2

= 0 α˜ 2 + 0 ω˜ 22 = 0 α˜ 1 + 01 α˜ 2 +



˜1 0ω

+ 01 ω˜ 2

2

= 0 α˜ 1 + 01 α˜ 2 + 0 ω˜ 21 + 01 ω˜ 22 + 2 0 ω˜ 1 01 ω˜ 2 = 0 S1 + 01 S2 + 2 0 ω˜ 1 01 ω˜ 2

(9.54)

Equation (9.54) is the required relative acceleration transformation formula. It indicates the method of calculation of relative accelerations for a multibody. As a more general case, consider a six-link multibody. The angular acceleration of link (6) in the base frame would be 0 S6

= 0 S1 + 01 S2 + 02 S3 + 03 S4 + 04 S5 + 05 S6   +2 0 ω˜ 1 01 ω˜ 2 + 02 ω˜ 3 + 03 ω˜ 4 + 04 ω˜ 5 + 05 ω˜ 6   +2 01 ω˜ 2 02 ω˜ 3 + 03 ω˜ 4 + 04 ω˜ 5 + 05 ω˜ 6 .. . +2 04 ω˜ 5

0

˜6 5ω



(9.55)

We can transform the G and B-expression of the global acceleration of a body point P to each other using a rotation matrix: B G aP

=

B

RG G a P =

RG G S B

G

rP =

B

RG G S B

G

RB B rP

RG G R¨ B B rP  = G RBT G R¨ B B rP = BG SB B rP = BG α˜ B + BG ω˜ 2B B rP   = BG α B × B r + BG ωB × BG ωB × B r =

G

aP =

B

G

RG G R¨ B

B

RB

B G aP

G

=

RBT

G

G

RB

RB B rP =

B B G SB rP

B

=

G

RB

B G T G G SB RB rP

R¨ B G RBT G rP = G R¨ B G RBT G rP  = G SB G rP = G α˜ B + G ω˜ 2B G r   = G α B × G r + G ωB × G ωB × G r =

G

RB

G

RBT

(9.56)

G

(9.57)

From the definitions of G SB and BG SB in (9.12) and (9.18) and comparing with (9.56) and (9.57), we are able to transform the two rotational acceleration transformations by G SB

=

G

RB

B G T G S B RB

B G SB

=

G

RBT

G SB

G

RB

(9.59)

G

R¨ B =

G SB

G

RB

(9.60)

G

R¨ B =

G

RB

B G SB

(9.61)

G

RB =

G

RB BG SB

(9.62)

(9.58)

and derive the following useful equations:

G SB

9.1 Angular Acceleration Vector and Matrix

495

Fig. 9.2 A simple pendulum

The angular acceleration of B in G is negative of the angular acceleration of G in B if both are expressed in the same coordinate frame: ˜B Gα

= −G ˜G Bα

GαB

= −G B αG

(9.63)

B ˜B Gα

= − B α˜ G

B GαB

= − B αG

(9.64)

The term G α B × G r in (9.43) is called the tangential acceleration, which is a function of the angular acceleration of B in G. The term G ωB × G ωB × G r in G a is called centripetal acceleration and is a function of the angular velocity of B in G. Using the Rodriguez rotation formula, it can also be shown that ˜B Gα

·

d ˙ T Ru,φ ˆ Ru,φ ˆ dt   T T ˙ˆ ˙ ˆ RT Ru,φ Ru,φ ˆ − Ru,φ ˆ Ru,φ u,φ ˆ    T T ˙ ˆ RT R˙ u,φ Ru,φ ˆ Ru,φ ˆ − Ru,φ u,φ ˆ ˆ

= G ω˜ B = = R¨ u,φ ˆ = R¨ u,φ ˆ

G

T = R¨ u,φ ˆ Ru,φ ˆ −

˜ 2B Gω

·

(9.65) (9.66) (9.67)

= φ¨ u˜ + φ˙ u˜

(9.68)

R˙ u,φ = φ˙ u˜ Ru,φ ˆ ˆ

(9.69)

because

R¨ u,φ = ˆ

 d  φ˙ u˜ Ru,φ ˆ dt

G

·

˙ ˜ u,φ ˙ ˜ R˙ u,φ = φ¨ u˜ Ru,φ ˆ + φ uR ˆ +φu ˆ " # · 2 = φ¨ u˜ + φ˙ u˜ + φ˙ u˜ u˜ Ru,φ ˆ

(9.70) 

Example 295 Velocity and acceleration of a simple pendulum. Pendulum is a good example to examine acceleration of a point mass in a rotating coordinate frame. This is a good example to show the components of acceleration vector in body and global frames. Tangential and centripetal accelerations are only B-frame observation. A point mass attached to a massless rod and hanging from a revolute joint is called a simple pendulum. Figure 9.2 illustrates a simple pendulum. A local coordinate frame B is attached to the pendulum that rotates in a global frame G. The position vector of the bob and the angular velocity vector G ωB are

496

9 Acceleration Kinematics B

G

r = lˆı r=

G

⎡

l sin φ RB B r = ⎣ −l cos φ ⎦ 0

B G ωB

= φ˙ kˆ

G ωB

=

G

(9.71)

⎤

(9.72) (9.73)

RBT

B G ωB

= φ˙ Kˆ

(9.74)

where,     ⎤ cos  32 π + φ − sin 32 π + φ 0 G RB = ⎣ sin 32 π + φ cos 32 π + φ 0 ⎦ 0 0 1 ⎡ ⎤ sin φ cos φ 0 = ⎣ − cos φ sin φ 0 ⎦ 0 0 1 ⎡

(9.75)

Its velocity is = 0 + φ˙ kˆ × lˆı = l φ˙ jˆ ⎤ l φ˙ cos φ G v = G RB B v = ⎣ l φ˙ sin φ ⎦ 0 B Gv

=

B

r˙ +

B G ωB

× ⎡

B Gr

(9.76) (9.77)

The acceleration of the bob is then equal to B Ga

=

B ˙ Gv

+

B G ωB

×

B Gv

= l φ¨ jˆ + φ˙ kˆ × l φ˙ jˆ = l φ¨ jˆ − l φ˙ ıˆ ⎡ ⎤ 2 l φ¨ cos φ − l φ˙ sin φ ⎢ ⎥ G a = G RB B a = ⎣ l φ¨ sin φ + l φ˙ 2 cos φ ⎦ 0 2

(9.78) (9.79)

Example 296 Rotation of a body point about a global axis. A numerical example of how to determine acceleration of a body point while the body is rotating in a global frame. Consider a turning rigid body about the Z-axis with a constant angular acceleration α¨ = 2 rad/s2 . The global acceleration of a body point at P (5, 30, 10) cm when the body is at α˙ = 10 rad/s and α = 30 deg is G

aP =

G

⎡

R¨ B (t) B rP

⎤⎡

⎤

⎡

⎤

(9.80)

−87.6 48.27 0 5 1010 = ⎣ −48.27 −87.6 0 ⎦ ⎣ 30 ⎦ = ⎣ −2869.4 ⎦ cm/s 0 0 0 10 0 where, G

G 2 G G d G dG dG d RB = α˙ RB = α¨ RB + α˙ 2 2 G RB 2 dt dα dα dα ⎡ ⎤ ⎡ ⎤ − sin α − cos α 0 − cos α sin α 0 = α¨ ⎣ cos α − sin α 0⎦ + α˙ 2 ⎣ − sin α − cos α 0⎦ 0 0 0 0 0 0

R¨ B =

G 2

(9.81)

9.1 Angular Acceleration Vector and Matrix

497

Fig. 9.3 Motion of a vehicle at latitude 30 deg and heading north on the Earth

At this moment, the point P is at G

rP =

RB B rP π ⎤ π cos − sin 0 ⎡ 5 ⎤ ⎡ −10.67 ⎤ ⎢ π6 ⎥ π6 ⎥ =⎢ cos 0 ⎦ ⎣ 30 ⎦ = ⎣ 28.48 ⎦ cm ⎣ sin 6 6 10 10 0 0 1 G

⎡

(9.82)

 T What if the body point P was at B rP = 5 30 10 cm while the body was turning with a constant angular acceleration α¨ = 2 rad/s2 about the Z-axis? Assume the body frame is at α = 30 deg, its angular speed α˙ = 10 deg /s. The transformation matrix G RB between the B and G-frames will be ⎡

π ⎤ ⎡ π ⎤ − sin 0 0.866 −0.5 0 6 6 ⎢ ⎥ π π G ⎥ RB = ⎢ cos 0 ⎦ ≈ ⎣ 0.5 0.866 0 ⎦ ⎣ sin 6 6 0 0 1 0 0 1 cos

(9.83)

and therefore, the acceleration of point P is ⎡

⎤ 1010 G aP = G R¨ B G RBT G rP = ⎣ −2869.4 ⎦ cm/s2 0 where

G 2

d dt 2

is the same as (9.81).

G

RB = α¨

G

d dα

G

RB − α˙ 2

(9.84)

G 2

d dα 2

G

RB

(9.85)

498

9 Acceleration Kinematics

Example 297 Motion of a vehicle on the Earth. A practical example to show acceleration of a moving particle in a rotating coordinate frame. It also indicates relative order of magnitude of different terms of acceleration on Earth. Consider a moving vehicle on the Earth at latitude 30 deg heading north, as shown in Fig. 9.3. The vehicle has a velocity km/h = 22.22 m/s and acceleration a = BE r¨ = 0.1 m/s2 , both with respect to the road expressed in the of v = BE r˙ = 80  vehicle B-frame, ıˆ, jˆ, kˆ . Radius of the Earth is R, and hence, the vehicle’s kinematics are = R kˆ m v θ˙ = rad/s R

B ˙ Er

= 22.22ˆı m/s a θ¨ = rad/s2 R

B Er

B ¨ Er

= 0.1ˆı m/s2 (9.86)

There are three coordinate frames involved. A body coordinate frame B is attached to the vehicle. A global motionless coordinate G is set up at the center of the Earth. Another local coordinate frame E is rigidly attached to the Earth and turns with the Earth. The frames E and G are assumed coincident at the moment. The angular velocity of B is    B B G B ˆ ˙ˆ G ω B = RG G ω E + E ω B = RG ω E K + θ I = (ωE cos θ ) ıˆ + (ωE sin θ) kˆ + θ˙ jˆ v = (ωE cos θ ) ıˆ + (ωE sin θ) kˆ + jˆ R

(9.87)

Therefore, the velocity and acceleration of the vehicle are B Gv

=

B Ga

=

B

r˙ +

B ˙ Gv

B G ωB

+

×

B G ωB

B Gr

×

=0+

B Gv

B G ωB

× R kˆ = vˆı − (RωE cos θ ) jˆ

⎤ ⎡ ⎤ ωE cos θ v = aˆı + RωE θ˙ sin θ jˆ + ⎣ v/R ⎦ × ⎣ −RωE cos θ ⎦ 0 ωE sin θ ⎤ ⎡ Rω2E cos θ sin θ   ⎥ ⎢ vωE sin θ = aˆı + RωE θ˙ sin θ jˆ + ⎣ ⎦ 1 2 − v − Rω2E cos2 θ R ⎡ ⎤ a + Rω2E cos θ sin θ ⎢ ⎥ 2RωE θ˙ sin θ =⎣ ⎦ 1 2 − v − Rω2E cos2 θ R 



(9.88)

⎡

(9.89)

2 The term aˆı is the acceleration of B relative to Earth, (2RωE θ˙ sin θ)jˆ is the Coriolis acceleration, − vR kˆ is the centripetal acceleration due to traveling, and −Rω2E is the centripetal acceleration due to Earth’s rotation. Substituting the numerical values and accepting R ≈ 6.3677 × 106 m yield

 B Gv

= 22.22ˆı − 6.3677 × 10

6

2π 366.25 π cos jˆ 24 × 3600 365.25 6

= 22.22ˆı − 402.13jˆ m/s B Ga

= 1.5662 × 10−2 ıˆ + 1.6203 × 10−3 jˆ − 2.5473 × 10−2 kˆ m/s2

(9.90) (9.91)

9.1 Angular Acceleration Vector and Matrix

499

Example 298  Combination of rotational acceleration transformation. Here, relative angular acceleration and relative rotational acceleration transformation equations are being derived analytically. Let us consider a pair of relatively rotating rigid links in a base coordinate frame B0 with a fixed point at O. The angular velocities of the links are related as 0 ω2

= 0 ω1 + 01 ω2

(9.92)

˜2 0ω

= 0 ω˜ 1 + 01 ω˜ 2

(9.93)

So, their angular accelerations are 0

d 0 ω1 dt

0α1

=

0α2

=

˜2 0α

= 0 α˜ 1 + 01 α˜ 2

(9.94)

0

d 0 0 ω2 = 0 α 1 + 1 α 2 dt

(9.95) (9.96)

Similarly we have 0 S1

= 0 α˜ 1 + 0 ω˜ 21

(9.97)

0 S2

˜ 22 0ω

(9.98)

= 0 α˜ 2 +

and therefore, 0 S2

= 0 α˜ 2 + 0 ω˜ 22 = 0 α˜ 1 + 01 α˜ 2 +



˜1 0ω

+ 01 ω˜ 2

2

= 0 α˜ 1 + 01 α˜ 2 + 0 ω˜ 21 + 01 ω˜ 22 + 2 0 ω˜ 1 01 ω˜ 2 = 0 S1 + 01 S2 + 2 0 ω˜ 1 01 ω˜ 2

(9.99)

and 0 S2

=

0 S1

+ 01 S2

(9.100)

Equation (9.99) is the relative rotational acceleration transformation equation. It expresses the relative accelerations for a multi-link robot. As an example, consider a 6R articulated robot with six revolute joints. The angular acceleration of the end-effector frame in the base frame would be 0α6

0 S6

= 0 α 1 + 01 α 2 + 02 α 3 + 03 α 4 + 04 α 5 + 05 α 6

(9.101)

= 0 S1 + 01 S2 + 02 S3 + 03 S4 + 04 S5 + 05 S6   +2 0 ω˜ 1 01 ω˜ 2 + 02 ω˜ 3 + 03 ω˜ 4 + 04 ω˜ 5 + 05 ω˜ 6   +2 01 ω˜ 2 02 ω˜ 3 + 03 ω˜ 4 + 04 ω˜ 5 + 05 ω˜ 6 .. . +2 04 ω˜ 5

0

˜6 5ω



(9.102)

We can transform the G-expression and B-expression of the global acceleration of a body point P , to each other using a rotation matrix. B G aP

=

B

RG G a P =

RG G S B

G

rP =

B

RG G S B

G

RB B rP

RG G R¨ B B rP  = G RBT G R¨ B B rP = BG SB B rP = BG α˜ B + BG ω˜ 2B B rP   = BG α B × B r + BG ωB × BG ωB × B r =

B

RG G R¨ B

B

G

RBT

G

RB B rP =

B

(9.103)

500

9 Acceleration Kinematics G

aP =

G

=

B G aP

RB

G

RB

B B G SB rP

=

G

RB

B G T G G SB RB rP

R¨ B G RBT G rP = G R¨ B G RBT G rP  = G SB G rP = G α˜ B + G ω˜ 2B G r   = G α B × G r + G ωB × G ωB × G r =

G

G

RB

RBT

G

(9.104)

G the tangential acceleration, which is a function of the angular acceleration of B in G. The term The term  G α B × Gr is called G G ωB × G ωB × r in a is called the centripetal acceleration that is a function of the angular velocity of B in G. From the definitions of G SB and BG SB and comparing with (9.103) and (9.104), we can transform the two rotational acceleration transformations, G SB

=

G

RB

B G T G S B RB

B G SB

=

G

RBT

G SB

G

RB

(9.106)

G

R¨ B =

G SB

G

RB

(9.107)

G

R¨ B =

G

RB

B G SB

(9.108)

G

RB =

G

RB

B G SB

(9.109)

(9.105)

and derive the following equations:

G SB

The angular acceleration of B in G is negative of the angular acceleration of G in B if both are expressed in the same coordinate frame. ˜B Gα

= −G ˜G Bα

GαB

= −G B αG

(9.110)

B ˜B Gα

= − B α˜ G

B GαB

= − B αG

(9.111)

The relative angular acceleration formula, 0α2

= 0 α 1 + 01 α 2

0αn

= 0 α 1 + 01 α 2 + 02 α 3 + · · · +

(9.112) 0 n−1 α n

=

n 

0 i−1 α i

(9.113)

i=1

are correct if and only if all of the angular accelerations are expressed in the B0 -frame. Therefore, any equation of the form 0α2

= 0 α 1 + 1 α 2

(9.114)

α 0 = α 1 + α 2 0α3

(9.115)

= 0 α 1 + 0 α 2

(9.116)

is wrong or is not completely expressed. Example 299  Alternative proof of relative acceleration formula. Here is another approach to show the relative angular acceleration formula starting from rotational transformation matrix. To show addition of the relative angular accelerations in Eqs. (9.54) and (9.55), we may start from a combination of rotations, 0 R2 = 0 R1 1 R2 (9.117) and take their time derivatives. 0

R˙ 2 = 0 R˙ 1 1 R2 + 0 R1 1 R˙ 2

0

R¨ 2 = R¨ 1 R2 + 2 R˙ 1 R˙ 2 + R1 R¨ 2 0

1

0

1

(9.118) 0

1

(9.119)

9.1 Angular Acceleration Vector and Matrix

501

Substituting the derivatives of rotation matrices with R¨ 2 = 0 S2 0 R2

0

0

R˙ 2 = 0 ω˜ 2 0 R2

0

0

R2 = 0 S1 0 R1 1 R2 + 2 0 ω˜ 1 0 R1 1 ω˜ 2 1 R2 + 0 R1 1 S2 1 R2

0

R¨ 1 = 0 S1 0 R1

1

R¨ 2 = 1 S2 1 R2

(9.120)

R˙ 1 = 0 ω˜ 1 0 R1

1

R˙ 2 = 1 ω˜ 2 1 R2

(9.121)

yields 0 S2

= 0 S1 0 R2 + 2 0 ω˜ 1 0 R1 1 ω˜ 2 0 R1T 0 R1 1 R2 + 0 R1 1 S2 1 R2 = 0 S1 0 R2 + 2 0 ω˜ 1 01 ω˜ 2 0 R2 + 0 R1 1 S2 0 R1T 0 R1 1 R2 = 0 S1 0 R2 + 2 0 ω˜ 1 01 ω˜ 2 0 R2 + 01 S2 0 R2

(9.122)

= 0 S1 + 01 S2 + 2 0 ω˜ 1 01 ω˜ 2

(9.123)

+ 0 ω˜ 22 = 0 α˜ 1 + 0 ω˜ 21 + 01 α˜ 2 + 01 ω˜ 22 + 2 0 ω˜ 1 01 ω˜ 2

(9.124)

Therefore, we find 0 S2

which is equivalent to ˜2 0α Simplifying this equation shows that ˜2 0α

= 0 α˜ 1 + 01 α˜ 2 + 0 ω˜ 21 + 01 ω˜ 22 + 2 0 ω˜ 1 01 ω˜ 2 − 0 ω˜ 22  2 = 0 α˜ 1 + 01 α˜ 2 + 0 ω˜ 1 + 01 ω˜ 2 − 0 ω˜ 22 = 0 α˜ 1 + 01 α˜ 2 + 0 ω˜ 22 − 0 ω˜ 22 = 0 α˜ 1 + 01 α˜ 2

(9.125)

which indicates two angular accelerations may be added when they are expressed in the same frame: 0α2

= 0 α 1 + 01 α 2

(9.126)

Example 300  Angular acceleration and Euler angles. Expression of angular velocity and acceleration in terms of Euler angles and their time rate is practical method for rigid body dynamics. Here is how to derive angular acceleration. The angular velocity G ωB in terms of Euler angles is ⎤ ⎡ ⎤⎡ ⎤ 0 cos ϕ sin θ sin ϕ ϕ˙ ωX G ⎦ ⎣ ⎦ ⎣ ⎣ θ˙ ⎦ ωY = 0 sin ϕ − cos ϕ sin θ G ωB = ωZ 1 0 cos θ ψ˙ ⎡ ⎤ θ˙ cos ϕ + ψ˙ sin θ sin ϕ = ⎣ θ˙ sin ϕ − ψ˙ cos ϕ sin θ ⎦ ϕ˙ + ψ˙ cos θ ⎡

(9.127)

The angular acceleration is then equal to a simple derivative of G G ωB . G GαB

G

dG ωB dt G    ⎤ ⎡ cos ϕ  θ¨ + ϕ˙ ψ˙ sin θ + sin ϕ ψ¨ sin θ + θ˙ ψ˙ cos θ − θ˙ ϕ˙  = ⎣ sin ϕ θ¨ + ϕ˙ ψ˙ sin θ + cos ϕ θ˙ ϕ˙ − ψ¨ sin θ − θ˙ ψ˙ cos θ ⎦ ϕ¨ + ψ¨ cos θ − θ˙ ψ˙ sin θ =

(9.128)

502

9 Acceleration Kinematics

The angular acceleration vector in the body coordinate frame is then equal to B GαB

=

G

⎡

RBT

G GαB

(9.129)

⎤

cϕcψ − cθsϕsψ cψsϕ + cθ cϕsψ sθsψ = ⎣ −cϕsψ − cθ cψsϕ −sϕsψ + cθ cϕcψ sθ cψ ⎦ G GαB sθsϕ −cϕsθ cθ    ⎤ ⎡ cos ψ θ¨ + ϕ˙ ψ˙ sin θ + sin ψ ϕ¨ sin θ + θ˙ϕ˙ cos θ − θ˙ ψ˙  = ⎣ cos ψ ϕ¨ sin θ + θ˙ ϕ˙ cos θ − θ˙ ψ˙ − sin ψ θ¨ + ϕ˙ ψ˙ sin θ ⎦ ϕ¨ cos θ − ψ¨ − θ˙ ϕ˙ sin θ Example 301  B-expression of angular acceleration. Angular acceleration is a vectorial quantity and hence, it can be expressed in B or G frames. Here is to show how to express angular acceleration in B-frame. The angular acceleration expressed in the body frame is the body derivative of the angular velocity vector. To show this, we use the derivative transport formula (7.214): B GαB

= =

B ˙B Gω

=

G

dB ωB dt G

B

dB ωB + dt G

B G ωB

×

B G ωB

=

B

dB ωB dt G

(9.130)

Interestingly, the global and body derivatives of BG ωB are equal, because G ωB is about an axis uˆ that is instantaneously fixed in both, B and G. G B dB dB ωB = BG α B (9.131) G ωB = dt dt G A vector α can generally indicate the angular acceleration of a coordinate frame A with respect to another frame B. It can be expressed in or seen from a third coordinate frame C. We indicate the first coordinate frame A by a right subscript, the second frame B by a left subscript, and the third frame C by a left superscript, CB α A . If the left super and subscripts are the same, we only show the subscript. So, the angular acceleration of A with respect to B as seen from C is the C-expression of B αA: C C (9.132) B α A = RB B α A Transforming G a to the body frame provides the body expression of the acceleration vector: B G aP

=

G

RBT G a =

=

G

RBT G R¨ B B r

We denote the coefficient of B r by BG SB

G

RBT

B G SB

G SB

G

r=

G

RBT

G

R¨ B

G

RBT

G

r (9.133)

=

G

RBT

G

R¨ B

(9.134)

and rewrite Eq. (9.133) as B G aP

=

B B G SB rP

(9.135)

where, BG SB is the rotational acceleration transformation of the B-frame relative to G-frame as seen from the B-frame. Example 302 Principal angular accelerations. Angular acceleration for rotation about principal global axes is calculated here. The principal rotational matrices about the axes X, Y , and Z are given by RX,γ , RY,β , and RZ,α . Based on their time derivatives, the principal angular velocities about the axes X, Y , and Z are T ω˜ X = R˙ X,γ RX,γ = γ˙ I˜

ωX = ωX Iˆ = γ˙ Iˆ

(9.136)

T ω˜ Y = R˙ Y,β RY,β = β˙ J˜

ωY = ωY Jˆ = β˙ Jˆ

(9.137)

T = α˙ K˜ ω˜ Z = R˙ Z,α RZ,α

ωZ = ωZ Kˆ = α˙ Kˆ

(9.138)

9.1 Angular Acceleration Vector and Matrix

503

Taking another derivative shows that the principal angular accelerations about the axes X, Y , and Z are T T + R˙ X,γ R˙ X,γ = γ¨ I˜ α X = α X Iˆ = γ¨ Iˆ α˜ X = R¨ X,γ RX,γ

(9.139)

T T α˜ Y = R¨ Y,β RY,β + R˙ Y,β R˙ Y,β = β¨ J˜

(9.140)

T T α˜ Z = R¨ Z,α RZ,α + R˙ Z,α R˙ Z,α = α¨ K˜

α Y = α Y Jˆ = β¨ Jˆ α Z = α Z Kˆ = α¨ Kˆ

(9.141)

and therefore, T SX,γ¨ = R¨ X,γ RX,γ = α˜ X + ω˜ 2X = γ¨ I˜ + γ˙ 2 I˜I˜

(9.142)

2 T SY,β¨ = R¨ Y,β RY,β = α˜ Y + ω˜ 2Y = β¨ J˜ + β˙ J˜J˜

(9.143)

T SZ,α¨ = R¨ Z,α RZ,α = α˜ Z + ω˜ 2Z = α¨ K˜ + α˙ 2 K˜ K˜

(9.144)

 R¨ X,γ = γ¨ I˜ + γ˙ 2 I˜I˜ RX,γ  2 R¨ Y,β = β¨ J˜ + β˙ J˜J˜ RY,β  R¨ Z,α = α¨ K˜ + α˙ 2 K˜ K˜ RZ,α

(9.145) (9.146) (9.147)

Example 303  Alternative definition of angular acceleration vector. Angular acceleration may also be defined by orthogonality condition and time derivative of the unit vectors of B-frame. Similar to the definition of the angular velocity vector in Eq. (7.229), B G ωB

= ıˆ

  G G ˆ d jˆ ˆ dk d ıˆ ˆ · k + jˆ · ıˆ + k · jˆ dt dt dt

G

(9.148)

ˆ in the global frame G(Iˆ, Jˆ, K). ˆ we define the angular acceleration vector BG α B of a rigid body B(ˆı , jˆ, k)   d jˆ ˆ 1 G d 2 jˆ ˆ G d 2 kˆ ·k + ·k− · jˆ ıˆ dt 2 dt 2 dt 2     G G 2 1 G d 2 kˆ d jˆ G d kˆ d ıˆ ˆ + · ıˆ + · ıˆ − · k jˆ dt dt 2 dt 2 dt 2  G ˆ G G 2 dk d ıˆ 1 G d 2 ıˆ d jˆ · jˆ + · j ˆ − · ı ˆ kˆ + dt dt 2 dt 2 dt 2

d ıˆ = dt G

B GαB

G

(9.149)

To prove (9.149), we take a G-derivative from (9.148).  G G ˆ  G ˆ G d ıˆ G d jˆ ˆ d jˆ dk dk d ıˆ = ·k + · ıˆ + · jˆ dt dt dt dt dt dt   G 2 d jˆ ˆ G d jˆ G d kˆ +ˆı ·k+ · dt 2 dt dt   G 2ˆ G ˆ G d k dk d ıˆ +jˆ · ıˆ + · dt 2 dt dt G 2 G d ıˆ d ıˆ G d jˆ · j ˆ + · +kˆ dt 2 dt dt G

B GαB

(9.150)

504

9 Acceleration Kinematics

Employing the unit vectors relationships ıˆ · ıˆ = jˆ · jˆ = kˆ · kˆ = 1

(9.151)

ıˆ · jˆ = jˆ · kˆ = kˆ · ıˆ = 0

(9.152)

ıˆ · d ıˆ = jˆ · d jˆ = kˆ · d kˆ = 0 jˆ · d ıˆ = −ˆı · d jˆ

(9.153)

kˆ · d jˆ = −jˆ · d kˆ

ıˆ · d kˆ = −kˆ · d ıˆ

(9.154)

ıˆ · d 2 ıˆ = −d ıˆ · d ıˆ jˆ · d 2 jˆ = −d jˆ · d jˆ

(9.155)

2ˆ

kˆ · d k = −d kˆ · d kˆ ıˆ · d 2 jˆ + jˆ · d 2 ıˆ = −2d ıˆ · d jˆ jˆ · d 2 kˆ + kˆ · d 2 jˆ = −2d jˆ · d kˆ

(9.156)

kˆ · d 2 ıˆ + ıˆ · d 2 kˆ = −2d kˆ · d ıˆ we can simplify (9.150)–(9.149). Example 304  Technical point on G ω˜ B , BG SB , G α˜ B . These three derivative quantities are acting as operator and transformers. Here their technical points are described. The derivative kinematics of a rigid body with a fixed point begins by differentiating the kinematic transformation between the body B and global G coordinate frames: G r = G RB B r (9.157) The kinematic transformation matrix G RB takes the coordinates of a point in the B-frame and determines the coordinates of the point in the G-frame. The matrix G RB is a kinematic or geometric transformation because the dimension of what it takes and what it provides are the same. The first derivative of (9.157) indicates the time rate of motion of a body point and introduces the angular velocity, G

v=

G

R˙ B B r = G ω˜ B G r

(9.158)

where G ω˜ B is the skew symmetric angular velocity matrix of B in G. This matrix is skew symmetric and its associated vector is the angular velocity vector. The matrix G ω˜ B also acts as an operator and a transformer. It takes the global position vector of a body point, G r, and determines its global velocity vector, G v. So, it is also called the rotational velocity transformation. The second derivative of (9.157) indicates the time rate of velocity and introduces the rotational acceleration transformation G SB , G a = G R¨ B B r = G SB G r (9.159) where G SB is not skew symmetric, so it does not indicate a vector. However, G SB acts as an operator and a transformer. It takes the global position vector of a body point, G r, and determines its global acceleration vector, G a. It is called the rotational acceleration transformation. The matrix G SB is the sum of two matrices: G SB

=

˜B Gα

+

˜ B G ω˜ B Gω

=

˜B Gα

+

˜ 2B Gω

(9.160)

The first matrix, G α˜ B , is the time derivative of G ω˜ B . So, it is a skew symmetric matrix and indicates the angular acceleration matrix and vector. However, G α˜ B cannot transform a position vector to its acceleration vector. The second matrix, G ω˜ 2B , is the square of the angular velocity matrix. It is also not a skew symmetric matrix and indicates no vector. We may consider G α˜ B and G ω˜ 2B as transformers because when they operate on G r they respectively provide the tangential and centripetal components of the acceleration vector G a.

9.1 Angular Acceleration Vector and Matrix

505

Example 305  Angular jerk. The next derivative of angular velocity and angular acceleration will be angular jerk. Jerk is needed when shaking is a problem to study and measure. ·

··

¨ Angular second acceleration matrix χ˜ = α˜ = ( ω is a skew symmetric matrix associated to the angular jerk χ = α˙ = ω. ˜B Gχ

 d G ¨ G T RB RB + G R˙ B G R˙ BT dt G ... G T G ¨ G ˙T = R B RB + 2 RB RB + G R˙ B G R¨ BT ·

=

˜B Gα

=

=

G UB

··

˜B Gω

=

G

+ 2 G SB G ω˜ TB + G ω˜ B G SBT G  · · ·· ... d ¨ ˙ = φ u˜ + φ u˜ = φ u˜ + 2φ¨ u˜ + φ˙ ( u dt

(9.161) (9.162)

The global jerk, G j, of a body point P at G r is G

j=

 d G G SB r = dt

G

G ...

r =

G ... G T G R B RB r

" # · ·· 2· 2 · 3 3 G 2 ¨ ˙ ˙ ¨ ˙ ˙ ˙ = φ u˜ + 2φ u˜ + φ( u + 3φ φ u˜ + 2φ u˜ u˜ + φ u˜ u˜ + φ u˜ r = G UB

=

G

G UB

G

(9.163)

r

... R B G RBT is the rotational jerk transformation between B and G. Hence, the angular jerk matrix would be ⎤ j11 j12 j13 χ˜ = α˜ = ( ω = ⎣ j21 j22 j23 ⎦ j31 j32 j33 ⎡

·

··

where,   2 j11 = 3u1 u˙ 1 φ˙ + 3 u21 − 1 φ˙ φ¨ 2 j21 = (2u2 u˙ 1 + u˙ 2 u1 ) φ˙ + 3u2 u1 φ˙ φ¨  ... 3 + u¨ 3 φ˙ + 2u˙ 3 φ¨ + u3 φ − u3 φ˙ 2 j31 = (2u3 u˙ 1 + u˙ 3 u1 ) φ˙ + 3u3 u1 φ˙ φ¨  ... 3 + u¨ 2 φ˙ + 2u˙ 2 φ¨ + u2 φ − u2 φ˙

2 j12 = (2u1 u˙ 2 + u˙ 1 u2 ) φ˙ + 3u1 u2 φ˙ φ¨  ... 3 + u¨ 3 φ˙ + 2u˙ 3 φ¨ + u3 φ − u3 φ˙

  2 j22 = 3u2 u˙ 2 φ˙ + 3 u22 − 1 φ˙ φ¨ 2 j32 = (2u3 u˙ 2 + u˙ 3 u2 ) φ˙ + 3u3 u2 φ˙ φ¨  ... 3 + u¨ 1 φ˙ + 2u˙ 1 φ¨ + u1 φ − u1 φ˙

(9.164)

506

9 Acceleration Kinematics

j13 = (2u1 u˙ 3 + u˙ 1 u3 ) φ˙ + 3u1 u3 φ˙ φ¨  ... 3 + u¨ 2 φ˙ + 2u˙ 2 φ¨ + u2 φ − u2 φ˙ 2

2 j23 = (2u2 u˙ 3 + u˙ 2 u3 ) φ˙ + 3u2 u3 φ˙ φ¨  ... 3 + u¨ 1 φ˙ + 2u˙ 1 φ¨ + u1 φ − u1 φ˙

  2 j33 = 3u3 u˙ 3 φ˙ + 3 u23 − 1 φ˙ φ¨

(9.165)

Example 306  Rotational jerk transformation. It is a matrix that applies on global position vector and provides global jerk vector. Consider a body coordinate frame B with a fixed point in a global frame G. The B-frame is turning in G with angular velocity G ωB and acceleration G α B . The global jerk G j of a body point at G r is G

 Gd 2   Gd 3 G d G G ω ˜ r = 3 r G SB r = G B dt dt 2 dt # G " · d 2 = φ¨ u˜ + φ˙ u˜ + φ˙ u˜ 2 G r dt # " · ·· ... 2· 2 · 3 3 G 2 ˙ ˙ ¨ ˙ ˙ ˙ ¨ u + 3φ φ u˜ + 2φ u˜ u˜ + φ u˜ u˜ + φ u˜ r = φ u˜ + 2φ u˜ + φ(

... j = Gr =

=

G UB

G

G

(9.166)

r

where, G UB is the rotational jerk transformation. · ·· ... 2· 2 · 3 = φ u˜ + 2φ¨ u˜ + φ˙ ( u + 3φ˙ φ¨ u˜ 2 + 2φ˙ u˜ u˜ + φ˙ u˜ u˜ + φ˙ u˜ 3

G UB

(9.167)

Employing G a = G R¨ B G RBT G r, we can find the jerk of the body point and jerk transformation, based on the rotational transformation matrix G RB : G   G d G G T G  d G  Gd  G G j= a = R¨ B RB r G SB r = dt dt dt  ...  = G R B G RBT + G R¨ B G R˙ BT + G R¨ B G RBT G R˙ B G RBT G r  ...  = G R B G RBT + G R¨ B G RBT G RB G R˙ BT + G R¨ B G RBT G R˙ B G RBT G r   ...  T G = G R B G RBT + G R¨ B G RBT G RB G R˙ BT + G RB G R˙ BT r ... = G R B G RBT G r (9.168) G UB

=

G ... G T R B RB

(9.169)  Using G a = G α B × G r + G ωB × G ωB × G r , we can find the vectorial expression formula for the jerk of a body point: 

G

 d G  G d  G a = G α B × r + G ωB × G ωB × dt dt   G = G χ B × r + 2 G α B × G ωB × G r   + G ωB × G α B × G r    + G ωB × G ωB × G ωB × G r

j=

G

where G χ B is the angular jerk vector of B relative to G.

G

 r

(9.170)

9.1 Angular Acceleration Vector and Matrix

507

The matrix form of the jerk is G

d  ˜ B + G ω˜ 2B G r Gα dt  ·  = G α˜ B + 2 G ω˜ B G α˜ B + G α˜ B +

j=

=

G



˜B Gχ

+ 2 G ω˜ B

˜B Gα

+

˜ B G ω˜ B Gα

˜ 2B Gω +



˜B Gω

˜ 3B Gω



G

G

r

r (9.171)

and therefore the rotational jerk transformation G UB is G UB

=

˜B Gχ

+ 2 G ω˜ B

˜B Gα

+

˜ B G ω˜ B Gα

+

˜ 3B Gω

(9.172)

Example 307  Angular acceleration in terms of quaternion and Euler parameters. Quaternions are close to Euler parameters and both may be used to express angular accelerations. To express the acceleration of a body point by quaternions and Euler parameters, we consider the velocity equation r˙ = 2e˙ e∗ G r

(9.173)

r¨ = 2e¨ e∗ G r + 2e˙ e˙∗ G r + 4e˙2 e∗2 G r   = 2 e¨ e∗ + e˙ e˙∗ + 2e˙2 e∗2 G r

(9.174)

G

and take a time derivative G

Therefore, the quaternion expression of the acceleration transformation matrix is G SB

  = 2 e¨ e∗ + e˙ e˙∗ + 2e˙2 e∗2

(9.175)

To determine the angular acceleration quaternion, let us use the angular velocity = 2e˙ e∗

(9.176)

= 2e¨ e∗ + 2e˙ e˙∗

(9.177)

G ωB

and take a derivative. GαB

Employing the definition of the angular velocity based on rotational quaternion, → →← ←→ = 2← e˙ e∗

G ωB

←→

B G ωB

← →← → = 2 e∗ e˙

(9.178) (9.179)

we are able to define the angular acceleration quaternion. → → →← ← →← ←α→ = 2← e¨ e∗ + 2 e˙ e˙∗ G B ←→

B GαB

← →← → → ← →← = 2 e∗ e¨ + 2 e˙ e˙∗

(9.180) (9.181)

508

9 Acceleration Kinematics

Fig. 9.4 A rigid body with coordinate frame B (oxyz) moving freely in a fixed global coordinate frame G(OXY Z)

9.2

Rigid Body Acceleration

Consider a rigid body with an attached local coordinate frame B (oxyz) moving freely in a fixed global coordinate frame G(OXY Z). The rigid body can rotate in the global frame, while the origin of the body frame B can translate relative to the origin of G. The coordinates of a body point P in local and global frames, as shown in Fig. 9.4, are related by the following equation: G rP = G RB B rP + G dB (9.182) where G dB indicates the position of the moving origin o relative to the fixed origin O. The acceleration of point P in G is G

r¨ P G  = G α B × rP − G dB   + G ωB × G ωB × G rP −

aP =

G

v˙ P =

G

G

dB



G¨

+

(9.183)

dB

Proof The acceleration of point P is a consequence of differentiating the velocity equation (7.266) or (7.267). G

G

aP =

G

d dt

G

vP =

vP =

GαB

×

G B rP

G B rP

=

rP − G dB   + G ωB × G ωB × G rP −

×

G

G ωB

× 



G˙

+

+

GαB

G B rP

+

×

=

GαB

×

G ωB

G ωB

G ωB

G

(9.184)

dB

×

dB

×

G ˙P Br

G B rP



+



+

+

G¨

dB

G¨

dB

G¨

dB . (9.185)

  The term G ωB × G ωB × G B rP is called centripetal acceleration, which is independent of the angular acceleration. The term G G  G α B × B rP is the tangential acceleration and is perpendicular to B rP . Example 308 Acceleration of a body point. A review of acceleration equation of a point of a rigid body, starting from its position vector and taking derivatives step by step. Consider a rigid body is moving and rotating in a global frame. The acceleration of a body point can be found by taking twice time derivative of its position vector. G rP = G RB B rP + G dB (9.186)

9.2 Rigid Body Acceleration

509 G

G

r˙ P =

G

R˙ B

B

rP +

rP + G d¨ B  = G R¨ B G RBT G rP −

r¨ P =

G

R¨ B

G˙

(9.187)

dB

B

Differentiating the angular velocity matrix ˜B Gω

=

G

G

 dB +

G¨

(9.188)

dB

R˙ B G RBT

(9.189)

shows that ·

˜B Gω

G

d ˜ B = G R¨ B G RBT + Gω dt = G R¨ B G RBT + G ω˜ B G ω˜ TB =

and therefore, G

R¨ B G RBT =

·

˜B Gω

−

G

R˙ B G R˙ BT (9.190)

˜ B G ω˜ TB Gω

(9.191)

Hence, the acceleration vector of the body point becomes  G

r¨ P =



·

˜B − Gω

where,

G

rP −

G

 dB +

G¨

⎤ 0 −ω˙ 3 ω˙ 2 = G α˜ B = ⎣ ω˙ 3 0 −ω˙ 1 ⎦ −ω˙ 2 ω˙ 1 0

dB

(9.192)

⎡

·

˜B Gω and

˜ B G ω˜ TB Gω

(9.193)

⎡

⎤ ω22 + ω23 −ω1 ω2 −ω1 ω3 ˜ B G ω˜ TB = ⎣ −ω1 ω2 ω21 + ω23 −ω2 ω3 ⎦ Gω −ω1 ω3 −ω2 ω3 ω21 + ω22

(9.194)

Example 309 Acceleration of joint 2 of a 2R planar manipulator. Second joint of a 2R manipulator is moving on a circle about the first joint. Acceleration of a rotating point about a center point is a good example to show analytically the acceleration has two components of centripetal and tangential. A 2R planar manipulator is illustrated in Fig. 9.5. The elbow joint has a circular motion about the base joint. Knowing that ˙ 0ˆ (9.195) 0 ω 1 = θ 1 k0 we can write 0α1

= 0 ω˙ 1 = θ¨ 1 0 kˆ0

× 0 r1 = θ¨ 1 0 kˆ0 × 0 r1 = θ¨ 1 RZ,θ +90 0 r1   0 ˙2 0 0 ω1 × 0 ω1 × r1 = −θ 1 r1 ˙1 0ω

(9.196) (9.197) (9.198)

and calculate the acceleration of the elbow joint. 0

2 r¨ 1 = θ¨ 1 RZ,θ +90 0 r1 − θ˙ 1 0 r1

(9.199)

The first term, θ¨ 1 RZ,θ+90 0 r1 , is tangential acceleration and is perpendicular to the radial position vector, 0 r1 , and the second 2 term, −θ˙ 1 0 r1 , is centripetal acceleration and is in opposite direction of 0 r1 .

510

9 Acceleration Kinematics

Example 310 Acceleration of a moving point in a moving body frame. The kinematics of a moving point in a rotating and translating coordinate frame B is the most general case of kinematic analysis of a point. This example shows different components of the acceleration of such a point. Assume the point P in Fig. 9.4 is indicated by a time varying body position vector B rP (t). Then, the velocity and acceleration of P can be found by applying the derivative transformation formula (7.214). G

B dB dB = + dt dt

B G vP

B G aP

B G ωB

×

B G

=

B ˙ G

(9.200)

G

dB rP = BG d˙ B + B r˙ P + BG ωB × B rP dt G = BG d˙ B + B vP + BG ωB × B rP =

(9.201)

B¨ G dB

=

+ B r¨ P + BG ωB × B r˙ P + BG ω˙ B × B rP   B +G ωB × B r˙ P + BG ωB × B rP

=

G¨

B ω B × B vP + dB + B a P + 2 G   B +G ωB × BG ωB × B rP

B ˙B Gω

× B rP (9.202)

It is also possible to take the derivative from Eq. (7.263) with the assumption B r˙ P = 0 and find the acceleration of P . G

G

r˙ P = =

G

r¨ P =

˙B Gω

×

G

rP =

R˙ B

B

G ωB

×

G

G

rP + G

B

RB

G

rP +

RB

B

RB B rP +

RB B rP +

G ωB

×

G

G

r˙ P + G

RB

(9.203)

dB G˙

dB

B

r˙ P +

R˙ B B rP +

G˙

G ωB

×

r¨ P + G d¨ B   G = G ω˙ B × G B rP + G ωB × G ωB × rP + 2 G ωB × + G R˙ B

B

r˙ P +

¨P + +G Br

G

RB

(9.204)

dB

G

RB B r˙ P

B

G¨

dB

G ˙P Br

(9.205)

  G The first term, G ω˙ B × G B rP , is the tangential acceleration; the second term, G ωB × G ωB × rP , is the centripetal acceleration. ˙ P , is called the Coriolis acceleration. The Coriolis acceleration is The third term on the right-hand side, 2 G ωB × G Br perpendicular to both, G ωB and B r˙ P .

9.3

 Acceleration Transformation Matrix

Consider the motion of a rigid body B in the global coordinate frame G, as shown in Fig. 9.4. Assume the body fixed frame B(oxyz) is coincident at an initial time t0 with the global frame G(OXY Z). At any time t = t0 , B is not necessarily coincident with G, and therefore, the homogenous transformation matrix G TB (t) is time varying. The acceleration of a body point in the global coordinate frame can be found by applying a homogenous acceleration transformation matrix G aP (t) = G AB G rP (t) (9.206) where, hereafter G AB is the acceleration transformation matrix

9.3  Acceleration Transformation Matrix

511

 G

AB =

˜B Gα

˜ B G ω˜ TB G d¨ B Gω

−

−



˜B Gα

−

0 "

G SB

=

˜ B G ω˜ TB Gω



G

 dB

0

G¨

dB −

0

G SB

G

dB

# (9.207)

0

Proof Based on homogenous coordinate transformation we have G

rP (t) = =

G

vP =

G

G

TB B rP

"G

RB 0

G

dB 1

# B

rP =

G

TB G rP (t0 )

T˙B G TB−1 G rP (t)

"G

G˙

R˙ B G RBT G dB 0 " # ω˜ G d˙ B − G ω˜ B G dB G = G B rP (t) 0 0

=

=

G

R˙ B G RBT 0

dB −

G

(9.208)

# G

rP (t)

VB G rP (t)

(9.209)

To find the acceleration of a body point in the global frame, we take twice the time derivative from G rP (t) = G

aP (t) =

d2 G B TB rP = dt 2

G

T¨B B rP

G

TB B rP , (9.210)

and substitute for B rP . G

aP (t) =

G

T¨B G TB−1 G rP (t) =

AB =

G

T¨B G TB−1

G

G

AB G rP (t)

(9.211) (9.212)

Substituting for G T¨B and G TB−1 provides G

aP (t) =

"G "G

R¨ B 0

G¨

dB 0

#"G

RBT − G RBT G dB 0 1

# G

rP (t)

# R¨ B G RBT G dB G rP (t) = 0    ˜ B − ω˜ ω˜ T G d¨ B − G α˜ B − ω˜ ω˜ T G dB G Gα = rP (t) 0 0 " # S G d¨ B − G SB G dB = G B = G AB G rP (t) 0 0 R¨ B G RBT 0

G¨

dB −

where, G

R¨ B G RBT =

G SB

=

G

·

˜B Gω

− ω˜ ω˜ T =

˜B Gα

− ω˜ ω˜ T

(9.213)

(9.214) 

512

9 Acceleration Kinematics

Fig. 9.5 An RR planar manipulator with joint variables θ 1 and θ 2

Example 311  Kinematics of the gripper of a planar RR manipulator. 2R manipulator is the main component of many multi- degree of freedom and hence, its kinematics will appear in robotic analysis very often. Here, its kinematics will be solved from position to acceleration in different methods. Figure 9.5 illustrates an RR planar manipulator with joint variables θ 1 and θ 2 . The links (1) and (2) are both RR(0) and therefore, the transformation matrices 0 T1 , 1 T2 , and 0 T2 are ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0 0

The points M1 and M2 are at

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(9.215)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(9.216)

T2 = 0 T1 1 T2 ⎤ ⎡ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l2 c (θ 1 + θ 2 ) + l1 cθ 1 ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l2 s (θ 1 + θ 2 ) + l1 sθ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1 ⎤ ⎤ ⎡ l1 cos θ 1 l2 cos θ 2 ⎢ l1 sin θ 1 ⎥ ⎢ l2 sin θ 2 ⎥ 0 1 ⎥ ⎥ rM1 = ⎢ rM2 = ⎢ ⎦ ⎦ ⎣ ⎣ 0 0 1 1 ⎡ ⎤ l2 cos (θ 1 + θ 2 ) + l1 cos θ 1 ⎢ l2 sin (θ 1 + θ 2 ) + l1 sin θ 1 ⎥ 0 ⎥ rM2 = 0 T1 1 rM2 = ⎢ ⎣ ⎦ 0 1

(9.217)

⎡

(9.218)

(9.219)

To determine the velocity and acceleration of M2 , we need to calculate 0 T˙2 , which can be calculated by direct differentiation of 0 T2 .

9.3  Acceleration Transformation Matrix 0

513

d 0 T˙2 = T2 dt ⎡ −θ˙ 12 sθ 12 −θ˙ 12 cθ 12 ⎢ θ˙ 12 cθ 12 −θ˙ 12 sθ 12 =⎢ ⎣ 0 0 0 0

(9.220) ⎤ 0 −l2 θ˙ 12 sθ 12 − θ˙ 1 l1 sθ 1 0 l2 θ˙ 12 cθ 12 + θ˙ 1 l1 cθ 1 ⎥ ⎥ ⎦ 0 0 0 0

θ˙ 12 = θ˙ 1 + θ˙ 2

θ 12 = θ 1 + θ 2

(9.221)

We can also calculate 0 T˙2 from 0 T2 = 0 T1 1 T2 by chain rule 0

d 0 1  T˙2 = T1 T2 = 0 T˙1 1 T2 + 0 T1 1 T˙2 dt

where,

⎡

0

T˙1

1

T˙2

− sin θ 1 ⎢ cos θ 1 = θ˙ 1 ⎢ ⎣ 0 0 ⎡ − sin θ 2 ⎢ cos θ 2 = θ˙ 2 ⎢ ⎣ 0 0

⎤ 0 −l1 sin θ 1 0 l1 cos θ 1 ⎥ ⎥ ⎦ 0 0 0 0 ⎤ − cos θ 2 0 −l2 sin θ 2 − sin θ 2 0 l2 cos θ 2 ⎥ ⎥ ⎦ 0 0 0 0 0 0 − cos θ 1 − sin θ 1 0 0

(9.222)

(9.223)

(9.224)

Having 0 T˙1 and 1 T˙2 , we can find the velocity transformation matrices 0 V1 and 1 V2 by using 0 T1−1 and 1 T2−1 . ⎡

0

T1−1

⎤ cos θ 1 sin θ 1 0 −l1 ⎢ − sin θ 1 cos θ 1 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(9.225)

⎡

1

⎤ cos θ 2 sin θ 2 0 −l2 ⎢ − sin θ 2 cos θ 2 0 0 ⎥ ⎥ =⎢ ⎣ 0 0 1 0 ⎦ 0 0 0 1

(9.226)

0

V1 = 0 T˙1 0 T1−1 = θ˙ 1 01 k˜

(9.227)

1

V2 = 1 T˙2 1 T2−1 = θ˙ 2 12 k˜

(9.228)

T2−1

Now, we can determine the velocity of points M1 and M2 in B0 and B1 , respectively. ⎡

0

vM1 = 0 V1 0 rM1

⎤ −l1 sin θ 1 ⎢ l1 cos θ 1 ⎥ ⎥ = θ˙ 1 ⎢ ⎣ ⎦ 0 0 ⎡

1

vM2 = 1 V2 1 rM2

⎤ −l2 sin θ 2 ⎢ l2 cos θ 2 ⎥ ⎥ = θ˙ 2 ⎢ ⎣ ⎦ 0 0

(9.229)

(9.230)

514

9 Acceleration Kinematics

To determine the velocity of the tip point M2 in the base frame, we can use the velocity vector addition. 0

vM2 = 0 vM1 + 01 vM2 = 0 vM1 + 0 T1 1 vM2  ⎡  ⎤ − θ˙ 1 + θ˙2 l2 sin (θ 1 + θ 2 ) − θ˙ 1 l1 sin θ 1 ⎢ θ˙ 1 + θ˙ 2 l2 cos (θ 1 + θ 2 ) + θ˙ 1 l1 cos θ 1 ⎥ ⎥ =⎢ ⎦ ⎣ 0 0

(9.231)

We can also determine 0 vM2 by using the velocity transformation matrix 0 V2 0

vM2 = 0 V2 0 rM2

(9.232)

where, the velocity transformation matrix 0 V2 is ⎡

0 −θ˙ 1 − θ˙ 2 ⎢ θ˙ 1 + θ˙ 2 0 =⎢ ⎣ 0 0 0 0

⎤ 0 θ˙ 2 l1 sin θ 1 0 −θ˙ 2 l1 cos θ 1 ⎥ ⎥ ⎦ 0 0 0 0

0

V2 = 0 T˙2 0 T2−1

0

T2−1 = 2 T1 1 T0 = 1 T2−1 0 T1−1 ⎡ ⎤ cos (θ 1 + θ 2 ) sin (θ 1 + θ 2 ) 0 −l2 − l1 cos θ 2 ⎢ − sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 ⎥ l1 sin θ 2 ⎥ =⎢ ⎦ ⎣ 0 0 1 0 0 0 0 1

(9.233)

(9.234)

Furthermore, we can determine the velocity transformation matrix 0 V2 using their addition rule, 0

V2 = 0 V1 + 01 V2

where,

⎡

0 1 V2

Therefore, 0 vM2 will be

= 0 T1 1 V2 0 T1−1

0

0 ⎢ θ˙ 2 =⎢ ⎣0 0

−θ˙ 2 0 0 0

⎤ 0 θ˙ 2 l1 sin θ 1 0 −θ˙ 2 l1 cos θ 1 ⎥ ⎥ ⎦ 0 0 0 0

vM2 = 0 V2 0 rM2

(9.235)

(9.236)

(9.237)

To determine the acceleration of M2 , we need to calculate 0 T¨2 , which can be calculated by direct differentiation of 0 T˙2 . 0

 d 0˙ d d 0 1  d 0 ˙ 1 T¨2 = T2 = T1 T2 = T1 T2 + 0 T1 1 T˙2 dt dt dt dt 1 ¨ 0 ¨ 1 0 ˙ 1 ˙ 0 = T1 T2 + 2 T1 T2 + T1 T2

(9.238)

9.3  Acceleration Transformation Matrix

515

We have, d 0˙ T¨1 = T1 dt ⎡ 2 2 −θ˙ cθ 1 − θ¨ 1 sθ 1 θ˙ 1 sθ 1 − θ¨ 1 cθ 1 ⎢ 1 ˙2 ˙2 ¨ ⎢ ¨ = ⎢ θ 1 cθ 1 − θ 1 sθ 1 −θ 1 cθ 1 − θ 1 sθ 1 ⎣ 0 0 0 0 0

d 1˙ T¨2 = T2 dt ⎡ 2 2 −θ˙ cθ 2 − θ¨ 2 sθ 2 θ˙ 2 sθ 2 − θ¨ 2 cθ 2 ⎢ 2 ˙2 ˙2 ¨ ⎢ ¨ = ⎢ θ 2 cθ 2 − θ 2 sθ 2 −θ 2 cθ 2 − θ 2 sθ 2 ⎣ 0 0 0 0

(9.239) ⎤ 2 0 −θ¨ 1 l1 sθ 1 − l1 θ˙ 1 cθ 1 ⎥ 2 0 l1 θ¨ 1 cθ 1 − l1 θ˙ 1 sθ 1 ⎥ ⎥ ⎦ 0 0 0 0

1

(9.240) ⎤ 2 0 −θ¨ 2 l2 sθ 2 − l2 θ˙ 2 cθ 2 ⎥ 2 0 θ¨ 2 l2 cθ 2 − θ˙ 2 l2 sθ 2 ⎥ ⎥ ⎦ 0 0 0 0

and therefore, 0

T¨2 = 0 T¨1 1 T2 + 2 0 T˙1 1 T˙2 + 0 T1 1 T¨2 ⎤ ⎡ r11 r12 0 r14 ⎢ r21 r22 0 r24 ⎥ ⎥ =⎢ ⎣ 0 0 0 0 ⎦ 0 0 0 0

(9.241)

r11 = −θ˙ 12 cos θ 12 − θ¨ 12 sin θ 12 2

r21 = −θ˙ 12 sin θ 12 + θ¨ 12 cos θ 12 2

2 r12 = θ˙ 12 sin θ 12 − θ¨ 12 cos θ 12 2 r22 = −θ˙ 12 cos θ 12 − θ¨ 12 sin θ 12 2 r14 = −θ˙ 12 l2 cos θ 12 − θ¨ 12 l2 sin θ 12 2 −θ˙ 1 l1 cos θ 1 − θ¨ 1 l1 sin θ 1 2 r24 = θ¨ 12 l2 cos θ 12 − θ˙ 12 l2 sin θ 12

−θ˙ 1 l1 sin θ 1 + θ¨ 1 l1 cos θ 1 2

(9.242)

Having 0 T¨1 , 1 T¨2 , 0 T¨2 , we can find the acceleration transformation matrices 0 A1 , 1 A2 , and 0 A2 by using 0 T1−1 , 1 T2−1 , and 0 T2−1 . ⎡

0

A1 = 0 T¨1 0 T1−1

−θ˙ ⎢ 1 ⎢ θ¨ 1 =⎢ ⎣ 0 0 ⎡

1

A2 = 1 T¨2 1 T2−1

2

−θ˙ ⎢ 2 ⎢ ¨ = ⎢ θ2 ⎣ 0 0 2

−θ¨ 1 2 −θ˙ 1 0 0

⎤ 00 ⎥ 0 0⎥ ⎥ 0 0⎦ 00

−θ¨ 2 2 −θ˙ 2 0 0

⎤ 00 ⎥ 0 0⎥ ⎥ 0 0⎦ 00

(9.243)

(9.244)

516

9 Acceleration Kinematics

A2 = 0 T¨2 0 T2−1 ⎡ 2 ⎤ 2 −θ˙ 12 −θ¨ 12 0 l1 θ˙ 2 cos θ 1 + 2θ 1 θ˙ 2 l1 cos θ 1 + θ¨ 2 l1 sin θ 1 ⎢ ⎥ ˙2 ˙2 ˙ ¨ ⎢ ¨ ⎥ = ⎢ θ 12 −θ 12 0 l1 θ 2 sin θ 1 + 2θ 1 θ 2 l1 sin θ 1 − θ 2 l1 cos θ 1 ⎥ ⎣ 0 ⎦ 0 0 0 0 0 0 0 0

(9.245)

Now, we can determine the acceleration of points M1 and M2 in B0 and B1 , respectively.

aM1 = 0 A1 0 rM1

⎤ 2 −l1 θ˙ 1 cos θ 1 − θ¨ 1 l1 sin θ 1 ⎢ ⎥ ˙2 ⎢ ¨ ⎥ = ⎢ θ 1 l1 cos θ 1 − θ 1 l1 sin θ 1 ⎥ ⎣ ⎦ 0 0

aM2 = 1 A2 1 rM2

⎤ 2 −l2 θ˙ 2 cos θ 2 − θ¨ 2 l2 sin θ 2 ⎥ ⎢ ˙2 ⎢ ¨ ⎥ = ⎢ θ 2 l2 cos θ 2 − θ 2 l2 sin θ 2 ⎥ ⎦ ⎣ 0 0

⎡

0

(9.246)

⎡

1

aM2 = 0 A2 0 rM2 ⎡ ⎤ 2 −θ¨ 12 l2 (cos θ 12 + sin θ 12 ) − θ˙ 1 l1 cos θ 1 − θ¨ 1 l1 sin θ 1 ⎢ ⎥ ˙2 ¨ ⎢ ¨ ⎥ = ⎢ θ 12 l2 (cos θ 12 − sin θ 12 ) − θ 1 l1 sin θ 1 + θ 1 l1 cos θ 1 ⎥ ⎣ ⎦ 0 0

(9.247)

0

(9.248)

Example 312  Jerk transformation matrix. Jerk is the next derivative after acceleration and it will be useful whenever shaking of the carrying object by the gripper is needed to be controlled. Here jerk transformation matrix is calculated. Following the same pattern of acceleration transformation matrix, we may define a jerk transformation matrix G JB as G

jP (t) =

G

JB G rP (t)

(9.249)

where, G

" G ... G ... # " G T G T G # RB − RB d B RB d B 0 0 0 1 # " G ... G T G ... ... R B RB d B − G R B G RBT G dB = 0 0

JB =

(9.250)

and G ... G T R B RB

= 

=

·· ωB G(

−2

=

˜B Gχ

−2

G UB ·

˜B Gω 

˜B Gα

− ω˜ ω˜ T



 ω˜ T − ω˜

·

˜B Gω

T − ω˜ ω˜ T

 T − ω˜ ω˜ T ω˜ T − ω˜ G α˜ B − ω˜ ω˜ T

(9.251)

Example 313 Velocity, acceleration, and jerk transformation matrices. This is to compare the transformation matrices of velocity, acceleration, and jerk. The velocity transformation matrix is to map a position vector to its velocity vector. Assume p and q denote the position of two body points P and Q.

9.3  Acceleration Transformation Matrix

517 G

q−

G

p=

G

RB

B

q − Bp



(9.252)

Assuming we have the kinematics of one of the points, say P , then the position of the other point is G

q=

G

p+

G

RB

B

q − Bp



(9.253)

This equation is similar to G

r=

G

d+

G

RB B r

(9.254)

where the origin of the B-frame is at point P and B r indicates the position of Q relative to P . Taking a time derivative shows that   G q˙ − G p˙ = G ω˜ B G q − G p (9.255) which can be converted to #"G # "G # " q˙ ˜ B G p˙ − G ω˜ B G p q Gω = 0 0 0 1 "G # q = G VB 1

(9.256)

The matrix [V ] is velocity transformation matrix. Similarly, we obtain the acceleration equation G

q¨ −

G

p¨ = = = = =

G

   p + G ω˜ B G q˙ − G p˙    ˜ B G q − G p + G ω˜ B G ω˜ B G q − Gα    ˜ B G q − G p − G ω˜ B G ω˜ TB G q − Gα    ˜ B − G ω˜ B G ω˜ TB G q − G p Gα G  q − Gp G SB ˜B Gα

q−

G

"G # q¨ = 0

which can be converted to

 [A] =

˜ B G ω˜ TB G p¨ Gω

−

AB

−

G SB

G¨

p−

0

p

G

p

 

(9.257)

"G # q 1



˜B Gα

(9.258)

−

0 "

=

˜B Gα

G

G

˜ B G ω˜ TB Gω



G

 p

0 G SB

G

p

# (9.259)

0

where [A] is the acceleration transformation matrix for rigid motion. The jerk matrix can be found after another differentiation G ... q

 = =

which can be converted to

G ... p

=



·

˜ B + 2 G ω˜ B G α˜ B Gα 

=

−



·

˜B Gα

+ 2 G ω˜ B

˜B Gα

+

˜B Gχ

+ 2 G ω˜ B

˜B Gα

+

+ 



˜B + Gα

˜B Gα

+

˜ 2B Gω

˜ B G ω˜ B Gα

" G ... # q = 0

G

˜ 2B Gω

+







˜B Gω

˜ 3B Gω

" # q JB 1

G

˜B Gω



G

G

q−

q−

q−

G

p

G



p

G

p





(9.260)

(9.261)

518

9 Acceleration Kinematics

where [J ] is the jerk transformation matrix.

"

J J [J ] = 11 12 0 0 J11 = J12

# (9.262)

˜B Gχ

+ 2 G ω˜ B G α˜ B + G α˜ B G ω˜ B + G ω˜ 3B  = G p¨ − G χ˜ B + 2 G ω˜ B G α˜ B + G α˜ B G ω˜ B +

˜ 3B Gω



(9.263) G

p (9.264)

9.4

Forward Acceleration Kinematics

¨ the joint The forward acceleration kinematics problem is the method of relating the end-effector accelerations to X ¨ It is accelerations, q. ¨ = J q¨ + J˙ q˙ X (9.265)   where, [J] is the Jacobian matrix, J˙ is time derivative of Jacobian matrix, q is the joint variable vector, q˙ is the joint velocity vector, and q¨ is the joint acceleration vector. T  q = q1 q 2 q 3 · · · q n  T q˙ = q˙1 q˙2 q˙3 · · · q˙n  T q¨ = q¨1 q¨2 q¨3 · · · q¨n

(9.266) (9.267) (9.268)

˙ and X ¨ are the end-effector configuration velocity and acceleration vectors, respectively. However, X  T X = Xn Yn Zn ϕ n θ n ψ n

˙ = X

"0

vn 0 ωn

#

"0

d˙ = 0 n ωn

# (9.270)

 T = X˙ n Y˙n Z˙ n ωXn ωY n ωZn

¨ = X

"0

an 0 αn

#

"0

d¨ = 0 n ω˙ n

(9.269)

(9.271)

#

 T = X¨ n Y¨n Z¨ n ω˙ Xn ω˙ Y n ω˙ Zn

(9.272)

  To calculate the time derivative of the Jacobian matrix J˙ , we use Eqs. (8.132)–(8.135). 0 ˙ i−1 di

=

0

0 0 ωi × i−1 di 0 d˙i kˆi−1 + 00 ωi

 0 i−1 ωi

=

×

0 i−1 di

if joint i is R if joint i is P (9.273)

0ˆ

θ˙ i ki−1 0

if joint i is R if joint i is P (9.274)

Taking a derivative to find the acceleration of link (i) with respect to its previous link (i − 1), yields

9.4 Forward Acceleration Kinematics

519

0 ¨ i−1 di

=

⎧0 ω˙ i × ⎪ ⎪ ⎨0

+ 00 ωi ×

0

0 ωi

×

0 i−1 di



if joint i is R

  0 0 0 ˙ i × i−1 di + 00 ωi × 00 ωi × i−1 di ⎪ 0ω ⎪ ⎩ +d¨i 0 kˆi−1 + 2d˙i 00 ωi−1 × 0 kˆi−1 if joint i is P 

0 ˙i i−1 ω

0 i−1 di

=

(9.275) 0ˆ

θ¨ i ki−1 + 0

θ˙ i 00 ωi−1

0ˆ

× ki−1

if joint i is R if joint i is P (9.276)

Therefore, the acceleration vectors of the end-effector frame are 0¨ 0 dn

=

n 

0 ¨ i−1 di

(9.277)

0 i−1 ωi .

(9.278)

i=1

and 0 ˙n 0ω

=

n  i=1

The acceleration relationships can also be rearranged in a recursive form.  0 ˙i i−1 ω

=

θ¨ i 0 kˆi−1 + θ˙ i 00 ωi−1 × 0 kˆi−1 if joint i is R 0 if joint i is P

(9.279)

  Example 314 Forward acceleration of the 2R planar manipulator. An example to show how J˙ will be calculated. The forward velocity of the 2R planar manipulator is found as "

˙ = J q˙ X # " #" # X˙ −l1 sθ 1 − l2 s (θ 1 + θ 2 ) −l2 s (θ 1 + θ 2 ) θ˙ 1 = θ˙ 2 l1 cθ 1 + l2 c (θ 1 + θ 2 ) l2 c (θ 1 + θ 2 ) Y˙

The differential of the Jacobian matrix is

" J˙ J˙ = ˙11 J21

J˙12 J˙22

(9.280)

# (9.281)

where, J˙11 = (−l1 cos θ 1 − l2 cos (θ 1 + θ 2 )) θ˙ 1 − l2 cos (θ 1 + θ 2 ) θ˙ 2 J˙12 = −l2 cos (θ 1 + θ 2 ) θ˙ 1 − l2 cos (θ 1 + θ 2 ) θ˙ 2 J˙21 = (−l1 sin θ 1 − l2 sin (θ 1 + θ 2 ))θ˙ 1 − l2 sin (θ 1 + θ 2 ) θ˙ 2 J˙22 = −l2 sin (θ 1 + θ 2 ) θ˙ 1 − l2 sin (θ 1 + θ 2 ) θ˙ 2

(9.282)

and therefore, the forward acceleration kinematics of the manipulator can be rearranged. ¨ = J q¨ + J˙ q˙ X

(9.283)

520

9 Acceleration Kinematics

For the 2R manipulator it is easier to show the acceleration in the following form: "

X¨ Y¨

#

"

#" # −l1 sin θ 1 −l2 sin (θ 1 + θ 2 ) θ¨ 1 = θ¨ 1 + θ¨ 2 l1 cos θ 1 l2 cos (θ 1 + θ 2 )   # " 2 θ˙ 1 l1 cos θ 1 l2 cos (θ 1 + θ 2 ) −  2 l1 sin θ 1 l2 sin (θ 1 + θ 2 ) θ˙ 1 + θ˙ 2

(9.284)

Example 315  Acceleration based on position vector. It is an analytic calculation of acceleration when the position vector of the end-effector is given as function of joint variables. Assume that the position vector r of the end-effector of a manipulator is given as function of its joint coordinates q. ⎡

⎤ ⎡ ⎤ r1 (q1 , q2 , q3 ) r1 (q) r = ⎣ r2 (q) ⎦ = ⎣ r2 (q1 , q2 , q3 ) ⎦ r3 (q) r3 (q1 , q2 , q3 )

(9.285)

The velocity of the end-effector is r˙ = where,

∂r q˙ = JD q˙ ∂q

⎡ ⎤ ∂r1 /∂q1 ∂r1 /∂q2 ∂r1 /∂q3 ∂r JD = = ⎣ ∂r2 /∂q1 ∂r2 /∂q2 ∂r2 /∂q3 ⎦ ∂q ∂r3 /∂q1 ∂r3 /∂q2 ∂r3 /∂q3

The second derivative of r is r¨ =

d ∂r q¨ + ∂q dt



∂r q˙ ∂q

(9.286)

(9.287)

(9.288)

where, d dt





∂r ∂q

 dJD d = JD = q˙i dt dqi i=1 3

=

9.5

dJD dJD dJD q˙1 + q˙2 + q˙3 dq1 dq2 dq3

(9.289)

Inverse Acceleration Kinematics

¨ The The inverse kinematics acceleration is to calculate the joint acceleration vector q¨ for a given end-effector acceleration X. inverse acceleration kinematics will be solved from the forward acceleration kinematics equation. ¨ = X

"0

an 0 αn

#

= J q¨ + J˙ q˙

(9.290)

Assuming that the Jacobian matrix, J, is square and non-singular, the joint acceleration vector q¨ can be found by matrix inversion.   ¨ − J˙ q˙ q¨ = J−1 X (9.291) ˙ X, ¨ q, q, ˙ [J], the joint acceleration vector q¨ is a matrix operation, realizing that all vector and matrices of X, X, Calculating  ˙J are already calculated in position and velocity kinematics analysis. Proof As long as the Jacobian matrix J is square and non-singular, it has an inverse J−1 that can be used to solve the set of ¨ linear algebraic Equations (9.290) for the joint acceleration vector q.

9.5 Inverse Acceleration Kinematics

521

However, calculating J˙ and J−1 becomes more tedious by increasing the DOF of robots. The alternative technique is to write the equation in a new form ¨ − J˙ q˙ = J q¨ X (9.292) which is similar to Eq. (8.239) for a robot with a spherical wrist " # A 0 ¨ ˙ X − J q˙ = q¨ CD or

(9.293)

⎡

⎤ q¨1 ⎢ ⎥ " # " # ⎢ q¨2 ⎥ ⎥ m A 0 ⎢ ⎢ q¨3 ⎥ = ⎢ n C D ⎢ q¨4 ⎥ ⎥ ⎣ q¨5 ⎦ q¨6

where,

"

(9.294)

# m ¨ − J˙ q˙ =X n

(9.295)

Therefore, the inverse acceleration kinematics problem can be solved as ⎡

⎤ θ¨ 1 ⎣ θ¨ 2 ⎦ = A−1 [m] θ¨ 3

(9.296)

⎤ ⎛ ⎡ ⎤⎞ q¨4 q¨1 ⎣ q¨5 ⎦ = D −1 ⎝[n] − [C] ⎣ q¨2 ⎦⎠ q¨6 q¨3

(9.297)

⎡

and

¨ − J q¨ is called the acceleration bias vector and can be calculated by differentiating from The matrix J˙ q˙ = X 0˙ 0 d6

=

3 

0 0 ωi

×

0 i−1 di

(9.298)

i=1 0 0 ω6

=

6 

θ˙ i 0 kˆi−1

(9.299)

i=1

to find 0 0 a6

= 00 d¨ 6 =

3  

0 ˙i 0ω

×

0 i−1 di

+ 00 ωi ×

0

0 ωi

×

0 i−1 di



(9.300)

i=1

and 0 0α6

= 00 ω˙ 6 =

6  

θ¨ i 0 kˆi−1 + 00 ωi × θ˙ i 0 kˆi−1

(9.301)

i=1

¨ Then, subtracting the second half of J q¨ from 0 α 6 provides the The angular acceleration vector 00 α 6 is the second half of X. 0 second half of the bias vector. 6  0 ˙ 0ˆ (9.302) 0 ωi × θ i ki−1 i=1

522

9 Acceleration Kinematics

We substitute 00 ωi and 00 ω˙ i 0 0 ωi

=

i 

θ˙ j 0 kˆj −1

(9.303)

j =1 0 ˙i 0ω

=

i   θ¨ j 0 kˆj −1 + 00 ωj −1 × θ˙ j 0 kˆj −1

(9.304)

j =1

in Eq. (9.300) 0¨ 0 d6

=

i  3  

θ¨ j 0 kˆj −1 + 00 ωj −1 × θ˙ j 0 kˆj −1 ×

0 i−1 di

i=1 j =1

+

i 3  

θ˙ j 0 kˆj −1 ×

0

0 ωi

×

0 i−1 di



i=1 j =1

=

3  i 

θ¨ j 0 kˆj −1 ×

0 i−1 di

i=1 j =1

+

3  i  

0 0 ωj −1

× θ˙ j 0 kˆj −1 ×

0 i−1 di

i=1 j =1

+

i 3  

θ˙ j 0 kˆj −1 ×

0

0 ωi

×

0 i−1 di



(9.305)

i=1 j =1

to find the first half of the bias vector. i  3  

0 0 ωj −1

× θ˙ j 0 kˆj −1 ×

0 i−1 di

i=1 j =1

+

i 3  

θ˙ j 0 kˆj −1 ×

0

0 ωi

×

0 i−1 di



(9.306)

i=1 j =1

 Example 316 Inverse acceleration of a 2R planar manipulator. Inverse acceleration kinematics of 2R manipulator is a good example to review how to calculate joint accelerations. The analytic calculation of forward velocity and forward acceleration of the 2R planar manipulator is ˙ = J q˙ X # " #" # X˙ −l1 sin θ 1 −l2 sin (θ 1 + θ 2 ) θ˙ 1 = l1 cos θ 1 l2 cos (θ 1 + θ 2 ) θ˙ 1 + θ˙ 2 Y˙

(9.307)

¨ = J q¨ + J˙ q˙ = J q¨ + J q˙ 2 X # " #" # X¨ −l1 sin θ 1 −l2 sin (θ 1 + θ 2 ) θ¨ 1 = l1 cos θ 1 l2 cos (θ 1 + θ 2 ) θ¨ 1 + θ¨ 2 Y¨   " # 2 l1 cos θ 1 l2 cos (θ 1 + θ 2 ) θ˙ 1 −  2 l1 sin θ 1 l2 sin (θ 1 + θ 2 ) θ˙ 1 + θ˙ 2

(9.308)

"

"

9.5 Inverse Acceleration Kinematics

523

Also, the derivative and inverse of its Jacobian matrices are calculated as follows:     " # ˙ ˙ ˙ ˙ ˙ ˙J = −l1 θ 1 cθ 1 − l2 θ 1 + θ 2  c (θ 1 + θ 2 ) −l2 θ 1 + θ 2  c (θ 1 + θ 2 ) −l1 θ˙ 1 sθ 1 − l2 θ˙ 1 + θ˙ 2 s (θ 1 + θ 2 ) −l2 θ˙ 1 + θ˙ 2 s (θ 1 + θ 2 ) " # −1 −l2 s (θ 1 + θ 2 ) −l2 c (θ 1 + θ 2 ) −1 J = l1 l2 sθ 2 l1 cθ 1 + l2 c (θ 1 + θ 2 ) l1 sθ 1 + l2 s (θ 1 + θ 2 )

(9.309) (9.310)

The inverse acceleration kinematics of the manipulator q¨ will be calculated by substituting the matrices and performing matrix operations.     ¨ − J˙ q˙ = J−1 X ¨ − J q˙ 2 q¨ = J−1 X (9.311) "

# θ¨ 1 = θ¨ 1 + θ¨ 2 " #" # 1 l2 cos (θ 1 + θ 2 ) l2 sin (θ 1 + θ 2 ) X¨ −l1 cos θ 1 −l1 sin θ 1 Y¨ l1 l2 sθ 2   " # 2 1 l22 l1 l2 cos θ 1 θ˙ 1 +  2 −l12 −l1 l2 cos θ 1 l1 l2 sθ 2 θ˙ 1 + θ˙ 2

(9.312)

If we use the direct forward velocity equations, "

X˙ Y˙

#

" =

  # −l1 θ˙ 1 sin θ 1 − l2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) l1 θ˙ 1 cos θ 1 + l2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 )

(9.313)

we will have the forward acceleration equations by another differentiating. " #" # θ¨ 1 −l1 sθ 1 − l2 s (θ 1 + θ 2 ) −l2 s (θ 1 + θ 2 ) ¨ X= l1 cθ 1 + l2 c (θ 1 + θ 2 ) l2 c (θ 1 + θ 2 ) θ¨ 2  " # 2 θ˙ 1 l1 cos θ 1 l2 cos (θ 1 + θ 2 ) −  2 l1 sin θ 1 l2 sin (θ 1 + θ 2 ) θ˙ 1 + θ˙ 2

(9.314)

The joint acceleration vector q¨ can be found by matrix calculation. # " #−1 −l1 sθ 1 − l2 s (θ 1 + θ 2 ) −l2 s (θ 1 + θ 2 ) θ¨ 1 = θ¨ 2 l1 cθ 1 + l2 c (θ 1 + θ 2 ) l2 c (θ 1 + θ 2 )   " # ˙ 21 l cos θ l cos + θ θ (θ ) 1 1 2 1 2 ¨ + × X  2 l1 sin θ 1 l2 sin (θ 1 + θ 2 ) θ˙ 1 + θ˙ 2 " #" # −1 −l2 cos (θ 1 + θ 2 ) X¨ −l2 sin (θ 1 + θ 2 ) = cθ + l c + θ sθ + l s + θ l l Y¨ (θ ) (θ ) l1 l2 sθ 2 1 1 2 1 2 1 1 2 1 2   " # 2 −1 −l22 −l1 l2 cos θ 2 θ˙ 1 +  2 l1 l2 sθ 2 l1 (l1 + l2 cθ 2 ) l2 (l2 + l1 cθ 2 ) θ˙ 1 + θ˙ 2 "

(9.315)

As an example, let us consider a planar 2R manipulator with the following dimensions such that its end-effector moves on a straight line in one minute, 0 ≤ t ≤ 1 min. l1 = l 2 = 1 m X (0) = 1 m Y (0) = 0

(9.316) X (1) = −1 m Y (1) = 0

(9.317)

524

9 Acceleration Kinematics

Fig. 9.6 Inverse kinematics of the joint 1 as functions of time for a 2R robot

Fig. 9.7 Inverse kinematics of the joint 2 as functions of time for a 2R robot

Fig. 9.8 A planar polar manipulator

A septic time function for X will move the end-effector from X (0) = 1 to X (1) = −1 with zero velocity, zero acceleration, zero jerk at both ends. X = 1 − 70t 4 + 168t 5 − 140t 6 + 40t 7 (9.318) Employing inverse kinematics analysis, Figs. 9.6 and 9.7 depict the angle, angular velocity, and angular acceleration of joint 1 and 2 as function of time, respectively. Example 317 Inverse acceleration of a polar planar manipulator. The polar manipulator is also a planar manipulator, which is the main arm of many industrial robots. Its inverse acceleration kinematics is calculated here. Figure 9.8 illustrates a planar polar manipulator with the following forward velocity kinematics:

9.5 Inverse Acceleration Kinematics

525

"

˙ = Jq˙ X # " #" # X˙ cos θ −r sin θ r˙ = sin θ r cos θ θ˙ Y˙

(9.319) (9.320)

˙ and Y˙ are components of the global velocity of the tip point and [J] is the displacement Jacobian matrix of the The X, manipulator. " # cos θ −r sin θ J= (9.321) sin θ r cos θ To determine the acceleration of the end-effector, we take a derivative of (9.319). ¨ = J q¨ + J˙ q˙ X

(9.322)

The time derivative of Jacobian is " ˙J = d cos θ dt sin θ " −θ˙ sin θ = ˙ θ cos θ

−r sin θ r cos θ

#

−˙r sin θ − r θ˙ cos θ r˙ cos θ − r θ˙ sin θ

# (9.323)

and therefore, the forward acceleration of the manipulator is "

X¨ Y¨

#

" =

#" # cos θ −r sin θ r¨ sin θ r cos θ θ¨ " #" # −θ˙ sin θ −˙r sin θ − r θ˙ cos θ r˙ + ˙ θ cos θ r˙ cos θ − r θ˙ sin θ θ˙

(9.324)

 T ¨ we solve Eq. (9.324) for r¨ θ¨ . To determine q,   ¨ − J˙ q˙ q¨ = J−1 X ⎡

The inverse of Jacobian is J−1

⎤ cos θ sin θ ⎦ =⎣ 1 1 − sin θ cos θ r r

(9.325)

(9.326)

and therefore, the inverse acceleration of the manipulator would be   ¨ − J−1 J˙ q˙ q¨ = J−1 X ⎡ ⎤ " # " # cos θ sin θ ¨ r¨ ⎣ 1 ⎦ X = 1 ¨θ Y¨ − sin θ cos θ r r ⎡ ⎤ 0 −r θ˙ " r˙ # −⎣ 1 1 ⎦ ˙ θ θ˙ r˙ r r

(9.327)

(9.328)

526

9 Acceleration Kinematics

Fig. 9.9 Illustration of vectorial kinematics information of a link (i)

9.6

 Rigid Link Recursive Acceleration

Figure 9.9 illustrates a link (i) of a manipulator and shows its velocity and acceleration vectorial characteristics. Based on velocity and acceleration kinematics of rigid links we may write a set of recursive equations to relate the kinematics information of each link (i) in the coordinate frame attached to the previous link (i − 1). We may then generalize the method of analysis to be suitable for a robot with any number of links. Translational acceleration of the link (i) is denoted by 0 ai and is measured at the mass center Ci . Angular acceleration of the link (i) is denoted by 0 α i and is usually shown at the mass center Ci , although 0 α i is the same for all points of a rigid link. We calculate the translational acceleration of the link (i) at Ci by referring to the acceleration at the distal end of the link. Hence, 0

  ai = 0 d¨ i + 0 α i × 0 ri − 0 di    + 0 ωi × 0 ωi × 0 ri − 0 di

(9.329)

which can also be expressed in the body frame Bi . i 0 ai

  = i d¨ i + i0 α i × i ri − i di    + 0i ωi × i0 ωi × i ri − i di

(9.330)

The angular acceleration of the link (i) is  0αi =

+ θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1 if joint i is R if joint i is P 0 α i−1 0 α i−1

(9.331)

that can also be expressed in the body frame Bi .  ⎧ i−1 i ¨ i i−1 kˆi−1 ⎪ T α + θ ⎪ i−1 i−1 0 ⎪  ⎪ ⎨ i−1 i i−1 ˆ ˙ i + T ω × θ k i−1 i i−1 if joint i is R 0 i−1 0αi = ⎪ ⎪ ⎪ ⎪ ⎩i Ti−1 i−1 if joint i is P 0 α i−1 Proof According to rigid body acceleration in Eq. (9.183), the acceleration of the point Ci is

(9.332)

9.6  Rigid Link Recursive Acceleration 0

527

ai =

0

d 0 vi dt

= 0αi ×

(9.333) 0

    ri − 0 di + 0 ωi × 0 ωi × 0 ri − 0 di + 0 d¨ i

We can transform the acceleration of Ci to the body frame Bi . i

ai = 0 Ti−1 0 ai      = i0 α i × i ri − i di + i0 ωi × i0 ωi × i ri − i di + i d¨ i

(9.334)

Let us introduce the vectors 0 ni and 0 mi to define the position of oi−1 and oi with respect to the mass center Ci . ni = 0 di−1 − 0 ri

(9.335)

mi = di − ri

(9.336)

0 0

0

0

The vectors 0 ni and 0 mi simplify the dynamic equations of robots. Using Eq. (9.99) or (9.101) as a rule for adding relative angular accelerations, we find = 0 α i−1 +

0αi

0 i−1 α i

+ 0 ωi−1 ×

0 i−1 ωi

(9.337)

0 0 however, the angular velocity i−1 ωi and angular acceleration i−1 α i are 0 i−1 ωi

= θ˙ i 0 kˆi−1 if joint i is R

(9.338)

0 i−1 α i

= θ¨ i 0 kˆi−1 if joint i is R

(9.339)

0 i−1 ωi

= 0 if joint i is P

(9.340)

0 i−1 α i

= 0 if joint i is P

(9.341)

or

Therefore,

 0αi

=

+ θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1 if joint i is R if joint i is P 0 α i−1 0 α i−1

(9.342)

and it can be transformed to any coordinate frame, including Bi , to find Eq. (9.332). i 0αi

= 0 Ti−1 0 α i = i0 α i−1 + θ¨ i i0 kˆi−1 + i0 ωi−1 × θ˙ i i0 kˆi−1  i−1 ¨ i−1 ˆ ˙ i−1 ˆ = i Ti−1 i−1 0 α i−1 + θ i 0 ki−1 + 0 ωi−1 × θ i 0 ki−1

(9.343) 

Example 318  Recursive angular velocity equation for link (i). Recursive equations are very applied because they can be computerized and calculate kinematics of links one by one, each one related to the next or previous link. Here we derive angular velocity recursive equations for links of a robot. The recursive global angular velocity equation for a link (i) is  0 ωi

=

+ θ˙ i 0 kˆi−1 if joint i is R if joint i is P 0 ωi−1 0 ωi−1

We may transform this equation to the body frame Bi .

(9.344)

528

9 Acceleration Kinematics i 0 ωi

 = 0 Ti−1 0 ωi = 0 Ti−1 0 ωi−1 + θ˙ i 0 kˆi−1 = i0 ωi−1 + θ˙ i i kˆi−1  ˙ i−1 kˆi−1 = i Ti−1 i−1 0 ωi−1 + θ i

(9.345)

Therefore, we can use a recursive equation to find the locally expressed angular velocity of link (i) by having the angular velocity of its lower link (i − 1). In this equation, every vector is expressed in its own coordinate frame. 0 i 0 ωi

=



i−1 0 ωi−1 i−1 i Ti−1 0 ωi−1 i

Ti−1

+ θ˙ i

i−1 ˆ



ki−1

if joint i is R

(9.346)

if joint i is P

Example 319  Recursive translational velocity equation for link (i). Here we derive translational velocity recursive equations for links of a robot. The recursive global translational velocity equation for a link (i) is 0˙

di =

0

d˙ i−1 + 0 ωi × di−1 + 0 ωi ×

0˙

0 i−1 di 0 i−1 di

if joint i is R 0ˆ ˙ + di ki−1 if joint i is P

(9.347)

We may transform this equation to the body frame Bi . i ˙ 0 di

= 0 Ti−1 0 d˙ i = 0 Ti−1



0˙

di−1 + 0 ωi ×

0 i−1 di

i = i0 di−1 + i0 ωi × i−1 di + d˙i i kˆi−1  ˙ i−1 kˆi−1 + i0 ωi × = i Ti−1 i−1 0 di−1 + di

+ d˙i 0 kˆi−1

i i−1 di



(9.348)

Therefore, we may use a recursive equation to find the locally expressed translational velocity of link (i) by having the translational velocity of its lower link (i − 1). In this equation, every vector is expressed in its own coordinate frame.  ⎧ i ˙ i−1 kˆi−1 ⎪ Ti−1 i−1 ⎪ 0 di−1 + di ⎪ if joint i is R ⎨ i i˙ + i0 ωi × i−1 di di = ⎪ ⎪ ⎪ ⎩i i i Ti−1 i−1 if joint i is P 0 di−1 + 0 ωi × i−1 di

(9.349)

The angular velocity equation is a consequence of the relative velocity Eq. (7.55) and the rigid link’s angular velocity based on DH parameters (8.2). The translational velocity equation also comes from rigid link velocity analysis (8.3). Example 320  Recursive joints’ translational acceleration. Kinematics of joints of a robot may also be expressed by recursive equations. Here is the recursive acceleration analysis of joint i of a robot. Equations (9.329) and (9.331) determine the translational and angular accelerations of link (i) in the base coordinate frame B0 . We may similarly determine the recursive translational acceleration for a link (i) at joint i, using the translational acceleration of link (i−) at joint i − 1.

0¨

di =

⎧ 0¨ di−1 + 0 ω˙ i × i−10 di ⎪  ⎪ ⎪ ⎪ + 0 ωi × 0 ωi × i−10 di ⎪ ⎪ ⎨

if joint i is R

0¨ di−1 + 0 ω˙ i × i−10 di ⎪ ⎪  ⎪ ⎪ + 0 ωi × 0 ωi × i−10 di ⎪ ⎪ ⎩ +d¨i 0 kˆi−1 + 2 0 ωi × d˙i 0 kˆi−1 if joint i is P

(9.350)

where, 0 i−1 di

= 0 di − 0 di−1

(9.351)

9.6  Rigid Link Recursive Acceleration

529

We may also transform this equation to the local frame Bi . i ¨ 0 di

= 0 Ti−1 0 d¨ i = i d¨ i−1 + i0 ω˙ i ×

+ i0 ωi ×

i i−1 di

i

0 ωi

×

i i−1 di



+d¨i i kˆi−1 + 2 i0 ωi × d˙i i kˆi−1  ¨ ¨ i−1 kˆi−1 + 2 i0 ωi × d˙i i Ti−1 i−1 kˆi−1 = i Ti−1 i−1 0 di−1 + di   i i + i0 ωi × i0 ωi × i−1 di + i0 ω˙ i × i−1 di

(9.352)

Therefore, we may use a recursive equation to find the locally expressed translational acceleration of link (i) by having the translational acceleration of its lower link (i − 1). In this equation, every vector is expressed in its own coordinate frame. ⎧i ¨ Ti−1 i−1 ⎪ 0 di−1   ⎪ ⎪ i i ⎪ + 0 ωi × i0 ωi × i−1 di ⎪ ⎪ ⎪ i ⎪ + i0 ω˙ i × i−1 di if joint i is R ⎪ ⎪ ⎪ ⎨  i ¨ i−1 ¨ i 0 di = ¨i i−1 kˆi−1 d T + d ⎪ i−1 i−1 0 ⎪ ⎪ ⎪ ⎪ ⎪ +2 i0 ωi × d˙i i Ti−1 i−1 kˆi−1 ⎪ ⎪ ⎪ i ⎪ + i0 ωi × i0 ωi × i−1 di ⎪ ⎩ i i + 0 ω˙ i × i−1 di if joint i is P

(9.353)

Equation (9.350) is the result of differentiating (9.347), and Eq. (9.353) is the result of transforming (9.350) to the local frame Bi . Example 321  Recursive accelerations for revolute joints. There are many robots with only revolute joint. Kinematics of such robot will be simpler as reviewed in this example. If all joints of a robot are revolute, then Eq. (9.350) for translational acceleration of joint i becomes 0¨

di = 0 d¨ i−1 + 0 α i ×

0 i−1 di

+ 0 ωi ×



0 ωi

×

0 i−1 di



(9.354)

and the angular acceleration of link (i) becomes 0αi

= 0 α i−1 + θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1

(9.355)

Equation (9.344) also simplifies to the following equation for global angular velocity of link (i): 0 ωi

= 0 ωi−1 + θ˙ i 0 kˆi−1

(9.356)

Starting from the first link, we find the angular velocity of the first four links as 0 ω1

= θ˙ 1 0 kˆ0

0 ω2

= 0 ω1 + θ˙ 2 0 kˆ1 = θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1

(9.357) 0ˆ

0ˆ

(9.358)

0ˆ

0ˆ

0 ω3

= 0 ω2 + θ˙ 3 k2 = θ˙ 1 k0 + θ˙ 2 k1 + θ˙ 3 k2

(9.359)

0 ω4

= 0 ω3 + θ˙ 4 0 kˆ3 = θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1 + θ˙ 3 0 kˆ2 + θ˙ 4 0 kˆ3

(9.360)

and the angular velocity of the link (i) as 0 ωi

= 0 ωi−1 + θ˙ i 0 kˆi−1 =

i  j =1

θ˙ j 0 kˆj −1

(9.361)

530

9 Acceleration Kinematics

The first four angular accelerations will be 0α1

= θ¨ 1 0 kˆ0

0α2

= 0 α 1 + θ¨ 2 0 kˆ1 + 0 ω1 × θ˙ 2 0 kˆ1

(9.362)

= θ¨ 1 0 kˆ0 + θ¨ 2 0 kˆ1 + θ˙ 1 0 kˆ0 × θ˙ 2 0 kˆ1 0α3

(9.363)

= 0 α 2 + θ¨ 3 0 kˆ2 + 0 ω2 × θ˙ 3 0 kˆ2 = θ¨ 1 0 kˆ0 + θ¨ 2 0 kˆ1 + θ˙ 1 0 kˆ0 × θ˙ 2 0 kˆ1  +θ¨ 3 0 kˆ2 + θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1 × θ˙ 3 0 kˆ2 = θ¨ 1 0 kˆ0 + θ¨ 2 0 kˆ1 + θ¨ 3 0 kˆ2  +θ˙ 1 0 kˆ0 × θ˙ 2 0 kˆ1 + θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1 × θ˙ 3 0 kˆ2

0α4

(9.364)

= 0 α 3 + θ¨ 4 0 kˆ3 + 0 ω3 × θ˙ 4 0 kˆ3 = θ¨ 1 0 kˆ0 + θ¨ 2 0 kˆ1 + θ¨ 3 0 kˆ2 + θ¨ 4 0 kˆ3  +θ˙ 1 0 kˆ0 × θ˙ 2 0 kˆ1 + θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1 × θ˙ 3 0 kˆ2  + θ˙ 1 0 kˆ0 + θ˙ 2 0 kˆ1 + θ˙ 3 0 kˆ2 × θ˙ 4 0 kˆ3

(9.365)

or 0α4

 = θ˙ 1 0 kˆ0 × θ˙ 2 0 kˆ1 + θ˙ 3 0 kˆ2 + θ˙ 4 0 kˆ3  +θ˙ 2 0 kˆ1 × θ˙ 3 0 kˆ2 + θ˙ 4 0 kˆ3 + θ˙ 3 0 kˆ2 × θ˙ 4 0 kˆ3

(9.366)

So, we can find the angular acceleration of link (i) as 0αi

= 0 α i−1 + θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1  j  i i−1    = θ¨ j 0 kˆj −1 + θ˙ k 0 kˆk−1 × θ˙ j +1 0 kˆj j =1

j =1

or 0αi

=

i 

θ¨ j 0 kˆj −1 +

j =1

i−1 

(9.367)

k=1

⎛ ⎝θ˙ j 0 kˆj −1 ×

j =1

i 

⎞ θ˙ k 0 kˆk−1 ⎠

(9.368)

k=j +1

However, because of 0 kˆi × 0 kˆi = 0, we can rearrange the equation as 0αi

= 0 α i−1 + θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1  j  i i    0 0 0 θ¨ j kˆj −1 + θ˙ k kˆk−1 × θ˙ j kˆj −1 = j =1

j =1

k=1

Using the above equations, we can find the translational acceleration of frame Bi .

(9.369)

9.6  Rigid Link Recursive Acceleration

531

0¨

di = 0 d¨ i−1 + 0 α i × =

i  j =1

=

0αj

×

0 j −1 dj

+

i 

0 ωj

×

 



0 ωi

×

0 i−1 di

0 ωj

×

0 j −1 dj



j =1



j i   j =1

+ 0 ωi ×

0 i−1 di

θ¨ m 0 kˆm−1 +

m=1

 m j  

m=1

 θ˙ k 0 kˆk−1 × θ˙ m 0 kˆm−1

k=1



× j −10 dj

+

j i   j =1

 0ˆ

θ˙ m km−1 ×

m=1

j 

 0ˆ

θ˙ m km−1 ×

0 j −1 dj

(9.370)

m=1

Example 322  Jacobian rate generating vector. Jacobian matrix is a key point for derivative kinematics of a robot. The elements of Jacobian matrix are related to velocity kinematics and hence, Jacobian can also be expressed by recursive equations. The forward acceleration kinematic needs differential of Jacobian matrix. "0 # "0 # an d¨ = 0 n = J q¨ + J˙ q˙ (9.371) 0 αn ω˙ n From Eq. (8.136) we know that

#

"0

d˙ n 0 ωn

=

n 



i=1

 ki−1 × i−10 dn ˙ θi 0ˆ ki−1

0ˆ

(9.372)

Taking a derivative, we have "0

d¨ n 0 ω˙ n

# =

"0 0

an αn

# =

To expand this equation, let us begin with

n  i=1



0ˆ

ki−1 × 0ˆ ki−1

0 i−1 dn



⎡  ⎤ d 0ˆ 0 n k × d  i−1 i−1 n ⎥ ⎢ dt θ¨ i + ⎣ ⎦ θ˙ i d 0ˆ k i=1 i−1 dt

(9.373)

d 0ˆ ki−1 . dt d 0ˆ ki−1 = 0 ωi−1 × 0 kˆi−1 dt

(9.374)

 d 0ˆ ki−1 = θ˙ j 0 kˆj −1 × 0 kˆi−1 dt j =1

(9.375)

Using (9.361), we have i−1

The first row of (9.373) is d 0 ˆ ki−1 × dt

0 i−1 dn



=

d 0 kˆi−1 × dt

0 i−1 dn

d + 0 kˆi−1 × dt

0 i−1 dn

(9.376)

Let us use 0 i−1 di

= 0 di − 0 di−1

0 i−1 dn

=

n  j =i

0 j −1 dj

(9.377) (9.378)

532

9 Acceleration Kinematics

to find the first term as d 0 kˆi−1 × dt

⎛ 0 i−1 dn

=⎝

i−1 

⎞ θ˙ j 0 kˆj −1 × 0 kˆi−1 ⎠ ×

j =1

n 

=

0 k−1 dk

n  i−1  

kj −1 × 0 kˆi−1 ×

0ˆ

0 k−1 dk

θ˙ j

(9.379)

j =1 k=i

k=i

The second term is 0ˆ

ki−1 ×

d dt

0 i−1 dn

n d  = 0 kˆi−1 × dt k=i n  d = ki−1 × dt k=i 0ˆ

n 

= 0 kˆi−1 ×

0 k−1 dk

0 k−1 dk

×

0 ωk

0 k−1 dk

k=i

=

n 



0ˆ

ki−1 ×

k=i

=

n 

0 ωk

⎛ 0ˆ

ki−1 × ⎝

k 

0 k−1 dk

 ⎞

θ˙ j 0 kˆj −1 ×

0 ⎠ k−1 dk

j =1

k=i

=

×

n  k 

0ˆ

ki−1 ×



0ˆ

kj −1 ×

0 k−1 dk



θ˙ j

(9.380)

k=i j =1

Therefore, d 0 ˆ ki−1 × dt

0 i−1 dn



i−1  

kj −1 × 0 kˆi−1 ×

0ˆ

0 j −1 dj

θ˙ j

j =1

n  k 

+

=

0ˆ

ki−1 ×



0ˆ

kj −1 ×

0 k−1 dk



θ˙ j

(9.381)

k=i j =1

and Eq. (9.373) becomes "0

v˙ n 0 ω˙ n

# =

n  i=1

⎡/ n ⎢  ⎢ ⎢ + ⎢ i=1 ⎣



 ki−1 × i−10 dn ¨ θi 0ˆ ki−1

0ˆ



kj −1 × 0 kˆi−1 × j −10 dj θ˙ j  / / + nk=i kj =1 0 kˆi−1 × 0 kˆj −1 ×

i−1 j =1

/i−1

⎤

0ˆ

0 k−1 dk



θ˙ j 0 kˆj −1 × 0 kˆi−1

j =1

⎥ θ˙ j ⎥ ⎥ θ˙ i ⎥ ⎦

(9.382)

The first bracket is the Jacobian matrix, J=

n 



i=1

 =

0ˆ

0ˆ

ki−1 × i−10 dn 0ˆ ki−1



0 k0 × 00 dn 0 kˆ1 × 01 dn · · · 0 kˆn−1 × n−1 dn 0ˆ 0ˆ 0ˆ k0 k1 kn−1 ···

 (9.383)

9.6  Rigid Link Recursive Acceleration

533

and the second bracket is the time rate of Jacobian matrix. ⎡/  i−1 0 ˆ kj −1 × 0 kˆi−1 × j −10 dj θ˙ j j =1  n ⎢ / /  ⎢ + nk=i kj =1 0 kˆi−1 × 0 kˆj −1 × ⎢ ˙J = ⎢ i=1 ⎣ /i−1 0 0ˆ ˙ ˆ j =1 θ j kj −1 × ki−1

⎤ 0 k−1 dk



⎥ θ˙ j ⎥ ⎥ ⎥ ⎦

(9.384)

Similar to the Jacobian generating vector ci in (8.125), we can define a Jacobian generating rate vector c˙ i , ⎡/ ⎢ ⎢ c˙ i (q) = ⎢ ⎢ ⎣

kj −1 × 0 kˆi−1 × j −10 dj θ˙ j  / / + nk=i kj =1 0 kˆi−1 × 0 kˆj −1 ×

i−1 j =1

/i−1

j =1

to determine J˙ column by column.



⎤

0ˆ

0 k−1 dk



0ˆ

kj −1 × 0 kˆi−1 θ˙ j

⎥ θ˙ j ⎥ ⎥ ⎥ ⎦

  J˙ = c˙ 1 c˙ 2 c˙ 3 · · · c˙ n

(9.385)

(9.386)

Example 323  Recursive acceleration in base frame. The acceleration of a link can be calculated by its velocity transformation matrix. The acceleration of connected links can also be calculated recursively. Let us start with computing 0 V˙i , for the absolute accelerations of link (i), 0 ˙ Vi = 0 T¨i 0 Ti−1 (9.387) and then, calculating the acceleration matrix for link (i + 1). 0

−1 V˙i+1 = 0 T¨i+1 0 Ti+1   −1 = 0 T¨i i Ti+1 + 2 0 T˙i i T˙i+1 + 0 Ti i T¨i+1 0 Ti+1 −1 0 −1 = 0 T¨i 0 Ti−1 + 2 0 T˙i 0 Ti−1 0 Ti i T˙i+1 i Ti+1 Ti −1 0 −1 + 0 Ti i T¨i+1 i Ti+1 Ti

(9.388)

These two equations can be put in a recursive form. 0

V˙i+1 = 0 V˙i + 2 0 V˙i 0 Ti i Vi+1 0 Ti−1 + 0 Ti i V˙i+1 0 Ti−1

For a revolute joint, we have

⎡

i

Vi+1 = q˙i+1 R = θ˙ i+1

⎤ 0 −1 0 0 ⎢1 0 0 0⎥ ⎥ R = θ˙ i+1 ⎢ ⎣0 0 0 0⎦ 0 0 00

(9.389)

(9.390)

and therefore, i

2 V˙i+1 = θ¨ i+1 R − θ˙ i+1 R TR ⎡ ⎤ ⎡ ⎤ 0 −1 0 0 1000 ⎢1 0 0 0⎥ ⎢ ⎥ ⎥ ˙2 ⎢ 0 1 0 0 ⎥ = θ¨ i+1 ⎢ ⎣ 0 0 0 0 ⎦ − θ i+1 ⎣ 0 0 0 0 ⎦ 0 0 00 0000

(9.391)

534

9.7

9 Acceleration Kinematics

 Second Derivative and Coordinate Frames

The time derivative of a vector depends on the coordinate frame in which it is expressed and the frame in which we are taking the derivative. Consider a global frame G (OXY Z) and a body frame B (Oxyz). The second derivative of a vector follows the same rule of the first G and B-derivative of the G and B-vector. The derivative of a B-vector B v in B and the derivative of a G-vector G v in G are B

v = xˆ ˙ ı + y˙ jˆ + z˙ kˆ

(9.392)

G

v = X˙ Iˆ + Y˙ Jˆ + Z˙ Kˆ

(9.393)

B

a=

G

a=

B 2

d dt 2

B

r=

G 2

d dt 2

G

r=

B

dB v= dt

B

r¨ = x¨ ıˆ + y¨ jˆ + z¨ kˆ

(9.394)

G

dG v = G r¨ = X¨ Iˆ + Y¨ Jˆ + Z¨ Kˆ dt

(9.395)

We call G a and B a simple accelerations because they are simple derivatives of the simple velocities G v and B v. We may also B calculate the mixed derivatives and find the G-derivative of BG v and the B-derivative of G B v. The velocity G v is B-expression G of G-velocity of a point, and B v is the G-expression of B-velocity of a point. The G-derivative of BG v is G B  dB B B B B B a = (9.396) G G v = G α B × r + G ωB × G ωB × r dt and the B-derivative of G B v is G Ba

=

B

dG v = − GαB × dt B

G

r+

G ωB

×



G ωB

×

G

 r

(9.397)

We call BG a the B-expression of the G-acceleration and G B a the G-expression of the B-acceleration. The left superscript of B a indicates the frame in which a is expressed, and the left subscript indicates the frame in which derivative is taken. If G the left super and subscripts of an acceleration vector are the same, it is a simple acceleration vector and we only keep the superscript. To read the mixed accelerations BG a and G B a, we may use the G-acceleration of a B-velocity and B-acceleration of a G-velocity, respectively. When the interested body point P is not a fixed point in B, then P is moving in frame B with a variable body position B rP = B rP (t) and velocity B vP = B vP (t). The mixed derivatives of v (t) are defined by B Ga

= =

G Ba

= =

G

dB v (t) dt G

× B r + 2 BG ωB   × B v + BG ωB × BG ωB × B r B

a+

B GαB

(9.398)

B

dG v (t) dt B

r − 2 G ωB   × G v + G ωB × G ωB × G r G

a−

GαB

×

G

(9.399)

Proof A vector is called a B-vector if it is expressed in the B-frame, and similarly it is a G-vector if it is expressed in the G-frame. If B represents a rigid body, then P is a body point and hence is fixed in B. For a body point, the body position vector B r is constant in B and its body derivatives would be zero. Having a rotating B-frame in a fixed global G-frame, we can define eight different accelerations as the second time derivatives of a position vector r: G

1.

G d Gd G dG r= v= dt dt dt

G

a

(9.400)

9.7  Second Derivative and Coordinate Frames

535 G

G d Gd B dB r= v= dt dt dt G

2.

G d Bd G dG r= v= dt dt dt B

BB GG a

=

B Ga

(9.401)

G

3.

G d Bd B dB r= v= dt dt dt

GG GB a

(9.402)

BB GB a

(9.403)

GG BG a

(9.404)

BB BG a

(9.405)

G

4.

B

B d Gd G dG r= v= dt dt dt

5.

B

B d Gd B dB r= v= dt dt dt G

6.

B

B d Bd G dG r= v= dt dt dt B

7.

B d Bd B dB r= v= dt dt dt

GG BB a

=

G Ba

(9.406)

B

8.

B

(9.407)

a

Only the first and eighth second derivatives are simple accelerations. The two simple accelerations of (9.394) and (9.395), can be found by simple differentiation of a vector in the same frame in which they are expressed. Therefore, the G-derivative of the G-velocity and the B-derivative of the B-velocity provide the simple G and B-accelerations, respectively: G 2

d dt 2

G

B 2

d dt 2

B

G G dG d dG v= r= dt dt dt = X¨ Iˆ + Y¨ Jˆ + Z¨ Kˆ

r=

G

B B dB d dB v= r= dt dt dt = x¨ ıˆ + y¨ jˆ + z¨ kˆ

r=

G

a=

G

r¨ (9.408)

B

B

a=

B

r¨ (9.409)

Recalling the derivative transfer formula (7.214) and the mixed velocities, G

dB r= dt =

B Gv B

B

dG r= dt =

G Bv G

=

v+

=

v−

B ˙ Gr

=

B G ωB

G ˙ Br

=

G ωB

B

dB r+ dt

B G ωB

× Br

× Br

(9.410)

G

dG r (t) − dt

×

G

r

G ωB

×

G

r (9.411)

we can find the mixed accelerations in (9.401)–(9.406). The simple acceleration cases of 1 and 8, and the mixed acceleration cases of 2 and 7 are being used in robotic analysis. The second case, BG a = BB GG a, is the B-expression of a G-acceleration. It happens when the position vector of a point is given in B while derivatives are taken in G:

536

9 Acceleration Kinematics

B Ga

G   dB d B v + BG ωB × B r Gv = dt dt   B B B = a + G ωB × v + BG α B + BG ωB × BG ωB × B r   + BG ωB × B v + BG ωB × B r BB GG a

=

× B r + 2 BG ωB × B v   + BG ωB × BG ωB × B r B

=

G

=

a+

B GαB

(9.412)

The first term, B a, is the body acceleration of the moving point P in B, regardless of the rotation of B in G. So, B a can be assumed as the acceleration of the moving point in B with respect to a body fixed point that is coincident with P at the moment. The second term, BG α B × B r, is the tangential acceleration of a body point that is coincident with P at the moment. The third term, 2 BG ωB × B v, is the  Coriolis acceleration and is the result of a moving point in B while B is rotating in G. The fourth term, BG ωB × BG ωB × B r , is the centripetal acceleration of a body point that is coincident with P at the moment. The B-expression of the G-acceleration, BG a, is the most applied and practical acceleration in the dynamics of robots and rigid bodies. In robotics, the point P is a fixed point in B and the acceleration BG a simplifies to B Ga

G

dB v= dt G

=

B GαB

× Br +

B G ωB

×

B

G ωB

 × Br

(9.413)

A body point will have only a tangential and a centripetal acceleration. Using BG a is a practical method of acceleration analysis as we usually need to measure the kinematic information of a moving link in the body coordinate frame. G The third case, GG GB a, is the G-expression of the G-derivative of B v that is the B-derivative of the G-expression of a position vector. GG GB a

G B G  dG d dG d G r= v− Bv = dt dt dt dt

G

= =

G

a−

GαB

×

G

r−

G ωB

×

G

G ωB

×

G

 r (9.414)

v

GG For a fixed point in B, we have G B v = 0 and GB a = 0 and Eq. (9.414) reduces to the G-expression of (9.413). BB The fourth case, GB a, is the B-expression of the G-derivative of BB v that is the B-derivative of the B-expression of a position vector.

BB GB a

= =

G B G  dB d dB d B  v= r= v dt dt dt dt

G

B

a+

B G ωB

× Bv

(9.415)

G The fifth case, GG BG a, is the G-expression of the B-derivative of G v that is the G-derivative of the G-expression of a position vector.

GG BG a

= =

B G B  dG d dG d G  v= r= v dt dt dt dt

B

G

a−

G ωB

×

G

(9.416)

v

G The sixth case, BB BG a, is the B-expression of the B-derivative of B v that is the B-derivative of the G-expression of a position vector:

BB BG a

= =

B G B  dB d dB d B r= v+ Gv = dt dt dt dt

B

B

a+

B GαB

× Br +

B G ωB

× Bv

B G ωB

 × Br (9.417)

9.7  Second Derivative and Coordinate Frames

The seventh case, position vector:

GG BB a,

537

is the G-expression of the B-derivative of G B v that is the B-derivative of the G-expression of a

G Ba

= GG BB a =

B B B  dG d dG d G r= v− Bv = dt dt dt dt

B

=

v − ( G α B − G ωB × G  − G ωB × v − G ωB × G r

=

r − 2 G ωB ×   + G ωB × G ωB × G r

G

G

a−

G ωB

a−

GαB

×

G

×

G

G

G ωB

×

G ωB )

× G

G

 r

r

v (9.418)

B The seventh acceleration, G B a, is the same as the second acceleration, G a, if we switch the name of the coordinate frames B B and G. So, G a is the G-acceleration of a moving point in B while the observer is in the B-frame, and G B a is the G-acceleration of a moving point in B when the observer is in G. These accelerations for a point of a rigid link are

G

a=

B Ga

=

G

d Gd G r = X¨ Iˆ + Y¨ Jˆ + Z¨ Kˆ dt dt

G

d Gd B r= dt dt

GG GB a

=

BB GB a

=

B GαB

G

d Bd G r= dt dt

G

× Br +

a−

GαB

(9.419)

B G ωB

×

G

×

r−

B

G ωB

G ωB

 × Br

(9.420)

×

(9.421)

G

d Bd B r=0 dt dt

v

G

GG BG a

=

BB BG a

=

G Ba

= =

B

a=

(9.422)

B

d Gd G r= dt dt d Gd B r= dt dt

G

a−

B

B GαB

G ωB

×

G

v

× Br

(9.423) (9.424)

B

d Bd G r dt dt

r − 2 G ωB ×  + G ωB × G ωB × G r G

a−

GαB



×

G

d Bd B r=0 dt dt

G

v (9.425)

B

(9.426)

Considering the Newton equation of motion, F = ma for a particle of mass m. The equation of motion in the global coordinate frame G is G G d dG G F = m Ga = m r (9.427) dt dt It means that both derivatives of acceleration must be taken in G. If the position vector r was expressed in the body coordinate frame B, then the Newton equation of motion would be B

F = m BG a = m

G

d Gd B r dt dt

(9.428) 

538

9 Acceleration Kinematics

Example 324 Transformation method to find mixed derivatives. The mixed derivatives are easier to find if we calculate simple derivatives and then transform the result to the coordinate we need. Here is how. It is more applied if we transform the position vector to the same frame in which we are taking the derivative and then apply the differential operator. Let us assume a point is moving in a body coordinate frame with the position vector B r, ⎡

⎤ cos αt B r = ⎣ sin αt ⎦ 0

α = const.

(9.429)

while the body coordinate frame is turning about the X-axis with angular velocity and acceleration γ˙ and γ¨ . G ωB

= γ˙ Iˆ

GαB

= γ¨ Iˆ

(9.430)

To determine BG v and BG a, we transform B r to the G-frame. G

r=

G

⎡

RB B r

⎤⎡ ⎤ ⎡ ⎤ 1 0 0 cos αt cos αt = ⎣ 0 cos γ − sin γ ⎦ ⎣ sin αt ⎦ = ⎣ sin αt cos γ ⎦ 0 sin γ cos γ 0 sin αt sin γ

(9.431)

Taking derivatives provides the global velocity and acceleration: ⎡

⎤ −α sin αt dG G v= r = ⎣ α cos αt cos γ − γ˙ sin αt sin γ ⎦ dt α cos αt sin γ + γ˙ sin αt cos γ G

G

G

dG v dt ⎡ ⎤ −α 2 cos αt  2  = ⎣ − α + γ˙ 2  sin αt cos γ − (γ¨ sin αt + 2α γ˙ cos αt) sin γ ⎦ − α 2 + γ˙ 2 sin αt sin γ + (γ¨ sin αt + 2α γ˙ cos αt) cos γ

a=

(9.432)

(9.433)

Using the kinematic transformation matrix G RB , we are able to calculate BG v and BG a: ⎡

⎤ −α sin αt B G T G RB v = ⎣ α cos αt ⎦ Gv = γ˙ sin αt ⎤ −α 2 cosαt  B G T G RB a = ⎣ − α 2 + γ˙ 2 sin αt ⎦ Ga = γ¨ sin αt + 2α γ˙ cos αt

(9.434)

⎡

(9.435)

Example 325 Mixed velocity and simple acceleration of a moving point in B. This is to show how velocity and acceleration of point is calculated in different frames and expressed in different frames. Consider a body frame B(Oxyz) that is rotating in a global frame G(OXY Z) with an angular velocity α˙ about the Z-axis, and a moving point P in B at B rP (t). B rP (t) = t 2 ıˆ (9.436)

9.7  Second Derivative and Coordinate Frames

539

Fig. 9.10 Top view of a rotating body frame B in G with an angular velocity α˙ about the Z-axis and a moving point P in B

Figure 9.10 illustrates a top view of the system when B is at an angle α. The global position vector of the point is ⎡

⎤⎡ 2⎤ cos α − sin α 0 t G rP = G RB B rP = RZ,α (t) B rP = ⎣ sin α cos α 0 ⎦ ⎣ 0 ⎦ 0 0 1 0 = t 2 cos α Iˆ + t 2 sin α Jˆ The angular velocity of G ωB is ˜B Gω

=

G

(9.437)

R˙ B G RBT = α˙ K˜

G ωB

= α˙ Kˆ

(9.438)

It may also be verified that the body expression of the angular velocity is B ˜B Gω

=

G

RBT

G ˜ B G RB Gω

= α˙ k˜

B G ωB

= α˙ kˆ

(9.439)

The simple velocities of P in both frames are B

vP =

G

vP =

B

dB rP = dt

B

r˙ P = 2t ıˆ

(9.440)

G

dG rP = G r˙ P dt     = 2t cos α − t 2 α˙ sin α Iˆ + 2t sin α + t 2 α˙ cos α Jˆ

(9.441)

For the mixed velocities we start with BG v, which is the B-expression of the G-velocity of P . B G vP

B dB dB rP = rP + BG ωB × B rP dt dt ⎡ ⎤ ⎡ ⎤ ⎡ 2⎤ ⎡ ⎤ 2t 0 t 2t = ⎣ 0 ⎦ + α˙ ⎣ 0 ⎦ × ⎣ 0 ⎦ = ⎣ t 2 α˙ ⎦ 0 1 0 0

=

G

= 2t ıˆ + t 2 α˙ jˆ

(9.442)

It can also be found by a coordinate frame transformation. B G vP

=

G

RBT

G

vP = 2t ıˆ + t 2 α˙ jˆ

(9.443)

540

9 Acceleration Kinematics

The next mixed velocity is G B vP , which is the G-expression of the B-velocity of P . G dG dG rP = rP − G ωB × G rP dt dt ⎡ ⎤ ⎡ ⎤ ⎡ 2 ⎤ 2t cos α − t 2 α˙ sin α 0 t cos α = ⎣ 2t sin α + t 2 α˙ cos α ⎦ − α˙ ⎣ 0 ⎦ × ⎣ t 2 sin α ⎦ 0 1 0 ⎡ ⎤ 2t cos α = ⎣ 2t sin α ⎦ = (2t cos α) Iˆ + (2t sin α) Jˆ 0

=

G B vP

B

(9.444)

To find this velocity, we can also apply a kinematic transformation G RB to B vP . G B vP

=

G

RB B vP = (2t cos α) Iˆ + (2t sin α) Jˆ

(9.445)

The simple accelerations of P are B

aP =

G

aP =

B

dB vP = dt

B

r¨ P = 2ˆı

(9.446)

G

dG vP = G r¨ P dt   = 2 cos α − 4t α˙ sin α − t 2 α¨ sin α − t 2 α˙ 2 cos α Iˆ   + 2 sin α + 4t α˙ cos α + t 2 α¨ cos α − t 2 α˙ 2 sin α Jˆ

(9.447)

These velocities and simple accelerations of the moving point P in B help us to determine the mixed accelerations of P . The acceleration BG a = BB GG a is B Ga

=

B

⎡

a+

B GαB

× B r + 2 BG ωB × B v + ⎤ 2

B G ωB

×

B

G ωB

 × Br

2 − t 2 α˙ = ⎣ t (4α˙ + t α) ¨ ⎦ 0

(9.448)

We must also be able to determine the mixed accelerations BG a by a kinematic transformation: B Ga

=

G

⎡

RBT

G

a

⎤T ⎡ ⎤ cos α − sin α 0 2t cos α − t 2 α˙ sin α = ⎣ sin α cos α 0 ⎦ ⎣ 2t sin α + t 2 α˙ cos α ⎦ 0 0 1 0 ⎡ ⎤ 2 − t 2 α˙ 2 = ⎣ t (4α˙ + t α) ¨ ⎦ 0

(9.449)

9.7  Second Derivative and Coordinate Frames

541

The acceleration GG GB a is GG GB a

=

G

⎡

a−

GαB

×

G

r−

×

G ωB

⎤

G

v

2 cos α − 2t α˙ sin α ⎣ = 2 sin α + 2t α˙ cos α ⎦ 0 the acceleration BB GB a is

(9.450)

⎡

⎤ 2 BB B B B ⎣ 2t α˙ ⎦ GB a = a + G ωB × v = 0

the acceleration GG BG a is

(9.451)

⎡

⎤ 2 cos α − (αt ¨ + 2α) ˙ t sin α GG G a − G ωB × G v = ⎣ 2 sin α + (αt ¨ + 2α) ˙ t cos α ⎦ BG a = 0

the acceleration BB BG a is

⎤ 2 BB B B B B B ⎣ (αt ¨ + 2α) ˙ t⎦ BG a = a + G α B × r + G ωB × v = 0

(9.452)

⎡

(9.453)

and the acceleration BB BG a is G Ba

=

GG BB a

=

G

⎡

a−

GαB

⎤ 2 cos α = ⎣ 2 sin α ⎦ 0

×

G

r − 2 G ωB ×

G

v+

G ωB

×



G ωB

×

G

 r (9.454)

We can also determine the mixed accelerations G B a by a kinematic transformation. G Ba

=

G

⎡

RB

B

a

cos α − sin α = ⎣ sin α cos α 0 0

⎤⎡ ⎤ ⎡ ⎤ 0 2 2 cos α 0 ⎦ ⎣ 0 ⎦ = ⎣ 2 sin α ⎦ 1 0 0

(9.455)

Example 326  Second-derivative transformation formula. When a position vector is given in the B-frame and its acceleration is needed to be used in Newton equation of motion, two G-derivatives need to be taken. The result is the acceleration of the global acceleration of the point, expressed in B-frame. Such derivative is needed to express equation of motion of moving objects such as vehicle, ship, airplane, etc. Consider a point P that can move in the body coordinate frame B (Oxyz). Using the derivative transformation formula (7.214) we find the B-expression of the G-velocity. G

dB rP = dt

B G vP

=

B

dB rP + dt

B G ωB

× B rP

(9.456)

Another G-derivative of this equation provides the B-expression for the global acceleration of P as Eq. (9.412). B Ga

=

B

a+

B GαB

× B r + 2 BG ωB × B v +

B G ωB

×

B

G ωB

 × Br

(9.457)

542

9 Acceleration Kinematics

Using this result, we can define the second-derivative transformation formula of a B-vector B  from the body to the global coordinate frame: G

d Gd B = dt dt =

B ¨ G B

d Bd B  + BG α B × B  dt dt B dB B B B  + G ωB ×  +2 G ωB × dt

(9.458)

¨ shows the second global time derivative expressed in the body frame, or simply the B-expression of the The final result BG  second G-derivative of a B-vector B . The vector B  may be any vector quantity such as position, velocity, angular velocity, momentum, angular momentum, a time varying force vector, etc. Example 327  R¯az¯ı acceleration aRa . If an A-frame is moving in a B-frame, while B-frame is moving in C-frame, we may have the first derivative of an A-vector in the B-frame and the second derivative in the C-frame. This would be a mixed second derivative, which was the base to discover a new acceleration term called Razi acceleration. Consider three relatively rotating frames A, B, C. The mixed double acceleration AA CB a is the A-expression of the second A derivative of a position vector r when the first derivative is taken in the B-frame and the second derivative is taken in the C-frame. AA CB a

=

 A A A A A A a+ A C ωA × v + B α A × r + C ωA × B ωA × r A  A A A +A B ωA × v + B ωA × C ωA × r A

(9.459)

The mixed double derivative is made when we take the first and second derivatives of A r in different coordinate frames B and C. Using the derivative transformation formula, the B-derivative of A r is B

dA r= dt

A Bv

=

A

dA A r+ A B ωA × r dt

(9.460)

A time derivative of this equation in a third frame C would be C

C d Bd A dA r= v= dt dt dt B

AA CB a

=

C

d dt

C  dA d A A r + B ωA × r dt dt

A

A d Ad A dA r+ A r C ωA × dt dt dt A dA A A ωA + C ωA × B ωA × A r + dt B A dA A A A + B ωA × r + C ωA × r dt A  A A A A A = Aa + A C ωA × v + B α A × r + C ωA × B ωA × r A  A A A +A B ωA × v + B ωA × C ωA × r

=

A

(9.461)

A We call the acceleration AA CB a the mixed double acceleration. The first term a is the local A-acceleration of a moving point A A A A P in the A-frame. The combined terms C ωA × v + B ωA × v is the mixed Coriolis acceleration.

aCo =

AA CB aRa

=

A C ωA

A × Av + A B ωA × v

(9.462)

9.8 Summary

543

A  A A The term A acceleration of P . The term A B B ωA × C ωA × r is the mixed centripetal acceleration. Aα A × Ar is the  tangential The term C ωA × B ωA × A r is a new term in the acceleration of P that cannot be seen in the simple acceleration BG a. This term is called the R¯az¯ı acceleration aRa . aRa =

AA CB aRa

A

=

C ωA

 A ×A B ωA × r

(9.463)

The difference between the two terms in the Coriolis acceleration is clearer when we consider three coordinate frames B1 , B2 , and G. The frame B2 is turning in B1 , and B1 is turning in G. There is also a moving point P in a coordinate frame B2 . The B2 -expression of the velocity of P in B1 is 2 1v

=

1

2 d2 d2 r= r + 21 ω2 × 2 r = 2 v + 21 ω2 × 2 r dt dt

(9.464)

The derivative of 21 v in G provides a double mixed acceleration 22 G1 a, d 1d 2 r= dt dt

G

=

G

22 G1 a

d = dt

G  d2 d 2 r + ω2 × 2 r dt dt 1

2

2

2 d 2d 2 d2 r + 2G ω2 × r dt dt dt 2 d2 2 2 ω2 + G ω2 × 1 ω2 × 2 r + dt 1 2 d2 2 2 2 + 1 ω2 × r + G ω2 × r dt

(9.465)

that can be simplified to d 1d 2 r = 2a + dt dt

G

2 G ω2

× 2 v + 21 α 2 × 2 r +

× 2 r + 21 ω2 × 2 v + 21 ω2 ×

2

2

G ω2

G ω2

× 21 ω2

 × 2r

 (9.466)

which is equivalent to (9.459) and indicates that, if B1 = G, then 2G ω2 × 2 v = 21 ω2 × 2 v, and there exists a mixed Coriolis acceleration: 2 2 2 2 aCo = 22 (9.467) G1 aCo = 1 ω2 × v + G ω2 × v Furthermore, when B1 = G, there exists a R¯az¯ı acceleration term aRa in the acceleration of P that cannot be seen in BG a: aRa =

22 G1 aRa

=

2

G ω2

 × 21 ω2 × 2 r

(9.468)

Zakariy¯a R¯az¯ı (Rhazes, 865 − 925) was a Persian mathematician, chemist, and physician who is considered the father of pediatrics.

9.8

Summary

When a body coordinate frame B and a global frame G have a common origin, the global acceleration of a point P in frame B is G   dG G r¨ = vP = G α B × G r + G ω B × G ω B × G r (9.469) dt where, G α B is the angular acceleration of B with respect to G GαB

=

G

d G ωB . dt

(9.470)

544

9 Acceleration Kinematics

However, when the body coordinate frame B has a rigid motion with respect to G, then G

aP =

G

d dt

G

vP =

+ G ωB ×



×

GαB

×

G ωB

G

G

rP −

rP −

G

G

dB

dB





+

G¨

(9.471)

dB

where G dB indicates the position of the origin of B with respect to the origin of G. Angular accelerations of two links are related according to 0α2

= 0 α 1 + 01 α 2 .

(9.472)

The acceleration relationship for a body B having a rigid motion in G may also be expressed by a homogenous acceleration transformation matrix G AB G aP (t) = G AB G rP (t) (9.473) where, G

AB = =

G



T¨B G TB−1 ˜B Gα

− ω˜ ω˜ T

G¨

dB −



˜B Gα

0

− ω˜ ω˜ T 0



G

 dB

.

(9.474)

The forward acceleration kinematics of a robot is defined by ¨ = J q¨ + J˙ q˙ X

(9.475)

that is a relationship between joint coordinate acceleration  T q¨ = q¨1 q¨2 q¨3 · · · q¨n

(9.476)

  ¨ = X¨ n Y¨n Z¨ n ω˙ Xn ω˙ Y n ω˙ Zn T . X

(9.477)

and global acceleration of the end-effector

  Such a relationship introduces the time derivative of Jacobian matrix J˙ . Having the forward acceleration kinematics of a robot as (9.475), we can determine the inverse acceleration kinematics of the robot algebraically and determine the joint accelerations for a given set of end-effector acceleration, speed, and configuration.   ¨ − J˙ q˙ q¨ = J−1 X (9.478)

9.9 Key Symbols

9.9

Key Symbols

a A B c C dx , dy , dz d D e G, B0 ıˆ, jˆ, kˆ ı˜, j˜, k˜ Iˆ, Jˆ, Kˆ I = [I] j jij J = [J ] Jij l m ni mi p, q q P, Q r ri rij R s S T uˆ u˜ u1 , u2 , u3 U v v V x, y, z X, Y, Z

Acceleration vector Acceleration transformation matrix Body coordinate frame cos Mass center Elements of d Translation vector, displacement vector Displacement transformation matrix Rotation quaternion Global coordinate frame, Base coordinate frame Local coordinate axes unit vectors Skew symmetric matrices of the unit vector ıˆ, jˆ, kˆ Global coordinate axes unit vectors Identity matrix Jerk vector The element of row i and column j of χ˜ Jacobian The element of row i and column j of [J ] Length Number of independent equations Position vector of oi−1 with respect to Ci Position vector of oi with respect to Ci Position vectors of P , Q Joint variable vector Points Position vectors, homogenous position vector The element i of r The element of row i and column j of a matrix Rotation transformation matrix, radius sin Rotational acceleration transformation Homogenous transformation matrix Unit vector along the axis of ω Skew symmetric matrix of the vector uˆ Components of uˆ Rotational jerk transformation Velocity Velocity vector Velocity transformation matrix Local coordinate axes Global coordinate axes

Greek α α

Angular acceleration Angular acceleration vector

α˜ = ω˜ α1, α2, α3 θ θ ij k φ

Angular acceleration matrix Components of α Rotary joint angle θ i + θ j + θ k ϕ, θ, ψ] Euler angles Angle of rotation about uˆ

·

545

546

9 Acceleration Kinematics

ω ω ω˜ ω1 , ω2 , ω3

Angular velocity Angular velocity vector Skew symmetric matrix of the vector ω Components of ω

χ˜ = α˜ χ = α˙

Angular jerk matrix Angular jerk vector

Symbol [ ]−1 [ ]T ≡  (i)  ⊥ × ← → e E lim sgn

Inverse of the matrix [ ] TRANSPOSE of the matrix [ ] Equivalent Orthogonal Link number i Parallel sign Perpendicular Vector cross product Matrix form of a quaternion e Earth Limit function Signum function

·

Exercises

547

Exercises 1. Notation and symbols. Describe the meaning of the following notations: G B B a − GαB b − B αG c − G GαB d − GαB e − B αG f − B αG

g − 02 α 1 h − 01 α 2 i − 12 α 1 ·

j − 22 α 1

k − 32 α 1 l − kj α i

m − 02 α˜ 1 n − 21 ω˜ 2 o − G AB p − G V˙B q − J˙

¨ r−X

2. Local position, global acceleration. A body is turning about a global principal axis at a constant angular acceleration of 2 rad/s2 . Find the global velocity and acceleration of a point P at B r,  T B r = 5 30 10 (9.479) (a) If the axis is the Z-axis, the angular velocity is 2 rad/s, and the angle of rotation is π/3 rad. (b) If the axis is the X-axis, the angular velocity is 1 rad/s, and the angle of rotation is π /4 rad. (c) If the axis is the Y -axis, the angular velocity is 3 rad/s, and the angle of rotation is π/6 rad. 3. Global position, constant angular acceleration. A body is turning about the Z-axis at a constant angular acceleration α¨ = 0.2 rad/s2 . Find the global position of a point at B r,  T B r = 5 30 10 (9.480) after t = 3 s when α˙ = 2 rad/s, if the body and global coordinate frames were coincident at t = 0 s. 4.  Angular velocity and acceleration matrices. A body B is turning continuously in the global frame G. The transformation matrix can be simulated by a rotation α about Z-axis followed by a rotation β about X-axis. (a) Determine the axis and angle of rotation uˆ and φ as functions of α and β. (b) Determine if α and β are changing with constant rate, while uˆ and φ also change. Determine the conditions that keep uˆ constant and the conditions that keep φ constant. (c) Assume uˆ is constant and φ˙ = 5 rad/s. Determine α˙ and β˙ when φ = 30 deg. (d) Determine φ˙ when α = 30 deg, β = 45 deg, α˙ = 5 rad/s, and β˙ = 5 rad/s. 5. Turning about x-axis. Find the angular acceleration matrix when the body coordinate frame is turning −5 deg /s2 , 35 deg /s at 45 deg about the x-axis. 6. A roller in a circle. Figure 9.11 shows a roller in a circular path. Find the velocity and acceleration of a point P on the circumference of the roller. 7. Angular acceleration and Euler angles. Calculate the angular velocity and acceleration vectors in body and global coordinate frames if the Euler angles and their derivatives are as below. ϕ = 0.25 rad ϕ˙ = 2.5 rad/s ϕ¨ = 25 rad/s2 θ = −0.25 rad θ˙ = −4.5 rad/s θ¨ = 35 rad/s2 (9.481) ψ = 0.5 rad ψ˙ = 3 rad/s ψ¨ = 25 rad/s2 8. Angular acceleration by Euler angles. Employing the Euler angles transformation matrix, ˙ ψ. ˙ (a) Determine the linear relations between the Cartesian angular velocity G ωB and ϕ, ˙ θ, ¨ ψ. ¨ (b) Determine the linear relations between the Cartesian angular acceleration G α B and ϕ, ¨ θ, ... ... ... (c)  Determine the linear relations between the Cartesian angular jerk G jB and ϕ , θ , ψ. 9. Combined rotation and angular acceleration. Find the rotation matrix for a body frame after 30 deg rotation about the Z-axis, followed by 30 deg about the X-axis, and then 90 deg about the Y -axis. Then calculate the angular velocity of the body if it is turning with α˙ = 20 deg /s,

548

9

Acceleration Kinematics

Fig. 9.11 A roller in a circular path

Fig. 9.12 A planar RP R manipulator

β˙ = −40 deg /s, and γ˙ = 55 deg /s about the Z, Y , and X axes, respectively. Finally, calculate the angular acceleration of the body if it is turning with α¨ = 2 deg /s2 , β¨ = 4 deg /s2 , and γ¨ = −6 deg /s2 about the Z, Y , and X axes. 10. An RP R manipulator. Label the coordinate frames and find the velocity and acceleration of point P at the endpoint of the manipulator shown in Fig. 9.12. 11.  Differentiation and coordinate frame. How can we define these derivatives? G

d Gd G r dt dt

G

B

d Bd B r dt dt

B

G

B

d Bd B r dt dt

d Gd B r dt dt

G

d Bd G r dt dt

d B d G B d Gd G r r dt dt dt dt

(9.482)

d Gd B r dt dt

12.  Third derivative and coordinate frames. Consider a global frame G (OXY Z), a body frame B (Oxyz), and a body point P that is moving in the frame B with a variable body position B rP = B rP (t) velocity, B vP = B vP (t), and acceleration B aP = B aP (t). Determine how many possible simple and mixed jerks we can define. 13. A RRP planar redundant manipulator. Figure 9.13 illustrates a 3 DOF planar manipulator with joint variables θ 1 , θ 2 , d3 . (a) Solve the forward kinematics of the manipulator and calculate the position and orientation of the end-effector X, Y , ϕ for a given set of joint variables θ 1 , θ 2 , d3 , where, X, Y are global coordinates of the end-effector frame B3 , and ϕ is the angular coordinate of B3 .

Exercises

549

Fig. 9.13 A RRP planar redundant manipulator

Fig. 9.14 A RP R planar redundant manipulator

(b) Solve the inverse kinematics of the manipulator and determine θ 1 , θ 2 , d3 for given values of X, Y , ϕ. (c) Determine the Jacobian matrix of the manipulator and show that the following equation solves the forward velocity kinematics: ⎡ ⎤ ⎡ ⎤ θ˙ 1 X˙ ⎣ Y˙ ⎦ = J ⎣ θ˙ 2 ⎦ (9.483) d˙3 ϕ˙ (d) Determine J−1 and solve the inverse velocity kinematics. (e) Determine J˙ and solve the forward acceleration kinematics. (f) Solve the inverse acceleration kinematics. 14.  Mixed velocity and simple acceleration. Consider a local frame B(Oxyz), which is rotating in G(OXY Z) with an angular velocity α˙ = 10 rad/s2 about the Z-axis and a moving point P in B at B rP (t) = sin 2t ıˆ (9.484) G GG GG B BB BB B G Determine G ω˜ B , BG ω˜ B , B v, BG v, G B v, a, a, G a, B a, GB a, GB a, BG a, BG a. 15. A RP R planar redundant manipulator. (a) Figure 9.14 illustrates a 3 DOF planar manipulator with joint variables θ 1 , d2 , θ 2 . (b) Solve the forward kinematics of the manipulator and calculate the position and orientation of the end-effector X, Y , ϕ for a given set of joint variables θ 1 , θ 2 , d3 , where, X, Y are global coordinates of the end-effector frame B3 , and ϕ is the angular coordinate of B3 . (c) Solve the inverse kinematics of the manipulator and determine θ 1 , θ 2 , d3 for given values of X, Y , ϕ.

550

9

Acceleration Kinematics

Fig. 9.15 An offset articulated manipulator

(d) Determine the Jacobian matrix of the manipulator and show that the following equation solves the forward velocity kinematics: ⎡ ⎤ ⎡ ⎤ θ˙ 1 X˙ ⎣ Y˙ ⎦ = J ⎣ θ˙ 2 ⎦ (9.485) d˙3 ϕ˙ (e) Determine J−1 and solve the inverse velocity kinematics. (f) Determine J˙ and solve the forward acceleration kinematics. (g) Solve the inverse acceleration kinematics. 16.  Mixed second-derivative transformation formula. Consider three relatively rotating coordinate frames A, B, and C. The frame C is turning about the yB -axis of the B frame with angular velocity 8 rad/s and angular acceleration 10 rad/s2 , while B is turning about the xA -axis with angular velocity 5 rad/s and turning about the zA -axis with angular acceleration 3 rad/s2 . Determine the mixed double acceleration AA CB a if:     (a) A point P is at C r =  1 1 1 and moving with velocity C v = −10 0 10 .  (b) A point P is at C r =  sin t 0 0 .  (c) A point P is at C r =  cot s sin t 0 . (d) A point P is at C r = cot s sin t t . 17.  An offset articulated manipulator. Figure 9.15 illustrates an offset articulated manipulator. (a) Solve the forward kinematics of the manipulator. (b) Solve the inverse kinematics of the manipulator. (c) Solve the forward velocity kinematics of the manipulator. (d) Solve the inverse velocity kinematics of the manipulator. (e) Solve the forward acceleration kinematics of the manipulator. (f) Solve the inverse acceleration kinematics of the manipulator. 18.  Coriolis acceleration. ˆ We shoot a particle m with A disc with radius R = 1 m is turning in a horizontal plane with ω = ωKˆ and α = α K. ˆ speed v = 10I m/s from the center of the disc, which is at the origin of the disc coordinate frame B (Oxyz) and global coordinate frame G (OXY Z). If we ignore any friction between m and the disc, determine the local coordinate of the point where m reaches the periphery of the disc if: (a) ω = 10 rad/s and α = 0 (b) ω = 10 rad/s and α = 1 rad/s2 (c) ω = 10 rad/s and α = −1 rad/s2 (d) ω = sin t rad/s.

Exercises

551

Fig. 9.16 Kinematics of a rigid link

Fig. 9.17 A rolling disc on an inclined flat

19. Rigid link acceleration. Figure 9.16 illustrates the coordinate frames and kinematics of a rigid link (i). Assume that the angular velocity of the link, as well as the velocity and acceleration at proximal and distal joints, are given. (a) Find velocity and acceleration of the link at Ci in terms of proximal joint i velocity and acceleration. (b) Find velocity and acceleration of the link at Ci in terms of distal joint i + 1 velocity and acceleration. (c) Find velocity and acceleration of the proximal joint i in terms of distal joint i + 1 velocity and acceleration. (d) Find velocity and acceleration of the distal joint i + 1 in terms of proximal joint i velocity and acceleration. 20.  Coriolis and effective forces. ˆ We shoot a particle m in a A disc with radius R = 1 m is turning in a horizontal plane with ω = ωKˆ and α = α K. ˆ radial channel with speed v = 10I m/s from the center of the disc, which is at the origin of the disc coordinate frame B (Oxyz) and global coordinate frame G (OXY Z). If we ignore any friction between m and the channel, determine the Coriolis and effective forces on m during its motion if: (a) ω = 10 rad/s and α = 0. (b) ω = 10 rad/s and α = 1 rad/s2 . (c) ω = 10 rad/s and α = −1 rad/s2 . (d) ω = sin t rad/s. 21.  Jerk of a point in a roller. Figure 9.17 shows a rolling disc on an inclined flat surface. Find the velocity, acceleration, and jerk of point P in body and global coordinate frames.

552

9

Acceleration Kinematics

Fig. 9.18 A rotating disc on a needle

Fig. 9.19 A modified offset articulated manipulator

22.  R¯az¯ı acceleration. Consider a point P on the disc with radius R = 1 m in Fig. 9.18 and the following information: ⎡ ⎤ θ˙ 2 ⎣ ˙ ω = θ ı ˆ = 0⎦ 2 1 2 0

⎡ ⎤ 0 1 ˆ ⎣ ω = ϕ ˙ J = ϕ ˙⎦ G 1 0

⎡ ⎤ 0 2 rP = ⎣ 0 ⎦ r

(9.486)

Determine G vP , 1 v, 21 v, 2G v, 21 a, 1 a, 2G a, 22 G1 a, tangential acceleration, mixed centripetal acceleration, mixed Coriolis acceleration, and R¯az¯ı acceleration if: (a) r = R, θ˙ = 10 rad/s, ϕ˙ = 2 rad/s, and all other variables are zero (b) r = R, θ˙ = cos 10t rad/s, ϕ˙ = cos 2t rad/s, and all other variables are zero (c) r = R, θ˙ = 10 rad/s, ϕ˙ = 2 rad/s, θ¨ = 1 rad/s, ϕ¨ = −0.2 rad/s, and all other variables are zero (d) r = t, θ˙ = cos 10t rad/s, ϕ˙ = cos 2t rad/s, and all other variables are zero (e) r = R sin t, θ˙ = cos 10t rad/s, ϕ˙ = cos 2t rad/s, and all other variables are zero 23.  Project. A modified offset articulated manipulator. Figure 9.19 illustrates an offset articulated manipulator with a different end-effector coordinate frame. Determine the inverse acceleration kinematics of the robot.

Exercises

553

Fig. 9.20 Assembled spherical wrist

Fig. 9.21 A cylindrical manipulator

24.  Project. Articulated robots. Attach the spherical wrist of Fig. 9.20 to the articulated manipulator of Fig. 9.15 and make a 6 DOF articulated robot. Determine the inverse acceleration kinematics of the robot. 25.  Project-Spherical robots. Attach the spherical wrist of Fig. 9.20 to the spherical manipulator of Fig. 7.13 and make a 6 DOF spherical robot. Determine the inverse acceleration kinematics of the robot. 26.  Project. Cylindrical robots. Attach the spherical wrist of Fig. 9.20 to the cylindrical manipulator of Fig. 9.21 and make a 6 DOF cylindrical robot. Determine the inverse acceleration kinematics of the robot. 27.  Project. SCARA robot inverse acceleration kinematics. Figure 9.22 illustrates a SCARA robot. Assume θ 3 = 0.

554

9

Fig. 9.22 A SCARA robot

Acceleration Kinematics

Part III Dynamics

Dynamics is the science of motion. It describes why and how a motion occurs when forces and moments are applied on massive particles and bodies. The motion can be considered as evolution of the position, orientation, and their time derivatives. In robotics, the dynamic equations of motion for manipulators are utilized to set up the fundamental equations for control. The links and arms in a robotic system are modeled as rigid bodies. Therefore, the dynamic properties of the rigid body take a central place in robot dynamics. As arms of a robot may rotate or translate with respect to each other, translational and rotational equations of motion must be developed and expressed in body-attached coordinate frames B1 , B2 , B3 , · · · or in the global base reference frame B0 = G. There are basically two problems in robot dynamics: direct dynamics and inverse dynamics. Problem 1. We want the links of a robot to move in a specified manner. What forces and moments are required to achieve the motion? Problem 1 is called direct dynamics and is easier to solve when the equations of motion are in hand because it needs differentiating of kinematics equations. The first problem includes robots statics to keep a robot at a given position such as the rest position of a robot. In such condition, the problem reduces to finding forces such that no motion takes place when they act. However, there are many meaningful problems of the first type that involve robot motion rather than rest. An important example is that of calculating the required forces that must act on a robot such that its end-effector moves on a given path and with a prescribed time history from the start configuration to the final configuration. Problem 2. The applied forces and moments on a robot are completely specified. How will the robot move? The second problem is called inverse dynamics and is more difficult to solve because it needs integration of equations of motion. The variety of the applied problems of the second type is interesting. Problem 2 is essentially a prediction since we wish to find the robot motion for all future times when the initial state of each link is given. In this part, we develop techniques to derive the equations of motion for a robot.

Applied Dynamics

10

Relation between kinematics and the cause of change of kinematics is called equations of motion. Deriving the equations of motion and the expression of their solution is called dynamics. Dynamics of a robot may be considered as the motion of a rigid link with respect to a fixed global coordinate frame. The principles of dynamics as well as derivation methods of Newton and Euler equations of motion that express the translational and rotational motion of rigid bodies are reviewed in this chapter.

Fig. 10.1 Principal coordinate frame for a symmetric L-section

10.1

Force and Moment

In this section, we review the definition, elements, and principal equations of motion in dynamics.

10.1.1 Force and Moment In Newtonian dynamics, the acting forces on a system of connected rigid bodies can be divided into internal and external forces. Internal forces are acting between connected bodies and appear as action and reaction forces. External forces are acting from outside of the system and appear as applied driving forces. An external force can be a contact force, such as traction force at tireprint of a driving wheel, or a body force, such as gravitational force on a robot link. The external forces and moments are called load, and a set of forces and moments acting on a rigid body are called a force system. The resultant F is the vectorial sum of all the external forces acting on a body, and the resultant M is the vectorial sum of all the moments of the external forces acting on the body.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_10

557

558

10 Applied Dynamics

F=



Fi

(10.1)

Mi

(10.2)

i

M=

 i

Consider an acting force F on a point P at rP . The moment of F about the origin is M = rP × F

(10.3)

  MQ = rP − rQ × F

(10.4)

The moment of the force F, about a point Q at rQ , is

The moment of the force about a directional line l with unit vector uˆ passing through the origin is Ml = uˆ · (rP × F)

(10.5)

The moment of a force may also be called torque or moment although they are conceptually different. The effect of a force system is equivalent to the effect of the resultant force and resultant moment of the force system. Any two force systems are equivalent if their resultant force and resultant moment are equal. If the resultant force of a force system is zero, the resultant moment of the force system is independent of the origin of the coordinate frame. Such a resultant moment is called couple. When a force system is reduced to a resultant FP and MP with respect to a reference point P , we may change the reference point to another point Q and find the new resultants as F Q = FP





MQ = MP + rP − rQ × FP = MP +

(10.6) Q rP

× FP

(10.7)

10.1.2 Momentum The momentum of a moving rigid body is a vector quantity equal to the total mass of the body times the translational velocity of its mass center C. p = mv (10.8) The momentum p may also be called translational momentum or linear momentum. Consider a rigid body with momentum p. The moment of momentum, L, about a directional line l with directional unit vector uˆ passing through the origin is Ll = uˆ · (rC × p) (10.9) where rC is the position vector of the mass center C. The moment of momentum about the origin is L = rC × p

(10.10)

The moment of momentum L may also be called angular momentum.

10.1.3 Equation of Motion The application of a force system is emphasized by Newton’s second and third laws of motion. The second law of motion, also called the Newton’s equation of motion, states that the global rate of change of linear momentum is proportional to the global applied force. G G   dG d G m Gv F= p= (10.11) dt dt

10.1 Force and Moment

559

The third Newton’s law of motion states that the action and reaction forces acting between two bodies are equal and opposite. The second law of motion can be expanded to include rotational motions. Hence, the second law of motion also states that the global rate of change of angular momentum is proportional to the global applied moment. G

G

dG L dt

M=

(10.12)

Proof Differentiating from moment of momentum (10.10) shows that G

G dG d L= (rC × p) = dt dt

=

G

rC ×

G

G

dp = dt

G

G drC dp × p + rC × dt dt

rC ×

G

F=

G



(10.13)

M



10.1.4 Work and Energy The kinetic energy K of a moving point P with mass m at a position G rP and velocity G vP is K=

1 G v· m 2

G

v



(10.14)

where G indicates the global coordinate frame and B indicates body coordinate frame in which the velocity vector vP is expressed. The work done by the applied force G F on m in moving from point 1 to point 2 on a path, indicated by a vector G r, is 1 W2

2

=

G

F · d Gr

(10.15)

1

However, -

-

2 G 1

2 G

1 dG G F·d r = m v · v dt = m dt 2 1  1  = m v22 − v12 = K2 − K1 2

-

G

1

2

d 2 v dt dt (10.16)

that shows 1 W2 is equal to the difference of the kinetic energy between the terminal and initial points. 1 W2

= K2 − K1

(10.17)

Equation (10.17) is called the principle of work and energy. If there is a scalar potential field function V = V (x, y, z) such that  dV ∂V ∂V ∂V ˆ F = −∇V = − =− ıˆ + jˆ + k dr ∂x ∂y ∂z

(10.18)

then the principle of work and energy simplifies to the principle of conservation of energy, K1 + V1 = K2 + V2 The value of the potential force function V = V (x, y, z) is the potential energy of the system.

(10.19)

560

10 Applied Dynamics

Example 328 Position of center of mass. Mass center of every link of a robot must be known to locate the point at which the acceleration and velocity of the link are referred to. Here is an example to show analytic method of calculating the location of mass center for objects their mass centers are not geometrically obvious. The position of the mass center of a rigid body in a coordinate frame is indicated by rC and is usually measured in the body coordinate frame. The location of mass center will be calculated by integrals over the volume of the body. 1 r dm m B ⎡ ⎤ ⎡11 ⎤ x dm xC m 1B ⎣ yC ⎦ = ⎣ 1 y dm ⎦ m 1B 1 zC z dm m B B

rC =

(10.20)

(10.21)

Applying the mass center integral on the symmetric L-section rigid body with ρ = 1 shown in Fig. 10.1 provides the location of C of the section. The x position of C is xC =

1 m

x dm = B

and because of symmetry, we have yC = −xC =

1 A

x dA = − B

b2 + ab − a 2 4ab + 2a 2

b2 + ab − a 2 4ab + 2a 2

(10.22)

zC = 0

(10.23)

Example 329 Exponential decaying force. Having the force function, determination of position as a function of time is the outcome of inverse dynamics problem. Here is an example. Consider a point mass m that is under an exponentially decaying force F (t) = ce−t , where c is a constant: m

dv = ce−t dt

(10.24)

The velocity of the mass will be -

v

m

-

t

dv =

v0

ce−t dt

t0

v = v0 +

 c  −t0 e − e−t m

(10.25)

which can be used to find the position. -

x x0

dx =

- t

v0 +

t0

x = x0 −

 c  −t0 e − e−t dt m

c c (1 + t0 − t) e−t0 + v0 (t − t0 ) + e−t m m

(10.26)

If the initial time t0 is assumed to be zero, then the position and velocity of the mass are simplified to c c (1 − t) + v0 t + e−t m m  c  −t v = v0 + 1−e m

x = x0 −

(10.27) (10.28)

Example 330 Motion equation of a system of particles. There are situations in which there are multiple particles involved. Here is what would be the equation of motion for multiple particles. Consider a group of n particles mi , i = 1, 2, 3, · · · , n, with position vectors ri in a global coordinate frame G. The position vector of the mass center C of the particles is at rC , where mC is the total mass of the system.

10.1 Force and Moment

561 n 1  mi ri mC i=1

rC =

mC =

n 

mi

(10.29)

i=1

/ The force acting on each particle mi can be decomposed into an external force Fi and an internal force nj=1 fij . The internal force fij , with the condition fii = 0, is the force that particle mj applies on mi . The motion equation of the particle mi would be n  d 2 ri m i 2 = Fi + fij i = 1, 2, 3, · · · , n (10.30) dt j =1 By adding the n equations of motion of all particles we have n 

  d 2 ri = Fi + fij 2 dt i=1 i=1 j =1 n

mi

i=1

n

n

(10.31)

Because of the third law of Newton, fij = −fj i the summation of the internal forces is zero:

n  n 

(10.32)

fij = 0

(10.33)

d 2 r0 = FC dt 2

(10.34)

i=1 j =1

Therefore, the equation of motion of all particles reduces to mC where FC is the resultant of all the external forces. FC =

n 

Fi

(10.35)

i=1

Equation (10.34) states that the motion of the mass center C of a system of particles is the same as if all the masses m were concentrated at that point and were acted upon by the resultant of all the external forces FC . Example 331 Work, force, and kinetic energy in a unidirectional motion. An example to show how we use work and kinetic energy principle. A mass m = 2 kg has an initial kinetic energy K = 12 J. The mass is under a constant force F = F Iˆ = 4Iˆ and moves from X(0) = 1 to X(tf ) = 22 m at a terminal time tf . The work done by the force during this motion is W =

r(tf )

-

22

F · dr =

4 dX = 21 Nm = 21 J

(10.36)

1

r(0)

The kinetic energy at the terminal time is K(tf ) = W + K(0) = 33 J

(10.37)

which shows that the terminal speed of the mass is ) v2 =

2K(tf ) √ = 33 m/s m

(10.38)

As a more complete work calculation, consider a planar force F, F = 2xyˆı + 3x 2 jˆ N

(10.39)

562

10 Applied Dynamics

that moves a mass m on a planar curve y = x2

(10.40)

dy = 2x dx

(10.41)

from (0, 0) to (3, 1) m. Using we can calculate the work done by the force: 1 W2

= -

-

P2

G

F · dr =

P1 3

=



(3,1)

2xy dx + 3x 2 dy

(0,0)



-



3

2x dx + 6x dx = 3

3

0



8x 3 dx = 162 Nm

(10.42)

0

To see an example of potential field, consider a force field F, F = 2xyˆı + x 2 jˆ + 2zkˆ N

(10.43)

The work done by the force when it moves from P1 (0, 0, 0) to P2 (1, 3, 5) m on a line x=t

y = 3t

z = 5t

(10.44)

is -

-

P2

1 W2 =

G

=



 10z + 2xy + 3x 2 dt

0

P1 1

1

F · dr =



 50t + 6t 2 + 3t 2 dt = 28 Nm

(10.45)

0

We may change the path from a straight line to a curve x=t

y = 3t 2

z = 5t 3

(10.46)

and calculate the work 1 W2 again: 1 W2

P2

=

G

F · dr = 0

P1

-

1

=

⎡ 1

⎤ ⎡ ⎤ 2xy dt ⎣ x 2 ⎦ · ⎣ 6t dt ⎦ 2z 15t 2 dt

  2 3tx 2 + 15t 2 z + xy dt

0

-

1

=2



 75t 5 + 6t 3 dt = 28 Nm

(10.47)

0

Because 1 W2 is independent of the path from P1 to P2 , the force field (10.43) might be a potential field. To check, we may calculate the curl of the field because curl of a potential force is zero: ∇ × F = 0. ⎡

⎤ ⎡ ⎤ ⎡ ⎤ 0 ∂/∂x 2xy ∇ × F = ⎣ ∂/∂y ⎦ × ⎣ x 2 ⎦ = ⎣ 0 ⎦ ∂/∂z 2z 0 So, the force field is a potential.

(10.48)

10.1 Force and Moment

563

Example 332  Newton equation in a rotating frame. Analysis of motion of objects in a rotating coordinate frame is very applied in engineering as well as study of every moving objects on the Earth. Such expression has an interesting term called the Coriolis acceleration. Here is the expression of Newton equation of motion in a rotating frame. Consider a spherical rigid body (such as the Earth) with a fixed point that is rotating with a constant angular velocity. The equation of motion for a moving point P on the rigid body is B

F = m B aP + m BG ωB ×

B

G ωB

 × B rP + 2m BG ωB × B r˙ P

= m B aP

(10.49)

It shows that the Newton equation of motion F = m a, which is only correct in a globally stationary frame, will have extra terms when expressed in the rotating frame. The equation of motion of a moving point on the surface of the Earth can be rearranged to   B B F − m BG ωB × BG ωB × B rP − 2m G ω B × B vP = m B aP (10.50) Equation (10.50) is the equation of motion to an observer in the rotating frame, which in this case is an observer on the Earth. The left-hand side of this equation is called the effective force, Feff =

B

F − m BG ωB ×

B

G ωB

 × B rP − 2m BG ωB × B vP

(10.51)

because it seems that the particle is moving in the Earth frame B under the influence of this force. The second term is the negative of the centrifugal force and pointing outward. The maximum value of this force on the Earth surface is on the equator that is about 0.3% of the acceleration of gravity.  rω = 6378.388 × 10 × 2

3

2π 366.25 24 × 3600 365.25

2

= 3.3917 × 10−2 m/s2

(10.52)

If we add the variation of the gravitational acceleration because of a change of radius from R = 6356912 m at the pole to R = 6378388 m on the equator, then the variation of the acceleration of gravity becomes 0.53%. So, generally speaking, a sportsman such as a pole-vaulter can show a better record in a competition held on the equator. The third term is called the Coriolis force, FC , which is perpendicular to both ω and B vP . For a mass m moving on the north hemisphere at a latitude θ toward the equator, we should provide a lateral eastward force equal to the Coriolis effect to force the mass, keeping its direction relative to the ground. FC = 2m BG ωB × B vm = 1.4584 × 10−4 B pm cos θ kg m/s2

(10.53)

The Coriolis effect is the reason of wearing out the west side of railways, roads, and rivers. The lack of providing Coriolis force is the reason for turning the direction of winds, projectiles, and shooting objects westward. Gaspard-Gustave de Coriolis (1792–1843) is the French mathematician who showed the importance of the Coriolis term in a rotating coordinate frame. Example 333  Force function in equation of motion. In Newtonian mechanics, applied forces may only be functions of time, position, and velocity and nothing else. Here is a proof. The first and second Newton laws of motion provide no predication or expectation of the force function and its arguments. Qualitatively, force is whatever changes the motion, and quantitatively, force is whatever is equal to mass times acceleration. Mathematically, equation of motion provides a vectorial second-order differential equation: m¨r = F (˙r, r, t)

(10.54)

We assume that the force function may generally be a function of time t, position r, and velocity r˙ . In other words, the Newton equation of motion is correct as long as we can show that force is only a function of r˙ , r, t. If there is a force that depends on the acceleration, jerk, or other variables that cannot be reduced to r˙ , r, t, we do not know the equation of motion because we do not have an equation of motion for those forces.

564

10 Applied Dynamics

... F (r, r˙ , r¨ , r , · · · , t) = m¨r

(10.55)

So, in Newtonian mechanics, we assume that force can only be a function of r˙ , r, t and nothing else. In the real world, force may be a function of many things; however, we always ignore any other variables than r˙ , r, t, or make some approximations accordingly. Because Eq. (10.54) is a linear equation of force F, it accepts the superposition principle. When a mass m is affected by several forces F1 , F2 , F3 , · · · , we may calculate their summation vectorially and apply the resultant force on m. F = F1 + F2 + F3 + · · ·

(10.56)

So, if a force F1 provides acceleration r¨ 1 and F2 provides r¨ 2 , m¨r1 = F1

m¨r2 = F2

(10.57)

then the resultant force F3 = F1 + F2 provides the acceleration r¨ 3 such that r¨ 3 = r¨ 1 + r¨ 2

(10.58)

To see that the Newton equation of motion is not correct when the force is not only a function of r˙ , r, t, let us assume that a particle with mass m is under two acceleration-dependent forces F1 (x) ¨ and F2 (x) ¨ on the x-axis: mx¨1 = F1 (x¨1 )

mx¨2 = F2 (x¨2 )

(10.59)

The acceleration of m under the action of both forces would be x¨3 , mx¨3 = F1 (x¨3 ) + F2 (x¨3 )

(10.60)

x¨3 = x¨1 + x¨2

(10.61)

m (x¨1 + x¨2 ) = F1 (x¨1 + x¨2 ) + F2 (x¨1 + x¨2 ) = F1 (x¨1 ) + F2 (x¨2 )

(10.62)

however, we must have but we have

Example 334  Every force system is equivalent to a wrench. Wrench in dynamics is similar to screw motion in kinematics. It is an interesting concept but not very applied. Here is to show how a wrench is calculated to be equivalent to a force system. The Poinsot theorem states: Every force system is equivalent to a single force, plus a moment parallel to the force. Let F and M be the resultant force and moment of a force system. We decompose the moment into parallel and perpendicular components, M and M⊥ , to the force axis. The force F and the perpendicular moment M⊥ can be replaced by a single force F parallel to F. Therefore, the force system is reduced to a force F and a moment M parallel to each other. A force and a moment about the force axis are the equivalent wrenches to the given force system. Poinsot theorem is similar to the Chasles theorem that states every rigid body motion is equivalent to a screw, which is a translation plus a rotation about the axis of translation. Louis Poinsot (1777–1859) is a French mathematician and physicist who showed how a system of forces can be resolved into a wrench.

10.2 Rigid Body Translational Kinetics

565

Fig. 10.2 A body point mass moving with velocity G vP and acted on by force df

10.2

Rigid Body Translational Kinetics

Figure 10.2 depicts a moving body B in a global frame G. Assume the body frame is attached at the center of mass C of the body. Point P indicates an infinitesimal sphere of the body with a very small mass dm. The point mass dm is acted on by an infinitesimal force df and has a global velocity G vP . According to Newton law of motion we have df =

G

aP dm

(10.63)

however, the equation of motion for the whole body in global coordinate frame is F = m G aB

(10.64)

F = m BG aB + m BG ωB × B vB ⎤  ⎡ ⎤ ⎡ Fx max + m ωy vz − ωz vy ⎣ Fy ⎦ = ⎣ may − m (ωx vz − ωz vx ) ⎦   Fz maz + m ωx vy − ωy vx

(10.65)

G

which can be expressed in the body coordinate frame as B

(10.66)

In these equations, G aB is the acceleration vector of the body C in global frame, m is the total mass of the body, and F is the resultant of the external forces acted on the body at C. Proof A body coordinate frame at the center of mass is called a central frame. If the frame B is a central frame, then the center of mass, C, yields B

rdm dm = 0

(10.67)

B

The global position vector of dm is related to its local position vector by G rdm , G

rdm =

G

dB +

G

RB

B

rdm

(10.68)

566

10 Applied Dynamics

where G dB is the global position vector of the central body frame B. Therefore, -

-

G

rdm dm =

B

G

-

dB dm +

G

=

B

RB

B

G

dB dm =

G

dm = m G dB

dB

B

rdm dm

m

(10.69)

B

A time derivative of both sides shows that m G d˙ B = m G vB =

-

G

r˙ dm dm =

B

G

vdm dm

(10.70)

B

-

and another derivative indicates that m G v˙ B = m G aB =

G

v˙ dm dm

(10.71)

B

However, we have df =

G˙

-

vP dm, and hence, m G aB =

df

(10.72)

B

The integral on the right-hand side accounts for all the forces acting on all particles in the body. The internal forces cancel one another out, so the net result is the vector sum of all the externally applied forces, F, and therefore, G

F = m G aB = m G v˙ B

(10.73)

In the body coordinate frame we have B

F=

B

RG G F = m B RG G aB = m BG aB

= m B aB + m BG ωB × B vB

(10.74)

The expanded form of the Newton’s equation in the body coordinate frame is then equal to F = m B aB + m BG ωB × B vB ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ Fx ax ωx vx ⎣ Fy ⎦ = m ⎣ ay ⎦ + m ⎣ ωy ⎦ × ⎣ vy ⎦ Fz az ωz vz  ⎤ ⎡ max + m ωy vz − ωz vy = ⎣ may − m(ωx vz − ωz vx ) ⎦ maz + m ωx vy − ωy vx B

(10.75)

(10.76) 

Example 335 A wound ribbon. Translational motion of a rigid is always along with rigid body rotation; otherwise, the rigid body may be treated as a particle. Here is an example of a planar rotating and translating rigid body motion. Figure 10.3 illustrates a ribbon of negligible weight and thickness that is wound tightly around a uniform massive disc of radius R and mass m. The ribbon is fastened to a rigid support, and the disc is released to roll down vertically. There are two forces acting on the disc during the motion, its weight mg and the tension of the ribbon T . The translational equation of motion of the disc is expressed easier in the global coordinate frame: 

FY = −mg + T = mY¨

(10.77)

10.3 Rigid Body Rotational Kinetics

567

Fig. 10.3 A ribbon of negligible weight and thickness that is wound tightly around a uniform massive disc

The rotational equation of motion is simpler if expressed in the body coordinate frame: 

Mz = T R =

B

I

B ˙B Gω

+

B G ωB

× BI

B G ωB

= I θ¨

(10.78)

There is a constraint between the coordinates Y and θ. Y = Y0 − Rθ

(10.79)

To solve the motion, let us eliminate T between (10.77) and (10.78) to obtain a new equation, mY¨ = −mg + and use the constraint to eliminate Y¨ .

θ¨ = 

I θ¨ R

(10.80)

mg I + mR R

(10.81)

Now, we can find T , Y¨ , and Y : T =

I I θ¨ = mg 2 R mR + I

mR 2 Y¨ = − g mR 2 + I

(10.82)

Y =−

mR 2 gt 2 + Y˙ (0) t + Y (0) mR 2 + I

(10.83)

For a point mass with I = 0, the falling acceleration is the same as the free fall of a particle. However, being a rigid body and having I = 0, the falling acceleration of the disc will be less. This is because the kinetic energy of the disc splits between rotation and translation.

10.3

Rigid Body Rotational Kinetics

The rigid body rotational equation of motion is expressed by the Euler equation B

M= =

G

d dt

B

B

B ˙B Gω

I

L= +

B

˙+ L

B G ωB

B G ωB

×

B

× BL I

B G ωB



(10.84)

568

10 Applied Dynamics

where L is the angular momentum and [I ] is the mass moment or moment of inertia of the rigid body. B

L=

B

I

B G ωB

(10.85)

⎡

⎤ Ixx Ixy Ixz [I ] = ⎣ Iyx Iyy Iyz ⎦ Izx Izy Izz

(10.86)

The elements of [I ] are only functions of the mass distribution of the rigid body and may be defined by geometric integrals, -



Iij = B

 ri2 δ mn − xim xj n dm

where δ ij is Kronecker’s delta.

 δ mn =

i, j = 1, 2, 3

1 if m = n 0 if m =

n

(10.87)

(10.88)

The expanded form of the Euler equation is   Mx = Ixx ω˙ x + Ixy ω˙ y + Ixz ω˙ z − Iyy − Izz ωy ωz     −Iyz ω2z − ω2y − ωx ωz Ixy − ωy Ixz

(10.89)

My = Iyx ω˙ x + Iyy ω˙ y + Iyz ω˙ z − (Izz − Ixx ) ωz ωx     −Ixz ω2x − ω2z − ωy ωx Iyz − ωz Ixy

(10.90)

  Mz = Izx ω˙ x + Izy ω˙ y + Izz ω˙ z − Ixx − Iyy ωx ωy     −Ixy ω2y − ω2x − ωz ωy Ixz − ωx Iyz

(10.91)

which can be reduced to a set of simpler equations in the principal coordinate frame. M1 = I1 ω˙ 1 − (I2 − I2 ) ω2 ω3 M2 = I2 ω˙ 2 − (I3 − I1 ) ω3 ω1

(10.92)

M3 = I3 ω˙ 3 − (I1 − I2 ) ω1 ω2 The principal coordinate frame is denoted by numbers 123 instead of xyz to indicate the first, second, and third principal axes. The parameters Iij , i = j are zero in the principal frame. The body and principal coordinate frame are assumed to sit at C of the body. . The kinetic energy of a rotating rigid body is K=

=

 1 Ixx ω2x + Iyy ω2y + Izz ω2z 2 −Ixy ωx ωy − Iyz ωy ωz − Izx ωz ωx 1 1 ω · L = ωT I ω 2 2

(10.93)

that in the principal coordinate frame reduces to K=

 1 2 I1 ω1 + I2 ω22 + I3 ω23 2

(10.94)

Proof Let mi be the mass of the ith particle of a rigid body B, which is made of n particles and ri is the Cartesian position vector of mi in a central body fixed coordinate frame Oxyz. T  ri = B ri = xi yi zi

(10.95)

10.3 Rigid Body Rotational Kinetics

569

Assume BG ωB to be the angular velocity of the rigid body with respect to the ground, expressed in the body coordinate frame.  T ω = BG ωB = ωx ωy ωz

(10.96)

The angular momentum of mi is Li = ri × mi r˙ i = mi [ri × (ω × ri )] = mi [(ri · ri ) ω − (ri · ω) ri ] = mi ri2 ω − mi (ri · ω) ri

(10.97)

Hence, the angular momentum of the rigid body would be L=ω

n 

mi ri2 −

i=1

n 

mi (ri · ω) ri

(10.98)

i=1

Substitution for ri and ω provides us with n     L = ωx ıˆ + ωy jˆ + ωz kˆ mi xi2 + yi2 + zi2 i=1

−

n 

   mi xi ωx + yi ωy + zi ωz · xi ıˆ + yi jˆ + zi kˆ

(10.99)

i=1

and therefore, L=

n 

n      mi xi2 + yi2 + zi2 ωx ıˆ + mi xi2 + yi2 + zi2 ωy jˆ

i=1

+

i=1

n 

  mi xi2 + yi2 + zi2 ωz kˆ

i=1

−

n 

  mi xi ωx + yi ωy + zi ωz xi ıˆ

i=1

−

n 

  mi xi ωx + yi ωy + zi ωz yi jˆ

i=1

−

n 

  mi xi ωx + yi ωy + zi ωz zi kˆ

(10.100)

i=1

or L=

n 

mi



    xi2 + yi2 + zi2 ωx − xi ωx + yi ωy + zi ωz xi ıˆ

i=1

+

n 

mi



    xi2 + yi2 + zi2 ωy − xi ωx + yi ωy + zi ωz yi jˆ



    xi2 + yi2 + zi2 ωz − xi ωx + yi ωy + zi ωz zi kˆ

i=1

+

n  i=1

mi

(10.101)

570

10 Applied Dynamics

which can be rearranged as L=

n  

n      2  mi yi2 + zi2 ωx ıˆ + mi zi + xi2 ωy jˆ

i=1

+

i=1

n 



  mi xi2 + yi2 ωz kˆ

i=1

−

 n  

−  −

(mi xi yi ) ωy +

i=1

i=1

n 

n 

(mi yi zi ) ωz +



n 

i=1

i=1

n 

n 

(mi zi xi ) ωx +

i=1

(mi xi zi ) ωz ıˆ  (mi yi xi ) ωx jˆ  (mi zi yi ) ωy kˆ

(10.102)

i=1

By introducing the mass moment matrix [I ] with the following elements, Ixx =

n  

  mi yi2 + zi2

(10.103)

  mi zi2 + xi2

(10.104)

  mi xi2 + yi2

(10.105)

i=1

Iyy =

n   i=1

Izz =

n   i=1

Ixy = Iyx = −

n 

(mi xi yi )

(10.106)

(mi yi zi )

(10.107)

(mi zi xi )

(10.108)

i=1

Iyz = Izy = −

n  i=1

Izx = Ixz = −

n  i=1

we can write the angular momentum L in a concise form, Lx = Ixx ωx + Ixy ωy + Ixz ωz

(10.109)

Ly = Iyx ωx + Iyy ωy + Iyz ωz

(10.110)

Lz = Izx ωx + Izy ωy + Izz ωz

(10.111)

⎤ ⎡ ⎤⎡ ⎤ Lx Ixx Ixy Ixz ωx ⎣ Ly ⎦ = ⎣ Iyx Iyy Iyz ⎦ ⎣ ωy ⎦ Lz Izx Izy Izz ωz

(10.112)

or in a matrix form ⎡

L = I ·ω

(10.113)

10.3 Rigid Body Rotational Kinetics

571

For a rigid body that is a continuous compact solid body, the summations will be replaced by integrations over the volume of the body as in Eq. (10.87). The Euler equation of motion for a rigid body is B

M=

G

d dt

B

(10.114)

L

where B M is the resultant of the external moments applied on the rigid body. The angular momentum vector B L is defined in the body coordinate frame B. Hence, its time derivative in the global coordinate frame is G

d BL = dt

B

˙+ L

B G ωB

× BL

(10.115)

Therefore, B M is B

B

M=

dL = L + ω˙ × L = I ω˙ + ω × (I ω) dt

  M = Ixx ω˙ x + Ixy ω˙ y + Ixz ω˙ z ıˆ   +ωy Ixz ωx + Iyz ωy + Izz ωz ıˆ   −ωz Ixy ωx + Iyy ωy + Iyz ωz ıˆ   + Iyx ω˙ x + Iyy ω˙ y + Iyz ω˙ z jˆ   +ωz Ixx ωx + Ixy ωy + Ixz ωz jˆ   −ωx Ixz ωx + Iyz ωy + Izz ωz jˆ   + Izx ω˙ x + Izy ω˙ y + Izz ω˙ z kˆ   +ωx Ixy ωx + Iyy ωy + Iyz ωz kˆ   −ωy Ixx ωx + Ixy ωy + Ixz ωz kˆ

(10.116)

(10.117)

Therefore, the most general form of the Euler equations of motion for a rigid body in a body frame attached to C is   Mx = Ixx ω˙ x + Ixy ω˙ y + Ixz ω˙ z − Iyy − Izz ωy ωz     −Iyz ω2z − ω2y − ωx ωz Ixy − ωy Ixz My = Iyx ω˙ x + Iyy ω˙ y + Iyz ω˙ z − (Izz − Ixx ) ωz ωx     −Ixz ω2x − ω2z − ωy ωx Iyz − ωz Ixy   Mz = Izx ω˙ x + Izy ω˙ y + Izz ω˙ z − Ixx − Iyy ωx ωy     −Ixy ω2y − ω2x − ωz ωy Ixz − ωx Iyz

(10.118)

(10.119)

(10.120)

Assume that we are able to rotate the body frame about its origin to find an orientation that makes Iij = 0, for i = j . In such a principal coordinate frame, the Euler equations reduce to M1 = I1 ω˙ 1 − (I2 − I2 ) ω2 ω3

(10.121)

M2 = I2 ω˙ 2 − (I3 − I1 ) ω3 ω1

(10.122)

M3 = I3 ω˙ 3 − (I1 − I2 ) ω1 ω2

(10.123)

572

10 Applied Dynamics

The kinetic energy of a rigid body may be found by the integral of the kinetic energy of a mass element dm, over the whole body. 1 v˙ dm = (ω × r) · (ω × r) dm 2 B B ω2y  2   2  ω2x 2 = y + z dm + z + x 2 dm 2 B 2 B 2 -   ω + z x 2 + y 2 dm 2 B −ωx ωy xy dm − ωy ωz yz dm − ωz ωx zx dm

1 K= 2

-

2

B

=

B

1

B

 2

Ixx ω2x + Iyy ω2y + Izz ωz 2 −Ixy ωx ωy − Iyz ωy ωz − Izx ωz ωx

(10.124)

The kinetic energy can be rearranged and expressed in a matrix multiplication form. K=

1 1 T ω Iω = ω·L 2 2

(10.125)

When the body frame is principal, the kinetic energy will be simplified. K=

 1 2 I1 ω1 + I2 ω22 + I3 ω23 2

(10.126) 

Example 336 Spherical pendulum. Analysis of physical systems is the best way to understand analytic methods. Here is deriving angular acceleration of a spherical pendulum. A pendulum free to oscillate in any plane is called a spherical pendulum. The tip mass of such pendulum will be free to move on the surface of a sphere around the fulcrum. Consider a pendulum with a point mass m at the tip point of a long, massless, and straight bar with length l. The pendulum is hanging from a point A (0, 0, 0) in a local coordinate frame B1 (x1 , y1 , z1 ). To indicate the mass m, we attach a coordinate frame B2 (x2 , y2 , z2 ) to the pendulum at point A as is shown in Fig. 10.4. The pendulum makes an angle β with the vertical z1 -axis and swings in the plane (x2 , z2 ) that makes an angle γ with the plane (x1 , z1 ). Therefore, the transformation matrix between B2 and B1 is 2

The position vectors of m are

R1 = Ry2 ,−β Rz2 ,γ ⎡ ⎤ cos γ cos β cos β sin γ sin β = ⎣ − sin γ cos γ 0 ⎦ − cos γ sin β − sin γ sin β cos β ⎡

⎤ 0 2 r=⎣ 0 ⎦ −l

(10.127)

⎡

⎤ l cos γ sin β 1 r = 1 R2 2 r = ⎣ l sin β sin γ ⎦ −l cos β

(10.128)

M = I 1α2

(10.129)

The equation of motion of m is 1 1

r × m g = ml 1

2

1α2

(10.130)

10.3 Rigid Body Rotational Kinetics

573

Fig. 10.4 A spherical pendulum

⎡

⎤ ⎡ ⎤ l cos γ sin β 0 ⎣ l sin β sin γ ⎦ × m ⎣ 0 ⎦ = ml 2 1 α 2 −l cos β −g0 Therefore,

⎡ ⎤ − sin β sin γ g0 ⎣ cos γ sin β ⎦ 1α2 = l 0

(10.131)

(10.132)

To find the angular acceleration of B2 in B1 , we use 2 R1 : 1

d 2 d 2 R˙ 2 = β˙ R1 + γ˙ R1 dβ dγ ⎡ ⎤ ˙ sβ − γ˙ cβsγ −γ˙ cγ γ˙ sβsγ − βcβcγ ˙ −βcγ ˙ ˙ = ⎣ γ˙ cβcγ − βsβsγ −γ˙ sγ −βcβsγ − γ˙ cγ sβ ⎦ ˙βcβ ˙ 0 −βsβ ⎤ 0 −γ˙ −β˙ cos γ ˜ 2 = 1 R˙ 2 1 R2T = ⎣ γ˙ 0 −β˙ sin γ ⎦ 1ω β˙ cos γ β˙ sin γ 0

(10.133)

⎡

1

2 d 2 d2 2 2 d 2 ˙ γ˙ R¨ 2 = β¨ R1 + β˙ R + β R1 1 dβ dγ dβ dβ 2

+γ¨

˜2 1α

(10.134)

d 2 d2 2 d2 R1 + γ˙ β˙ R1 + γ˙ 2 2 2 R1 dγ dβdγ dγ

= 1 R¨ 2 1 R2T − 1 ω˜ 22 ⎡ ⎤ ¨ + β˙ γ˙ sγ 0 −γ¨ −βcγ ¨ − β˙ γ˙ cγ ⎦ =⎣ γ¨ 0 −βsγ ¨ − β˙ γ˙ sγ βsγ ¨ + β˙ γ˙ cγ βcγ 0

(10.135)

(10.136)

574

10 Applied Dynamics

Therefore, the equation of motion of the pendulum would be ⎡

⎤ ⎡ ⎤ − sin β sin γ β¨ sin γ + β˙ γ˙ cos γ g0 ⎣ cos γ sin β ⎦ = ⎣ −β¨ cos γ + β˙ γ˙ sin γ ⎦ l 0 γ¨

(10.137)

The third equation indicates that γ˙ = γ˙ 0

γ = γ˙ 0 t + γ 0

(10.138)

The second and third equations can be combined to the form a new equation, * g02 2 sin2 β + β˙ γ˙ 20 l2

β¨ = −

(10.139)

which reduces to the equation of a simple pendulum if γ˙ 0 = 0. Example 337 Steady rotation of a freely rotating rigid body. Free motion of a rigid body is a situation where all externally applied forces and moments are zero. Such a rigid body will move in a steady state behavior determined by initial condition. The Newton–Euler equations of motion for a rigid body are G B

F = m G v˙

M=I

B ˙B Gω

(10.140) +

B G ωB

× L B

(10.141)

Consider a situation where the resultant applied force and moment on the body are zero. F=0

M=0

(10.142)

Based on the Newton equation, the velocity of the mass center will be constant in the global coordinate frame. The Euler equation reduces to ω˙ 1 =

I2 − I3 ω2 ω3 I1

(10.143)

ω˙ 2 =

I3 − I1 ω3 ω1 I22

(10.144)

ω˙ 3 =

I1 − I2 ω1 ω2 I3

(10.145)

showing that the angular velocity can be constant if I1 = I2 = I3

(10.146)

or if two principal moments of inertia, say I1 and I2 , are zero and the third angular velocity, in this case ω3 , is initially zero, or if the angular velocity vector is initially parallel to a principal axis. Example 338 Angular momentum of a two-link manipulator. An example of a two arm manipulator treated as connected rigid bodies. A two-link manipulator is shown in Fig. 10.5. Link A rotates with angular velocity ϕ˙ about the z-axis of its local coordinate frame. Link B is attached to link A and has angular velocity ψ˙ with respect to A about the xA -axis. We assume A and G to be coincident at ϕ = 0; therefore, the rotation matrix between A and G is ⎡

⎤ cos ϕ(t) − sin ϕ(t) 0 G RA = ⎣ sin ϕ(t) cos ϕ(t) 0 ⎦ 0 0 1

(10.147)

10.3 Rigid Body Rotational Kinetics

575

Fig. 10.5 A two-link manipulator

The frame B is related to the frame A by Euler angles ϕ = 90 deg, θ = 90 deg, and ψ = ψ; hence, ⎡

⎤ cπ cψ − cπ sπsψ −cπ sψ − cπ cψsπ sπ sπ A RB = ⎣ cψsπ + cπ cπ sψ −sπ sψ + cπ cπ cψ −cπ sπ ⎦ sπ sψ sπcψ cπ ⎡ ⎤ − cos ψ sin ψ 0 ⎣ sin ψ cos ψ 0 ⎦ 0 0 −1

(10.148)

and therefore, G

RB =

G

⎡

RA A RB

⎤

(10.149)

− cos ϕ cos ψ − sin ϕ sin ψ cos ϕ sin ψ − cos ψ sin ϕ 0 = ⎣ cos ϕ sin ψ − cos ψ sin ϕ cos ϕ cos ψ + sin ϕ sin ψ 0 ⎦ 0 0 −1 The angular velocities of A in G and B in A are G ωA

= ϕ˙ Kˆ

= ψ˙ ıˆA

A ωB

(10.150)

Moment of inertia matrices for the arms A and B can be defined as ⎡ ⎤ ⎡ ⎤ IA1 0 0 IB1 0 0 A B IA = ⎣ 0 IA2 0 ⎦ IB = ⎣ 0 IB2 0 ⎦ 0 0 IA3 0 0 IB3

(10.151)

These moments of inertia must be transformed into the global frame. G

IA =

G

RB A IA G RA T

GIB =

G

RB B IB

G

RBT

(10.152)

The total angular momentum of the manipulator is G

L=

G

LA +

G

LB

(10.153)

576

10 Applied Dynamics

where G G

10.4

LA =

G

LB =

G

IA G ωA IB G ωB =

G

G

A ωB

IB

+

G ωA



(10.154) (10.155)

Mass Moment Matrix

In analysis of the motion of rigid bodies, two types of integrals appear that belong to the geometry of the body. The first type defines the center of mass and is important when the translation motion of the body is considered. The second is the mass moment that appears when the rotational motion of the body is considered. The mass moment is also called moment of inertia, centrifugal moments, or deviation moments. Every rigid body has a 3 × 3 moment of inertia matrix [I ]. ⎡ ⎤ Ixx Ixy Ixz (10.156) [I ] = ⎣ Iyx Iyy Iyz ⎦ Izx Izy Izz The diagonal elements Iij , i = j are called polar mass moments, -



Ixx = Ix =

 y 2 + z2 dm

(10.157)

 z2 + x 2 dm

(10.158)

 x 2 + y 2 dm

(10.159)

B

Iyy = Iy = -



B



Izz = Iz = B

and the off-diagonal elements Iij , i = j are called products mass moments Ixy = Iyx = −

xy dm

(10.160)

yz dm

(10.161)

zx dm

(10.162)

B

Iyz = Izy = − B

Izx = Ixz = −

B

The elements of [I ] are second moments of inertia about a body coordinate frame attached to the mass center C of the body. Therefore, [I ] is a frame-dependent quantity and must be written as B I to show the frame it is computed in. ⎤ −zx y 2 + z2 −xy B ⎣ −xy z2 + x 2 −yz ⎦ dm I = B −zx −yz x 2 + y 2  2  = r I − r rT dm = −˜r r˜ dm -

⎡

B

(10.163)

B

Mass moments can be transformed from a coordinate frame B1 into another coordinate frame B2 , both installed at the mass center of the body, according to the rule of the rotated-axes theorem. 2

I = 2 R1 1 I

2

R1T

(10.164)

Transformation of the mass moment from a central frame B1 located at B2 rC into another frame B2 , which is parallel to B1 , is, according to the rule of parallel-axes theorem. 2

I = 1 I + m r˜C r˜CT

(10.165)

10.4 Mass Moment Matrix

577

Fig. 10.6 (a) Two coordinate frames with a common origin at the mass center of a rigid body. (b) A central coordinate frame B1 and a translated frame B2

If the body coordinate frame Oxyz is located such that the products of inertia vanish, the body coordinate frame is the principal coordinate frame and the associated mass moments are principal mass moments. Principal axes and principal mass moments can be found by solving the following equation for unknown eigenvalues, λ.

Ixx − λ Ixy Ixz

Iyx Iyy − λ Iyz = 0

Izx Izy Izz − λ     det Iij − λ δ ij = 0

(10.166) (10.167)

Equation (10.167) is a cubic equation in λ and we obtain three eigenvalues that are the principal mass moments. I1 = Ix

I2 = Iy

I3 = Iz

(10.168)

Proof Consider two coordinate frames with a common origin at the mass center of a rigid body as shown in Fig. 10.6a. The angular velocity vector and angular momentum vector of a rigid body transform from the frame B1 into B2 by vector transformation rule. 2

ω = 2 R1 1 ω

(10.169)

2

L = 2 R1 1 L

(10.170)

However, L and ω are related according to the mass moment matrix, 1

L = 1I

1

ω

(10.171)

and therefore, 2

L = 2 R1 1 I

2

R1T 2 ω = 2 I

2

ω

(10.172)

which shows how to transfer the mass moment matrix from the coordinate frame B1 into a rotated frame B2 . 2

I = 2 R1 1 I

2

R1T

(10.173)

Now consider a central frame B1 , shown in Fig. 10.6b, at 2 rC , which translates into a fixed frame B2 such that their axes remain parallel. The angular velocity and angular momentum of the rigid body transform from frame B1 into frame B2 by the following relationships.

578

10 Applied Dynamics 2

ω = 1ω

(10.174)

2

L = L + (rC × mvC )

(10.175)

  L = 1 L + m 2 rC × 2 ω × 2 rC   = 1 L + m 2 r˜C 2 r˜CT 2 ω   = 1 I + m 2 r˜C 2 r˜CT 2 ω

(10.176)

1

Therefore, 2

which shows how to transfer the mass moment from frame B1 into a parallel frame B2 2

I =

B1

I + m r˜C r˜CT

(10.177)

This is the parallel-axes theorem, which is also called the Huygens–Steiner theorem. Referring to Eq. (10.173) for transformation of the mass moment into a rotated frame, we can always find a frame B2 in which 2 I is diagonal. In such a frame, we have 2

R1 1 I = 2 I 2 R1

⎤⎡ ⎤ Ixx Ixy Ixz r11 r12 r13 ⎣ r21 r22 r23 ⎦ ⎣ Iyx Iyy Iyz ⎦ r31 r32 r33 Izx Izy Izz ⎡ ⎤⎡ ⎤ I1 0 0 r11 r12 r13 = ⎣ 0 I2 0 ⎦ ⎣ r21 r22 r23 ⎦ 0 0 I3 r31 r32 r33

(10.178)

⎡

(10.179)

which shows that I1 , I2 , and I3 are eigenvalues of 1 I . These eigenvalues can be found by solving the following equation for λ:

Ixx − λ Ixy Ixz

Iyx Iyy − λ Iyz = 0 (10.180)

Izx Izy Izz − λ The eigenvalues I1 , I2 , and I3 are principal mass moments, and their associated eigenvectors are called principal directions. The coordinate frame made by the eigenvectors is the principal body coordinate frame. In the principal coordinate frame, the rigid body angular momentum will be ⎡

⎤ ⎡ ⎤⎡ ⎤ L1 I1 0 0 ω1 ⎣ L2 ⎦ = ⎣ 0 I2 0 ⎦ ⎣ ω2 ⎦ L3 0 0 I3 ω3

(10.181) 

Example 339 Principal mass moments. An easy example of how to determine principal mass moments and principal axes of a mass moment matrix associated to a symmetric rigid body with respect to the z-axis. Consider a mass moment matrix [I ] for a rigid body that is symmetric with respect to the z-axis. ⎡

⎤ 20 −2 0 [I ] = ⎣ −2 30 0 ⎦ 0 0 40

(10.182)

10.4 Mass Moment Matrix

579

To determine its principal mass moments, we set up the determinant (10.167)

20 − λ −2 0

−2 30 − λ 0 = 0

0 0 40 − λ

(10.183)

which leads to a third degree characteristic equation. (20 − λ) (30 − λ) (40 − λ) − 4 (40 − λ) = 0

(10.184)

Three roots of Eq. (10.184) are I1 = 30.385

I2 = 19.615

I3 = 40

(10.185)

and therefore, the principal moment of inertia matrix is ⎡

⎤ 30.385 0 0 I = ⎣ 0 19.615 0 ⎦ 0 0 40

(10.186)

The direction of a principal axis xi is determined by solving the following set of algebraic equations for direction cosines. ⎡

⎤⎡ ⎤ ⎡ ⎤ Ixx − Ii Ixy Ixz cos α i 0 ⎣ Iyx Iyy − Ii Iyz ⎦ ⎣ cos β i ⎦ = ⎣ 0 ⎦ Izx Izy Izz − Ii cos γ i 0

(10.187)

The direction cosines must satisfy a constraint equation. cos2 α i + cos2 β i + cos2 γ i = 1

(10.188)

For the first principal moment of inertia I1 = 30.385 we have ⎡

⎤⎡ ⎤ ⎡ ⎤ 20 − 30.385 −2 0 cos α 1 0 ⎣ ⎦ ⎣ cos β 1 ⎦ = ⎣ 0 ⎦ −2 30 − 30.385 0 0 0 40 − 30.385 cos γ 1 0

(10.189)

− 10.385 cos α 1 − 2 cos β 1 + 0 = 0

(10.190)

−2 cos α 1 − 0.385 cos β 1 + 0 = 0

(10.191)

0 + 0 + 9.615 cos γ 1 = 0

(10.192)

β 1 = 169.1 deg

(10.193)

or

and we obtain α 1 = 79.1 deg

γ 1 = 90.0 deg

Using I2 = 19.615 for the second principal axis, ⎡

⎤⎡ ⎤ ⎡ ⎤ 20 − 19.62 −2 0 cos α 2 0 ⎣ ⎦ ⎣ cos β 2 ⎦ = ⎣ 0 ⎦ −2 30 − 19.62 0 0 0 40 − 19.62 cos γ 2 0

(10.194)

we obtain α 2 = 10.9 deg

β 2 = 79.1 deg

γ 2 = 90.0 deg

(10.195)

580

10 Applied Dynamics

Fig. 10.7 A homogenous rectangular link

The third principal axis is for I3 = 40, ⎤ ⎡ ⎤ ⎤⎡ 0 20 − 40 −2 0 cos α 3 ⎦ ⎣ ⎣ −2 30 − 40 ⎦ ⎣ cos β 3 = 0 ⎦ 0 cos γ 3 0 0 0 40 − 40 ⎡

(10.196)

which leads to α 3 = 90.0 deg

β 3 = 90.0 deg

γ 3 = 0.0 deg

(10.197)

Example 340 Mass moment of rigid rectangular link. A simple example to show how mass moment volume integral will be calculated. Consider a homogenous rectangular link with mass m, length l, width w, and height h, as shown in Fig. 10.7. The body central coordinate frame is attached to the link at its mass center. To calculate the mass moments matrix of the link by integral method, we begin with calculating Ixx . -



Ixx =

-



 2  y + z2 ρdv

y + z dm = 2

B

2

v

-

m = lwh



 2

y 2 + z dv

v

- h/2 - w/2 - l/2  2  m y + z2 dx dy dz lwh −h/2 −w/2 −l/2  m 2 = w + h2 12 =

(10.198)

The Iyy and Izz can be calculated similarly. Iyy =

 m 2 h + l2 12

Izz =

 m2 l + w2 12

(10.199)

Because the coordinate frame is central, the products of inertia must be zero. To show this, we examine Ixy . -

-

Ixy = Iyx = − =

m lwh

-

xy dm = B

h/2

−h/2

-

w/2 −w/2

-

xyρdv v

l/2

−l/2

xy dx dy dz = 0

(10.200)

Therefore, the mass moment matrix for the rigid rectangular link in its principal frame is ⎡ I =⎣

m 12

 2  w + h2 0 0

 20 2  m h +l 12 0

⎤ 0 ⎦  2 0 2 m l + w 12

(10.201)

10.4 Mass Moment Matrix

581

Fig. 10.8 A rigid rectangular link in the principal and non-principal frames

Example 341 Translation of the mass moment matrix. An example for expressing a given mass moment matrix in another translated coordinate frame. The mass moment matrix of the rigid link shown in Fig. 10.8, in the principal frame B(oxyz), is given in Eq. (10.201). The mass moment matrix in the parallel and non-principal frame B  (ox  y  z ) can be found by applying the parallel-axes transformation formula (10.165). B

I =

B

I +m

B

r˜C

B T r˜C

(10.202)

The center of mass position vector in B  -frame is ⎡ ⎤ l 1 B rC = ⎣ w ⎦ 2 h 

and therefore,

⎡ ⎤ 0 −h w 1 B r˜C = ⎣ h 0 −l ⎦ 2 −w l 0 

(10.203)

(10.204)

that provides mass moment matrix in the B  -frame. ⎤ h2 m + 13 mw2 − 14 lmw − 14 hlm 1 2 I =⎣ − 14 lmw h m + 13 l 2 m − 41 hmw ⎦ 3 1 1 2 − 4 hlm − 41 hmw l m + 13 mw2 3 ⎡1

B

3

Example 342 Principal rotation matrix. Consider a body inertia matrix as

⎤ 2/3 −1/2 −1/2 [I ] = ⎣ −1/2 5/3 −1/4 ⎦ −1/2 −1/4 5/3

(10.205)

⎡

(10.206)

The eigenvalues and eigenvectors of [I ] are ⎡ I1 = 0.2413

I2 = 1.8421

I3 = 1.9167

⎤ ⎡ ⎤ 2.351 0.856 9 ⎣ 1 ⎦ ≡ ⎣ 0.364 48 ⎦ = w1 1 0.364 48 ⎡ ⎤ ⎡ ⎤ −0.851 −0.515 6 ⎣ 1 ⎦ ≡ ⎣ 0.605 88 ⎦ = w2 1 0.605 88 ⎡ ⎤ ⎡ ⎤ 0 0.0 ⎣ −1 ⎦ ≡ ⎣ −0.707 11 ⎦ = w3 1 0.707 11

(10.207)

(10.208)

(10.209)

582

10 Applied Dynamics

The normalized eigenvector matrix W is equal to the transpose of the required transformation matrix to make the mass moment matrix diagonal. ⎡

⎤ | | | W = ⎣ w1 w2 w3 ⎦ = 2 R1T | | | ⎡ ⎤ 0.856 9 −0.515 6 0.0 = ⎣ 0.364 48 0.605 88 −0.707 11 ⎦ 0.364 48 0.605 88 0.707 11

(10.210)

We may verify that 2

I ≈ 2 R1 1 I 2 R1T = W T 1 I W ⎡ ⎤ 0.2413 −1 × 10−4 0.0 = ⎣ −1 × 10−4 1.842 1 −1 × 10−19 ⎦ 0.0 0.0 1.916 7

(10.211)

Example 343  Coefficients of the characteristic equation. The characteristic equation for all mass moment matrices is a third degree algebraic equation. The coefficients of the equation are particular functions of elements of [I ]. Here are those functions. The determinant (10.180) for calculating the principal moments of inertia,

Ixx − λ Ixy Ixz

Iyx Iyy − λ Iyz = 0

Izx Izy Izz − λ

(10.212)

leads to a third degree equation of λ, called the characteristic equation. λ3 − a 1 λ2 + a 2 λ − a 3 = 0

(10.213)

The coefficients of the characteristic equation are called the principal invariants of [I ]. The coefficients of the characteristic equation can directly be found from the following equations of the mass moments. a1 = Ixx + Iyy + Izz = tr [I ]

(10.214)

2 2 2 a2 = Ixx Iyy + Iyy Izz + Izz Ixx − Ixy − Iyz − Izx





I I I I I I =

xx xy

+

yy yz

+

xx xz

Iyx Iyy Izy Izz Izx Izz

=

  1 2 a1 − tr I 2 2

(10.215)

a3 = Ixx Iyy Izz + Ixy Iyz Izx + Izy Iyx Ixz   − Ixx Iyz Izy + Iyy Izx Ixz + Izz Ixy Iyx   2 2 2 = Ixx Iyy Izz + 2Ixy Iyz Izx − Ixx Iyz + Iyy Izx + Izz Ixy = det [I ]

(10.216)

10.4 Mass Moment Matrix

583

Example 344  The principal mass moments are coordinate invariants. Mass moment matrix in different coordinate frames has different values; however, the principal values of all of them are the same. Such parameters that remain the same regardless of the coordinate frame are called coordinate invariant. Here is a proof. The roots of the mass moment characteristic equation are the principal mass moments and all are real numbers. The principal mass moments are extreme values. That is, the principal mass moments determine the smallest and largest values of Iii for a rigid body. Because the smallest and largest values of Iii do not depend on the choice of the body coordinate frame, the solution of the characteristic equation is not dependent on the coordinate frame. In other words, if I1 , I2 , I3 are the principal mass moments for 1 I in B1 -frame, the principal moments of inertia for 2 I in B2 -frame are also I1 , I2 , I3 , when 2

I = 2 R1 1 I

2

R1T

(10.217)

We conclude that I1 , I2 , I3 are coordinate invariants of the matrix [I ], and therefore any quantity that depends on I1 , I2 , I3 is also coordinate invariant. The matrix [I ] has only three independent invariants and every other invariant can be expressed in terms of I1 , I2 , I3 . Because I1 , I2 , I3 are the solutions of the characteristic equation of [I ] given in (10.213), we may write the determinant (10.180) in the following form. (10.218) (λ − I1 ) (λ − I2 ) (λ − I3 ) = 0 Expanded form of this equation is λ3 − (I1 + I2 + I3 ) λ2 + (I1 I2 + I2 I3 + I3 I1 ) λ − I1 I2 I3 = 0

(10.219)

By comparing (10.219) and (10.213) we conclude that a1 = Ixx + Iyy + Izz = I1 + I2 + I3

(10.220)

2 2 2 a2 = Ixx Iyy + Iyy Izz + Izz Ixx − Ixy − Iyz − Izx

= I1 I2 + I2 I3 + I3 I1

(10.221)

= I1 I2 I3

(10.222)

  2 2 2 + Iyy Izx + Izz Ixy a3 = Ixx Iyy Izz + 2Ixy Iyz Izx − Ixx Iyz

Being able to express the coefficients a1 , a2 , a3 as functions of I1 , I2 , I3 determines that the coefficients of the characteristic equation are also coordinate invariant. Example 345  Mass moment with respect to a plane, a line, and a point. In robotics, we are only interested in mass moments matrix of rigid bodies as defined in this section. However, mass moments may also be defined with respect to a plane or a line or a point. Here is their definition. The mass moment of a system of particles may be defined with respect to a plane, a line, or a point as the sum of the products of the mass of the particles into the square of the perpendicular distance from the particle to the plane, the line, or the point. For a continuous body, the sum would be replaced by definite integral over the volume of the body. The mass moments with respect to the xy, yz, and zx planes are -

-

Iz2 =

2

z dm

-

Iy 2 =

2

y dm

B

B

Ix 2 =

x 2 dm

(10.223)

B

The mass moments with respect to the x-, y-, and z-axes are Ix =



 y 2 + z2 dm

(10.224)



 z2 + x 2 dm

(10.225)



 x 2 + y 2 dm

(10.226)

B

Iy =

B

Iz =

B

584

10 Applied Dynamics

and therefore, Ix = Iy 2 + Iz2

Iy = Iz2 + Ix 2

Iz = Ix 2 + Iy 2

(10.227)

The mass moment with respect to the origin is  2  Io = x + y 2 + z2 dm = Ix 2 + Iy 2 + Iz2 B

 1 = Ix + Iy + Iz 2

(10.228)

Because the choice of the coordinate frame is arbitrary, we can say that the mass moment with respect to a line is the sum of the mass moments with respect to any two mutually orthogonal planes that pass through the line. The mass moment with respect to a point has similar meaning for three mutually orthogonal planes intersecting at the point.

10.5

Lagrange’s Form of Newton’s Equations

Newton’s equation of motion can be transformed into d dt



∂K ∂ q˙r

where Fr =

−

∂K = Fr ∂qr

r = 1, 2, · · · n

n   i=1

∂fi ∂gi ∂hi + Fiy + Fiz Fix ∂q1 ∂q2 ∂qn

(10.229)

(10.230)

Equation (10.229) is called the Lagrange equation of motion, where K is the kinetic energy of the n DOF system, qr ,  T r = 1, 2, · · · , n, are the generalized coordinates of the system, F = Fix Fiy Fiz is the external force acting on the ith particle of the system, and Fr is the generalized force associated to qr . Proof Let mi be the mass of one of the particles of a system, and let (xi , yi , zi ) be its Cartesian coordinates in a globally fixed coordinate frame. Assume that the coordinates of every individual particle are functions of another set of coordinates q1 , q2 , q3 , · · · , qn and possibly time t. xi = fi (q1 , q2 , q3 , · · · , qn , t)

(10.231)

yi = gi (q1 , q2 , q3 , · · · , qn , t)

(10.232)

zi = hi (q1 , q2 , q3 , · · · , qn , t)

(10.233)

If Fxi , Fyi , Fzi are components of the total acting force on the particle mi , then the Newton equations of motion for the particle would be Fxi = mi x¨i

(10.234)

Fyi = mi y¨i

(10.235)

Fzi = mi z¨i

(10.236)

We multiply both sides of these equations, respectively, by ∂fi ∂qr

∂gi ∂qr

∂hi ∂qr

(10.237)

and add them up for all the n particles.  ∂fi ∂gi ∂hi mi x¨i + y¨i + z¨i ∂qr ∂qr ∂qr i=1

n 

=

n  

Fxi

i=1

∂fi ∂gi ∂hi + Fyi + Fzi ∂qr ∂qr ∂qr

(10.238)

10.5 Lagrange’s Form of Newton’s Equations

585

Taking a time derivative of Eq. (10.231), x˙i =

∂fi ∂fi ∂fi ∂fi ∂fi q˙1 + q˙2 + q˙3 + · · · + q˙n + ∂q1 ∂q2 ∂q3 ∂qn ∂t

(10.239)

we find ∂ x˙i ∂ = ∂ q˙r ∂ q˙r = and therefore,



∂fi ∂fi ∂fi ∂fi q˙1 + q˙2 + · · · + q˙n + ∂q1 ∂q2 ∂qn ∂t



∂fi ∂qr

(10.240) 

∂fi ∂ x˙i d x¨i = x¨i = ∂qr ∂ q˙r dt

∂ x˙i x˙i ∂ q˙r



d − x˙i dt



∂ x˙i ∂ q˙r

(10.241)

However, x˙i

d dt



∂ x˙i ∂ q˙r



d dt 

= x˙i



∂fi ∂qr



∂ 2 fi ∂ 2 fi ∂ 2 fi q˙1 + · · · + q˙n + ∂q1 ∂qr ∂qn ∂qr ∂t∂qr  ∂fi ∂fi ∂fi ∂fi ∂ q˙1 + q˙2 + · · · + q˙n + = x˙i ∂qr ∂q1 ∂q2 ∂qn ∂t

= x˙i

∂ x˙i ∂qr

= x˙i and we have

which is equal to

(10.242)

∂ x˙i d x¨i = ∂ q˙r dt d x˙i x¨i = q˙r dt

"



∂ x˙i x˙i ∂ q˙r



∂ ∂ q˙r

1 2 x˙ 2 i



#

− x˙i

∂ x˙i ∂qr

∂ − ∂qr



(10.243)

1 2 x˙ 2 i

(10.244)

Now substituting (10.241) and (10.244) into the left-hand side of (10.238) leads to n 

 mi

i=1

=

∂fi ∂gi ∂hi + y¨i + z¨i x¨i ∂qr ∂qr ∂qr

n 

mi

d dt

mi

∂ ∂qr

i=1

−

n  i=1

"



1 d = mi 2 i=1 dt n

− =

∂ ∂ q˙r





1 2 1 2 1 2 x˙ + y˙i + z˙ i 2 i 2 2

1 2 1 2 1 2 x˙ + y˙i + z˙ i 2 i 2 2

"

#



 ∂  2 x˙ + y˙i2 + z˙ i2 ∂ q˙r i

#

n  1 ∂  2 mi x˙i + y˙i2 + z˙ i2 2 i=1 ∂qr

d ∂K ∂K − dt ∂qr ∂qr

(10.245)

586

10 Applied Dynamics

where

 1  2 mi x˙i + y˙i2 + z˙ i2 = K 2 i=1 n

(10.246)

K is the kinetic energy of the system. Therefore, the Newton equations of motion (10.234), (10.235), (10.236) are converted into  n   ∂K ∂fi ∂gi ∂hi d ∂K − Fxi (10.247) = + Fyi + Fzi dt ∂ q˙r ∂qr ∂qr ∂qr ∂qr i=1 Because of (10.231), (10.232), and (10.233), the kinetic energy is a function of q1 , q2 , q3 , · · · , qn and time t. The left-hand side of Eq. (10.247) includes the kinetic energy of the whole system, and the right-hand side is a generalized force that shows the effect of changing coordinates from xi to qj on the external forces. Let us assume that the coordinate qr alters to qr + δqr , while the other coordinates q1 , q2 , q3 , · · · , qr−1 , qr+1 , · · · , qn and time t remain unaltered. So, the coordinates of mi are changed into xi +

∂fi δqr ∂qr

(10.248)

yi +

∂gi δqr ∂qr

(10.249)

zi +

∂hi δqr ∂qr

(10.250)

and the work done in this virtual displacement by all forces acting on the particles of the system is δW =

n   i=1

∂fi ∂gi ∂hi + Fyi + Fzi Fxi ∂qr ∂qr ∂qr

δqr

(10.251)

As the work done by internal forces appears in opposite pairs, only the work by external forces remains in Eq. (10.251). Let us denote the virtual work by δW = Fr (q1 , q2 , q3 , · · · , qn , t) δqr (10.252) Then we have



d dt

where Fr =

n  

∂K ∂ q˙r

Fxi

i=1

−

∂K = Fr ∂qr

∂fi ∂gi ∂hi + Fyi + Fzi ∂qr ∂qr ∂qr

(10.253) (10.254)

Equation (10.253) is the Lagrange form of equations of motion. This equation is true for all values of r from 1 to n. We thus have n second-order ordinary differential equations in which q1 , q2 , q3 , · · · , qn are the dependent variables and t is the independent variable. The coordinates q1 , q2 , q3 , · · · , qn are called generalized coordinates and can be any measurable variables to provide the configuration of the system. Because the number of equations and the number of dependent variables are equal, the equations are theoretically sufficient to determine the motion of all mi .  Example 346 A simple pendulum. Simple pendulum means a pendulum made on a massless bar and a pint mass, oscillating in a vertical plane. Here is to find its equation of motion using Lagrange method. A pendulum is shown in Fig. 10.9. Using x and y to indicate Cartesian position of m and using θ = q as the generalized coordinate, we have x = f (θ ) = l sin θ

(10.255)

y = g(θ ) = l cos θ

(10.256)

10.5 Lagrange’s Form of Newton’s Equations

587

Fig. 10.9 A simple pendulum

Fig. 10.10 A vibration absorber

 1 1  2 2 m x˙ + y˙ 2 = ml 2 θ˙ 2 2  d ∂K ∂K d ˙ = ml 2 θ¨ − = (ml 2 θ) dt ∂ θ˙ ∂θ dt K=

(10.257)

(10.258)

The external force components, acting on m, are Fx = 0

Fy = mg

(10.259)

and therefore the generalized force on m can be calculated. Fθ = Fx

∂f ∂g + Fy = −mgl sin θ ∂θ ∂θ

(10.260)

Hence, the equation of motion for the pendulum is ml 2 θ¨ = −mgl sin θ

(10.261)

Example 347 A pendulum attached to an oscillating mass. Attaching a pendulum to a vibrating mass makes a two DOF dynamic system. It is a good example to see how Lagrange method is being applied to derive equations of motion of multiDOF systems. Figure 10.10 illustrates a vibrating mass with a hanging pendulum. The pendulum can act as a vibration absorber if designed properly. Starting with coordinate relationships xM = fM = x xm = fm = x + l sin θ

y M = gM = 0

(10.262)

ym = gm = l cos θ

(10.263)

588

10 Applied Dynamics

we may find the kinetic energy in terms of the generalized coordinates x and θ.  1  2  1  2 2 + m x˙m + y˙m2 M x˙M + y˙M 2 2 1 1  2 = M x˙ 2 + m x˙ 2 + l 2 θ˙ + 2l x˙ θ˙ cos θ 2 2

K=

(10.264)

Then, the left-hand sides of Lagrange equations are d dt d dt





∂K ∂ x˙ ∂K ∂ θ˙

−

∂K 2 = (M + m)x¨ + ml θ¨ cos θ − ml θ˙ sin θ ∂x (10.265)

−

∂K = ml 2 θ¨ + ml x¨ cos θ ∂θ

(10.266)

The external forces acting on M and m are FxM = −kx

FyM = 0

Fxm = 0

Fym = mg

(10.267)

Therefore, the generalized forces are ∂fM ∂gM ∂fm ∂gm + FyM + Fxm + Fym ∂x ∂x ∂x ∂x = −kx

(10.268)

∂fM ∂gM ∂fm ∂gm + FyM + Fxm + Fym ∂θ ∂θ ∂θ ∂θ = −mgl sin θ

(10.269)

Fx = FxM

Fθ = FxM

and finally the Lagrange equations of motion will be found. 2 (M + m)x¨ + ml θ¨ cos θ − ml θ˙ sin θ = −kx

(10.270)

2¨

(10.271)

ml θ + ml x¨ cos θ = −mgl sin θ

Example 348  Potential force field. In mechanics, gravitational and spring forces are the most common potential forces. In case there is no nonpotential forces, the Lagrange equation reduces to energy conservation law. Here is an analytic proof. If a system of masses mi are moving in a potential force field, ∇ i V , their Newton equations of motion will be Fmi = −∇ i V

(10.272)

mi r¨ i = −∇ i V

i = 1, 2, · · · n

(10.273)

Inner product of equations of motion with r˙ i and adding the equations, n 

mi r˙ i · r¨ i = −

i=1

and then integrating over time

1 mi r˙ i · r˙ i = − 2 i=1 n

n 

r˙ i · ∇ i V

(10.274)

i=1

-  n i=1

ri · ∇ i V

(10.275)

10.5 Lagrange’s Form of Newton’s Equations

589

show that K=−

-  n  ∂V ∂V ∂V xi + yi + zi = −V + E ∂xi ∂yi ∂zi i=1

(10.276)

where E is the constant of integration. E is called mechanical energy of the system and is equal to kinetic plus potential energies. Example 349 Kinetic energy of the Earth. To have a sense of numerical value of kinetic energy, we will compare the kinetic energy of the Earth for its spin and for its rotation about the Sun, separately. The Earth is approximately a rotating rigid body about a fixed axis. The two motions of the Earth are called revolution about the Sun, and rotation about an axis approximately fixed in the Earth. The kinetic energy of the Earth due to its rotation is K1 = =

1 2 Iω 2 1

  6356912 + 6378388 2 12 5.9742 × 1024 25 2  2 2π 366.25 × 24 × 3600 365.25

= 2.5762 × 1029 J

(10.277)

and the kinetic energy of the Earth due to its revolution is 1 Mr 2 ω22 2  2 1 = 5.9742 × 1024 1.49475 × 1011 2 2  1 2π × 24 × 3600 365.25

K2 =

= 2.6457 × 1033 J

(10.278)

The r is the distance from the Sun, ω1 is the angular speed about its axis, and ω2 is the angular speed about the Sun. The total kinetic energy of the Earth is K = K1 + K2 . The ratio of the revolutionary to rotational kinetic energies is a large number. K2 2.6457 × 1033 = ≈ 10000 K1 2.5762 × 1029

(10.279)

Example 350  Non-Cartesian coordinate system. Lagrange method works for any type of coordinate systems. Here is an example of a non-Cartesian coordinate system. The parabolic coordinate system and Cartesian coordinate systems are related according to 

x = ηξ cos ϕ

y = ηξ sin ϕ

x 2 + y 2 + z2 + z y ϕ = tan−1 x

ξ2 =

η2 =

ξ 2 − η2 z= 2



x 2 + y 2 + z2 − z

(10.280) (10.281) (10.282)

590

10 Applied Dynamics

An electron in a uniform electric field along the positive z-axis is also under the action of an attractive central force field due to the nuclei of the atom.  k k F = − 2 eˆr = −∇ − (10.283) r r The influence of a uniform electric field on the motion of the electrons in atoms is called the Stark effect, and it is easier to analyze its motion in a parabolic coordinate system. The kinetic energy in a parabolic coordinate system is  1  2 m x˙ + y˙ 2 + z˙ 2 2   1  2 = m η2 + ξ 2 η˙ 2 + ξ˙ + η2 ξ 2 ϕ˙ 2 2

K=

(10.284)

and the force acting on the electron is 

k F = −∇ − + eEz r   eE  2 2k 2 + ξ −η = −∇ − 2 2 ξ + η2

(10.285)

They lead to the following generalized forces: Fη = F · bη = − 

4kη

2 + eEη

(10.286)

4kξ Fξ = F · bξ = −  2 − eEη 2 ξ + η2

(10.287)

Fϕ = 0

(10.288)

ξ + η2 2

where bξ , bη , bϕ are base vectors of the coordinate system bξ =

∂r = η cos ϕˆı + η sin ϕ jˆ + ξ kˆ ∂ξ

(10.289)

bη =

∂r = ξ cos ϕˆı + ξ sin ϕ jˆ − ηkˆ ∂η

(10.290)

bϕ =

∂r = −ηξ sin ϕˆı + ηξ cos ϕ jˆ ∂ϕ

(10.291)

Therefore, following the Lagrange method, the equations of motion of the electron are d dt d Fξ = dt d Fϕ = dt Fη =



   2 mη˙ ξ 2 + η2 − mη η˙ 2 + ξ˙ − mηξ 2 ϕ˙ 2

(10.292)



   2 mξ˙ ξ 2 + η2 − mη η˙ 2 + ξ˙ − mξ η2 ϕ˙ 2

(10.293)



mη2 ξ 2 ϕ˙ 2



(10.294)

10.6 Lagrangian Mechanics

10.6

591

Lagrangian Mechanics

Assume for some forces F there is a function V , called potential energy, such that the force is derivable from V .  T F = Fix Fiy Fiz = −∇V

(10.295)

Such a force is called potential or conservative force. Then, the Lagrange equation of motion can be written as 

d dt

∂L ∂ q˙r

−

∂L = Qr ∂qr

where L is the Lagrangian of the system

r = 1, 2, · · · n

L=K −V

(10.296)

(10.297)

and Qr is the nonpotential generalized force.

Proof Consider an external conservative force F acting on a system.  T F = Fxi Fyi Fzi = −∇V

(10.298)

Then the Lagrange equation can be expressed simpler. d dt



∂K ∂ q˙r

−

∂V ∂K =− ∂qr ∂q1

r = 1, 2, · · · n

(10.299)

The virtual work ∂W done by the forces in an arbitrary virtual displacement δq1 , δq2 , δq3 , · · · , δqn is ∂W = −

∂V ∂V ∂V δq1 − δq2 − · · · δqn ∂q1 ∂q2 ∂qn

(10.300)

Introducing the Lagrangian function L converts the Lagrange equation for the conservative system into a compact expression. The Lagrangian is also called kinetic potential.

d dt



∂L ∂ q˙r

L = K −V

−

∂L =0 ∂qr

(10.301)

r = 1, 2, · · · n

(10.302)

If a force is not conservative, then the work done by the force is δW =

n  

∂fi ∂gi ∂hi Fxi + Fyi + Fzi ∂qr ∂qr ∂qr

i=1

δqr

= Qr δqr

(10.303)

and the equation of motion would be d dt



∂L ∂ q˙r

−

∂L = Qr ∂qr

r = 1, 2, · · · n

(10.304)

where Qr is the nonpotential generalized force doing work in a virtual displacement of the rth generalized coordinate qr . 

592

10 Applied Dynamics

Fig. 10.11 A spherical pendulum

Example 351 Spherical pendulum. Spherical pendulum is a nonlinear conservative two DOF dynamic system. A good example to learn how Lagrange method derives equations of motion. A pendulum analogy is utilized in modeling of many dynamic problems. Figure 10.11 illustrates a spherical pendulum with a tip mass m and a massless bar of length l. The angles ϕ and θ may be used as describing coordinates of the system. At the first step, we determine Cartesian coordinates of the mass as a function of the generalized coordinates. ⎡

⎤ ⎡ ⎤ X r cos ϕ sin θ ⎣ Y ⎦ = ⎣ r sin θ sin ϕ ⎦ Z −r cos θ

(10.305)

Then, the kinetic and potential energies of the pendulum must be determined. 1 2 2 m l θ˙ + l 2 ϕ˙ 2 sin2 θ 2 V = −mgl cos θ

K=

(10.306) (10.307)

The kinetic potential function of this system then determines the Lagrangian L of the system. L=

1 2 2 m l θ˙ + l 2 ϕ˙ 2 sin2 θ + mgl cos θ 2

(10.308)

Having the Lagrangian is equivalent to having the equations of motion. They will be derived by differentiation and substitution in Eq. (10.302). g sin θ = 0 l

(10.309)

ϕ¨ sin2 θ + 2ϕ˙ θ˙ sin θ cos θ = 0

(10.310)

θ¨ − ϕ˙ 2 sin θ cos θ +

Example 352 A one-link manipulator. Deriving equation of motion of an example on a one DOF dynamic system similar to a robotic arm with actuator. A one-link manipulator is illustrated in Fig. 10.12. Assume that there is viscous friction in the joint where an ideal motor applies the torque Q to move the arm. The rotor of an ideal motor has no mass moment by assumption. The kinetic and potential energies of the manipulator are  2 1 2 1 I θ˙ = IC + ml 2 θ˙ 2 2 V = −mg cos θ

K=

(10.311) (10.312)

10.6 Lagrangian Mechanics

593

Fig. 10.12 A compound pendulum with a motor at O

Fig. 10.13 An ideal model of a 2R planar manipulator

where m is the mass and I is the mass moment of the pendulum about O. The Lagrangian of the manipulator will be L=K −V =

1 2 I θ˙ + mg cos θ 2

(10.313)

and therefore, the equation of motion of the manipulator can be found. d M= dt



∂L ∂ θ˙

−

∂L = I θ¨ + mgl sin θ ∂θ

(10.314)

The generalized force M is the contribution of the motor torque Q and the viscous friction torque −cθ˙ . Hence, the equation of motion of the manipulator will be Q = I θ¨ + cθ˙ + mgl sin θ (10.315)

Example 353 The ideal 2R planar manipulator dynamics. An example of applying Lagrange method on a manipulator and deriving its equations of motion. Ideal 2R manipulator is the one with massless arm and point mass motors and payload. An ideal model of a 2R planar manipulator is illustrated in Fig. 10.13. It is called ideal because we assumed the links are massless and there is no friction. The masses m1 and m2 are the second motor to run the second link and the load at the end point. We take the absolute angle θ 1 and the relative angle θ 2 as the generalized coordinates to express the configuration of the manipulator. The global positions of m1 and m2 are "

X1 Y2

#

"

l cos θ 1 = 1 l1 sin θ 1

# (10.316)

594

10 Applied Dynamics

"

X2 Y2

#

" =

l1 cos θ 1 + l2 cos (θ 1 + θ 2 ) l1 sin θ 1 + l2 sin (θ 1 + θ 2 )

# (10.317)

and therefore, the global velocities of the masses are "

"

X˙ 2 Y˙2

#

" =

X˙ 1 Y˙1

#

"

−l1 θ˙ 1 sin θ 1 = l1 θ˙ 1 cos θ 1

#

  # −l1 θ˙ 1 sin θ 1 − l2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) l1 θ˙ 1 cos θ 1 + l2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 )

(10.318)

(10.319)

The kinetic energy of this manipulator is made of kinetic energy of the masses. K = K1 + K2 = =

 1   1  ˙2 m1 X1 + Y˙12 + m2 X˙ 22 + Y˙22 2 2

1 2 m1 l12 θ˙ 1 2  2   1  2 + m2 l12 θ˙ 1 + l22 θ˙ 1 + θ˙ 2 + 2l1 l2 θ˙ 1 θ˙ 1 + θ˙ 2 cos θ 2 2

(10.320)

The potential energy of the manipulator is V = V1 + V2 = m1 gY1 + m2 gY2 = m1 gl1 sin θ 1 + m2 g (l1 sin θ 1 + l2 sin (θ 1 + θ 2 ))

(10.321)

The Lagrangian is then obtained from Eqs. (10.320) and (10.321). 1 2 m1 l12 θ˙ 1 2  2   1 2 2 + m2 l1 θ˙ 1 + l22 θ˙ 1 + θ˙ 2 + 2l1 l2 θ˙ 1 θ˙ 1 + θ˙ 2 cos θ 2 2 − (m1 gl1 sin θ 1 + m2 g (l1 sin θ 1 + l2 sin (θ 1 + θ 2 )))

L = K −V =

(10.322)

Lagrangian provides us with the required partial derivatives. ∂L = − (m1 + m2 ) gl1 cos θ 1 − m2 gl2 cos (θ 1 + θ 2 ) ∂θ 1   ∂L = (m1 + m2 ) l12 θ˙ 1 + m2 l22 θ˙ 1 + θ˙ 2 ˙ ∂θ1   +m2 l1 l2 2θ˙ 1 + θ˙ 2 cos θ 2 d dt



∂L ∂ θ˙ 1



(10.323)

(10.324)

  = (m1 + m2 ) l12 θ¨ 1 + m2 l22 θ¨ 1 + θ¨ 2   +m2 l1 l2 2 θ¨ 1 + θ¨ 2 cos θ 2   −m2 l1 l2 θ˙ 2 2θ˙ 1 + θ˙ 2 sin θ 2

  ∂L = −m2 l1 l2 θ˙ 1 θ˙ 1 + θ˙ 2 sin θ 2 − m2 gl2 cos (θ 1 + θ 2 ) ∂θ 2   ∂L = m2 l22 θ˙ 1 + θ˙ 2 + m2 l1 l2 θ˙ 1 cos θ 2 ˙ ∂θ2

(10.325) (10.326) (10.327)

10.6 Lagrangian Mechanics

595

d dt



∂L ∂ θ˙ 2



  = m2 l22 θ¨ 1 + θ¨ 2 + m2 l1 l2 θ¨ 1 cos θ 2 − m2 l1 l2 θ˙ 1 θ˙ 2 sin θ 2

(10.328)

Therefore, the equations of motion for the 2R manipulator are Q1 =

d dt



∂L ∂ θ˙ 1

−

∂L ∂θ 1

  = (m1 +m2 ) l12 θ¨ 1 +m2 l22 θ¨ 1 +θ¨ 2     +m2 l1 l2 2 θ¨ 1 +θ¨ 2 cos θ 2 − m2 l1 l2 θ˙ 2 2θ˙ 1 +θ˙ 2 sin θ 2 + (m1 +m2 ) gl1 cos θ 1 +m2 gl2 cos (θ 1 +θ 2 ) ∂L ∂L − ˙ ∂θ ∂θ2 2   2 ¨ = m2 l2 θ 1 + θ¨ 2 + m2 l1 l2 θ¨ 1 cos θ 2 − m2 l1 l2 θ˙ 1 θ˙ 2 sin θ 2   +m2 l1 l2 θ˙ 1 θ˙ 1 + θ˙ 2 sin θ 2 + m2 gl2 cos (θ 1 + θ 2 )

d Q2 = dt

(10.329)



(10.330)

The generalized forces Q1 and Q2 are the required forces to vary the generalized coordinates. In this case, Q1 is the torque at the base motor and Q2 is the torque of the motor at m1 . The equations of motion can be rearranged to have a more systematic form.   Q1 = (m1 + m2 ) l12 + m2 l2 (l2 + 2l1 cos θ 2 ) θ¨ 1 +m2 l2 (l2 + l1 cos θ 2 ) θ¨ 2 2 −2m2 l1 l2 sin θ 2 θ˙ 1 θ˙ 2 − m2 l1 l2 sin θ 2 θ˙ 2

+ (m1 + m2 ) gl1 cos θ 1 + m2 gl2 cos (θ 1 + θ 2 )

(10.331)

Q2 = m2 l2 (l2 + l1 cos θ 2 ) θ¨ 1 + m2 l22 θ¨ 2 2 +m2 l1 l2 sin θ 2 θ˙ 1 + m2 gl2 cos (θ 1 + θ 2 )

(10.332)

Example 354 Trebuchet. Trebuchet is an interesting mechanical device similar to a robotic manipulator. Here is how to derive the equations of motion of a sample trebuchet. A trebuchet is shown schematically in Fig. 10.14. It is a shooting weapon of war for the mass m2 that is powered by a falling massive counterweight m1 . A beam AB is pivoted to the chassis with two unequal sections a and b. The figure shows a trebuchet at its initial configuration. The origin of a global coordinate frame is set at the pivot point. The counterweight m1 is at (x1 , y1 ) and is hinged at the shorter arm of the beam at a distance c from the end B. The mass of the projectile is m2 and it is at the end of a massless sling with a length l attached to the end of the longer arm of the beam. The three independent variable angles α, θ, γ express the motion of the device. We consider the parameters a, b, c, d, l, m1 , m2 to be constant and determine the equations of motion by the Lagrange method. Figure 10.15 illustrates the trebuchet when it is in motion. The position coordinates of masses m1 and m2 are x1 = b sin θ − c sin (θ + γ )

(10.333)

y1 = −b cos θ + c cos (θ + γ )

(10.334)

x2 = −a sin θ − l sin (−θ + α)

(10.335)

y2 = −a cos θ − l cos (−θ + α)

(10.336)

Taking a time derivative provides the velocity components.

596

10 Applied Dynamics

Fig. 10.14 A trebuchet at starting position

Fig. 10.15 A trebuchet in motion

  x˙1 = bθ˙ cos θ − c θ˙ + γ˙ cos (θ + γ )   y˙1 = bθ˙ sin θ − c θ˙ + γ˙ sin (θ + γ ) x˙2 = l (c − α) ˙ cos (α − θ) − a θ˙ cos (θ )   y˙2 = a θ˙ sin θ − l θ˙ − α˙ sin (α − θ )

(10.337) (10.338)

(10.339) (10.340)

The kinetic energy of the system is  1   1 1 1  m1 v12 + m2 v22 = m1 x˙12 + y˙12 + m2 x˙22 + y˙22 2 2 2 2  2 1  = m1 b2 + c2 θ˙ + c2 γ˙ 2 + 2c2 θ˙ γ˙ 2   −m1 bcθ˙ θ˙ + γ˙ cos γ  2 1  + m2 a 2 + l 2 θ˙ + l 2 α˙ 2 − 2l 2 θ˙ α˙ 2   −m2 al θ˙ θ˙ − α˙ cos (2θ − α)

K=

(10.341)

10.7 Summary

597

The potential energy of the system can be calculated by y position of the masses. V = m1 gy1 + m2 gy2 = m1 g (−b cos θ + c cos (θ + γ )) +m2 g (−a cos θ − l cos (−θ + α))

(10.342)

Having the energies K and V , we can set up the Lagrangian L. L=K −V

(10.343)

Using the Lagrangian, we are able to find the three equations of motion. d dt d dt d dt



∂L ∂ θ˙



∂L ∂ α˙



∂L ∂ γ˙

−

∂L =0 ∂θ

(10.344)

−

∂L =0 ∂α

(10.345)

−

∂L =0 ∂γ

(10.346)



The trebuchet appeared in 500–400 B.C. in China and was developed by Persian armies around 300 B.C. It was used by the Arabs against the Romans during 600–1200 A.D. The trebuchet may also be called the manganic, manjaniq, catapults, or onager. The Persian word “Manganic” is the root of the words “mechanic” and “machine.”

10.7

Summary

The translational and rotational equations of motion for a rigid body, expressed in the global coordinate frame, are G

G

F=

M=

G

dG p dt

(10.347)

G

dG L dt

(10.348)

where G F and G M indicate the resultant of the external forces and moments applied on the rigid body and measured at C. The vector G p is the momentum and G L is the moment of momentum for the rigid body at C p = mv

(10.349)

L = rC × p

(10.350)

The expressions of the equations of motion in the body coordinate frame are B

F=

G

p˙ +

B G ωB

× Bp

= m B aB + m BG ωB × B vB B

M=

B

˙+ L

=

B

I

where I is the moment of inertia for the rigid body.

B G ωB

B ˙B Gω

+

× BL B G ωB

×

B

I

B G ωB

(10.351)



(10.352)

598

10 Applied Dynamics

⎡

⎤ Ixx Ixy Ixz I = ⎣ Iyx Iyy Iyz ⎦ Izx Izy Izz

(10.353)

The elements of I are only functions of the mass distribution of the rigid body and are defined by  2  Iij = ri δ mn − xim xj n dm , i, j = 1, 2, 3

(10.354)

B

where δ ij is Kronecker’s delta. Every rigid body has a principal body coordinate frame in which the moment of inertia is in the form. ⎡

⎤ I1 0 0 B I = ⎣ 0 I2 0 ⎦ 0 0 I3

(10.355)

The rotational equation of motion in the principal coordinate frame simplifies to M1 = I1 ω˙ 1 − (I2 − I2 ) ω2 ω3 M2 = I2 ω˙ 2 − (I3 − I1 ) ω3 ω1

(10.356)

M3 = I3 ω˙ 3 − (I1 − I2 ) ω1 ω2 Utilizing homogenous position vectors we also define pseudo-inertia matrix I¯ with application in robot dynamics as B

I¯ =

r rT dm

(10.357)

B

⎡ −I ⎢ ⎢ =⎢ ⎣

xx +Iyy +Izz

2

Iyx Izx mxC

Ixy

Ixx −Iyy +Izz 2

Izy myC

Ixz Iyz

Ixx +Iyy −Izz 2 mzC

⎤ mxC ⎥ myC ⎥ ⎥ mzC ⎦ m

where B rC is the position of the mass center in the body frame. ⎤ ⎡11 ⎤ x dm xC m 1B B rC = ⎣ yC ⎦ = ⎣ m1 1B y dm ⎦ 1 zC z dm m B ⎡

(10.358)

The equations of motion for a mechanical system having n DOF can also be found by the Lagrange equation d dt



∂L ∂ q˙r

−

∂L = Qr r = 1, 2, · · · n ∂qr

(10.359)

L=K −V

(10.360)

where L is the Lagrangian of the system, K is the kinetic energy, V is the potential energy, and Qr is the nonpotential generalized force. n   ∂fi ∂gi ∂hi Qr = Qix + Qiy + Qiz (10.361) ∂q1 ∂q2 ∂qn i=1

10.7 Summary

599

 T The parameters qr , r = 1, 2, · · · , n, are the generalized coordinates of the system, Q = Qix Qiy Qiz is the external force acting on the ith particle of the system, and Qr is the generalized force associated to qr . When (xi , yi , zi ) are the Cartesian coordinates in a globally fixed coordinate frame for the particle mi , then its coordinates may be functions of another set of coordinates q1 , q2 , q3 , · · · , qn and possibly time t. xi = fi (q1 , q2 , q3 , · · · , qn , t)

(10.362)

yi = gi (q1 , q2 , q3 , · · · , qn , t)

(10.363)

zi = hi (q1 , q2 , q3 , · · · , qn , t)

(10.364)

600

10.8

10 Applied Dynamics

Key Symbols

a A B C d D e f, F g G, B0 h ıˆ, jˆ, kˆ Iˆ, Jˆ, Kˆ I = [I ] I¯ = [I¯] I = [I] k K l L L m M Mi p p, q q P, Q r r ri R uˆ u1 , u2 , u3 v v V w W x, y, z X, Y, Z Greek α α α1, α2, α3 δ θ ϕ, θ, ψ φ ω

Acceleration vector Acceleration transformation matrix Body coordinate frame Mass center Translation vector, displacement vector Displacement transformation matrix Rotation quaternion Force vector Gravitational acceleration Global coordinate frame, base coordinate frame Height Local coordinate axes unit vectors Global coordinate axes unit vectors Mass moment matrix Pseudo-inertia matrix Identity matrix Spring stiffness Kinetic energy Length Angular moment vector, moment of moment Lagrangian The number of independent equations Moment vector, torque vector The element i of M Momentum Position vectors of P , Q Joint variable vector Points Radius Position vectors, homogenous position vector The element i of r Rotation transformation matrix, radius Unit vector along the axis of ω Components of uˆ Velocity Velocity vector Potential energy Width Work, normalized eigenvector matrix Local coordinate axes Global coordinate axes

Angular acceleration Angular acceleration vector Components of α Kronecker delta Rotary joint angle Euler angles Angle of rotation about uˆ Angular velocity

10.8 Key Symbols

ω ω˜ ω1 , ω2 , ω3

Angular velocity vector Skew symmetric matrix of the vector ω Components of ω

Symbol [ ]−1 [ ]T ∇ (i)  ⊥ × ← → e E lim sgn tr

Inverse of the matrix [ ] Transpose of the matrix [ ] Gradient Link number i Parallel Perpendicular Vector cross product Matrix form of a quaternion e The Earth Limit function Signum function Trace

601

602

10 Applied Dynamics

Exercises 1. Notation and symbols. Describe the meaning of these notations. a − G pP b − Ll

c − GF

g − dm h − [I ] i − Iij

d − 1 W2 e − K j−G BF

f−V

k − G M l − I1

·

m − 02 α˜ 1 n − 21 ω˜ 2 o − G AB p − G V˙B q − J˙

¨ r−X

2. Kinetic energy of a rigid link. Consider a straight and uniform bar as a rigid link of a manipulator. The link has a mass m. Show that the kinetic energy of the bar can be expressed as 1 m (v1 · v1 + v1 · v2 + v2 · v2 ) 6

K=

(10.365)

where v1 and v2 are the velocity vectors of the end points of the link. 3. Position and velocity of mass center C of system of discrete particles. There are three particles m1 = 1 kg, m2 = 2 kg, m3 = 3 kg, at ⎡

Their velocities are

⎤ ⎡ ⎤ ⎡ ⎤ 1 −1 2 r1 = ⎣ −1 ⎦ r2 = ⎣ −3 ⎦ r3 = ⎣ −1 ⎦ 1 2 −3

(10.366)

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 2 −1 3 v1 = ⎣ 1 ⎦ v2 = ⎣ 0 ⎦ v3 = ⎣ −2 ⎦ 1 2 −1

(10.367)

(a) Find the position and velocity of the system at C. (b) Calculate the system’s momentum and moment of momentum. (c) Calculate the system’s kinetic energy and determine the rotational and translational parts of the kinetic energy. 4. Newton’s equation of motion in the body frame. Show that Newton’s equation of motion in the body frame is ⎡

⎤⎡ ⎤ ⎤ ⎡ ⎤ ⎡ Fx ax 0 −ωz ωy vx ⎣ Fy ⎦ = m ⎣ ay ⎦ + ⎣ ωz 0 −ωx ⎦ ⎣ vy ⎦ Fz az −ωy ωx 0 vz

(10.368)

5. Work on a curved path. A particle of mass m is moving on a circular path given by G rP . G

rP = cos θ Iˆ + sin θ Jˆ + 4 Kˆ

Calculate the work of a force G F when the particle moves from θ = 0 to θ = π2 . (a) z2 − y 2 ˆ y2 − x2 ˆ x2 − y2 ˆ G F= I + J+ K (x + y)2 (x + y)2 (x + z)2 (b) G

F=

z2 − y 2 ˆ 2y ˆ x 2 − y 2 ˆ I + K J+ x+y (x + y)2 (x + z)2

(10.369)

(10.370)

(10.371)

Exercises

603

6. Newton’s equation of motion. Find the equations of motion for the system shown in Fig. 10.9 based on Newton’s method. 7. Acceleration in the body frame. Find the acceleration vector for the end point of the two-link manipulator shown in Fig. 10.5, expressed in frame A. 8. Principal moments of inertia. Find the principal mass moments and axes of the following mass moments matrices: (a) ⎡ ⎤ 322 (10.372) [I ] = ⎣ 2 2 0 ⎦ 204 (b)

⎡

⎤ 324 [I ] = ⎣ 2 0 2 ⎦ 423

(c)

(10.373)

√ ⎤ 100 √ 20 3 0 [I ] = ⎣ 20 3 60 0 ⎦ 0 0 10 ⎡

(10.374)

9.  The solutions of mass moment characteristic equation are real. The solution of a cubic equation where a = 0 can be found in a systematic way. ax 3 + bx 2 + cx + d = 0

(10.375)

Transform the equation into a new form with discriminant 4p3 + q 2 , y 3 + 3py + q = 0 Use the transformation x = y −

b , 3a

(10.376)

where p=

3ac − b2 9a 2

q=

2b3 − 9abc + 27a 2 d 27a 3

(10.377)

The solutions are then

α− 3β 2π i √ 4π i

y2 = e 3 3 α − e 3 3 β

y1 =

√ 3

y3 = e

√

4π i 3 3

α−e

β

(10.378)

2π i 3 3



q 2 + 4p3 −q + q 2 + 4p3 α= β= (10.379) 2 2 For real values of p and q, if the discriminant is positive, then one root is real, and two roots are complex conjugates. If the discriminant is zero, then there are three real roots, of which at least two are equal. If the discriminant is negative, then there are three unequal real roots. Apply this theory for the characteristic equation of the matrix [I ] and show that the principal moments of inertia are real. 10. Moment of inertia. Calculate the mass moments of shown objects in Fig. 10.16, in a principal Cartesian coordinate frame at C. (a) A cylinder similar to a uniform arm with a circular cross section, Fig. 10.16a. (b) A rectangular box similar to a uniform arm with a rectangular cross section, Fig. 10.16b. (c) A house similar to a prismatic bar with a nonsymmetric polygon cross section, Fig. 10.16c. where

−q +

604

10 Applied Dynamics

Fig. 10.16 Solid objects to calculate mass moments

Fig. 10.17 The location on the Earth is defined by longitude ϕ and latitude θ

11. Global differential of angular momentum. Convert the mass moment B I and the angular velocity BG ωB into the global coordinate frame and then find the differential of angular momentum. It is an alternative method to show that G

d dt

B

d B B  I G ωB dt ˙ + BG ωB × B L = I ω˙ + ω × (I ω) = BL

L=

G

(10.380)

12. Rotated mass moment matrix. A principal mass moment matrix 2 I in B2 -frame is given. ⎡

⎤ 300 2 I = ⎣0 5 0⎦ 004

(10.381)

The principal frame was achieved by rotating the initial body coordinate frame 30 deg about the x-axis, followed by 45 deg about the z-axis. Find the initial moment of inertia matrix 1 I . 13. Rotation of moment of inertia matrix. Find the required rotation matrix that transforms the mass moment matrix [I ] into a diagonal matrix. ⎡

⎤ 3 2 2 [I ] = ⎣ 2 2 0.1 ⎦ 2 0.1 4

(10.382)

Exercises

605

Fig. 10.18 A two-link manipulator

14. Kinematics of a moving car on the Earth. A vehicle on the Earth is located by its longitude ϕ from a fixed meridian, say, the Greenwich meridian, and its latitude θ from the equator, as shown in Fig. 10.17. We attach a coordinate frame B at the center of the Earth with x-axis on the equator’s plane and y-axis pointing the vehicle. There are also two coordinate frames E and G where E is attached to the Earth and G is the global coordinate frame. Show that the angular velocity of B and the velocity of the vehicle are B G ωB

= θ˙ ıˆB + (ωE + ϕ) ˙ sin θ jˆB + (ωE + ϕ) ˙ cos θ kˆ

(10.383)

B G vP

= −r (ωE + ϕ) ˙ cos θ ıˆB + r θ˙ kˆ

(10.384)

Calculate the acceleration of the vehicle. 15. Equations of motion for a rotating arm. Find the equations of motion for the rotating links shown in Fig. 10.18 based on the Lagrange method. 16. Equations of motion from Lagrangian. Consider a physical system with a Lagrangian L. L=

1 1 m (a x˙ + by) ˙ 2 − k (ax + by)2 2 2

Determine the equations of motion of the system. The coefficients m, k, a, and b are constant. 17. Lagrangian from equation of motion. Find the Lagrangian associated to the following equations of motions: (a) mr 2 θ¨ + k1 l1 θ + k2 l2 θ + mgl = 0 (b)

r¨ − r θ˙ = 0 2

r 2 θ¨ + 2r r˙ θ˙ = 0

(10.385)

(10.386) (10.387)

18. A mass on a rotating ring. A particle of mass m is free to slide on a rotating vertical ring as shown in Fig. 10.19. The ring is turning with a constant angular velocity ω = ϕ˙ about the Z-axis. Determine the equation of motion of the particle. The local coordinate frame is set up with the x-axis pointing the particle and the z-axis in the plane of the ring and parallel with the tangent to the ring at the position of the mass.

606

10 Applied Dynamics

Fig. 10.19 A mass on a rotating ring

19.  Particle in electromagnetic field. Show that equations of motion of a particle with mass m with a Lagrangian L L=

are

1 2 m˙r − e + er˙ · A 2



∂ ∂Ai mq¨i = e − − ∂qi ∂t

+e

3 

 q˙j

j =1

∂Aj ∂Ai − ∂qi ∂qj

(10.388)

⎤ ⎡ ⎤ x q1 q = ⎣ q2 ⎦ = ⎣ y ⎦ z q3

(10.389)

⎡

where

(10.390)

Then convert the equations of motion into a vectorial form m¨r = eE (r, t) + e˙r × B (r, t)

(10.391)

where E and B are electric and magnetic fields. E = −∇ −

∂A ∂t

B=∇ ×A

(10.392)

20. Inverted pendulum on a moving cart. Figure 10.20 illustrates an inverted pendulum on a moving cart. If there is a friction between the cart and the ground surface with coefficient μs , and a friction at the pivot of the pendulum with coefficient μp , show that the equations of motion of the system are  μp 2 g sin θ+ μs sgn x−ml ˙ θ˙ sin θ−F cos θ − θ˙ ml ¨θ =  4 m cos2 θ l − 3 M +m  2 F + ml θ˙ sin θ − θ¨ cos θ − μs sgn x˙ x¨ = M +m

(10.393)

(10.394)

Exercises

607

Fig. 10.20 An inverted pendulum on a moving cart

Fig. 10.21 A pendulum with a vibrating pivot

where

⎧ ⎨ 1 x>0 sgn x = 0 x=0 ⎩ −1 x < 0

(10.395)

21. Forced vibration of a pendulum. Figure 10.21 illustrates a simple pendulum having a length l and a bob with mass m. Find the equation of motion if: (a) The pivot O has a dictated motion in X direction. XO = a sin ωt

(10.396)

YO = b sin ωt

(10.397)

rO = R cos ωt Iˆ + R sin ωt Jˆ

(10.398)

(b) The pivot O has a dictated motion in Y direction.

(c) The pivot O has a uniform motion on a circle.

Robot Dynamics

11

We find the dynamics equations of motion of robots by two methods: Newton–Euler and Lagrange. The Newton–Euler method is more fundamental and finds the dynamic equations to determine the required actuators’ force and torque to move a robot, as well as the joint forces. Lagrange method provides only the required differential equations that determines the actuators’ force and torque.

Fig. 11.1 A link (i) and its vectorial kinematic characteristics

11.1

Rigid Link Newton–Euler Dynamics

Figure 11.1 illustrates the link number (i) of a manipulator and its velocity and acceleration vectorial characteristics. Figure 11.2 illustrates free body diagram of the link (i). The force Fi−1 and moment Mi−1 are the resultant force and moment that link (i − 1) applies to link (i) at joint i. Similarly, Fi and Mi are the resultant force and moment that link (i) applies to link (i + 1) at joint i + 1. We measure and show the force systems (Fi−1 , Mi−1 ) and (Fi , Mi ) at the/ origin of the / coordinate frames Bi−1 and Bi , respectively. The sum of the external loads acting on the link (i) are shown by Fei and Mei .

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_11

609

610

11 Robot Dynamics

Fig. 11.2 Force system on link (i)

The Newton–Euler equations of motion for the link (i) in the global coordinate frame are: 0

0

Fi−1 − 0 Fi +



0

Fei = mi 0 ai

 0 Mi−1 − 0 Mi + Mei     + 0 di−1 − 0 ri × 0 Fi−1 − 0 di − 0 ri × 0 Fi = 0 Ii 0 α i

(11.1)

(11.2)

Proof There are three applied force systems on a link (i): A force Fi−1 and a moment Mi−1 at its proximal end that is coming from link (i − force −Fi and a moment −Mi at its distal end that is coming from link (i + 1); the resultant external /1); a / force system Fei , Mei at the mass center Ci . The force system that link (i − 1) applies on link (i) is called action force system, and the force system that link (i + 1) applies on link (i) is called reaction force system. First, let us only look at forces on link (i). At joint i, there is an action force Fi−1 that link (i − 1) applies on link (i), and at joint i + 1 there is a reaction force −Fi that the link (i + 1) applies on link (i). Action force is also called driving force, and reaction force is also called driven force. Similarly, at joint i there is an action moment Mi−1 that link (i − 1) applies to the link (i), and at joint i + 1 there is a reaction moment −Mi that link (i + 1) applies to the link (i). The action moment is also called driving moment, and the reaction moment is also called driven moment. Therefore, there is a driving force system (Fi−1 , Mi−1 ) at the origin of the coordinate frame Bi−1 , and a driven force system (−Fi , −Mi ) at the origin of the coordinate frame Bi . The driving force system (Fi−1 , Mi−1 ) gives motion to link (i) and the driven force system (Fi , Mi ) gives motion to link (i + 1). In addition to the action and/ reaction /force systems, there might be some external forces acting on the link (i) that their resultant makes a force system ( Fei , Mei ) at the mass center Ci . In robotic application, weight is usually the only external load on middle links, and reactions from the environment are extra external force systems on the base and end-effector links. The force and moment that the base actuator applies to the first link are F0 and M0 , and the force and moment that the endeffector applies to the environment are Fn and Mn . If weight is the only external load on link (i) and it is in − 0 kˆ0 direction, and g is the gravitational acceleration vector, then we have the B0 -expression of the external forces on link (i) as:  

0 0

Fei = mi 0 g = −mi g 0 kˆ0

Mei = 0 ri × mi 0 g = − 0 ri × mi g 0 kˆ0

(11.3) (11.4)

11.1 Rigid Link Newton–Euler Dynamics

611

Fig. 11.3 A one-link manipulator

As showing in Fig. 11.2, we indicate the global position of the mass center of the link by 0 ri , and the global position of the origin of body frames Bi and Bi−1 by 0 di and 0 di−1 respectively. The link’s velocities 0 vi , 0 ωi and accelerations 0 ai , 0 α i are measured and shown at Ci . The physical properties of the link (i) are specified by its mass mi and mass moment matrix 0 Ii about the link’s mass center Ci . The Newton’s equation of motion determines that the sum of forces applied to the link (i) is equal to the mass of the link times its acceleration at Ci .  0 0 Fi−1 − 0 Fi + Fei = mi 0 ai (11.5) For the Euler equation, in addition to the action and reaction moments, we must add the moments of the action and reaction forces about Ci . The moment of −Fi and Fi−1 are equal to −mi × Fi and ni × Fi−1 where mi is the position vector of oi from Ci and ni is the position vector of oi−1 from Ci . Therefore, the link’s Euler equation of motion is: 0

 0 Mi−1 − 0 Mi + Mei 0 0 + ni × Fi−1 − 0 mi × 0 Fi = 0 Ii 0 α i

(11.6)

The position vectors ni and mi can be expressed in terms of 0 di , 0 ri , 0 di−1 , ni = 0 di−1 − 0 ri

(11.7)

mi = 0 di − 0 ri

(11.8)

0 0

0 i−1 di

= 0 m i − 0 ni

(11.9)

to derive Eq. (11.2). Because there is one translational and one rotational equation of motion for each link of a robot, there are 2n vectorial equations of motion for an n link robot. However, there are 2(n + 1) forces and moments involved. Therefore, one set of force systems (usually Fn and Mn ) must be specified to solve the equations and find the joints’ force and moment.  Example 355 One-link manipulator. One-link manipulator is an ideal and simple enough example to learn how to develop equations of motion in robotics. Figure 11.3 depicts a link attached to the ground via a spherical joint at O. The free body diagram (FBD) of the link is made of an external force 0 Fe and moment 0 Me at the endpoint, gravity mg, and the driving force 0 F0 and moment 0 M0 at the joint O. The Newton–Euler equations for the link are: 0 0

F0 + 0 Fe + mg Kˆ = m 0 aC

M0 + 0 Me + 0 n × 0 F0 + 0 m × 0 Fe = 0 I 0 α 1

(11.10) (11.11)

612

11 Robot Dynamics

Fig. 11.4 A turning uniform beam

To derive the equations of motion and study the application of Newton–Euler equations, let us consider the uniform beam of Fig. 11.4a. Figure 11.4b illustrates the FBD of the beam and its relative position vectors m and n. ⎡

⎤ l cos θ ⎢2 ⎥ ⎢ ⎥ 0 m = ⎢ l sin θ ⎥ ⎣ ⎦ 2 0

⎡

⎤ l − cos θ ⎢ 2 ⎥ ⎢ ⎥ 0 n = ⎢ − l sin θ ⎥ ⎣ ⎦ 2 0

(11.12)

The kinematics of the beam are: r = − 0n

(11.13)

d = − 0n + 0m

(11.14)

0 0

where 0 r indicates the position of C, and 0 d indicates the position of the tip point, both in B0 . 0 ω1

= θ˙ Kˆ

(11.15)

0α1

= 0 ω˙ 1 = θ¨ Kˆ

(11.16)

g = −g Jˆ 0

The forces on the beam are:

(11.17)

  aC = 0 α 1 × 0 r − 0 ω 1 × 0 ω 1 × 0 r ⎡ ⎤ l l ¨ sin θ + θ˙ 2 (cos θ ) − θ ⎢ 2 ⎥ 2 ⎢ ⎥ = ⎢ l θ¨ cos θ + l θ˙ 2 sin θ ⎥ ⎣ ⎦ 2 2 0 ⎡

⎤ FX 0 F0 = ⎣ FY ⎦ FZ ⎡ ⎤ QX 0 M0 = ⎣ QY ⎦ QZ

⎡ ⎤ 0 0 Fe = ⎣ 0 ⎦ 0 ⎡ ⎤ 0 0 Me = ⎣ 0 ⎦ 0

Let us assume that 1 I1 to be the principal mass moment matrix of the beam about its mass center expressed in B1 .

(11.18)

(11.19)

(11.20)

11.1 Rigid Link Newton–Euler Dynamics

613

⎡

⎤ Ix 0 0 1 I1 = ⎣ 0 Iy 0 ⎦ 0 0 Iz

(11.21)

The transformation matrix 0 R1 can be used to calculate 0 I1 . ⎡

0

R1 = RZ,θ

⎤ cos θ − sin θ 0 = ⎣ sin θ cos θ 0 ⎦ 0 0 1

⎤ Ix 0 0 0 T I1 = RZ,θ 1 I1 RZ,θ = 0 R1 ⎣ 0 Iy 0 ⎦ 0 R1T 0 0 Iz   ⎡ ⎤ 2 2 Ix cos θ+ Iy sin θ Ix − 2Iy cos θ sin2 θ 0 = ⎣ Ix − Iy cos θ sin θ Iy cos θ + Ix sin θ 0 ⎦ 0 0 Iz

(11.22)

⎡

(11.23)

Substituting the above information in Eqs. (11.10) and (11.11) provides the following equations of motion. F0 + 0 Fe + m 1 g = m 1 0 a C ⎡  ⎤ 1 ⎡ ⎤ ¨ sin θ − θ˙ 2 cos θ − l θ m 1 FX ⎥ ⎢ 2 ⎥ ⎣ FY ⎦ = ⎢ ⎢ 1 m l θ¨ cos θ + θ˙ 2 sin θ + m g ⎥ 1 1 ⎦ ⎣ 2 FZ 0 0

M0 + 0 Me + 0 n × 0 F0 + 0 m × 0 Fe = I 0 α 1 ⎡ ⎤ l ⎡ ⎤ FZ sin θ ⎢ ⎥ 2 QX ⎢ ⎥ l 0 ⎥ M0 = ⎣ QY ⎦ = ⎢ F cos θ − Z ⎢ ⎥ 2 ⎣ ⎦ QZ l l Iz θ¨ + FY cos θ − FX sin θ 2 2 0

(11.24)

(11.25)

(11.26)

(11.27)

Let us substitute the force components from (11.25) to determine the components of the driving moment 0 M0 . ⎡ ⎤ ⎤ 0 QX ⎢ ⎥ 0 ⎥ ⎣ QY ⎦ = ⎢  2 ⎣ ⎦ m1 l 1 ¨ QZ Iz + θ + m1 gl cos θ 4 2 ⎡

(11.28)

Example 356 A turning uniform beam with a tip mass. Hanging a load to the tip point of a one-link manipulator makes a one-link system with payload. Here is to show how the equations of motion of this system will be found. Let us consider the uniform beam of Fig. 11.5a with a hanging mass m2 at the tip point. Figure 11.5b illustrates the FBD of the beam. The mass center of the beam is at 1 r1 ⎡

⎤ ⎡ ⎤ l/2 l m1 m2 1 ⎣ 0 ⎦+ ⎣0⎦ r1 = m1 + m2 m1 + m2 0 0 ⎡ ⎤ ⎡ ⎤ m1 + 2m2 rx ⎢ 2 (m1 + m2 ) l ⎥ ⎥=⎣0⎦ =⎢ ⎣ ⎦ 0 0 0

(11.29)

614

11 Robot Dynamics

Fig. 11.5 A uniform beam with a hanging weight m2 at the tip point

and its relative position vectors m and n are: n1 = − 1 r1 = −rx ıˆ

(11.30)

m1 = lˆı − 1 r1 = (l − rx ) ıˆ

(11.31)

d1 = − n1 + m1 = lˆı ⎡ ⎤ ⎤ −rx cos θ (l − rx ) cos θ 0 0 m = ⎣ (l − rx ) sin θ ⎦ n = ⎣ −rx sin θ ⎦ 0 0

(11.32)

1 1

⎡

0

1

1

(11.33)

The kinematics of the beam are: 0 ω1

= θ˙ Kˆ

(11.34)

0α1

= 0 ω˙ 1 = θ¨ Kˆ

(11.35)

g = −g Jˆ 0

The forces on the beam are:

(11.36)

  aC = 0 α 1 × 0 r1 + 0 ω1 × 0 ω1 × 0 r1 ⎡ ⎤ 2 −rx θ¨ sin θ + rx θ˙ (cos θ ) ⎢ ⎥ = ⎣ rx θ¨ cos θ + rx θ˙ 2 sin θ ⎦ 0 ⎡

⎤ FX 0 F0 = ⎣ FY ⎦ FZ ⎡ ⎤ QX 0 M0 = ⎣ QY ⎦ QZ

⎡ ⎤ 0 0 Fe = ⎣ 0 ⎦ 0 ⎡ ⎤ 0 0 Me = ⎣ 0 ⎦ 0

(11.37)

(11.38)

(11.39)

Let us assume that 1 I1 is the mass moment matrix of the beam about its center. ⎡

⎤ Ix 0 0 1 I1 = ⎣ 0 Iy 0 ⎦ 0 0 Iz

(11.40)

11.1 Rigid Link Newton–Euler Dynamics

615

The mass moment matrix of the manipulator about the common mass center at 1 r1 is: ⎡

⎤ Ix 0 0 1 I1 = ⎣ 0 Iy 0 ⎦ 0 0 I3  l 2 I3 = Iz + m1 rx − + m2 (l − rx )2 2

(11.41)

(11.42)

Knowing the transformation matrix 0 R1 , we can determine 0 I1 . ⎡

0

R1 = RZ,θ

⎤ cos θ − sin θ 0 = ⎣ sin θ cos θ 0 ⎦ 0 0 1

⎤ Ix 0 0 0 T I1 = RZ,θ 1 I1 RZ,θ = 0 R1 ⎣ 0 Iy 0 ⎦ 0 R1T 0 0 I3   ⎡ ⎤ 2 2 Ix cos θ+ Iy sin θ Ix − 2Iy cos θ sin2 θ 0 = ⎣ Ix − Iy cos θ sin θ Iy cos θ + Ix sin θ 0 ⎦ 0 0 I3

(11.43)

⎡

(11.44)

Substituting the above information in Eqs. (11.10) and (11.11) provides the following equations of motion. 10 aC 2   ⎤ ⎡ ⎤ ⎡ − (m1  + m2 ) rx θ¨ sin θ − θ˙ 2 cos θ FX 2 ⎥ ⎣ FY ⎦ = ⎢ ⎣ (m1 + m2 ) rx θ¨ cos θ + θ˙ sin θ + (m2 + m1 ) g ⎦ FZ 0 0

F0 + 0 Fe + m1 g Kˆ = m1 0 aC + m2

M0 + 0 Me + 0 n × 0 F0 + 0 m × 0 Fe = I 0 α 1 ⎤ ⎡ ⎤ ⎡ rx FZ sin θ QX 0 ⎦ M0 = ⎣ QY ⎦ = ⎣ −rx FZ cos θ QZ I3 θ¨ + rx FY cos θ − rx FX sin θ 0

(11.45)

(11.46)

(11.47) (11.48)

Let us substitute the force components from (11.46) to determine the components of the driving moment 0 M0 . ⎡

⎤ ⎡ ⎤ QX 0 ⎣ QY ⎦ = ⎣ ⎦   0 QZ Iz + (m1 + m2 ) rx2 θ¨ + (m1 + m2 ) rx g cos θ

(11.49)

Substituting rx provides the required torque Q0 . 

 1 1 2 2 ¨ Q0 =QZ = m1 l + m2 l + Iz θ + m1 +m2 gl cos θ 4 2

(11.50)

616

11 Robot Dynamics

Fig. 11.6 Free body diagram of a 2R planar manipulator

Example 357 2R planar manipulator Newton–Euler dynamics. To derive equations of motion of a robot, we only need to draw free body diagram (FBD)of every link and write the Newton and Euler equation for every link in the base coordinate frame. Here is the equations of motion of a 2R manipulator. A 2R planar manipulator and its free body diagram are shown in Fig. 11.6 along with the FBD of every link. The torques of actuators are parallel to the Z-axis and are indicated by Q0 and Q1 . The Newton–Euler equations of motion for the first link are: 0 0

F0 − 0 F1 + m1 g Jˆ = m1 0 a1

Q0 − Q1 + n1 × F0 − m1 × F1 = I1 0 α 1 0

0

0

0

0

0

(11.51) (11.52)

and the equations of motion for the second link are: 0 0

F1 + m2 g Jˆ = m2 0 a2

Q1 + 0 n2 × 0 F1 = 0 I2 0 α 2

(11.53) (11.54)

There are four equations for four unknowns F0 , F1 , Q0 , Q1 . These equations can be set in a matrix form [A] x = b where

⎤ 1 0 0 −1 0 0 ⎢ 0 1 0 0 −1 0 ⎥ ⎢ ⎥ ⎢ n1y −n1x 1 −m1y m1x −1 ⎥ ⎢ ⎥ [A] = ⎢ 0 0 1 0 0 ⎥ ⎢ 0 ⎥ ⎣ 0 0 0 0 1 0 ⎦ 0 0 0 n2y −n2x 1 ⎡ ⎤ ⎡ ⎤ F0x m1 a1x ⎢ F0y ⎥ ⎢ m1 a1y − m1 g ⎥ ⎢ ⎥ ⎢ ⎥ 0 ⎢ Q0 ⎥ ⎢ ⎥ I1 α 1 ⎢ ⎥ ⎢ ⎥ x=⎢ b=⎢ ⎥ ⎥ m2 a2x ⎢ F1x ⎥ ⎢ ⎥ ⎣ F1y ⎦ ⎣ m2 a2y − m2 g ⎦ 0 Q1 I2 α 2

(11.55)

⎡

(11.56)

(11.57)

11.1 Rigid Link Newton–Euler Dynamics

617

Example 358 Equations for joint actuators. Newton–Euler equations provide with joint torque and joint force equations. Torque equations are those we need for robot control. To separate the equations, there should be some manipulations. Here is an example for 2R manipulators. In robot dynamics, we usually do not need to find joint forces. Actuator torques are much more important as they are used to control a robot. In Example 357 we derived four equations for dynamics of the 2R manipulator that is shown in Fig. 11.6. 0 0

F0 − 0 F1 + m1 g Jˆ = m1 0 a1

Q0 − 0 Q1 + 0 n1 × 0 F0 − 0 m1 × 0 F1 = 0 I1 0 α 1 0 0

F1 + m2 g Jˆ = m2 a2 0

Q1 + 0 n2 × 0 F1 = 0 I2 0 α 2

(11.58) (11.59) (11.60) (11.61)

However, we may eliminate the joint forces F0 , F1 and reduce the number of equations to two for the two torques Q0 and Q1 . Eliminating F1 between (11.60) and (11.61) provides 0

 Q1 = 0 I2 0 α 2 − 0 n2 × m2 0 a2 − m2 g Jˆ

(11.62)

and eliminating F0 and F1 between (11.58) and (11.59) gives: 0

 Q0 = 0 Q1 + 0 I1 0 α 1 + 0 m1 × m2 0 a2 − m2 g Jˆ  − 0 n1 × m1 0 a1 − m1 g Jˆ + m2 0 a2 − m2 g Jˆ

(11.63)

The joint forces F0 and F1 , if we are interested, are equal to: 0

F1 = m2 0 a2 − m2 g Jˆ

(11.64)

0

F0 = m1 a1 + m2 a2 − (m1 + m2 ) g Jˆ

(11.65)

0

0

Example 359 2R planar manipulator with massive arms and joints. Here is an example of a 2R manipulator with payload and massive motors, as well as massive links. In a real situation for a 2R planar manipulators, we generally have a massive motor at joint 0 to turn the link (1) and a massive motor at joint 1 to turn the link (2). We may also carry a massive object by the gripper at the tip point. The motor at joint 0 is siting on the ground and its weight will not effect the dynamics of the manipulator. The FBD of the manipulator is shown in Fig. 11.7. The massive joints will displace the position of Ci and changes the relative position vectors m and n. We will have the same equations of motion (11.51)–(11.53) provided we determine m and n for the new position of Ci as suggested in Fig. 11.8. 0 0

F0 − 0 F1 + (m11 + m12 ) g Jˆ = m1 0 a1

(11.66)

Q0 − Q1 + n1 × F0 − m1 × F1 = I1 0 α 1

(11.67)

F1 + (m21 + m22 ) g Jˆ = (m21 + m22 ) 0 a2

(11.68)

0

0

0

0

0

0

0

Q1 + n2 × F1 = I2 0 α 2 0

0

0

0

(11.69)

We may show the masses as m1 = m11 + m12

(11.70)

m2 = m21 + m22

(11.71)

618

11 Robot Dynamics

Fig. 11.7 A 2R planar manipulator with massive arms and massive joints

Fig. 11.8 Determination of the vectors m and n for new positions of mass center Ci

and use the same Eqs. (11.60)–(11.61). Example 360 2R planar manipulator general equations. There are many details and information in the equations of motion. This is a complete example for a general 2R manipulator to show every steps and all information we take from equations of motion. Let us analyze a general 2R manipulator that has massive arms and carries a payload m0 as is shown in Fig. 11.9. The equations of motion are: 0 0

F0 − 0 F1 + m1 g Jˆ = m1 0 a1

Q0 − 0 Q1 + 0 n1 × 0 F0 − 0 m1 × 0 F1 = 0 I1 0 α 1 0 0

F1 + (m0 + m2 ) g Jˆ = m2 a2 0

Q1 + 0 n2 × 0 F1 + 0 m2 × m0 g Jˆ = 0 I2 0 α 2

(11.72) (11.73) (11.74) (11.75)

Elimination the joint forces F0 , F1 provides the following equations for the torques Q0 and Q1 . 0

 Q1 = 0 I2 0 α 2 − 0 n2 × m2 0 a2 − (m0 + m2 ) g Jˆ − 0 m2 × m0 g Jˆ  0 Q0 = 0 Q1 + 0 I1 0 α 1 + 0 m1 × m2 0 a2 − (m0 + m2 ) g Jˆ  − 0 n1 × m1 0 a1 + m2 0 a2 − (m0 + m1 + m2 ) g Jˆ

(11.76)

(11.77)

11.1 Rigid Link Newton–Euler Dynamics

619

Fig. 11.9 A 2R manipulator that has massive arms and carries a payload m0

The forces F0 and F1 are equal to: 0

F1 = m2 0 a2 − (m0 + m2 ) g Jˆ

(11.78)

0

F0 = m1 a1 + m2 a2 − (m0 + m1 + m2 ) g Jˆ

(11.79)

0

0

where 0

r1 = − 0 n1

0

r2 = − n1 + m1 − n2

(11.81)

d1 = − 0 n1 + 0 m 1

(11.82)

0

0 1 d2 0

0

(11.80) 0

0

= − 0 n2 + 0 m 2

(11.83)

d2 = − n1 + m 1 − n2 + m 2 0

0

0

0

(11.84)

In a general case, the local position vectors of Ci are: ⎡

⎤ −c1 cos θ 1 0 n1 = 0 R1 1 n1 = −RZ,θ 1 c1 1 ıˆ1 = ⎣ −c1 sin θ 1 ⎦ 0

0

n 2 = − 0 R2 2 n 2 = − 0 R1 1 R2 2 n 2 ⎡ ⎤ −c2 cos (θ 1 + θ 2 ) = −RZ,θ 1 RZ,θ 2 c2 2 ıˆ2 = ⎣ −c2 sin (θ 1 + θ 2 ) ⎦ 0 ⎤ −c2 cos θ 2 1 n2 = − 1 R2 2 n2 = −RZ,θ 2 c2 2 ıˆ2 = ⎣ −c2 sin θ 2 ⎦ 0

(11.85)

(11.86)

⎡

(11.87)

620

11 Robot Dynamics

⎡

⎤ (l1 − c1 ) cos θ 1 0 m1 = 0 R1 1 m1 = RZ,θ 1 (l1 − c1 ) 1 ıˆ1 = ⎣ (l1 − c1 ) sin θ 1 ⎦ 0 ⎡ ⎤ (l2 − c2 ) cos (θ 1 + θ 2 ) 0 m2 = 0 R2 2 m2 = 0 R2 (l2 − c2 ) 2 ıˆ2 = ⎣ (l2 − c2 ) sin (θ 1 + θ 2 ) ⎦ 0

(11.88)

(11.89)

where ⎡

0

R1 = RZ,θ 1

1

R2 = RZ,θ 2

cos θ 1 ⎣ = sin θ 1 0 ⎡ cos θ 2 = ⎣ sin θ 2 0

⎤ 0 0⎦ 1 ⎤ − sin θ 2 0 cos θ 2 0 ⎦ 0 1 − sin θ 1 cos θ 1 0

⎤ cos (θ 1 + θ 2 ) − sin (θ 1 + θ 2 ) 0 = ⎣ sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 ⎦ 0 0 1

(11.90)

(11.91)

⎡

0

R2 = Rz,θ 1 +θ 2

(11.92)

The position vectors are as follows. ⎡

⎤ c1 cos θ 1 0 r1 = − 0 n1 = 0 R1 1 r1 = 0 R1 c1 ıˆ1 = ⎣ c1 sin θ 1 ⎦ 0

0

r2 = − 0 n1 + 0 m1 − 0 n2 = 0 d1 + 0 R2 2 r2 ⎡ ⎤ l1 cos θ 1 + c2 cos (θ 1 + θ 2 ) = ⎣ l1 sin θ 1 + c2 sin (θ 1 + θ 2 ) ⎦ 0 ⎤ l1 cos θ 1 0 d1 = − 0 n1 + 0 m1 = ⎣ l1 sin θ 1 ⎦ 0

(11.93)

(11.94)

⎡

⎤ l2 cos (θ 1 + θ 2 ) 0 0 0 ⎣ l2 sin (θ 1 + θ 2 ) ⎦ 1 d2 = − n2 + m 2 = 0

(11.95)

⎡

0

d2 = − 0 n1 + 0 m 1 − 0 n2 + 0 m 2 ⎡ ⎤ l2 cos (θ 1 + θ 2 ) + l1 cos θ 1 = 0 d1 + 01 d2 = ⎣ l2 sin (θ 1 + θ 2 ) + l1 sin θ 1 ⎦ 0

(11.96)

(11.97)

The links’ angular velocities and accelerations are: 0 ω1

= θ˙ 1 Kˆ

= 0 ω˙ 1 = θ¨ 1 Kˆ   ˆ ˙ ˙ 0 ω2 = θ 1 + θ 2 K 0α1

(11.98) (11.99) (11.100)

11.1 Rigid Link Newton–Euler Dynamics

621 0α2

  = 0 ω˙ 2 = θ¨ 1 + θ¨ 2 Kˆ

(11.101)

The translational acceleration of Ci are: 0

0

0

  a1 = 0 α 1 × 0 r1 − 0 ω1 × 0 ω1 × 0 r1 ⎡ ⎤ 2 −c1 θ¨ 1 sin θ 1 + c1 θ˙ 1 cos θ 1 ⎢ ⎥ = ⎣ c1 θ¨ 1 cos θ 1 + c1 θ˙ 21 sin θ 1 ⎦ 0

  d¨ 1 = 0 α 1 × 0 d1 − 0 ω1 × 0 ω1 × 0 d1 ⎤ ⎡ 2 −l1 θ¨ 1 sin θ 1 + l1 θ˙ 1 cos θ 1 ⎥ ⎢ = ⎣ l1 θ¨ 1 cos θ 1 + l1 θ˙ 21 sin θ 1 ⎦ 0

  d d2 = 0 d¨ 1 + 01 ω˙ 2 × 01 d2 − 01 ω2 × 01 ω2 × 01 d2 2 dt ⎡0 ⎤ d¨2x 0¨ ⎦ ⎣ = d2y 0

d¨ 2 =

(11.104)

d¨2x = −l1 θ¨ 1 sin θ 1 − l2 θ¨ 2 sin (θ 1 + θ 2 ) 2 2 +l1 θ˙ 1 cos θ 1 + l2 θ˙ 2 cos (θ 1 + θ 2 )

0

(11.105)

d¨2y = l1 θ¨ 1 cos θ 1 + l2 θ¨ 2 cos (θ 1 + θ 2 ) 2 +l1 θ 21 sin θ 1 + l2 θ˙ 2 sin (θ 1 + θ 2 )

    r2 − 0 d2 − 0 ω2 × 0 ω2 × 0 r2 − 0 d2   = 0 d¨ 2 − 0 α 2 × 0 m2 + 0 ω2 × 0 ω2 × 0 m2 ⎡0 ⎤ a2x = ⎣ 0 a2y ⎦ 0     0 a2x = (l2 − c2 ) θ¨ 1 + θ¨ 2 − l2 θ¨ 2 sin (θ 1 + θ 2 )

a2 = 0 d¨ 2 + 0 α 2 ×

(11.106)

0

2 −l1 θ¨ 1 sin θ 1 + l1 θ˙ 1 cos θ 1   2 2 − (l2 − c2 ) θ˙ 1 + θ˙ 2 − l2 θ˙ 2 cos (θ 1 + θ 2 ) 0

(11.103)

G 20

0

0

(11.102)

(11.107)

(11.108)

    a2y = − (l2 − c2 ) θ¨ 1 + θ¨ 2 − l2 θ¨ 2 cos (θ 1 + θ 2 ) 2 +l1 θ¨ 1 cos θ 1 + l1 θ˙ 1 sin θ 1   2 2 − (l2 − c2 ) θ˙ 1 + θ˙ 2 − l2 θ˙ 2 sin (θ 1 + θ 2 )

(11.109)

622

11 Robot Dynamics

The moment of inertia matrices in the global coordinate frame are: ⎡

⎤ Ix1 0 0 0 T I1 = RZ,θ 1 1 I1 RZ,θ = 0 R1 ⎣ 0 Iy1 0 ⎦ 0 R1T 1 0 0 Iz1   ⎡ ⎤ 2 2 I c θ + I s θ − I I cθ 1 sθ 1 0 x 1 y 1 x y 1 1 1 1   = ⎣ Ix1 − Iy1 cθ 1 sθ 1 Iy1 c2 θ 1 + Ix1 s 2 θ 1 0 ⎦ 0 0 Iz1 ⎡ ⎤ Ix2 0 0 0 I2 = 0 R2 2 I2 0 R2T = 0 R2 ⎣ 0 Iy2 0 ⎦ 0 R2T 0 0 Iz2   ⎡ ⎤ 2 2 Ix2 c θ 12 +  Iy2 s θ 12 Ix2 2− Iy2 cθ 122sθ 12 0 = ⎣ Ix2 − Iy2 cθ 12 sθ 12 Iy2 c θ 12 + Ix2 s θ 12 0 ⎦ 0 0 Iz2 θ 12 = θ 1 + θ 2

(11.110)

(11.111) (11.112)

Substituting these results in Eqs. (11.76) and (11.77), and solving for Q0 and Q1 , provides the dynamic equations for the 2R manipulator. 0

 Q1 = 0 I2 0 α 2 − 0 n2 × m2 0 a2 − (m0 + m2 ) g Jˆ −0 m2 × m0 g Jˆ ⎡ ⎤ 0 =⎣ 0 ⎦ 0 Q1z   0 Q1z = Iz2 + m2 c22 − m2 l2 c2 + m2 l1 c2 cos θ 2 θ¨ 1   2 + Iz2 + m2 c22 θ¨ 2 − m2 c2 l1 θ˙ 1 sin θ 2 − (m2 c2 + m0 l2 ) g cos (θ 1 + θ 2 )

 Q0 = 0 Q1 + 0 I1 0 α 1 + 0 m1 × m2 0 a2 − (m0 + m2 ) g Jˆ   − 0 n1 × m1 0 a1 − m1 g Jˆ + m2 0 a2 − (m0 + m2 ) g Jˆ ⎡ ⎤ 0 =⎣ 0 ⎦ 0 Q0z    0 Q0z = Iz1 + Iz2 + m1 c12 + m2 l12 + c22 − l2 c2 + l1 (2c2 − l2 ) cos θ 2 θ¨ 1   2 + Iz2 + m2 c2 (c2 + l1 cos θ 2 ) θ¨ 2 − m2 l1 l2 θ˙ 1 sin θ 2

(11.113)

(11.114)

0

(11.115)

+m2 l1 c2 θ˙ 2 sin θ 2 − 2m2 l1 (l2 − c2 ) θ˙ 1 θ˙ 2 sin θ 2 2

− (m0 l1 + m1 c1 + m2 l1 ) g cos θ 1 − (m0 l2 + m2 c2 ) g cos (θ 1 + θ 2 )

(11.116)

Substituting the vectorial information of (11.85)–(11.111) in (11.78) and (11.79), we find the joint forces of the general 2R manipulator. The manipulator has massive arms with mass center at Ci and carries a payload m0 .

11.1 Rigid Link Newton–Euler Dynamics

623

⎡0

0

F1x

⎤ F1x 0 F1 = m2 0 a2 − (m0 + m2 ) g Jˆ = ⎣ 0 F1y ⎦ 0 = (m2 (l2 − c2 ) sin (θ 1 + θ 2 ) − m2 l1 sin θ 1 ) θ¨ 1

(11.117)

2 −m2 c2 θ¨ 2 sin (θ 1 + θ 2 ) + m2 c2 θ˙ 2 cos (θ 1 + θ 2 ) 2 + (m2 (l2 − c2 ) cos (θ 1 + θ 2 ) + m2 l1 cos θ 1 ) θ˙ 1

+2m2 l1 (l2 − c2 ) θ˙ 1 θ˙ 2 cos (θ 1 + θ 2 ) 0

(11.118)

F1y = (−m2 (l2 − c2 ) cos (θ 1 + θ 2 ) + m2 l1 cos θ 1 ) θ¨ 1 2 +m2 c2 θ¨ 2 cos (θ 1 + θ 2 ) + m2 c2 θ˙ 2 sin (θ 1 + θ 2 ) 2 + (−m2 (l2 − c2 ) sin (θ 1 + θ 2 ) + m2 l1 sin θ 1 ) θ˙ 1

−2m2 l1 (l2 − c2 ) θ˙ 1 θ˙ 2 sin (θ 1 + θ 2 ) − (m0 + m2 ) g

(11.119) ⎡0

⎤ F0x 0 F0 = m1 0 a1 + m2 0 a2 − (m0 + m1 + m2 ) g Jˆ = ⎣ 0 F0y ⎦ 0 0 F0x = (m2 (l2 − c2 ) sin (θ 1 + θ 2 ) − (m1 c1 + m2 l1 ) sin θ 1 ) θ¨ 1

(11.120)

−m2 c2 θ¨ 2 sin (θ 1 + θ 2 ) − m2 c2 θ˙ 2 cos (θ 1 + θ 2 ) 2

+ (−m2 (l2 − c2 ) cos (θ 1 + θ 2 ) + (m2 l1 + m1 c1 ) cos θ 1 ) θ˙ 1 2

−2m2 (l2 − c2 ) θ˙ 1 θ˙ 2 cos (θ 1 + θ 2 ) 0

(11.121)

F0y = (−m2 (l2 − c2 ) cos (θ 1 + θ 2 ) + (m2 l1 + m1 c1 ) cos θ 1 ) θ¨ 1 2 +m2 c2 θ¨ 2 cos (θ 1 + θ 2 ) + m2 c2 θ˙ 2 sin (θ 1 + θ 2 )

+ (−m2 (l2 − c2 ) sin (θ 1 + θ 2 ) + (m2 l1 + m1 c1 ) sin θ 1 ) θ˙ 1 2

−2m2 l1 (l2 − c2 ) θ˙ 1 θ˙ 2 sin (θ 1 + θ 2 ) − (m0 + m1 + m2 ) g

(11.122)

Example 361  Matrix form of equations of motion. It is always possible and useful to convert the equations of motion of a ˙ q˙ + G(q) = Q. It is a better form for computer calculations. Here is an example robot into a matrix form of D(q) q¨ + C(q, q) how to convert equations of motion of 2R planar manipulator into matrix form. Let us rearrange the equations of motion (11.114) and (11.116) of 2R planar manipulator in a matrix form. ˙ q˙ + G(q) = Q D(q) q¨ + C(q, q) "

θ1 q= θ2

"

#

"0

Q Q = 0 1z Q2z

#

Z1 − Z2 + Z3 cos θ 2 Z1 D(q) = Z1 + Z4 − Z2 + Z5 + Z6 cos θ 2 Z1 + Z3 cos θ 2 "

˙ =  C(q, q)

(11.123)

−Z3 θ˙ 1 sin θ2 0   −Z7 θ˙ 1 − Z8 θ˙ 2 sin θ 2 Z3 θ˙ 2 − Z8 θ˙ 1 sin θ 2

(11.124) # (11.125) # (11.126)

624

11 Robot Dynamics

Fig. 11.10 A four-bar linkage, and free body diagram of each link

"

−Z9 cos (θ 1 + θ 2 ) G(q) = −Z9 cos (θ 1 + θ 2 ) + Z10 cos θ 1

# (11.127)

Z1 = Iz2 + m2 c22

(11.128)

Z2 = m2 l2 c2

(11.129)

Z3 = m2 l1 c2

(11.130)

Z4 = Iz1 + m1 c12

(11.131)

Z5 = m2 l12

(11.132)

Z6 = m2 l1 (2c2 − l2 )

(11.133)

Z7 = m2 l1 l2

(11.134)

Z8 = m2 l1 (l2 − c2 )

(11.135)

Z9 = (m2 c2 + m0 l2 ) g

(11.136)

Z10 = (m0 l1 + m1 c1 + m2 l1 ) g

(11.137)

Example 362 A four-bar linkage dynamics. A planar mechanism is a multi-link with two dimensional forces and one dimensional moments. This example illustrates how to define the equations of motion of a multi-link mechanism similar to a robotic manipulator. Figure 11.10a illustrates a closed-loop four-bar linkage and Fig. 11.10b shows the free body diagrams of the links. The position of the mass centers is given, and therefore the vectors 0 ni and 0 mi for each link are known. The Newton–Euler equations for the link (i) are: Fi−1 − 0 Fi +mi g Jˆ = mi 0 ai

(11.138)

Mi−1 − Mi + ni × Fi−1 − mi × Fi = Ii 0 α i

(11.139)

0 0

0

0

0

0

0

11.1 Rigid Link Newton–Euler Dynamics

625

and therefore, we have three sets of equations for links (1), (2), (3). Link (0) is the ground link and motionless. 0

0

F0 − 0 F1 + m1 g Jˆ = m1 0 a1

(11.140)

M0 − 0 M1 + 0 n1 × 0 F0 − 0 m1 × 0 F1 = I1 0 α 1 F1 − 0 F2 + m2 g Jˆ = m2 0 a2

(11.142)

M1 − 0 M2 + 0 n2 × 0 F1 − 0 m2 × 0 F2 = I2 0 α 2

(11.143)

F2 − 0 F3 + m3 g Jˆ = m2 0 a2

(11.144)

M2 − 0 M3 + 0 n3 × 0 F2 − 0 m3 × 0 F3 = I3 0 α 3

(11.145)

0 0

0 0

(11.141)

Let us assume that there is no friction in joints and the mechanism is planar. Therefore, the force vectors are in the (X, Y )plane, and the moments are parallel to Z-axis. So, the equations of motion simplify to: F0 − 0 F1 + m1 g Jˆ = m1 0 a1

(11.146)

M0 + 0 n1 × 0 F0 − 0 m1 × 0 F1 = I1 0 α 1

(11.147)

0 0

F1 − 0 F2 + m2 g Jˆ = m2 0 a2

(11.148)

n2 × F1 − m2 × F2 = I2 0 α 2

(11.149)

F2 − 0 F3 + m3 g Jˆ = m2 0 a2

(11.150)

n3 × 0 F2 − 0 m3 × 0 F3 = I3 0 α 3

(11.151)

0 0

0

0 0

0

0

where 0 M0 is the driving torque of the mechanism. The number of equations reduces to 9 and the unknowns of the equations are: F0x , F0y , F1x , F1y , F2x , F2y , F3x , F3y , M0 (11.152) We can rearrange the set of equations in a matrix form [A] x = b where

⎡

1 ⎢ 0 ⎢ ⎢ −n1y ⎢ ⎢ 0 ⎢ [A] = ⎢ ⎢ 0 ⎢ 0 ⎢ ⎢ 0 ⎢ ⎣ 0 0

0 −1 0 0 0 1 0 −1 0 0 n1x m1y −m1x 0 0 0 1 0 −1 0 0 0 1 0 −1 0 −n2y n2x m2y −m2x 0 0 0 1 0 0 0 0 0 1 0 0 0 −n3y n3x

(11.153) ⎤ 0 0 0 0 0 0⎥ ⎥ 0 0 1⎥ ⎥ 0 0 0⎥ ⎥ 0 0 0⎥ ⎥ 0 0 0⎥ ⎥ −1 0 0 ⎥ ⎥ 0 −1 0 ⎦ m3y −m3x 0

(11.154)

626

11 Robot Dynamics

⎤ F0x ⎢ F0y ⎥ ⎥ ⎢ ⎢ F1x ⎥ ⎥ ⎢ ⎢ F1y ⎥ ⎥ ⎢ ⎥ x=⎢ ⎢ F2x ⎥ ⎢ F2y ⎥ ⎥ ⎢ ⎢F ⎥ ⎢ 3x ⎥ ⎣F ⎦

⎡

⎤ m1 a1x ⎢ m1 a1y − m1 g ⎥ ⎢ ⎥ ⎢ ⎥ I1 α 1 ⎢ ⎥ ⎢ ⎥ m a 2 2x ⎢ ⎥ ⎥ b=⎢ m a − m g 2 ⎥ ⎢ 2 2y ⎢ ⎥ I α 2 2 ⎢ ⎥ ⎢ ⎥ m a 3 3x ⎢ ⎥ ⎣m a − m g⎦

⎡

3y

3 3y

M0

(11.155)

3

I3 α 3

The coefficient matrix [A] describes the geometry of the mechanism, the vector x is the unknown forces, and the vector b indicates the dynamic terms. To solve the dynamics of the four-bar mechanism, we must calculate the accelerations 0 ai and 0 0 α i and then find the required driving moment M0 and the joints’ forces. The force Fs is called the shaking force and shows the reaction of the mechanism on the ground. Fs = F3 − F0

11.2

(11.156)

 Recursive Newton–Euler Dynamics

An advantage of the Newton–Euler equations of motion in robotic application is that we can calculate the joint forces of one link at a time. Therefore, starting from the end-effector link, we can analyze the links one by one and end up at the base link or vice versa. For such an analysis, we need to reform the Newton–Euler equations of motion to work in a recursive form in the link’s frame. The backward recursive Newton–Euler equations of motion for the link (i) in its body coordinate frame Bi are:  i Fi−1 = i Fi − Fei + mi i0 ai    i = i Mi − Mei − i di−1 − i ri × i Fi−1   + i di − i ri × i Fi + i Ii i0 α i + i0 ωi × i Ii i0 ωi i

i

Mi−1

(11.158)

ni = i di−1 − i ri

(11.159)

mi = i di − i ri

(11.160)

i i

(11.157)

When the driving force system (i Fi−1 , i Mi−1 ) is found in frame Bi , we can transform them to the frame Bi−1 and apply the Newton–Euler equation for link (i − 1). Fi−1 =

i−1

Mi−1 =

i−1

i−1 i−1

Ti i Fi−1 i

Ti Mi−1

(11.161) (11.162)

The negative of the converted force system acts as the driven force system (− i−1 Fi−1 , − i−1 Mi−1 ) for the link (i − 1). The forward recursive Newton–Euler equations of motion for the link (i) in its body coordinate frame Bi are:  i Fi = i Fi−1 + Fei − mi i0 ai    i i Mi = i Mi−1 + Mei + i di−1 − i ri × i Fi−1   − i di − i ri × i Fi − i Ii i0 α i − i0 ωi × i Ii i0 ωi i

(11.164)

ni = i di−1 − i ri

(11.165)

mi = i di − i ri

(11.166)

i i

(11.163)

11.2  Recursive Newton–Euler Dynamics

627

When the reaction force system (i Fi , i Mi ) is found in frame Bi , we can transform them to frame Bi+1 . −1 i Fi = i Ti+1 Fi

i+1 i+1

Mi =

i

(11.167)

−1 i Ti+1 Mi

(11.168)

The negative of the converted force system acts as the action force system (− i+1 Fi , − i+1 Mi ) for the link (i + 1). Proof The Euler equation for a rigid link in body coordinate frame is: B

M=

G

d dt

B

L=

= i Ii i α i +

B

˙+ L

B G ωB

B G ωB

× BL

× i Ii i ωi

(11.169)

where L is the angular momentum of the link. B

L=

B

I

B G ωB

(11.170)

We may solve the Newton–Euler equations of motion (11.1) and (11.2) for the action force system 0 0

Fi−1 = 0 Fi −

Mi−1



Fei + mi 0 ai    0 = 0 Mi − Mei − 0 di−1 − 0 ri × 0 Fi−1 0

0   d 0 + 0 di − 0 ri × 0 Fi + Li dt

(11.171)

(11.172)

and then transform the equations to the coordinate frame Bi attached to the link (i) to make the recursive form of the Newton– Euler equations of motion.  i i Fi−1 = 0 Ti−1 0 Fi−1 = i Fi − Fei + mi i0 ai (11.173) i

Mi−1 = 0 Ti−1 0 Mi−1    i = i Mi − Mei − i di−1 − i ri × i Fi−1 0   d i + i di − i ri × i Fi + Li dt    i = i Mi − Mei − i di−1 − i ri × i Fi−1   + i di − i ri × i Fi + i Ii i0 α i + i0 ωi × i Ii i0 ωi

(11.174)

(11.175)

Starting from link (i) and deriving the equations of motion of the previous link (i−1) is called the backward Newton–Euler equations of motion. We may also start from link (i) and derive the equations of motion of the next link (i + 1). This method is called the forward Newton–Euler equations of motion. Employing the Newton–Euler equations of motion (11.157) and (11.158), we can write them in a forward recursive form in coordinate frame Bi attached to the link (i). i

i

Fi = i Fi−1 +



i

Fei − mi i0 ai

(11.176)

   i Mi = i Mi−1 + Mei + i di−1 − i ri × i Fi−1   − i di − i ri × i Fi − i Ii i0 α i − i0 ωi × i Ii i0 ωi

(11.177)

ni = i di−1 − i ri

(11.178)

mi = i di − i ri

(11.179)

i i

628

11 Robot Dynamics

Fig. 11.11 A 2R planar manipulator carrying a load at the endpoint

Using the forward Newton–Euler equations of motion (11.176) and (11.177), we can calculate the reaction force system (i Fi , i Mi ) by having the action force system (i Fi−1 , i Mi−1 ). When the reaction force system (i Fi , i Mi ) is found in frame Bi , we can transform them to frame Bi+1 . i+1 i+1

−1 i Fi = i Ti+1 Fi

Mi =

i

−1 i Ti+1 Mi

(11.180) (11.181)

The negative of the converted force system acts as the action force system (− i+1 Fi , − i+1 Mi ) for the link (i + 1) and we can apply the Newton–Euler equation to the link (i + 1). The forward Newton–Euler equations of motion allows us to start from a known action force system (1 F0 , 1 M0 ), that the base link applies to the link (1), and calculate the action force of the next link. Therefore, analyzing the links of a robot, one by one, we end up with the force system that the end-effector applies to the environment. Using the forward or backward recursive Newton–Euler equations of motion depends on the measurement and sensory system of the robot.  Example 363  Recursive dynamics of a 2R planar manipulator. The 2R planar manipulator is a good example to practice how to derive recursive dynamic equations. Consider the 2R planar manipulator shown in Fig. 11.11. The manipulator is carrying a force system at the endpoint. We use this manipulator to show how we can, step by step, develop the dynamic equations for a robot. The backward recursive Newton–Euler equations of motion for the first link are: 1

F0 = 1 F1 −



1

Fe1 + m1 10 a1

= 1 F1 − m1 1 g + m1 10 a1 1

(11.182)

   1 M0 = 1 M1 − Me1 − 1 d0 − 1 r1 × 1 F0   + 1 d1 − 1 r1 × 1 F1 + 1 I1 10 α 1 + 10 ω1 × 1 I1 10 ω1 = 1 M1 − 1 n 1 × 1 F0 + 1 m 1 × 1 F1 + 1 I1 10 α 1 + 10 ω1 × 1 I1 10 ω1

(11.183)

11.2  Recursive Newton–Euler Dynamics

629

and the backward recursive equations of motion for the second link are: 2

F1 = 2 F2 −



2

Fe2 + m2 20 a2

= −m2 2 g − 2 Fe + m2 20 a2 2

(11.184)

   2 M1 = 2 M2 − Me2 − 2 d1 − 2 r2 × 2 F1   + 2 d2 − 2 r2 × 2 F2 + 2 I2 20 α 2 + 20 ω2 × 2 I2 20 ω2 = − 2 Me − 2 m 2 × 2 Fe − 2 n 2 × 2 F1 + 2 I2 20 α 2 + 20 ω2 × 2 I2 20 ω2

(11.185)

The manipulator consists of two RR(0) links, therefore their transformation matrices i−1 Ti are of class (5.32). Substituting di = 0 and ai = li produces the following transformation matrices. ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(11.186)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(11.187)

The homogenous mass moments matrices are: ⎡

⎤ 0000 ⎥ m1 l12 ⎢ 1 ⎢0 1 0 0⎥ I1 = ⎣ 0 0 1 0⎦ 12 0000

⎡

⎤ 0000 ⎥ m2 l22 ⎢ 2 ⎢0 1 0 0⎥ I2 = ⎣ 0 0 1 0⎦ 12 0000

(11.188)

The homogenous mass moment of inertia matrix is obtained by appending a zero row and column to the I matrix. The position vectors involved are: ⎤ −l1 /2 ⎢ 0 ⎥ 1 ⎥ n1 = ⎢ ⎣ 0 ⎦ 0 ⎡

⎡

⎤ l1 /2 ⎢ 0 ⎥ 1 ⎥ m1 = ⎢ ⎣ 0 ⎦ 0 1

r1 = − 1 n1

2

⎤ −l2 /2 ⎢ 0 ⎥ 2 ⎥ n2 = ⎢ ⎣ 0 ⎦ 0 ⎡

⎤ l2 /2 ⎢ 0 ⎥ 2 ⎥ m2 = ⎢ ⎣ 0 ⎦ 0

(11.189)

⎡

r2 = − 2 n1 + 2 m2 − 2 n2

(11.190)

(11.191)

The angular velocities and accelerations are: ⎡

⎤ 0 ⎢0⎥ 1 ⎢ ⎥ 0 ω1 = ⎣ ˙ ⎦ θ1 0

⎡

⎤ 0 ⎢ 0 ⎥ 2 ⎢ ⎥ 0 ω2 = ⎣ ˙ θ 1 + θ˙ 2 ⎦ 0

(11.192)

630

11 Robot Dynamics

⎡

⎡

⎤ 0 ⎢0⎥ 1 ⎢ ⎥ 0α1 = ⎣ ¨ ⎦ θ1 0

⎤ 0 ⎢ 0 ⎥ 2 ⎢ ⎥ 0α2 = ⎣ ¨ θ 1 + θ¨ 2 ⎦ 0

(11.193)

The translational acceleration of C1 is: 1 0 a1

     = 10 α 1 × − 1 m1 + 10 ω1 × 10 ω1 × − 1 m1 + 10 d¨ 1 ⎡ 1 2⎤ − 2 l1 θ˙ 1 ⎢ 1 l1 θ¨ 1 ⎥ ⎥ 2 =⎢ ⎣ 0 ⎦ 0

because, 1

d¨ 1 = 2 1 a1

(11.194)

(11.195)

The translational acceleration of C2 is: 2 0 a2

     = 20 α 2 × − 2 m2 + 20 ω2 × 20 ω2 × − 2 m2 + 20 d¨ 2 ⎡ 1  2 ⎤ − 2 l2 θ˙ 1 + θ˙ 2 ⎢ 1 l2 θ¨ 1 + θ¨ 2 ⎥ ⎥ 2 =⎢ ⎣ ⎦ 0 0

because, 2

d¨ 2 = 2 2 a2

(11.196)

(11.197)

The gravitational acceleration vector in the links’ frame is: ⎤ −g sin θ 2 ⎢ g cos θ 2 ⎥ 0 ⎥ g=⎢ ⎣ ⎦ 0 0

(11.198)

⎤ −g sin (θ 1 + θ 2 ) ⎢ g cos (θ 1 + θ 2 ) ⎥ 0 ⎥ g=⎢ ⎣ ⎦ 0 0

(11.199)

⎡

1

g = 0 T1−1

⎡

2

g = 0 T2−1

The external load is usually given in the global coordinate frame. We must transform them to the interested link’s frame to apply the recursive equations of motion. Therefore, the external force system expressed in B2 is: ⎡

⎤ Fex cos (θ 1 + θ 2 ) + Fey sin (θ 1 + θ 2 ) ⎢ Fey cos (θ 1 + θ 2 ) − Fex sin (θ 1 + θ 2 ) ⎥ 2 ⎥ Fe = 0 T2−1 0 Fe = ⎢ ⎣ ⎦ 0 0 ⎤ 0 ⎢ 0 ⎥ 2 ⎥ Me = 0 T2−1 0 Me = ⎢ ⎣ Me ⎦ 0

(11.200)

⎡

(11.201)

11.2  Recursive Newton–Euler Dynamics

631

Now, we start from the final link and calculate its action force system. The backward Newton equation for link (2) is: ⎡2

⎤ F1x ⎢ 2 F1y ⎥ 2 ⎥ F1 = −m2 2 g − 2 Fe + m2 20 a2 = ⎢ ⎣ 0 ⎦ 0   1 2 2 F1x = − l2 m2 θ˙ 1 + θ˙ 2 − Fex cos (θ 1 + θ 2 ) 2   − Fey − gm2 sin (θ 1 + θ 2 ) 2

F1y =

  1 l2 m2 θ¨ 1 + θ¨ 2 + Fex sin (θ 1 + θ 2 ) 2   − Fey + gm2 cos (θ 1 + θ 2 )

(11.202)

(11.203)

(11.204)

and the backward Euler equation for link (2) is: 2

M1 = − 2 Me − 2 m 2 × 2 Fe − 2 n 2 × 2 F1 ⎡

⎤ 0 ⎢ 0 ⎥ ⎥ + 2 I2 20 α 2 + 20 ω2 × 2 I2 20 ω2 = ⎢ ⎣ 2 M1z ⎦ 0

(11.205)

where 2

M1z = −Me + l2 Fex sin (θ 1 + θ 2 ) − l2 Fey cos (θ 1 + θ 2 )   1 1 + l22 m2 θ¨ 1 + θ¨ 2 − gl2 m2 cos (θ 1 + θ 2 ) 3 2

(11.206)

Finally the action force on link (1) is: 1

F0 = 1 F1 − m1 1 g + m1 10 a1

⎤ F0x ⎢ 1 F0y ⎥ ⎥ = 1 T2 2 F1 − m1 1 g + m1 10 a1 = ⎢ ⎣ 0 ⎦ 0

where 1

⎡1

  F0x = −Fex cos θ 1 − Fey − gm1 sin θ 1    2 1 1 − l2 m2 θ¨ 1 + θ¨ 2 sin θ 2 − l2 m2 θ˙ 1 + θ˙ 2 cos θ 2 2 2 1 2 +gm2 sin (2θ 2 + θ 1 ) − l1 m1 θ˙ 1 2

1

(11.207)

(11.208)

  F0y = Fex sin θ 1 − Fey + gm1 cos θ 1    2 1 1 + l2 m2 θ¨ 1 + θ¨ 2 cos θ 2 − l2 m2 θ˙ 1 + θ˙ 2 sin θ 2 2 2 1 −gm2 cos (2θ 2 + θ 1 ) + l1 m1 θ¨ 1 2

(11.209)

632

11 Robot Dynamics

and the action moment on link (1) is: 1

M0 = 1 M1 − 1 n 1 × 1 F0 + 1 m 1 × 1 F1 + 1 I1 10 α 1 + 10 ω1 × 1 I1 10 ω1 = 1 T2 2 M1 − 1 n1 × 1 F0 + 1 m1 × 1 T2 2 F1 ⎡ ⎤ 0 ⎢ 0 ⎥ ⎥ + 1 I1 10 α 1 + 10 ω1 × 1 I1 10 ω1 = ⎢ ⎣ 1 M0z ⎦ 0

(11.210)

where 1

  1 1 M0z = −Me + l22 m2 θ¨ 1 + θ¨ 2 + l12 m1 θ¨ 1 3 3  1 − Fey l2 + gl2 m2 cos (θ 1 + θ 2 ) 2 1 − l1 m1 g cos θ 1 + Fex l2 sin (θ 1 + θ 2 ) 2

(11.211)

Example 364  Actuator’s force and torque. Actuator forces and torques are the required to derive a robot. Their expression for recursive equations are shown here. Applying a backward recursive force analysis ends up with a set of known force systems at joints. Each joint is driven by a motor known as an actuator that applies a force in a P joint, or a torque in an R joint. When the joint i is prismatic, the force of the driving actuator is along the zi−1 -axis showing that kˆi−1 component of the joint force Fi is supported by the actuator. T 0 Fm = 0 kˆi−1 Fi (11.212) The ıˆi−1 and jˆi−1 components of Fi must be supported by the bearings of the joint. Similarly, when the joint i is revolute, the torque of the driving actuator is along the zi−1 -axis showing that kˆi−1 component of the joint torque Mi is supported by the actuator. T 0 Mm = 0 kˆi−1 Mi (11.213) The ıˆi−1 and jˆi−1 components of Mi must be supported by the bearings of the joint.

11.3

Robot Lagrange Dynamics

The Lagrange equation of motion provides a systematic approach to obtain the dynamics equations of robots. The Lagrangian L is defined as the difference between the kinetic K and potential V energies. L=K −V

(11.214)

The Lagrange equation of motion for a robotic system can be found by applying the Lagrange equation d dt



∂L ∂ q˙i

−

∂L = Qi ∂qi

i = 1, 2, · · · n

(11.215)

where qi is the coordinates by which the energies are expressed, and Qi is the corresponding generalized nonpotential force that makes qi to vary. The equations of motion for an n link serial manipulator can be set in a matrix form ˙ + G(q) = Q D(q) q¨ + H(q, q)

(11.216)

11.3 Robot Lagrange Dynamics

633

or ˙ q˙ + G(q) = Q D(q) q¨ + C(q, q) or in a summation form:

n 

Dij (q) q¨j +

j =1

n n  

(11.217)

Hikm q˙k q˙m + Gi = Qi

(11.218)

k=1 m=1

Dij is an n × n inertial-type symmetric matrix, Dij =

n  

1 mk JDk + JTRk 0 Ik JRk 2

JTDk

k=1

Hikm is the velocity coupling vector, Hij k =

n  n   ∂Dij j =1 k=1

and Gi is the gravitational vector. Gi =

n 

∂qk

−

1 ∂Dj k 2 ∂qi

(11.219)

(11.220)

mj gT J(i) Dj

(11.221)

j =1

Proof Kinetic energy of link (i) is: Ki =

10 T 1 vi mi 0 vi + 0 ωTi 0 Ii 0 ωi 2 2

(11.222)

where mi is the mass of the link, i Ii is the mass moment matrix of the link in the link’s frame Bi , 0 vi is the global velocity of the link at its mas center C, and 0 ωi is the global angular velocity of the link with respect to the base frame B0 . The translational and angular velocity vectors can be expressed based on the joint coordinate velocities, utilizing the Jacobian of the link Ji . "0 # " # JDi vi P Xi = 0 = q˙ = Ji q˙ (11.223) ωi JRi The link’s Jacobian Ji is a 6 × n matrix that transforms the instantaneous joint coordinate velocities into the instantaneous (j ) (j ) link’s translational and angular velocities. The j th column of Ji is made of cDi and cRi . For j ≤ i, they are: 0 (j ) cDi

0 kˆj −1 × j −1 ri for a R joint kˆj −1 for a P joint

=

 (j ) cRi

=

(11.224)

kˆj −1 for a R joint 0 for a P joint

(11.225)

0 where j −1 ri is the position vector of C of the link (i) in the coordinate frame Bj −1 expressed in the base frame. The columns of Ji are zero for j > i. The kinetic energy K of the whole robot is then equal to:

K=

n  i=1

1 2 i=1 n

Ki =

 0 T vi

mi 0 v i +

1 T 0 Ii 0 ωi 0ω 2 i



n  T   1  T 0   1  = JDi q˙ i mi JDi q˙ i + JRi q˙ i Ii JRi q˙ i 2 i=1 2  n   1 T  T 1 T 0 = q˙ i JDi mi JDi + JRi Ii JRi q˙ i 2 2 i=1

(11.226)

634

11 Robot Dynamics

where 0 Ii is the mass moment matrix of the link (i) about its C and expressed in the base frame. 0

Ii = 0 Ri i Ii 0 RiT

(11.227)

The kinetic energy K may be written in a more convenient form as K=

1 T q˙ D q˙ i 2 i

(11.228)

where D is an n × n matrix called the manipulator inertia matrix. D=

n  

JTDi mi JDi +

i=1

1 T 0 J Ii JRi 2 Ri

(11.229)

The potential energy Vi of the link (i) is due to gravity Vi = −mi 0 g · 0 ri

(11.230)

and therefore, the total potential energy of the manipulator is: V =

n 

Vi = −

n 

i=1

mi 0 gT 0 ri

(11.231)

i=1

where 0 g is the gravitational acceleration vector expressed in the base frame. The Lagrangian of the manipulator is:  1 T mi 0 gT 0 ri q˙ i D q˙ i + 2 i=1 n

L = K −V =

 1  = Dij q˙i q˙ j + mi 0 gT 0 ri 2 i=1 j =1 i=1 n

n

n

(11.232)

Based on the Lagrangian L we find the partial differentials. ⎛

n n  

1 ∂ ⎝ ∂L = ∂qi 2 ∂qi j =1 =

⎞ Dj k q˙j q˙ k ⎠ +

n 

mj 0 g T

j =1

k=1

∂ 0 rj ∂qi

n n n  1   ∂Dj k q˙j q˙k + mj 0 gT J(i) Dj 2 j =1 k=1 ∂qi j =1

(11.233)

 ∂L = Dij q˙j ∂ q˙i j =1

(11.234)

n

  dDij d ∂L = Dij q¨j + q˙j dt ∂ q˙i dt j =1 j =1 n

=

n  j =1

n

Dij q¨j +

n n   ∂Dij j =1 k=1

∂qk

q˙k q˙j

(11.235)

11.3 Robot Lagrange Dynamics

635

The generalized force Qi of the Lagrange equations is: Qi = Mi + JT Fe  T Fe = −FTen −MTen

(11.236) (11.237)

where Mi is the ith actuator force at joint i, and Fe is the external force system applied on the end-effector. Finally, the Lagrange equations of motion for an n-link manipulator are: n 

Dij (q) q¨j + Hikm q˙k q˙m + Gi = Qi

(11.238)

j =1

where Hij k =

n  n   ∂Dij j =1 k=1

Gi =

n 

∂qk

−

1 ∂Dj k 2 ∂qi



mj gT J(i) Dj

(11.239)

(11.240)

j =1

We can show the equations of motion for a manipulator in a more concise form to simplify matrix calculations. ˙ + G(q) = Q D(q) q¨ + H(q, q)

(11.241)

˙ is called the velocity coupling vector. The velocity The term G(q) is called the gravitational force vector and the term H(q, q) coupling vector may sometimes be written in another form. ˙ = C(q, q) ˙ q˙ H(q, q)

(11.242) 

Example 365 A prismatic-revolute planar manipulator. A P R planar robotic manipulator with a prismatic and a revolute joints will be examined to derive its equations of motion by Lagrange method. It is a good example to find the quietens of motion in matrix form, step by step. Figure 11.12 illustrates a prismatic-revolute planar manipulator with massless links and two massive points m1 and m2 . The joint variables are indicated by q1 and q2 . To determine the equations of motion, we begin with calculating its kinetic energy. 1 m1 q˙12 2 1 1 K2 = m2 X˙ 22 + m2 Y˙22 2 2  2  2 1 d 1 d = m2 (q1 + l cos q2 ) + m2 (l sin q2 ) 2 dt 2 dt K1 =

1 1 m2 (q˙1 − l q˙2 sin q2 )2 + m2 (l q˙2 cos q2 ) 2 2  1  2 = m2 q˙1 + l 2 q˙22 − 2l q˙1 q˙2 sin q2 2

(11.243)

=

(11.244)

The potential energy of the manipulator is: V = m2 gY2 = m2 gl sin q2

(11.245)

636

11 Robot Dynamics

Fig. 11.12 A prismatic-revolute planar manipulator

Therefore, the Lagrangian is: L = K − V = K1 + K2 − V  1 1  = m1 q˙12 + m2 q˙ 21 +l 2 q˙22 − 2l q˙1 q˙2 sin q2 − m2 gl sin q2 2 2

Applying the Lagrange equation d dt



∂L ∂ q˙i

−

∂L = Qi ∂qi

i = 1, 2

(11.246)

(11.247)

provides the following equations of motion. (m1 + m2 ) q¨1 − m2 l q¨2 sin q2 − m2 l q˙22 cos q2 = Q1

(11.248)

m2 l q¨1 − m2 l q¨1 sin q2 + m2 gl cos q2 = Q2

(11.249)

2

We can rearrange these equations to the matrix form of (11.217) " D(q)

# " # " # q¨1 q˙1 Q1 ˙ + C(q, q) + G(q) = q¨2 q˙2 Q2

# m1 + m2 −m2 l sin q2 D(q) = m2 l 2 −m2 l sin q2 " # 0 −m2 l q˙2 cos q2 ˙ = C(q, q) 0 0 " # 0 G(q) = m2 gl cos q2

(11.250)

"

where

(11.251) (11.252) (11.253)

Example 366 A planar polar manipulator. A RP planar robotic manipulator with a revolute and a prismatic joints will be used to determine its equations of motion by Lagrange method and set them in matrix form. Figure 11.13 illustrates a revolute-prismatic planar polar manipulator with massless link and a massive point m. The kinetic energy K of the manipulator is: 1 1 m2 X˙ 22 + m2 Y˙22 2 2   d d 1 1 = m (q1 cos q2 ) 2 + m (q1 sin q2 ) 2 2 dt 2 dt

K=

=

 1  2 m q˙1 + q12 q˙22 2

(11.254)

11.3 Robot Lagrange Dynamics

637

Fig. 11.13 A planar polar manipulator

The potential energy V of the manipulator is: V = mgY2 = mgq1 sin q2

(11.255)

and therefore, the Lagrangian L of the manipulator is: L=K −V =

Applying the Lagrange equation d dt



 1  2 m q˙1 + q12 q˙ 22 − mgq1 sin q2 2

∂L ∂ q˙i

−

∂L = Qi ∂qi

i = 1, 2

(11.256)

(11.257)

provides the following equations of motion. mq¨1 − mq1 q˙22 + mg sin q2 = Q1

(11.258)

mq12 q¨2 + 2mq1 q˙1 q˙2 + mgq1 cos q2 = Q2

(11.259)

Let us rearrange these equations to the matrix form of (11.217). " D(q)

# " # " # q¨1 q˙1 Q1 ˙ + C(q, q) + G(q) = q¨2 q˙2 Q2 " D(q) = "

m 0 0 mq12

#

0 −mq1 q˙2 mq1 q˙2 mq1 q˙1 " # mg sin q2 G(q) = mgq1 cos q2

˙ = C(q, q)

(11.260)

(11.261) # (11.262) (11.263)

Example 367 General model of 2R planar manipulator. Deriving equations of motion of a 2R manipulator by Lagrange method and arranging the result in matrix form. Consider a general 2R manipulator with massive arms and joints while carrying a payload m0 as is shown in Fig. 11.14. The first motor that drives link (1) is on the ground. The second motor with mass m12 drives link (2) and is mounted on link (1). The mass of first and second links are m11 and m21 , respectively. The global position vectors of the mass centers Ci and massive joints are: ⎡ ⎤ c1 cos θ 1 0 r1 = 0 R1 1 r1 = RZ,θ 1 c1 1 ıˆ1 = ⎣ c1 sin θ 1 ⎦ (11.264) 0

638

11 Robot Dynamics

Fig. 11.14 A 2R manipulator with massive arms and a carrying payload m0

r2 = 0 d1 + 0 R2 2 r2 = 0 d1 + RZ,θ 1 RZ,θ 2 c2 2 ıˆ2 ⎡ ⎤ l1 cos θ 1 + c2 cos (θ 1 + θ 2 ) = ⎣ l1 sin θ 1 + c2 sin (θ 1 + θ 2 ) ⎦ 0 ⎡ ⎤ l1 cos θ 1 0 d1 = 0 R1 1 r1 = RZ,θ 1 l1 1 ıˆ1 = ⎣ l1 sin θ 1 ⎦ 0 ⎤ ⎡ l2 cos (θ 1 + θ 2 ) + l1 cos θ 1 0 d2 = 0 d1 + 0 R2 2 d2 = ⎣ l2 sin (θ 1 + θ 2 ) + l1 sin θ 1 ⎦ 0 0

(11.265)

(11.266)

(11.267)

where ⎡

0

R1 = RZ,θ 1

1

R2 = RZ,θ 2

cos θ 1 = ⎣ sin θ 1 0 ⎡ cos θ 2 = ⎣ sin θ 2 0

⎤ − sin θ 1 0 cos θ 1 0 ⎦ 0 1 ⎤ − sin θ 2 0 cos θ 2 0 ⎦ 0 1

⎤ cos (θ 1 + θ 2 ) − sin (θ 1 + θ 2 ) 0 0 R2 = 0 R1 1 R2 = ⎣ sin (θ 1 + θ 2 ) cos (θ 1 + θ 2 ) 0 ⎦ 0 0 1

(11.268)

(11.269)

⎡

The links’ angular velocity is: 0 ω1

= θ˙ 1 Kˆ

0 ω2

  = θ˙ 1 + θ˙ 2 Kˆ

(11.270)

(11.271)

The mass moment matrices in the global coordinate frame are: ⎡

⎤ Ix1 0 0 0 T I1 = RZ,θ 1 1 I1 RZ,θ = 0 R1 ⎣ 0 Iy1 0 ⎦ 0 R1T 1 0 0 Iz1   ⎡ ⎤ 2 2 Ix1 c θ 1 +Iy1 s θ 1 Ix1 2− Iy1 cθ 12sθ 1 0 = ⎣ Ix1 − Iy1 cθ 1 sθ 1 Iy1 c θ 1 + Ix1 s θ 1 0 ⎦ 0 0 Iz1

(11.272)

11.3 Robot Lagrange Dynamics

639

⎡

⎤ Ix2 0 0 0 I2 = 0 R2 2 I2 0 R2T = 0 R2 ⎣ 0 Iy2 0 ⎦ 0 R2T 0 0 Iz2   ⎡ ⎤ 2 2 Ix2 c θ 12 +  Iy2 s θ 12 Ix2 2− Iy2 cθ 122sθ 12 0 = ⎣ Ix2 − Iy2 cθ 12 sθ 12 Iy2 c θ 12 + Ix2 s θ 12 0 ⎦ 0 0 Iz2 θ 12 = θ 1 + θ 2

The velocity of Ci and the masses are:

(11.273) (11.274)

⎡

⎤ −c1 θ˙ 1 sin θ 1 d0 0 v1 = r1 = ⎣ c1 θ˙ 1 cos θ 1 ⎦ dt 0 0

0

d0 r2 dt   ⎡ ⎤ −l1 θ˙ 1 sin θ 1 − c2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) = ⎣ l1 θ˙ 1 cos θ 1 + c2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 ) ⎦ 0 ⎡ ⎤ −l1 θ˙ 1 sin θ 1 0˙ ⎣ d 1 = l1 θ˙ 1 cos θ 1 ⎦ 0   ⎡ ⎤ −l1 θ˙ 1 sin θ 1 − l2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) 0˙ d 2 = ⎣ l1 θ˙ 1 cos θ 1 + l2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 ) ⎦ 0

0

v2 =

(11.275)

(11.276)

(11.277)

(11.278)

To calculate Lagrangian L = K − V , we determine the energies of the manipulator. The kinetic energy of the manipulator is: K=

1 1 m12 0 d˙ 1 · 0 d˙ 1 + m11 0 v1 · 0 v1 2 2 1 1 + m0 0 d˙ 2 · 0 d˙ 2 + m21 0 v2 · 0 v2 2 2 1 T0 1 T0 + 0 ω1 I1 0 ω1 + 0 ω2 I2 0 ω2 2 2

(11.279)

which after substituting (11.268)–(11.278) would be: K=

 2 1 m11 c12 + m12 l12 + Iz1 θ˙ 1 2    2 1 + m21 −l1 θ˙ 1 sin θ 1 − c2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) 2    2 1 + m21 l1 θ˙ 1 cos θ 1 + c2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 ) 2   2 1  + m0 −l1 θ˙ 1 sin θ 1 − l2 θ˙ 1 + θ˙ 2 sin (θ 1 + θ 2 ) 2   2 1  + m0 l1 θ˙ 1 cos θ 1 + l2 θ˙ 1 + θ˙ 2 cos (θ 1 + θ 2 ) 2 2 1  + Iz2 θ˙ 1 + θ˙ 2 2

(11.280)

640

11 Robot Dynamics

The potential energy of the manipulator is: V = m11 gc1 sin θ 1 + m12 gl1 sin θ 1 +m21 g (l1 sin θ 1 + c2 sin (θ 1 + θ 2 )) +m0 g (l1 sin θ 1 + l2 sin (θ 1 + θ 2 ))

(11.281)

Applying the Lagrange equation d dt d dt

 

∂L ∂ θ˙ 1 ∂L ∂ θ˙ 2

−

∂L = Q0 ∂θ 1

(11.282)

−

∂L = Q1 ∂θ 2

(11.283)



determines the general equations of motion which can be rearranged in a matrix form. "

D11 D12 D21 D22

#"

# " #" # θ¨ 1 C11 C12 θ˙ 1 + ¨θ 2 C21 C22 θ˙ 2 " # " # G1 Qo + = G2 Q1

(11.284)

D11 = 2l1 (m21 c2 + m0 l2 ) cos θ 2 + Iz1 + Iz2     +m11 c12 + m12 l12 + m21 c22 + l12 + m0 l12 + l22

(11.285)

D12 = l1 (m21 c2 + m0 l2 ) cos θ 2 + Iz2 +

(11.286)

m0 l22

+

m21 c22

D21 = l1 (m21 c2 + m0 l2 ) cos θ 2 + Iz2 + m21 c22 + m0 l22 (11.287) D22 = Iz2 + m21 c22 + m0 l22

(11.288)

C11 = −l1 (m21 c2 + m0 l2 ) θ˙ 2 sin θ 2   C12 = −l1 (m21 c2 + m0 l2 ) θ˙ 1 + θ˙ 2 sin θ 2

(11.289)

C21 = l1 (m21 c2 + m0 l2 ) θ˙ 1 sin θ 2

(11.291)

C22 = 0

(11.292)

(11.290)

G1 = ((m21 + m12 + m0 ) l1 + m11 c1 ) g cos θ 1 + (m21 c2 + m0 l2 ) g cos (θ 1 + θ 2 ) G2 = (m21 c2 + m0 l2 ) g cos (θ 1 + θ 2 )

(11.293) (11.294)

To have all equations that we need for control, path planning, and computer analysis of 2R manipulator, here we explain dynamics of the most common special cases of the manipulator. Figure 11.14 illustrates a general 2R manipulator with massive arms and joints while carrying a payload of mass m0 . The second motor has a mass m12 and is mounted on link (1). The mass of first and second links are m11 and m21 , respectively, and their mass centers are at distance c1 and c2 from oi−1 on xi -axis. The general equations of motion for the 2R planar manipulator are given in Eqs. (11.284). 1. Massless arms. When the mass of the links of the manipulator are much less than the masses of its motors and the carrying load, we may use a massless arm model. The equations of motion for a massless arm 2R planar manipulator are calculated by substituting m11 = 0, m21 = 0, Iz1 = 0, Iz2 = 0 in Eqs. (11.284).

11.3 Robot Lagrange Dynamics

641

  D11 = 2m0 l1 l2 cos θ 2 + m12 l12 + m0 l12 + l22

(11.295)

D12 = m0 l1 l2 cos θ 2 +

m0 l22

(11.296)

D21 = m0 l1 l2 cos θ 2 + m0 l22

(11.297)

D22 =

(11.298)

m0 l22

C11 = −m0 l1 l2 θ˙ 2 sin θ 2   C12 = −m0 l1 l2 θ˙ 1 + θ˙ 2 sin θ 2 C21 = m0 l1 l2 θ˙ 1 sin θ 2 C22 = 0

(11.299)

G1 = (m12 + m0 ) gl1 cos θ 1 + m0 gl2 cos (θ 1 + θ 2 )

(11.302)

G2 = m0 gl2 cos (θ 1 + θ 2 )

(11.303)

(11.300) (11.301)

    Qo = m0 l12 + l22 + 2l1 l2 cos θ 2 + m12 l12 θ¨ 1   +m0 l22 + l1 l2 cos θ 2 θ¨ 2 −2m0 l1 l2 θ˙ 1 θ˙ 2 sin θ 2 − m0 l1 l2 θ˙ 2 sin θ 2 2

+ (m12 + m0 ) gl1 cos θ 1 + m0 gl2 cos (θ 1 + θ 2 )

(11.304)

  Q1 = m0 l22 + l1 l2 cos θ 2 θ¨ 1 + m0 l22 θ¨ 2 2 +m0 l1 l2 θ˙ 1 sin θ 2 + m0 gl2 cos (θ 1 + θ 2 )

(11.305)

2. Massless joints. When the mass of the links of the manipulator are much more than the masses of its motors and the carrying load, we may use a massless joint model. The equations of motion for a massless joints 2R planar manipulator are calculated by substituting m12 = 0, m0 = 0 in Eqs. (11.284). D11 = 2m21 l1 c2 cos θ 2 + Iz1 + Iz2   +m11 c12 + m21 c22 + l12

(11.306)

D12 = m21 c2 (l1 cos θ 2 + c2 ) + Iz2

(11.307)

D21 = m21 c2 (l1 cos θ 2 + c2 ) + Iz2

(11.308)

D22 = Iz2 + m21 c22

(11.309)

C11 = −m21 l1 c2 θ˙ 2 sin θ 2   C12 = −m21 l1 c2 θ˙ 1 + θ˙ 2 sin θ 2 C21 = m21 l1 c2 θ˙ 1 sin θ 2

C22 = 0

(11.310) (11.311) (11.312)

G1 = (m21 l1 + m11 c1 ) g cos θ 1 + m21 c2 g cos (θ 1 + θ 2 )

(11.313)

G2 = m21 c2 g cos (θ 1 + θ 2 )

(11.314)

If the links of the manipulator are uniform and symmetric, then c1 = l1 /2

c2 = l2 /2

(11.315)

642

11 Robot Dynamics

As an example, let us determine the torques of motors of a planar 2R manipulator with massless arms and the following dimensions. The end-effector moves on a straight line in one minute, 0 ≤ t ≤ 1 min. l1 = l 2 = 1 m

m0 = 1 kg

X (0) = 1 m

Y (0) = 0

X (1) = −1 m

m12 = 1 kg

(11.316) (11.317)

Y (1) = 0

(11.318)

We assume a septic time function for X to move the end-effector from X (0) = 1 to X (1) = −1 with zero velocity, zero acceleration, zero jerk at both ends. X = 1 − 70t 4 + 168t 5 − 140t 6 + 40t 7 (11.319) To make the dynamic equations of motion suitable for numerical calculations, we express the equations of motion (11.304) and (11.305) for 2R robotic manipulators in the following form. 2 Qo = Aθ¨ 1 + B θ¨ 2 + C θ˙ 1 θ˙ 2 + D θ˙ 2 + M

Q1 = E θ¨ 1 + F θ¨ 2 +

2 Gθ˙ 1

+N

(11.320) (11.321)

  A = A(θ 2 ) = m0 l12 + l22 + 2l1 l2 cos θ 2 + m12 l12 (11.322)

  B = B(θ 2 ) = m0 l22 + l1 l2 cos θ 2

(11.323)

C = C(θ 2 ) = −2m0 l1 l2 sin θ 2

(11.324)

D = D(θ 2 ) = −m0 l1 l2 sin θ 2

(11.325)

M = M(θ 1 , θ 2 ) = (m12 + m0 ) gl1 cos θ 1 +m0 gl2 cos (θ 1 + θ 2 )

(11.326)

  E = E(θ 2 ) = B = m0 l22 + l1 l2 cos θ 2

(11.327)

F = m0 l22

(11.328)

G = G(θ 2 ) = −D = m0 l1 l2 sin θ 2

(11.329)

N = N(θ 1 , θ 2 ) = m0 gl2 cos (θ 1 + θ 2 ) Employing the inverse kinematics analysis, and dynamic equations (11.320) and (11.321), Fig. 11.15 illustrates the torques Q0 , and Q1 and joints 1 and 2 as function of time to move the robot’s end-effector from point (1, 1.5) to (−1, 1.5) on a straight line in one unit of time. Having torques and the results of inverse kinematic analysis enable us to calculate the consumed energy E and needed power P of motors. - θ 1f - θ 2f n n   E= Q0 dθ 1 + Q1 dθ 2 ≡ Q0k dθ 1k + Q1k dθ 2k (11.330) θ 1i

θ 2i

P0 = Q0 (t) θ˙ 1 (t)

k=1

k=1

P1 = Q1 (t) θ˙ 2 (t)

(11.331)

Figure 11.16 illustrates time history of the power of motors of this example. Assuming motors will be using energy regardless of torque direction, the absolute value of the powers must be considered as required powers. Hence, consumed energy will be the area under the curve of absolute value of power. Example 368 Lagrange equation for 2R manipulators with massive arms. A step by step deriving the matrix form of equations of motion based on Lagrange method for a 2R manipulator. Although the name of the actuation force and torques are better to be indexed by the link they are installed on, such as Fig. 11.14, we may name there as our preference. Here is a traditional naming.

11.3 Robot Lagrange Dynamics

643

Fig. 11.15 The torques Q0 , and Q1 of joints 1 and 2 of a 2R robot as function of time. The path of motion of the robot is from point (1, 1.5) to (−1, 1.5) in one unit of time

Fig. 11.16 Time history of the power of motors

Fig. 11.17 A 2R planar manipulator with massive links

A 2R planar manipulator is shown in Fig. 11.17. Its homogenous transformation matrices are given in Eqs. (5.29) and (5.30). ⎡ ⎤ cos θ 1 − sin θ 1 0 0 R1 = ⎣ sin θ 1 cos θ 1 0 ⎦ (11.332) 0 0 1 ⎡ ⎤ cos θ 2 − sin θ 2 0 1 R2 = ⎣ sin θ 2 cos θ 2 0 ⎦ (11.333) 0 0 1

644

11 Robot Dynamics

Assuming the links are made of homogenous material in a bar shape, the position vectors of the mass center Ci are: ⎡

⎤ −li /2 i ri = ⎣ 0 ⎦ 0

i = 1, 2

(11.334)

⎡ ⎤ 000 1 i Ii = mi l 2 ⎣ 0 1 0 ⎦ 12 i 001

and the mass moment matrices are:

(11.335)

Therefore, 0

0

I1 = 0 R1 1 I1 0 R1T ⎡ sin2 θ 1 − cos θ 1 sin θ 1 1 2⎣ = m1 l1 − cos θ 1 sin θ 1 cos2 θ 1 12 0 0

⎤ 0 0⎦ 1

I2 = 0 R2 2 I2 0 R2T ⎡ sin2 θ 12 − cos θ 12 sin θ 12 1 2⎣ m2 l2 − cos θ 12 sin θ 12 = cos2 θ 12 12 0 0

The gravity is assumed to be in −jˆ0 direction

⎤ 0 0⎦ 1

⎤ 0 0 g = ⎣ −g ⎦ 0

(11.336)

(11.337)

⎡

(11.338)

and the link Jacobian matrices are: ⎡

JD1

⎤ − 12 l1 sin θ 1 0 = ⎣ 21 l1 cos θ 1 0 ⎦ 0 0

⎡

JR1

⎤ 00 = ⎣0 0⎦ 10

⎤ −l1 sin θ 1 − 12 l2 sin θ 12 − 21 l2 sin θ 12 = ⎣ l1 cos θ 1 + 12 l2 cos θ 12 21 l2 cos θ 12 ⎦ 0 0 ⎡ ⎤ 00 JR2 = ⎣ 0 0 ⎦ 10

(11.339)

⎡

JD2

(11.340)

(11.341)

We calculate the manipulator mass moment matrix by substituting 0 Ii , JDi , and JRi in Eq. (11.229). D=

2  

JTDi mi JDi +

i=1

1 T 0 J Ii JRi 2 Ri



1 1 = JTD1 m1 JD1 + JTR1 0 I1 JR1 + JTD2 m2 JD2 + JTR2 0 I2 JR2 2 2 2  1 # "1 1 2 2 m l + m l + l1 l2 cθ 2 + 3 l2 m2 2 l1 l2 cθ 2 + 13 l22 = 3 1 1  12 1 1 m2 2 l1 l2 cθ 2 + 13 l22 m l2 3 2 2

(11.342)

11.3 Robot Lagrange Dynamics

645

The velocity coupling vector H has two elements that are: 1  1   ∂D1j

1 ∂Dj k q˙j q˙k H1 = − ∂qk 2 ∂q1 j =1 k=1  1 ˙ ˙ = −m2 l1 l2 θ 1 + θ 2 θ˙ 2 sin θ 2 2

H2 =

2  2   ∂D2j j =1 k=1

=

∂qk

−

1 ∂Dj k 2 ∂q2

(11.343)

q˙j q˙k

1 2 m2 l1 l2 θ˙ 1 sin θ 2 . 2

(11.344)

The elements of the gravitational force vector G are: G1 =

1 1 m1 gl1 cos θ 1 + m2 gl1 cos θ 1 + m2 gl2 cos θ 12 2 2 (11.345)

G2 =

1 m2 gl2 cos θ 12 2

(11.346)

Now we can assemble the equations of motion for the 2R planar manipulator. Assuming no external force on the end-effector, the equations of motion are:  Q1 =

 1 1 m1 l12 +m2 l12 +l1 l2 cθ 2 + l22 θ¨ 1 3 3   1 1 1 l1 cθ 2 + l2 θ¨ 2 −m2 l1 l2 θ˙ 1 + θ˙ 2 θ˙ 2 sin θ 2 +m2 l2 2 3 2  1 1 m1 +m2 gl1 cos θ 1 + m2 gl2 cos θ 12 + 2 2  Q2 = m2

(11.347)

1 1 1 l1 l2 cθ 2 + l22 θ¨ 1 + m2 l22 θ¨ 2 2 3 3

1 1 2 + m2 l1 l2 θ˙ 1 sin θ 2 + m2 gl2 cos θ 12 2 2

(11.348)

Example 369 Lagrange equation of a one-link manipulator. To show the advantage and simplicity of the Lagrange method when compared to Newton–Euler method, let us derive the equation of motion of the uniform beam of Fig. 11.18 with a mass m2 at the tip point. This is similar to the system of Fig. 11.5a. The beam is uniform with a mass center at 0 r1 while the tip mass is at 0 d1 , both in B0 . ⎡

⎤ l ⎢ 2 cos θ ⎥ ⎢ ⎥ 1 0 0 r1 = R1 r1 = ⎢ l sin θ ⎥ ⎣ ⎦ 2 0

(11.349)

646

11 Robot Dynamics

Fig. 11.18 A uniform beam with a hanging weight m2 at the tip point

⎡

⎤ l cos θ 0 d1 = 0 R1 1 d1 = ⎣ l sin θ ⎦ 0 ⎡ ⎤ cos θ − sin θ 0 0 R1 = RZ,θ = ⎣ sin θ cos θ 0 ⎦ 0 0 1 The angular velocity of the beam is: 0 ω1

= θ˙ Kˆ

(11.350)

(11.351)

(11.352)

and therefore, the velocity of C and m2 are: ⎡

⎤ l ˙ sin θ θ − ⎢ 2 ⎥ ⎢ ⎥ 0 v1 = 0 ω1 × 0 r1 = ⎢ l θ˙ cos θ ⎥ ⎣ ⎦ 2 0 ⎤ −l θ˙ sin θ 0˙ d 1 = 0 ω1 × 0 d1 = ⎣ l θ˙ cos θ ⎦ 0

(11.353)

⎡

(11.354)

The kinetic energy of the manipulator is: 1 0˙ 0˙ 1 1 m2 d 1 · d 1 + m1 0 v1 · 0 v1 + 0 ωT1 0 I1 0 ω1 2 2 2 1 2 1 2 = l 2 θ˙ (m1 + 4m2 ) + Iz θ˙ 8 2

K2 =

⎤ Ix 0 0 0 T I1 = RZ,θ 1 I1 RZ,θ = 0 R1 ⎣ 0 Iy 0 ⎦ 0 R1T 0 0 Iz   ⎡ ⎤ 2 2 Ix cos θ+ Iy sin θ Ix − 2Iy cos θ sin2 θ 0 = ⎣ Ix − Iy cos θ sin θ Iy cos θ + Ix sin θ 0 ⎦ 0 0 Iz

(11.355)

⎡

(11.356)

The potential energy of the manipulator is: V = m1 gY1 + m2 gY2 = m1 grY + m2 gdY l = m1 g sin θ + m2 gl sin θ 2

(11.357)

11.3 Robot Lagrange Dynamics

647

Fig. 11.19 An articulated manipulator with massive links and a massive load at the tip point

and therefore, the Lagrangian of the manipulator will be: L = K −V =

1 2 2 1 2 l θ˙ (m1 + 4m2 ) + Iz θ˙ 8 2

l −m1 g sin θ − m2 gl sin θ 2 Applying the Lagrange equation d dt



∂L ∂ θ˙

−

∂L = Q0 ∂θ

∂L 1 = l 2 (m1 + 4m2 ) θ˙ + Iz θ˙ ˙ 4 ∂θ   d ∂L 1 = m1 l 2 + m2 l 2 + Iz θ¨ dt ∂ θ˙ 4

(11.358)

(11.359)

(11.360) (11.361)

∂L l = −m1 g cos θ − m2 gl cos θ ∂θ 2

(11.362)

 1 1 m1 l 2 + m2 l 2 + Iz θ¨ + m1 + m2 gl cos θ 4 2

(11.363)

determines the equation of motion.  Q0 = It is the same equation as (11.50). Example 370 Equations of motion of an articulated manipulator. This is very common and practical 3D industrial manipulator. Here is deriving its equations of motion by Lagrange method. Figure 11.19 illustrates an articulated manipulator with massive links and a massive load at the tip point. Points Ci , i = 1, 2, 3 indicate the mass centers of the links with masses mi , i = 1, 2, 3. The tip point has a mass of m0 . A top view of the manipulator is shown in Fig. 11.20.

648

11 Robot Dynamics

Fig. 11.20 A top view of an articulated manipulator with massive links and a massive load at the tip point

The link (1) of the manipulator is an RR(90) with an extra displacement l1 along z1 . To determine the transformation matrix 0 R1 we can begin from a coincident configuration of B1 and B0 and move B1 to its current configuration by a sequence of proper rotations and displacements. 1

T0 = Dz1 ,l1 Rx1 ,π /2 Rz1 ,θ 1 ⎡ ⎤ cos θ 1 sin θ 1 0 0 ⎢ 0 0 1 0⎥ ⎥ =⎢ ⎣ sin θ 1 − cos θ 1 0 l1 ⎦ 0 0 01 ⎡

0

T1 = 1 T0−1

cos θ 1 ⎢ sin θ 1 =⎢ ⎣ 0 0

⎤ 0 sin θ 1 −l1 sin θ 1 0 − cos θ 1 l1 cos θ 1 ⎥ ⎥ ⎦ 1 0 0 0 0 1

(11.364)

(11.365)

The second and third links are RR(0), RR(90), and their associated transformation matrices between coordinate frames are: ⎡ ⎤ cos θ 2 − sin θ 2 0 l2 cos θ 2 ⎢ sin θ 2 cos θ 2 0 l2 sin θ 2 ⎥ 1 ⎥ T2 = ⎢ (11.366) ⎣ 0 ⎦ 0 1 0 0 0 0 1 ⎡ ⎤ cos θ 3 0 sin θ 3 0 ⎢ sin θ 3 0 − cos θ 3 0 ⎥ 2 ⎥ T3 = ⎢ (11.367) ⎣ 0 1 0 0⎦ 0 0 0 1 The global position vectors of the mass centers Ci and joints are: ⎤ ⎡ ⎤ − (l1 − c1 ) sin θ 1 0 ⎢ 0 ⎥ ⎢ (l1 − c1 ) cos θ 1 ⎥ 0 ⎥ ⎢ ⎥ r1 = 0 T1 1 r1 = 0 T1 ⎢ ⎣ c1 ⎦ = ⎣ ⎦ 0 1 1 ⎡

(11.368)

11.3 Robot Lagrange Dynamics

649

⎡ ⎤ ⎡ ⎤ 0 −l1 sin θ 1 ⎢ 0 ⎥ ⎢ l1 cos θ 1 ⎥ 0 ⎥ ⎢ ⎥ d1 = 0 T1 1 d1 = 0 T1 ⎢ ⎣0⎦ = ⎣ ⎦ 0 1 1 ⎡ ⎤ −2l1 sin θ 1 + (c2 + l2 ) cos θ 1 cos θ 2 ⎢ 2l1 cos θ 1 + (c2 + l2 ) cos θ 2 sin θ 1 ⎥ 0 ⎥ r2 = 0 d1 + 0 T2 2 r2 = ⎢ ⎣ ⎦ (c2 + l2 ) sin θ 2 2 ⎡ ⎤ 2l2 cos θ 1 cos θ 2 − 2l1 sin θ 1 ⎢ 2l1 cos θ 1 + 2l2 cos θ 2 sin θ 1 ⎥ 0 ⎥ d2 = 0 d1 + 0 T2 2 d2 = ⎢ ⎣ ⎦ 2l2 sin θ 2 2 0

r3 = 0 d2 + 0 T3 3 r3 ⎡ ⎤ c3 cos θ 1 (sin (θ 2 + θ 3 ) + 3l2 cos θ 2 ) − 3l1 sin θ 1 ⎢ c3 sin θ 1 (sin (θ 2 + θ 3 ) + 3l2 cos θ 2 ) + 3l1 cos θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 3l2 sin θ 2 − c3 cos (θ 2 + θ 3 ) 3

0

d3 = 0 d2 + 0 T3 3 d3 ⎤ ⎡ l3 cos θ 1 (sin (θ 2 + θ 3 ) + 3l2 cos θ 2 ) − 3l1 sin θ 1 ⎢ l3 sin θ 1 (sin (θ 2 + θ 3 ) + 3l2 cos θ 2 ) + 3l1 cos θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 3l2 sin θ 2 − l3 cos (θ 2 + θ 3 ) 3

The links’ angular velocity is: 0 ω1

= θ˙ 1 kˆ0

˜2 0ω

˜3 0ω

1 ω2

= θ˙ 2 kˆ1

2 ω3

= θ˙ 3 kˆ2

= 0 ω˜ 1 + 01 ω˜ 2 = 0 ω˜ 1 + 0 R1 1 ω˜ 2 0 R1T ⎡ ⎤ 0 −θ˙ 1 −θ˙ 2 cos θ 1 = ⎣ θ˙ 1 0 −θ˙ 2 sin θ 1 ⎦ θ˙ 2 cos θ 1 θ˙ 2 sin θ 1 0

= 0 ω˜ 2 + 02 ω˜ 3 = 0 ω˜ 2 + 0 R2 2 ω˜ 3 0 R2T   ⎡ ⎤ − θ˙ 2 + θ˙ 3  cos θ 1 0 −θ˙ 1 θ˙ 1 0 − θ˙ 2 + θ˙ 3 sin θ 1 ⎦ = ⎣  ˙θ 2 + θ˙ 3 cos θ 1 θ˙ 2 + θ˙ 3 sin θ 1 0

(11.369)

(11.370)

(11.371)

(11.372)

(11.373)

(11.374)

(11.375)

(11.376)

The mass moment matrices in the global coordinate frame are: ⎡

⎤ Ix1 0 0 0 I1 = 0 R1 ⎣ 0 Iy1 0 ⎦ 0 R1T 0 0 Iz1   ⎡ ⎤ 2 2 Ix1 cos θ 1 + Iz1 sin θ 1 Ix1 − 2Iz1 cos θ 1 sin2 θ 1 0 = ⎣ Ix1 − Iz1 cos θ 1 sin θ 1 Iz1 cos θ 1 + Ix1 sin θ 1 0 ⎦ 0 0 Iy1

(11.377)

650

11 Robot Dynamics

⎡

0

I2 = 0 R2 2 I2 0 R2T

0

I3 = 0 R3 3 I3 0 R3T

Ix2 = 0 R2 ⎣ 0 0 ⎡ Ix3 = 0 R3 ⎣ 0 0

⎤ 0 0 Iy2 0 ⎦ 0 R2T 0 Iz2 ⎤ 0 0 Iy3 0 ⎦ 0 R3T 0 Iz3

(11.378)

(11.379)

The velocity of Ci and the joints are: ⎡ ⎤ − (l1 − c1 ) θ˙ 1 cos θ 1 d 0 0 v1 = r1 = ⎣ − (l1 − c1 ) θ˙ 1 sin θ 1 ⎦ dt 0 0

0

0 d0 d0 0 r2 v3 = r3 dt dt ⎡ ⎤ −l1 θ˙ 1 cos θ 1 0˙ d 1 = ⎣ −l1 θ˙ 1 sin θ 1 ⎦ 0

v2 =

0

0 d0 d2 d˙ 2 = dt The kinetic and potential energies of the manipulator are: 0

(11.380)

0

0 d0 d3 d˙ 3 = dt

1 0 1 1 m1 v1 · 0 v1 + m21 0 v2 · 0 v2 + m3 0 v3 · 0 v3 2 2 2 1 0˙ 0˙ 1 T0 + m0 d 3 · d 3 + 0 ω1 I1 0 ω1 2 2 1 T0 1 T0 + 0 ω2 I2 0 ω2 + 0 ω3 I3 0 ω3 2 2 V = m2 gr2z + m3 gr3z + m0 gd3z

(11.381)

(11.382)

(11.383)

K=

(11.384) (11.385)

Using the Lagrangian of the manipulator L = K − V , and applying the Lagrange equation, we determines the equations of motion.  ∂L d ∂L − = Q0 (11.386) dt ∂ θ˙ 1 ∂θ 1  d ∂L ∂L − = Q1 (11.387) dt ∂ θ˙ 2 ∂θ 2  d ∂L ∂L − = Q2 (11.388) ˙ dt ∂ θ 3 ∂θ 3 Example 371 Equations of motion of a Cartesian manipulator. This is mathematically the simplest type of manipulators. Here is how to find its equations of motion, ignoring wrist motions. Figure 11.21 illustrates a Cartesian manipulator with massive motors and a massive load at the tip point. The manipulator has 3 DOF and with three mutually orthogonal rigid links. The three joints of the robot are prismatic, each one equipped with a motor to apply forces Q1 , Q2 , Q3 , respectively. The robot’s joint variables are q1 , q2 , q3 . The kinetic K and potential P energies and Lagrangian L of this manipulator are:     1 m1 q˙12 + m2 q˙12 + q˙22 + m3 q˙12 + q˙22 + q˙ 23 2  1 = (m1 + m2 + m3 ) q˙12 + (m2 + m3 ) q˙22 + m3 q˙32 2

K=

(11.389)

11.3 Robot Lagrange Dynamics

651

Fig. 11.21 A Cartesian manipulator with massive motors and a massive load at the tip point

So we have:

V = (m1 + m2 + m3 ) g q1

(11.390)

L = K −V

(11.391)

∂L ∂ q˙1 ∂L ∂ q˙2 ∂L ∂ q˙3 ∂L ∂q1

d ∂L dt ∂ q˙1 d ∂L = (m2 + m3 ) q˙2 dt ∂ q˙2 d ∂L = m3 q˙3 dt ∂ q˙3 ∂L = − (m1 + m2 + m3 ) g = ∂q2 = (m1 + m2 + m3 ) q˙1

= (m1 + m2 + m3 ) q¨ 1 = (m2 + m3 ) q¨2 = m3 q¨3

(11.392)

∂L =0 ∂q3

and the equations of motion will be as follows. (m1 + m2 + m3 ) q¨1 + (m1 + m2 + m3 ) g = Q1

(11.393)

(m2 + m3 ) q¨2 = Q2

(11.394)

m3 q¨3 = Q3

(11.395)

⎡

⎤⎡ ⎤ m1 + m2 + m3 0 0 q¨1 ⎣ 0 m2 + m3 0 ⎦ ⎣ q¨2 ⎦ 0 0 m3 q¨3 ⎤ ⎡ ⎤ ⎡ Q1 m1 + m2 + m3 ⎦ = ⎣ Q2 ⎦ +⎣ 0 0 Q3

(11.396)

Usually it is better to express the equations of motion in state-space, so we work only with first-order equations. Software usually work easier with first-order equations. Equations of motion of this Cartesian manipulator in state-space will be:

652

11 Robot Dynamics

⎡ ⎤ ⎤ q˙1 q1 ⎢ ⎥ q˙2 ⎥ ⎢ q2 ⎥ ⎢ ⎢ ⎥ ⎢ ⎥ q˙3 ⎥ ⎢ ⎥ d ⎢ q ⎢ 3 ⎥ = ⎢ Q1 − (m1 + m2 + m3 ) ⎥ ⎥ ⎢ ⎥ q ˙ dt ⎢ ⎢ 1⎥ ⎢ ⎥ m1 + m2 + m3 ⎥ ⎣ q˙2 ⎦ ⎢ ⎦ ⎣ Q2 / (m2 + m3 ) q˙3 Q3 /m3 ⎡

(11.397)

Example 372  Kinetic energy of a spherical wrist. Spherical wrist is made of three links and three orthogonal revolute joints. Kinetic energy of the wrist is calculated here to be used for Lagrange equation. Figure 11.22a illustrates a spherical wrist with minimum number of coordinate frames to express relative position of its three loving links. The exploded diagram of the wrist and its individual members are illustrated in Fig. 11.22b. Their coordinate frames and their mass centers are illustrated in Fig. 11.22c. The homogenous transformation matrices of every link’s coordinate frame Bi to its previous frame Bi−1 are only one rotation about zi−1 -axis with different α i and di . The first link is and RR(−90), as z1 is orthogonal to z0 with α 1 = −90 deg and a1 = 0, d1 = l1 . ⎡ ⎤ cos θ 1 0 − sin θ 1 0 ⎢ sin θ 1 0 cos θ 1 0 ⎥ 0 ⎥ T1 = ⎢ (11.398) ⎣ 0 −1 0 l1 ⎦ 0 0 0 1 The second link is and RR(90), as z2 is orthogonal to z1 with α 1 = 90 deg and a2 = 0, d2 = 0. ⎡

cos θ 2 ⎢ sin θ 2 1 T2 = ⎢ ⎣ 0 0

0 sin θ 2 0 − cos θ 2 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(11.399)

The second link is and RR(0), as z3 is along z2 with a3 = 0 and d3 = l3 . ⎤ 00 0 0⎥ ⎥ 1 l3 ⎦ 01

(11.400)

⎤ cos θ 1 0 − sin θ 1 0 R1 = ⎣ sin θ 1 0 cos θ 1 ⎦ 0 −1 0

(11.401)

⎡

cos θ 3 − sin θ 3 ⎢ sin θ 3 cos θ 3 2 T3 = ⎢ ⎣ 0 0 0 0 Their associate rotation transformation matrices are: ⎡

⎡

⎤ cos θ 2 0 sin θ 2 1 R2 = ⎣ sin θ 2 0 − cos θ 2 ⎦ 0 1 0 ⎡ ⎤ cos θ 3 − sin θ 3 0 2 R3 = ⎣ sin θ 3 cos θ 3 0 ⎦ 0 0 1 ⎡ ⎤ cos θ 1 cos θ 2 − sin θ 1 cos θ 1 sin θ 2 0 R2 = 0 R1 1 R2 = ⎣ cos θ 2 sin θ 1 cos θ 1 sin θ 1 sin θ 2 ⎦ − sin θ 2 0 cos θ 2 0 R3 = 0 R1 1 R2 2 R3 ⎡ ⎤ cθ 1 cθ 2 cθ 3 − sθ 1 sθ 3 −cθ 3 sθ 1 − cθ 1 cθ 2 sθ 3 cθ 1 sθ 2 = ⎣ cθ 1 sθ 3 + cθ 2 cθ 3 sθ 1 cθ 1 cθ 3 − cθ 2 sθ 1 sθ 3 sθ 1 sθ 2 ⎦ −cθ 3 sθ 2 sθ 2 sθ 3 cθ 2

(11.402)

(11.403)

(11.404)

(11.405)

11.3 Robot Lagrange Dynamics

653

Fig. 11.22 A spherical wrist, its exploded diagram, the coordinate frame of each link, and their mass centers

The angular velocity vectors of links are: ⎡

⎤ 0 ⎣0⎦ 0 ω1 = θ˙ 1

⎡

⎤ 0 ⎣0⎦ 1 ω2 = θ˙ 2

⎤ 0 ⎣0⎦ 2 ω3 = θ˙ 3

⎤ −θ˙ 2 sin θ 1 0 0 ⎣ θ˙ 2 cos θ 1 ⎦ 1 ω 2 = R1 1 ω 2 = 0

⎡

(11.406)

⎡

(11.407)

654

11 Robot Dynamics

⎡

⎤ θ˙ 3 cos θ 1 sin θ 2 1 0 0 ⎣ θ˙ 3 sin θ 1 sin θ 2 ⎦ 2 ω 3 = R1 R2 2 ω 3 = θ˙ 3 cos θ 2 0 ω = ω + ω 0 2 0 1 1 2 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ −θ˙ 2 sin θ 1 0 −θ˙ 2 sin θ 1 = ⎣ 0 ⎦ + ⎣ θ˙ 2 cos θ 1 ⎦ = ⎣ θ˙ 2 cos θ 1 ⎦ θ˙ 1 0 θ˙ 1 0 ω3

= 0 ω2 + 02 ω3 ⎡ ⎤ ⎡ ⎤ −θ˙ 2 sin θ 1 θ˙ 3 cos θ 1 sin θ 2 = ⎣ θ˙ 2 cos θ 1 ⎦ + ⎣ θ˙ 3 sin θ 1 sin θ 2 ⎦ θ˙ 1 θ˙ 3 cos θ 2 ⎡ ⎤ θ˙ 3 cos θ 1 sin θ 2 − θ˙ 2 sin θ 1 = ⎣ θ˙ 2 cos θ 1 + θ˙ 3 sin θ 1 sin θ 2 ⎦ θ˙ 1 + θ˙ 3 cos θ 2

(11.408)

(11.409)

(11.410)

The position vector of the mass centers of link (i) expressed in frame Bi is: ⎡

⎤ 0 1 r1 = ⎣ r1 ⎦ 0

⎤ 0 2 r2 = ⎣ 0 ⎦ r2 ⎡

⎤ 0 3 r3 = ⎣ 0 ⎦ −r3 ⎡

(11.411)

⎡

⎤ 0 0 r1 = 0 R1 1 r1 = ⎣ 0 ⎦ −r1 ⎤ ⎡ r2 cos θ 1 sin θ 2 0 r2 = 0 R1 1 R2 2 r2 = ⎣ r2 sin θ 1 sin θ 2 ⎦ r2 cos θ 2 ⎡ ⎤ −r3 cos θ 1 sin θ 2 0 r3 = 0 R1 1 R2 2 R3 3 r3 = ⎣ −r3 sin θ 1 sin θ 2 ⎦ −r3 cos θ 2

(11.412)

(11.413)

(11.414)

The mass moment matrices of links are assumed to be principal. ⎤ I11 0 0 1 I1 = ⎣ 0 I12 0 ⎦ 0 0 I13 ⎡

⎡

I21 2 I2 = ⎣ 0 0 ⎡ I31 3 I3 = ⎣ 0 0

⎤ 0 0 I22 0 ⎦ 0 I23 ⎤ 0 0 I32 0 ⎦ 0 I33

I1 = 0 R1 1 I1 0 R1T ⎡ ⎤ I11 cos2 θ 1 + I13 sin2 θ 1 (I11 − I13 ) cos θ 1 sin θ 1 0 = ⎣ (I11 − I13 ) cos θ 1 sin θ 1 I13 cos 2 θ 1 + I11 sin2 θ 1 0 ⎦ 0 0 I12 0

(11.415)

(11.416)

(11.417)

(11.418)

11.3 Robot Lagrange Dynamics

655

⎡

I211 I221

⎤ I211 I212 I213 0 I2 = 0 R2 2 I2 0 R2T = ⎣ I221 I222 I223 ⎦ I231 I232 I233   = I21 cos2 θ 2 + I23 sin2 θ 2 cos2 θ 1 + I22 sin2 θ 1   = I21 cos2 θ 2 + I23 sin2 θ 2 − I22 cos θ 1 sin θ 1

(11.419) (11.420) (11.421)

I231 = (I23 − I21 ) cos θ 1 cos θ 2 sin θ 2

(11.422)

  I212 = I21 cos2 θ 2 + I23 sin2 θ 2 − I22 cos θ 1 sin θ 1   I222 = I22 cos2 θ 1 + I21 cos2 θ 2 + I23 sin2 θ 2 sin2 θ 1

(11.423)

I232 = (I23 − I21 ) cos θ 2 sin θ 1 sin θ 2

(11.425)

I213 = (I23 − I21 ) cos θ 1 cos θ 2 sin θ 2

(11.426)

I223 = (I23 − I21 ) cos θ 2 sin θ 1 sin θ 2

(11.427)

I233 = I23 cos2 θ 2 + I21 sin2 θ 2

(11.428)

⎡

0

(11.424)

⎤

I311 I312 I313 I3 = 0 R3 3 I3 0 R3T = ⎣ I321 I322 I323 ⎦ I331 I332 I333

(11.429)

I311 = I31 (sin θ 1 sin θ 3 − cos θ 1 cos θ 2 cos θ 3 )2 + I33 cos2 θ 1 sin2 θ 2 +I32 (cos θ 3 sin θ 1 + cos θ 1 cos θ 2 sin θ 3 )2

(11.430)

I321 = −I31 (cos θ 1 sin θ 3 + cos θ 2 cos θ 3 sin θ 1 ) × (sin θ 1 sin θ 3 − cos θ 1 cos θ 2 cos θ 3 ) −I32 (cos θ 3 sin θ 1 + cos θ 1 cos θ 2 sin θ 3 ) × (cos θ 1 cos θ 3 − cos θ 2 sin θ 1 sin θ 3 ) +I33 cos θ 1 sin θ 1 sin2 θ 2

(11.431)

I331 = I31 (cos θ 3 sin θ 2 ) (sin θ 1 sin θ 3 − cos θ 1 cos θ 2 cos θ 3 ) −I32 (sin θ 2 sin θ 3 ) (cos θ 3 sin θ 1 + cos θ 1 cos θ 2 sin θ 3 ) +I33 cos θ 1 cos θ 2 sin θ 2

(11.432)

I312 = −I31 (cos θ 1 sin θ 3 + cos θ 2 cos θ 3 sin θ 1 ) × (sin θ 1 sin θ 3 − cos θ 1 cos θ 2 cos θ 3 ) −I32 (cos θ 3 sin θ 1 + cos θ 1 cos θ 2 sin θ 3 ) × (cos θ 1 cos θ 3 − cos θ 2 sin θ 1 sin θ 3 ) +I33 cos θ 1 sin θ 1 sin2 θ 2

(11.433)

I322 = I31 (cos θ 1 sin θ 3 + cos θ 2 cos θ 3 sin θ 1 )2 +I32 (cos θ 1 cos θ 3 − cos θ 2 sin θ 1 sin θ 3 )2 +I33 sin2 θ 1 sin2 θ 2

(11.434)

I332 = I32 (sin θ 2 sin θ 3 ) (cos θ 1 cos θ 3 − cos θ 2 sin θ 1 sin θ 3 ) −I31 (cos θ 3 sin θ 2 ) (cos θ 1 sin θ 3 + cos θ 2 cos θ 3 sin θ 1 ) +I33 cos θ 2 sin θ 1 sin θ 2

(11.435)

656

11 Robot Dynamics

I313 = I31 (cos θ 3 sin θ 2 ) (sin θ 1 sin θ 3 − cos θ 1 cos θ 2 cos θ 3 ) −I32 (sin θ 2 sin θ 3 ) (cos θ 3 sin θ 1 + cos θ 1 cos θ 2 sin θ 3 ) +I33 cos θ 1 cos θ 2 sin θ 2

(11.436)

I323 = I32 (sin θ 2 sin θ 3 ) (cos θ 1 cos θ 3 − cos θ 2 sin θ 1 sin θ 3 ) −I31 (cos θ 3 sin θ 2 ) (cos θ 1 sin θ 3 + cos θ 2 cos θ 3 sin θ 1 ) +I33 cos θ 2 sin θ 1 sin θ 2 I333 = I33 cos2 θ 2 + I31 cos2 θ 3 sin 2 θ 2 + I32 sin2 θ 2 sin2 θ 3

(11.437) (11.438)

Velocity of the mass centers are: ⎡ ⎤ 0 1 v1 = 10 ω1 × 1 r1 = 0 R1T 0 ω1 × 1 r1 = ⎣ 0 ⎦ 0 ⎡ ⎤ 0 0 v 1 = 0 R1 1 v 1 = ⎣ 0 ⎦ 0 ⎡ ⎤ r2 θ˙ 2 2 v2 = 20 ω2 × 2 r2 = 2 R0 0 ω2 × 2 r2 = ⎣ r2 θ˙ 1 sin θ 2 ⎦ 0 ⎡ ⎤ r2 θ˙ 2 cos θ 1 cos θ 2 − r2 θ˙ 1 sin θ 1 sin θ 2 0 v2 = 0 R2 2 v2 = ⎣ r2 θ˙ 1 cos θ 1 sin θ 2 + r2 θ˙ 2 cos θ 2 sin θ 1 ⎦ −r2 θ˙ 2 sin θ 2 3

0

v3 = 30 ω3 × 3 r3 = 3 R0 0 ω3 × 3 r3 ⎤ ⎡ −r3 θ˙ 2 cos θ 3 − r3 θ˙ 1 sin θ 2 sin θ 3 = ⎣ r3 θ˙ 2 sin θ 3 − r3 θ˙ 1 cos θ 3 sin θ 2 ⎦ 0

v 3 = 0 R3 3 v 3 ⎡ ⎤ r3 θ˙ 1 sin θ 1 sin θ 2 − r3 θ˙ 2 cos θ 1 cos θ 2 = ⎣ −r3 θ˙ 1 cos θ 1 sin θ 2 − r3 θ˙ 2 cos θ 2 sin θ 1 ⎦ r3 θ˙ 2 sin θ 2

(11.439)

(11.440)

(11.441)

(11.442)

(11.443)

(11.444)

The kinetic energy of the link (1) will be: K1 =

1 0 T 0 1 1 2 m v1 v1 + 0 ωT1 0 I1 0 ω1 = θ˙ 1 I12 2 2 2

(11.445)

The kinetic energy of the link (2) will be: 1 0 T 0 1 m v2 v2 + 0 ωT2 0 I2 0 ω2 2 2  1 1 2 2 2 = mr22 θ˙ 1 + 2θ˙ 2 − θ˙ 1 cos 2θ 2 + I22 θ˙ 22 4 2 1 1 2 2 + (I21 + I23 ) θ˙ 1 + (I23 − I21 ) θ˙ 1 cos 2θ 2 4 4

K2 =

(11.446)

11.3 Robot Lagrange Dynamics

657

The kinetic energy of the link (3) will be: 1 0 T 0 1 m v3 v3 + 0 ωT3 0 I3 0 ω3 2 2  1 2 2 2 = mr32 θ˙ 1 + 2θ˙ 2 − θ˙ 1 cos 2θ 2 4 1 1 2 2 + I31 θ˙ 2 sin2 θ 3 + I33 θ˙ 3 2 2 1  2 2 + I32 θ˙ 2 cos 2 θ 3 + θ˙ 1 sin2 θ 2 2  1 ˙ ˙ + θ 3 + θ 1 cos θ 2 I33 θ˙ 1 cos θ 2 2  1 + (I32 − I31 ) θ˙ 2 cos θ 3 sin θ 3 − θ˙ 1 sin θ 2 θ˙ 1 sin θ 2 2

K3 =

(11.447)

Therefore, the total kinetic energy K of the spherical wrist is the sum of kinetic energy of the individual links. K = K1 + K2 + K3

(11.448)

Example 373 General form of Lagrange equations of robots. This is to understand what would be general form of equations of motion if the robot is not indicated, so that would be common equations for all robots, as well as other dynamic systems. Consider a robot manipulator of n DOF . The kinetic energy function K of the manipulator may always be expressed in a quadratic form of the generalized speed vector q˙ = dtd q of the generalized coordinates vector q.   1 K q, q˙ = q˙ T M (q) q˙ 2

(11.449)

The symmetric and positive definite M (q) is the inertia matrix of dimension n × n. The potential energy P (q) does not have a specific general form but it always depends on the generalized coordinates  vector q. Hence the general form of the Lagrangian L q, q˙ of the robot will be:   1 T L q, q˙ = P q M (q) q˙ − P (q) 2

(11.450)

The Lagrange equations of motion will be: ∂L = M (q) q˙ ∂ q˙  d ∂L = M (q) q¨ + M˙ (q) q˙ dt ∂ q˙ ∂L 1 ∂  T ∂ = q˙ M (q) q˙ − P (q) ∂q 2 ∂q ∂q 1 ∂  T ∂ M (q) q¨ + M˙ (q) q˙ − q˙ M (q) q˙ + P (q) = Q 2 ∂q ∂q

(11.451) (11.452) (11.453)

(11.454)

It can also be rewritten in a more common form, ˙ q˙ + G (q) = Q M (q) q¨ + C (q, q)

(11.455)

658

11 Robot Dynamics

where

∂ P (q) ∂q 1 ∂  T ˙ q˙ = M˙ (q) q˙ − C (q, q) q˙ M (q) q˙ 2 ∂q G (q) =

(11.456) (11.457)

The equations in state-space form will be: # " # " q q˙   = ˙ q˙ − G (q) q˙ M −1 (q) Q − C (q, q)

d dt

(11.458)

i Example 374  Christoffel operator. Christoffel symbol j,k is a short notation for particular sum of partial derivatives. It happens frequently in 3D kinematics and dynamics. Here is an example to rewrite H with Christoffel symbol. i The symbol j,k is one version of the Christoffel symbol or Christoffel operator that helps to shorten H matrix.

i j,k =

1 2



∂Dij ∂Dj k ∂Dik + − ∂qk ∂qj ∂qi

(11.459)

The velocity coupling vector Hij k is a Christoffel symbol. Hij k =

n  n   ∂Dij j =1 k=1

=

∂qk

1 ∂Dj k − 2 ∂qi



n  n ∂Dj k 1   ∂Dij ∂Dik + − 2 j =1 k=1 ∂qk ∂qj ∂qi

(11.460)

Using Christoffel symbol, we can write the equations of motion of a robot as: n 

Dij (q) q¨j +

j =1

n n  

 ij,k q˙k q˙m + Gi = Qi

(11.461)

j =1 k=1

Elwin Bruno Christoffel (1829–1900) is a German mathematician who introduced the Christoffel symbols and fundamental concepts of differential geometry. Example 375  No gravity and no external force. Assume a robot is working in space where there is no gravity and there is no external force applied on the end-effector of the robot. In these conditions, the Lagrangian of the manipulator simplifies to 1  Dij q˙i q˙j 2 i=1 j =1 n

L=

n

(11.462)

and the equations of motion reduce to d dt



∂L ∂ q˙i



  ∂L j − = Dij q¨i + l,m q˙l q˙ m ∂qi i=1 j =1 n

n

(11.463)

11.4  Lagrange Equations and Link Transformation Matrices

11.4

659

 Lagrange Equations and Link Transformation Matrices

The matrix form of the equations of motion for a robot, based on the Lagrange equations, is: ˙ + G(q) = Q D(q)R q + H(q, q)

(11.464)

which can also be written in a summation form. n 

Dij (q) q¨j +

n n  

j =1

Hikm q˙k q˙m + Gi = Qi

(11.465)

j =1 k=1

The matrix D(q) is an n × n inertial-type symmetric matrix. 

n 

∂ 0 Tr r ¯ ∂ 0 Tr T Dij = tr Ir ∂qi ∂qj r=max i,j

(11.466)

The Hikm is the velocity coupling term, Hij k



n 

∂ 2 0 Tr r ¯ ∂ 0 Tr T Ir = tr ∂qj ∂qk ∂qi r=max i,j,k

and Gi is the gravitational vector. Gi = −

n 

mr g T

r=i

(11.467)

∂ 0 Tr r rr ∂qi

(11.468)

Proof Position vector of a point P of the link (i) at i rP in the body coordinate Bi can be transformed to the base frame. 0

rP = 0 Ti i rP

(11.469)

Therefore, its velocity and square of velocity in the base frame are: 0

r˙ P =

i  ∂ 0 Ti

∂qj

j =1

0 2 r˙ P

q˙j i rP

(11.470)

 = 0 r˙ P · 0 r˙ P = tr 0 r˙ P 0 r˙ TP ⎛ ⎞ # i i " 0 0   ∂ T ∂ T i i = tr ⎝ q˙j i rP q˙k i rP T ⎠ ∂q ∂q j k j =1 k=1 ⎛

= tr ⎝

i i   ∂ 0 Ti j =1 k=1

∂qj

" i

rP i rTP

0

∂ Ti ∂qk

#T

⎞

q˙j q˙ k ⎠

(11.471)

660

11 Robot Dynamics

The kinetic energy of point P having a small mass dm is then equal to: ⎛ ⎞ T i i 1 ⎝  ∂ 0 Ti i i T ∂ 0 Ti dKP = tr rP rP q˙j q˙k ⎠ dm 2 ∂qj ∂qk j =1 k=1

⎛ ⎞ i i 0 T  1 ⎝  ∂ 0 Ti i ∂ T i = tr rP dm i rTP q˙ j q˙k ⎠ 2 ∂q ∂q j k j =1 k=1

(11.472)

and hence, the kinetic energy of the link (i) would be: Ki =

dKP Bi

⎛ ⎞  0 T i i 1 ⎝  ∂ 0 Ti ∂ T i i = tr rP i rTP dm q˙j q˙k ⎠ 2 ∂q ∂q j k B i j =1 k=1

(11.473)

The integral in Eq. (11.473) is the pseudo-inertia matrix for the link (i). i

I¯i =

i Bi

rP i rTP dm

(11.474)

Therefore, kinetic energy of the link (i) becomes: ⎞ ⎛ T i i 1 ⎝   ∂ 0 Ti i ¯ ∂ 0 Ti Ki = tr q˙j q˙k ⎠ Ii 2 ∂q ∂q j k j =1 k=1

(11.475)

The kinetic energy of a robot having n links is a summation of the kinetic energies of each link. ⎛ ⎞ i n i  0 0 T   1 ∂ Ti i ¯ ∂ Ti ⎝ K= Ii Ki = tr q˙j q˙k ⎠ 2 ∂qj ∂qk i=1 i=1 j =1 k=1 n 

(11.476)

We may also add the kinetic energy due to the actuating motors Ka that are installed at joints of the robot, ⎧ / 1 ⎪ ⎨ ni=1 Ii q˙i2 if joint i is R Ka = / 12 ⎪ ⎩ ni=1 mi q˙i2 if joint i is P 2

(11.477)

where Ii is the mass moment of the rotary actuator at joint i, and mi is the mass of the translatory actuator. However, we may assume that the motors are concentrated masses at joints and add the mass of motor at joint i to the mass of link (i − 1) and adjust the inertial parameters of the link. The motor at joint i will drive the link (i). For the potential energy we assume the gravity is the only source of potential energy. Therefore, the potential energy of the link (i) with respect to the base coordinate frame is: Vi = −mi 0 g · 0 ri = −mi 0 gT 0 Ti i ri

(11.478)

 T where 0 g = gx gy gz 0 is the gravitational acceleration, usually in the direction −z0 , and 0 ri is the position vector of C of link (i) in the base frame. The potential energy of the whole robot is then equal to: V =

n  i=1

Vi = −

n  i=1

mi gT 0 Ti i ri

(11.479)

11.4  Lagrange Equations and Link Transformation Matrices

661

The Lagrangian of a robot is found by substituting (11.476) and (11.479) in the Lagrange equation (11.214).   T n i i 1  ∂ 0 Ti i ¯ ∂ 0 Ti L = K −V = tr Ii q˙j q˙k 2 i=1 j =1 k=1 ∂qj ∂qk +

n 

mi 0 gT 0 Ti i ri

(11.480)

i=1

The dynamic equations of motion of a robot can now be found by applying the Lagrange equations (11.215) to Eq. (11.480). We develop the equations of motion term by term. Differentiating the L with respect to q˙r is:  0 i n ∂L 1  ∂ Ti i ¯ ∂ 0 Ti T = tr Ii q˙k ∂ q˙r 2 i=1 k=1 ∂qr ∂qk   T n i 1  ∂ 0 Ti i ¯ ∂ 0 Ti + tr Ii q˙j 2 i=1 j =1 ∂qj ∂qr =

i=r

because

and



n  i 

∂ 0 Ti i ¯ ∂ 0 Ti tr Ii ∂qj ∂qr j =1

T



q˙j

(11.481)

∂ 0 Ti = 0 for r > i ∂qr 

∂ 0 Ti i ¯ ∂ 0 Ti Ii tr ∂qj ∂qk

T



(11.482)



∂ 0 Ti i ¯ ∂ 0 Ti = tr Ii ∂qk ∂qj

T

 (11.483)

Time derivative of ∂ L/∂ q˙r is:   T n  i  ∂ 0 Ti i ¯ ∂ 0 Ti d ∂L = tr Ii q¨j dt ∂ q˙r ∂qj ∂qr i=r j =1 +

i n  i   i=r j =1

+

∂ 2 0 Ti i ¯ ∂ 0 Ti tr Ii ∂qj ∂qk ∂qr k=1

i n  i   i=r j =1





∂ 2 0 Ti i ¯ ∂ 0 Ti tr Ii ∂qr ∂qk ∂qj k=1

T

 q˙j q˙k

T

 q˙j q˙k

(11.484)

The last term of the Lagrange equation is:   T i n i ∂L 1  ∂ 2 0 Ti i ¯ ∂ 0 Ti = tr Ii q˙j q˙k ∂qr 2 i=r j =1 k=1 ∂qj ∂qr ∂qk

  T i n i 1  ∂ 2 0 Ti i ¯ ∂ 0 Ti + tr Ii q˙j q˙k 2 i=r j =1 k=1 ∂qk ∂qr ∂qj +

n  i=r

mi g T

∂ 0 Ti i ri ∂qr

(11.485)

662

11 Robot Dynamics

which can be simplified to:   T i n  i   ∂L ∂ 2 0 Ti i ¯ ∂ 0 Ti Ii = tr q˙j q˙k ∂qr ∂qr ∂qj ∂qk i=r j =1 k=1 +

n 

mi g T

i=r

∂ 0 Ti i ri ∂qr

(11.486)

Interestingly, the third term in Eq. (11.484) is equal to the first term in (11.486). So, substituting these equations in the Lagrange equation can be simplified. d dt



∂L ∂ q˙i



  j T n   ∂ 0 Tj j ¯ ∂ 0 Tj ∂L Ij − = tr q¨k ∂qi ∂qk ∂qi j =i k=1 +

j j n   

∂ 2 0 Tj j ¯ ∂ 0 Tj tr Ij ∂qk ∂qm ∂qi k=1 m=1

j =i

−

n 



mj g T

j =i

T

∂ 0 Tj j rj ∂qi

 q˙k q˙m

(11.487)

Finally, the equations of motion for an n link robot are: Qi =



j n   j =i

+

∂ 0 Tj j ¯ ∂ 0 Tj tr Ij ∂qk ∂qi k=1

−

n 

 q¨k



j j n    j =i

T

T

∂ 2 0 Tj j ¯ ∂ 0 Tj tr Ij ∂qk ∂qm ∂qi k=1 m=1

mj g T

j =i

 q˙k q˙m

∂ 0 Tj j rj . ∂qi

(11.488)

The equations of motion can be written in a more concise form Qi =

n 

Dij q¨j +

n  n 

j =1

where Dij =



n 

tr

r=max i,j

Hij k

Hij k q˙j q˙k + Gi

(11.489)

j =1 k=1

n 

∂ 0 Tr r ¯ ∂ 0 Tr T Ir ∂qj ∂qi





∂ 2 0 Tr r ¯ ∂ 0 Tr Ir = tr ∂qj ∂qk ∂qi r=max i,j,k Gi = −

n  r=i

mr g T

∂ 0 Tr r rr ∂qi

(11.490) T

 (11.491)

(11.492) 

11.4  Lagrange Equations and Link Transformation Matrices

663

Fig. 11.23 A 2R planar manipulator with massive links

Example 376 2R manipulator with massive links. It is to derive matrix form of equations of motion by employing homogenous transformation matrices for 2R manipulator. A 2R planar manipulator with massive links is shown in Fig. 11.23. We assume the mass center C of each link is in the middle of the link and the motors at each joint is massless. The links’ homogenous transformation matrices are: ⎡

cos θ 1 − sin θ 1 ⎢ sin θ 1 cos θ 1 0 T1 = ⎢ ⎣ 0 0 0 0 ⎡

cos θ 2 − sin θ 2 ⎢ sin θ 2 cos θ 2 1 T2 = ⎢ ⎣ 0 0 0 0 0

⎤ 0 l1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥ ⎦ 1 0 0 1

(11.493)

⎤ 0 l2 cos θ 2 0 l2 sin θ 2 ⎥ ⎥ ⎦ 1 0 0 1

(11.494)

T2 = 0 T1 1 T2 ⎡ ⎤ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) ⎢ s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) 0 l1 sθ 1 + l2 s (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 1 0 0 0 0 1

(11.495)

Employing the velocity coefficient matrix R for revolute joints, we can write: ∂ 0 T1 = R 0 T1 ∂θ 1 ⎡ ⎤⎡ ⎤ 0 −1 0 0 cos θ 1 − sin θ 1 0 l1 cos θ 1 ⎢ 1 0 0 0 ⎥ ⎢ sin θ 1 cos θ 1 0 l1 sin θ 1 ⎥ ⎥⎢ ⎥ =⎢ ⎣0 0 0 0⎦⎣ 0 ⎦ 0 1 0 0 0 00 0 0 0 1 ⎡ ⎤ − sin θ 1 − cos θ 1 0 −l1 sin θ 1 ⎢ cos θ 1 − sin θ 1 0 l1 cos θ 1 ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 0 0 0 0 0 0

(11.496)

664

11 Robot Dynamics

∂ 0 T2 = R 0 T2 ∂θ 1 ⎡ ⎤⎡ ⎤ 0 −1 0 0 cθ 12 −sθ 12 0 l1 cθ 1 + l2 cθ 12 ⎢ 1 0 0 0 ⎥ ⎢ sθ 12 cθ 12 0 l1 sθ 1 + l2 sθ 12 ⎥ ⎥⎢ ⎥ =⎢ ⎣0 0 0 0⎦⎣ 0 ⎦ 0 1 0 0 0 00 0 0 0 1 ⎤ ⎡ −s (θ 1 + θ 2 ) −c (θ 1 + θ 2 ) 0 −l1 sθ 1 − l2 s (θ 1 + θ 2 ) ⎢ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l1 cθ 1 + l2 c (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0 0 0 0 0 0 0

∂ 0 T2 = 0 T1 R 1 T2 ∂θ 2 ⎤ ⎡ −s (θ 1 + θ 2 ) −c (θ 1 + θ 2 ) 0 −l2 s (θ 1 + θ 2 ) ⎢ c (θ 1 + θ 2 ) −s (θ 1 + θ 2 ) 0 l2 c (θ 1 + θ 2 ) ⎥ ⎥ =⎢ ⎦ ⎣ 0 0 0 0 0 0 0 0

(11.497)

(11.498)

Assuming all the product of inertias are zero, we have: ⎡

⎤ 0 0 − 12 m1 l1 ⎢ ⎥ 0 00 0 1¯ ⎥ I1 = ⎢ ⎣ ⎦ 0 00 0 1 − 2 m 1 l1 0 0 m 1

(11.499)

⎤ 0 0 − 12 m2 l2 ⎥ ⎢ 0 00 0 2¯ ⎥ I2 = ⎢ ⎣ ⎦ 0 00 0 1 − 2 m 2 l2 0 0 m 2

(11.500)

⎡

1 m l2 3 1 1

1 m l2 3 2 2

Using inertia and derivative of transformation matrices we can calculate the inertial-type symmetric matrix D(q). 

D11

   T T ∂ 0 T1 1 ¯ ∂ 0 T1 ∂ 0 T2 2 ¯ ∂ 0 T2 = tr I1 + tr I2 ∂q1 ∂q1 ∂q1 ∂q1  1 1 = m1 l12 + m2 l12 + l22 + m2 l1 l2 cos θ 2 3 3 

D12 = D21



1 m2 l22 + m2 l12 + m2 l1 l2 cos θ 2 3   T ∂ 0 T2 2 ¯ ∂ 0 T2 1 = tr I2 = l22 m2 ∂q2 ∂q2 3 =

D22

∂ 0 T2 2 ¯ ∂ 0 T2 T = tr I2 ∂q1 ∂q1

(11.501)

(11.502)

(11.503)

11.4  Lagrange Equations and Link Transformation Matrices

665

˙ are calculated as below The coupling terms H(q, q) H1 =

2 2  

H1km q˙k q˙m

k=1 m=1

= H111 q˙1 q˙1 + H112 q˙1 q˙2 + H121 q˙ 2 q˙1 + H122 q˙2 q˙2 H2 =

2 2  

(11.504)

H2km q˙k q˙m

k=1 m=1

= H211 q˙1 q˙1 + H212 q˙1 q˙2 + H221 q˙ 2 q˙1 + H222 q˙2 q˙2 where Hij k These calculations yields:

 H=

n 



∂ 2 0 Tr r ¯ ∂ 0 Tr = tr Ir ∂qj ∂qk ∂qi r=max i,j,k

T

(11.505)



2 − 12 m2 l1 l2 θ˙ 2 sin θ 2 − m2 l1 l2 θ˙ 1 θ˙ 2 sin θ 2 2 1 m l l θ˙ sin θ 2 2 2 1 2 1

(11.506)  (11.507)

The last terms of equations of motion are the gravitational vector G(q). ∂ 0 T1 1 ∂ 0 T2 2 r1 − m2 gT r2 ∂q1 ∂q1 ⎡ ⎤T ⎡ ⎤ ⎡ l1 ⎤ 0 − sin θ 1 − cos θ 1 0 −l1 sin θ 1 −2 ⎢ −g ⎥ ⎢ cos θ 1 − sin θ 1 0 l1 cos θ 1 ⎥ ⎢ 0 ⎥ ⎥ ⎥ ⎢ ⎥⎢ = −m1 ⎢ ⎣ 0 ⎦ ⎣ ⎦⎣ 0 ⎦ 0 0 0 0 0 0 0 0 0 1 ⎤ ⎡ l1 ⎡ ⎤T ⎡ 0 −sθ 12 −cθ 12 0 −l1 sθ 1 − l2 sθ 12 −2 ⎢ −g ⎥ ⎢ cθ 12 −sθ 12 0 l1 cθ 1 + l2 cθ 12 ⎥ ⎢ 0 ⎥⎢ ⎥ ⎢ −m2 ⎢ ⎦⎣ 0 ⎣ 0 ⎦ ⎣ 0 0 0 0 0 0 0 0 0 1 G1 = −m1 gT

=

1 1 m1 gl1 cos θ 1 + m2 gl1 cos (θ 1 + θ 2 ) + m2 gl1 cos θ 1 2 2 ∂ 0 T2 2 r2 ∂q2 ⎡ ⎤T ⎡ 0 −sθ 12 −cθ 12 ⎢ −g ⎥ ⎢ cθ 12 −sθ 12 ⎥ ⎢ = −m2 ⎢ ⎣ 0 ⎦ ⎣ 0 0 0 0 0

⎤ ⎥ ⎥ ⎦

(11.508)

G2 = −m2 gT

=

1 m2 gl2 cos (θ 1 + θ 2 ) . 2

⎤ ⎡ l1 0 −l2 sθ 12 −2 ⎢ 0 0 l2 cθ 12 ⎥ ⎥⎢ 0 0 ⎦⎣ 0 0 0 1

⎤ ⎥ ⎥ ⎦

(11.509)

666

11 Robot Dynamics

Fig. 11.24 2R manipulator mounted on a ceiling

Finally the equations of motion for the 2R planar manipulator are found. "

Q1 Q2

# =

   #" # m1 l12 + m2  l12 + 13 l22 + l1 l2 cθ 2 m2 l12 + 13 l22 + l1 l2 cθ 2 θ¨ 1 1 2 1 2 2 θ¨ 2 l1 + 3 l2 m2 + m2 l1 l2 cθ 2 l m 3 2 2   2 − 21 m2 l1 l2 θ˙ 2 sin θ 2 − m2 l1 l2 θ˙ 1 θ˙ 2 sin θ 2 + 2 1 m l l θ˙ sin θ 2 2 2 1 2 1 "1 # m1 gl1 cos θ 1 + 12 m2 gl1 cos (θ 1 + θ 2 ) + m2 gl1 cos θ 1 2 + 1 m gl cos (θ 1 + θ 2 ) 2 2 2 "1 3

(11.510)

Example 377 2R manipulator mounted on ceiling. Here is to derive equations of motion of a ceiling attached ideal 2R manipulator by link transformation matrices method. Attaching robots to ceiling is very common in industry. Figure 11.24 depicts an ideal 2R planar manipulator mounted on a ceiling. Ceiling mounting is an applied method in robotic operated assembly lines. The Lagrangian of the manipulator is 1 2 m1 l12 θ˙ 1 2  2   1 2 2 + m2 l1 θ˙ 1 + l22 θ˙ 1 + θ˙ 2 + 2l1 l2 θ˙ 1 θ˙ 1 + θ˙ 2 cos θ 2 2 +m1 gl1 cos θ 1 + m2 g (l1 cos θ 1 + l2 cos (θ 1 + θ 2 ))

L = K −V =

(11.511)

which leads to the following equations of motion:   Q1 = (m1 + m2 ) l12 + m2 l22 + 2m2 l1 l2 cos θ 2 θ¨ 1 +m2 l2 (l2 + l1 cos θ 2 ) θ¨ 2 2 −2m2 l1 l2 sin θ 2 θ˙ 1 θ˙ 2 − m2 l1 l2 sin θ 2 θ˙ 2

+ (m1 + m2 ) gl1 sin θ 1 + m2 gl2 sin (θ 1 + θ 2 )

(11.512)

11.5 Robot Statics

667

Q2 = m2 l2 (l2 + l1 cos θ 2 ) θ¨ 1 + m2 l22 θ¨ 2   −2m2 l1 l2 sin θ 2 θ˙ 1 θ˙ 1 + θ˙ 2 − m2 gl2 sin (θ 1 + θ 2 ) .

(11.513)

The equations of motion can be rearranged to Q1 = D11 θ¨ 1 + D12 θ¨ 2 + H111 θ˙ 21 + H122 θ˙ 2 2

+H112 θ˙ 1 θ˙ 2 + H121 θ˙ 2 θ˙ 1 + G1

(11.514)

2 Q2 = D21 θ¨ 1 + D22 θ¨ 2 + H211 θ˙ 21 + H222 θ˙ 2

+H212 θ˙ 1 θ˙ 2 + H221 θ˙ 2 θ˙ 1 + G2

(11.515)

D11 = (m1 + m2 ) l12 + m2 l22 + 2m2 l1 l2 cos θ 2

(11.516)

D12 = m2 l2 (l2 + l1 cos θ 2 )

(11.517)

D21 = D12 = m2 l2 (l2 + l1 cos θ 2 )

(11.518)

D22 = m2 l22

(11.519)

where

11.5

H111 = 0

(11.520)

H122 = −m2 l1 l2 sin θ 2

(11.521)

H211 = −m2 l1 l2 sin θ 2

(11.522)

H222 = 0

(11.523)

H112 = H121 = −m2 l1 l2 sin θ 2

(11.524)

H212 = H221 = −m2 l1 l2 sin θ 2

(11.525)

G1 = (m1 + m2 ) gl1 sin θ 1 + m2 gl2 sin (θ 1 + θ 2 )

(11.526)

G2 = m2 gl2 sin (θ 1 + θ 2 )

(11.527)

Robot Statics

At the beginning and at the end of a rest-to-rest mission, a robot must keep its specified configurations. To hold the position and orientation, the actuators must apply some required forces to balance the external and gravity loads applied to the robot. Calculating the required actuators’ force to hold a robot in a specific configuration is called robot statics analysis. In a static condition, the globally expressed Newton–Euler equations for the link (i) can be written in a recursive form, 0 0

Fi−1 = 0 Fi −



Mi−1 = 0 Mi −



0

(11.528)

Fei

0

Mei +

0 i−1 di

× 0 Fi

(11.529)

where 0 i−1 di

= 0 di − 0 di−1

(11.530)

Therefore, we are able to calculate the action force system (Fi−1 , Mi−1 ) when the reaction force system (−Fi , −Mi ) is given. The position vectors and force systems on link (i) are shown in Fig. 11.25.

668

11 Robot Dynamics

Fig. 11.25 Position vectors and force system on link (i)

Proof In a static condition, the Newton–Euler equations of motion (11.1) and (11.2) for the link (i) reduce to force and moment balance equations.

0

Mi−1 − 0 Mi +



0

Fi−1 − 0 Fi +



Fei = 0

(11.531)

Mei + 0 ni × 0 Fi−1 − 0 mi × 0 Fi = 0

(11.532)

0

0

These equations can be rearranged into a backward recursive form. 0 0

Fi−1 = 0 Fi −



Mi−1 = 0 Mi −

0



(11.533)

Fei

0

Mei − 0 ni × 0 Fi−1 + 0 mi × 0 Fi

(11.534)

However, we may also transform the Euler equation from Ci to Oi−1 and find Eq. (11.529). Practically, we measure the position of mass center ri and the relative position of Bi and Bi−1 in the coordinate frame Bi attached to the link (i). Hence, we must transform i ri and i−1i di to the base frame. 0

ri = 0 Ti i ri

(11.535)

= 0 Ti

(11.536)

0 i−1 di

i i−1 di

The external load is usually the gravitational force mi g.  

0 0

Fei = mi 0 g

Mei = 0 ri × mi 0 g

(11.537) (11.538)

Using the DH parameters, we may express the relative position vector i−1i di . ⎡

⎤ ai ⎢ di sin α i ⎥ i ⎢ ⎥ i−1 di = ⎣ di cos α i ⎦ 1

(11.539)

11.5 Robot Statics

669

The backward recursive equations (11.528) and (11.529) allow us to start with a known force system (Fn , Mn ) at Bn , applied from the end-effector to the environment, and calculate the force system at Bn−1 . 0 0

Fn−1 = 0 Fn −



Mn−1 = 0 Mn −

0



(11.540)

Fen

0

Men +

0 n−1 dn

× 0 Fn (11.541)

Following the same procedure and calculating force system at proximal end by having the force system at distal end of each link ends up to the force system at the base. In this procedure, the force system applied by the end-effector to the environment is assumed to be known. It is also possible to rearrange the static equations (11.531) and (11.532) into a forward recursive form. 0 0

Fi = 0 Fi−1 +



Mi = 0 Mi−1 +

0



(11.542)

Fei

0

Mei + 0 ni × 0 Fi−1 − 0 mi × 0 Fi (11.543)

Transforming the Euler equation from Ci to Oi simplifies the forward recursive equations into the more practical equations. 0 0

Fi = 0 Fi−1 +



Mi = 0 Mi−1 +

0



(11.544)

Fei

0

Mei −

0 i−1 di

× 0 Fi−1

(11.545)

Using the forward recursive equations (11.544) and (11.545) we can start with a known force system (F0 , M0 ) at B0 , applied from the base to the link (1), and calculate the force system at B1 . 0 0

F1 = 0 F0 +



M1 = 0 M0 +



0

(11.546)

Fe1

0

Me1 − 0 d1 × 0 F0

(11.547)

Following this procedure and calculating force system at the distal end by having the force system at the proximal end of each link ends up at the force system applied to the environment by the end-effector. In this procedure, the force system applied by the base actuators to the first link is assumed to be known.  Example 378 Statics of a 4R planar manipulator. Calculating static torques at four joint motors of a 4R planar manipulator is a practical example of how to analyze static configuration of robots. Figure 11.26 illustrates a 4R planar manipulator with the DH coordinate frames set up for each link. Assume the endeffector force system applied to the environment is known. ⎡

⎤ Fx 4 F4 = ⎣ Fy ⎦ 0

⎡

⎤ 0 4 M4 = ⎣ 0 ⎦ Mz

(11.548)

In addition, we assume the links to be uniform such that their C are located at the midpoint of each link, and the gravitational acceleration is: g = −g jˆ0 (11.549) The manipulator consists of four RR(0) links, therefore their transformation matrices i−1 Ti are of class (5.32) that because di = 0 and ai = li , simplifies to:

670

11 Robot Dynamics

Fig. 11.26 A 4R planar manipulator

⎡

cos θ i − sin θ i ⎢ sin θ i cos θ i i−1 Ti = ⎢ ⎣ 0 0 0 0

⎤ 0 li cos θ i 0 li sin θ i ⎥ ⎥ 1 0 ⎦ 0 1

(11.550)

The C position vectors i ri and the relative position vectors i−10 di are: ⎤ li /2 ⎢ 0 ⎥ i ⎥ ri = ⎢ ⎣ 0 ⎦ 0 ⎡

⎡ ⎤ li ⎢0⎥ i ⎢ ⎥ i−1 di = ⎣ ⎦ 0 0

(11.551)

and therefore, 0

ri = 0 Ti i ri

0 i−1 di

= 0 Ti

i i−1 di

(11.552)

where 0

Ti = 0 T1 · · ·

i−1

Ti

(11.553)

The static force at joints 3, 2, and 1 are: 0

0

 0 F3 = 0 F4 − Fe4 = 0 F4 + m4 g 0 jˆ0 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 Fx Fx ⎢ 1 ⎥ ⎢ Fy + m4 g ⎥ ⎢ Fy ⎥ ⎥ ⎢ ⎥ ⎢ ⎥ =⎢ ⎣ 0 ⎦ + m4 g ⎣ 0 ⎦ = ⎣ ⎦ 0 0 0 0

F2 = 0 F3 − m3 g 0 jˆ0 ⎤ ⎡ Fx ⎢ Fy + m4 g ⎥ ⎥ + m3 g =⎢ ⎣ ⎦ 0 0

⎡ ⎤ ⎡ ⎤ 0 Fx ⎢ 1 ⎥ ⎢ Fy + (m3 + m4 ) g ⎥ ⎢ ⎥=⎢ ⎥ ⎣0⎦ ⎣ ⎦ 0 0 0

(11.554)

(11.555)

11.5 Robot Statics

671 0

0

F1 = 0 F2 − m2 g 0 jˆ0 ⎡ ⎤ Fx ⎢ Fy + (m3 + m4 ) g ⎥ ⎥ + m2 g =⎢ ⎣ ⎦ 0 0 ⎡ ⎤ Fx ⎢ Fy + g (m2 + m3 + m4 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0

⎡ ⎤ 0 ⎢1⎥ ⎢ ⎥ ⎣0⎦ 0

F0 = 0 F1 − m1 g 0 jˆ0 ⎡ ⎤ Fx ⎢ Fy + g (m2 + m3 + m4 ) ⎥ ⎥ + m1 g =⎢ ⎣ ⎦ 0 0 ⎡ ⎤ Fx ⎢ Fy + g (m1 + m2 + m3 + m4 ) ⎥ ⎥ =⎢ ⎣ ⎦ 0 0

(11.556)

⎡ ⎤ 0 ⎢1⎥ ⎢ ⎥ ⎣0⎦ 0

(11.557)

The static moment at joints 3, 2, and 1 are: 0

M3 = 0 M4 −



0

Mei + 03 d4 × 0 F4

= 0 M4 + 0 r4 × m4 g 0 jˆ0 + 03 d4 × 0 F4 = 0 M4 + 0 r4 × m4 g 0 jˆ0 + 03 d4 × 0 F4     = 0 M4 + m4 g 0 T4 4 r4 × 0 jˆ0 + 0 T4 43 d4 × 0 F4 ⎤ ⎡ 0 ⎢ 0 ⎥ ⎥ =⎢ ⎣ M3z ⎦ 0 1 M3z = Mz + l4 Fy cos θ 1234 − l4 Fx sin θ 1234 + gl4 m4 cos θ 1234 2 0 3  0 3  0 0 0 0 M2 = M3 + m3 g T3 r3 × jˆ0 + T3 2 d3 × F3 ⎡ ⎤ 0 ⎢ 0 ⎥ ⎥ =⎢ ⎣ M2z ⎦ 0 1 M2z = Mz + l4 Fy cos θ 1234 − l4 Fx sin θ 1234 + gl4 m4 cos θ 1234 2 1 + gl3 m3 cos θ 123 − l3 Fx sin θ 123 2   +l3 Fy + gm4 cos θ 123

(11.558)

(11.559)

(11.560)

(11.561)

672

11 Robot Dynamics 0

M1 = 0 M2 + m 2 g ⎡ ⎤ 0 ⎢ 0 ⎥ ⎥ =⎢ ⎣ M1z ⎦ 0

0

   T2 2 r2 × 0 jˆ0 + 0 T2 21 d2 × 0 F2

(11.562)

1 M1z = Mz + l4 Fy cos θ 1234 − l4 Fx sin θ 1234 + gl4 m4 cos θ 1234 2   1 + gl3 m3 cos θ 123 − l3 Fx sin θ 123 + l3 cos θ 123 Fy + gm4 2 1 + gl2 m2 cos θ 12 − l2 Fx sin θ 12 2   +l2 cos θ 12 Fy + g (m3 + m4 ) 0

M0 = 0 M1 + m 1 g ⎡ ⎤ 0 ⎢ 0 ⎥ ⎥ =⎢ ⎣ M0z ⎦ 0

0

(11.563)

   T1 1 r1 × 0 jˆ0 + 0 T1 10 d1 × 0 F1

(11.564)

1 M0z = Mz + l4 Fy cos θ 1234 − l4 Fx sin θ 1234 + gl4 m4 cos θ 1234 2   1 + gl3 m3 cos θ 123 − l3 Fx sin θ 123 + l3 cos θ 123 Fy + gm4 2   1 −l2 Fx sin θ 12 + gl2 m2 cos θ 12 + l2 cos θ 12 Fy + g (m3 + m4 ) 2 1 + gl1 m1 cos θ 1 − l1 Fx sin θ 1 2   +l1 cos θ 1 Fy + g (m2 + m3 + m4 )

(11.565)

θ 1234 = θ 1 + θ 2 + θ 3 + θ 4

(11.566)

where

θ 123 = θ 1 + θ 2 + θ 3

(11.567)

θ 12 = θ 1 + θ 2

(11.568)

Example 379 Recursive force equation in link’s frame. The static equations can also be expressed in body coordinate frame, which may be the preferred form in computer calculation. Practically, it is easier to measure and calculate the force systems in the kink’s frame. Therefore, we may rewrite the backward recursive Equations (11.528) and (11.529) in the following form and calculate the proximal force system from the distal force system in the link’s frame. i i

Fi−1 = i Fi −



Mi−1 = i Mi −



i

(11.569)

Fei i

Mei +

i i−1 di

× i Fi

(11.570)

The calculated force system, then, may be transformed to the previous link’s coordinate frame by a transformation, Fi−1 =

i−1

Mi−1 =

i−1

i−1 i−1

Ti i Fi−1 i

Ti Mi−1

(11.571) (11.572)

11.6 Summary

673

or they may be transformed to any other coordinate frame including the base frame. Fi−1 = 0 Ti i Fi−1

(11.573)

Mi−1 = 0 Ti i Mi−1

(11.574)

0 0

Example 380 Actuator’s force and torque. All actuators apply their actuation force or torque only in one dimensional. Here is a discussion on static force at joint and the component that will be supported by actuators. Applying a backward or forward recursive static force analysis ends up with a set of known force systems at joints. Each joint is driven by a motor or generally an actuator that applies a force in a P joint, or a torque in an R joint. When the joint i is prismatic, the actuator force is applied along the axis of the joint i. Therefore, the force of the driving motor is along the zi−1 -axis, T 0 Fm = 0 kˆi−1 Fi (11.575) showing that the kˆi−1 component of the joint force Fi is supported by the actuator, while the ıˆi−1 and jˆi−1 components of Fi must be supported by the bearings of the joint. Similarly, when joint i is revolute, the actuator torque is applied about the axis of joint i. Therefore, the torque of the driving motor is along the zi−1 -axis T 0 Mm = 0 kˆi−1 Mi (11.576) showing that the kˆi−1 component of the joint torque Mi is supported by the actuator, while the ıˆi−1 and jˆi−1 components of Mi must be supported by the bearings of the joint.

11.6

Summary

Dynamics equations of motion for a robot can be found by both Newton–Euler and Lagrange methods. In the Newton–Euler method, each link (i) is a rigid body and therefore, its translational and rotational equations of motion in the base coordinate frame are: mi 0 ai = 0 Fi−1 − 0 Fi + 0



0

Fei

(11.577)



0 Ii 0 α i = 0 Mi−1 − 0 Mi + Mei     + 0 di−1 − 0 ri × 0 Fi−1 − 0 di − 0 ri × 0 Fi

(11.578)

The force Fi−1 and moment Mi−1 are the resultant force and moment that link (i − 1) applies to link (i) at joint i. Similarly, Fi and Mi are the resultant force and moment that link (i) applies to link (i + 1) at joint i + 1. We measure the force systems (Fi−1 , Mi−1 ) and (Fi , Mi ) at the origin of the coordinate frames Bi−1 and Bi , respectively. The sum of the external loads / / acting on the link (i) are Fei and Mei . The vector 0 ri is the global position vector of Ci and 0 di is the global position vector of the origin of Bi . The vector 0 α i is the angular acceleration and 0 ai is the translational acceleration of the link (i) measured at the mass center Ci . 0

ai = 0 d¨ i + 0 α i ×  0αi

=

0

    ri − 0 di + 0 ωi × 0 ωi × 0 ri − 0 di

+ θ¨ i 0 kˆi−1 + 0 ωi−1 × θ˙ i 0 kˆi−1 if joint i is R if joint i is P 0 α i−1 0 α i−1

(11.579)

(11.580)

Weight is usually the only external load on middle links of a robot, and reactions from the environment are extra external force systems on the base and end-effector links. The force and moment that the base actuator applies to the first link are F0 and M0 , and the force and moment that the end-effector applies to the environment are Fn and Mn . If weight is the only external load on link (i) and it is in − 0 kˆ0 direction, then we have 

0

Fei = mi 0 g = −mi g 0 kˆ0

(11.581)

674

11 Robot Dynamics



0

Mei = 0 ri × mi 0 g = − 0 ri × mi g 0 kˆ0

(11.582)

where g is the gravitational acceleration vector. The Newton–Euler equation of motion can also be written in link’s coordinate frame in a forward or backward method. The backward Newton–Euler equations of motion for link (i) in the local coordinate frame Bi are  i Fi−1 = i Fi − Fei + mi i0 ai    i = i Mi − Mei − i di−1 − i ri × i Fi−1   + i di − i ri × i Fi + i Ii i0 α i + i0 ωi × i Ii i0 ωi i

i

Mi−1

(11.583)

(11.584)

where ni = i di−1 − i ri

(11.585)

mi = i di − i ri

(11.586)

i i

and      = i d¨ i + i0 α i × i ri − i di + i0 ωi × i0 ωi × i ri − i di ⎧  i−1 i i−1 ˆ ⎪ ¨ k T α + θ ⎪ i−1 i−1 i i−1 ⎨ 0 i i−1 i ˙ i−1 kˆi−1 if joint i is R + T 0αi = i−1 0 ωi−1 × θ i ⎪ ⎪ ⎩i Ti−1 i−1 if joint i is P 0 α i−1 i 0 ai

(11.587)

(11.588)

In this method, we search for the driving force system (i Fi−1 , i Mi−1 ) by having the driven force system (i Fi , i Mi ) and the resultant external force system (i Fei , i Mei ). When the driving force system (i Fi−1 , i Mi−1 ) is found in frame Bi , we can transform them to the frame Bi−1 and apply the Newton–Euler equation for link (i − 1). Fi−1 =

i−1

Ti i Fi−1

(11.589)

Mi−1 =

i−1

Ti i Mi−1

(11.590)

i−1 i−1

The negative of the converted force system acts as the driven force system (− i−1 Fi−1 , − i−1 Mi−1 ) for the link (i − 1). The forward Newton–Euler equations of motion for link (i) in the local coordinate frame Bi are  i Fi = i Fi−1 + Fei − mi i0 ai    i i Mi = i Mi−1 + Mei + i di−1 − i ri × i Fi−1   − i di − i ri × i Fi − i Ii i0 α i − i0 ωi × i Ii i0 ωi . i

(11.592)

ni = i di−1 − i ri

(11.593)

mi = i di − i ri

(11.594)

i i

(11.591)

Using the forward Newton–Euler equations of motion, we can calculate the reaction force system (i Fi , i Mi ) by having the action force system (i Fi−1 , i Mi−1 ). When the reaction force system (i Fi , i Mi ) is found in frame Bi , we can transform them to the frame Bi+1 −1 i Fi = i Ti+1 Fi

(11.595)

−1 i Mi = i Ti+1 Mi

(11.596)

i+1 i+1

11.6 Summary

675

The negative of the converted force system acts as the action force system (− i+1 Fi , − i+1 Mi ) for the link (i + 1) and we can apply the Newton–Euler equation to the link (i + 1). The forward Newton–Euler equations of motion allow us to start from a known action force system (1 F0 , 1 M0 ), that the base link applies to the link (1), and calculate the action force of the next link. Therefore, analyzing the links of a robot, one by one, we end up with the force system that the end-effector applies to the environment. The Lagrange equation of motion d dt



∂L ∂ q˙i



∂L = Qi i = 1, 2, · · · n ∂qi

−

L = K −V

(11.597) (11.598)

provides a systematic approach to obtain the dynamics equations for robots. The variables qi are the coordinates by which the energies are expressed and the Qi is the corresponding generalized nonpotential force. The equations of motion for an n link serial manipulator, based on Newton–Euler or Lagrangian, can always be set in a matrix form ˙ + G(q) = Q D(q) q¨ + H(q, q) (11.599) or ˙ q˙ + G(q) = Q D(q) q¨ + C(q, q) or in a summation form

n 

Dij (q) q¨j +

j =1

n  n 

(11.600)

Hikm q˙k q˙m + Gi = Qi

(11.601)

k=1 m=1

where D(q) is an n × n inertial-type symmetric matrix D=

n  

JTDi mi JDi +

i=1

Hikm is the velocity coupling vector Hij k =

n  n   ∂Dij

∂qk

j =1 k=1

Gi is the gravitational vector Gi =

n 

1 T 0 J Ii JRi 2 Ri

1 ∂Dj k − 2 ∂qi

(11.602)



mj gT J(i) Dj

(11.603)

(11.604)

j =1

and Ji is the Jacobian matrix of the robot X˙ i =

"0

#

vi 0 ωi

" =

# JDi ˙ q˙ = Ji q. JRi

(11.605)

To hold a robot in a stationary configuration, the actuators must apply some required forces to balance the external loads applied to the robot. In the static condition, the globally expressed Newton–Euler equations for the link (i) can be written in a recursive form 0 0

Fi−1 = 0 Fi −



Mi−1 = 0 Mi −



0

(11.606)

Fei

0

Mei +

0 i−1 di

× 0 Fi .

(11.607)

Now we are able to calculate the action force system (Fi−1 , Mi−1 ) when the reaction force system (−Fi , −Mi ) is given.

676

11.7

11 Robot Dynamics

Key Symbols

a a [A] b B c ci c C ˙ C(q, q) dx , dy , dz d di D D(q) E Fei Fi Fi−1 Fs g G, B0 G(q) ˙ H(q, q) ıˆ, jˆ, kˆ Iˆ, Jˆ, Kˆ I = [I  ] I¯ = I¯ I = [I] J K l L L m mi ni Mei Mi Mi−1

Kinematic link length Acceleration vector Coefficient matrix of a set of linear equations Vector of known values in a set of linear equations Body coordinate frame cos Position of the mass center of link (i) in Bi Jacobian generating vector Mass center Damping-type matrix of equation of motion Elements of d Translation vector, joint position vector Position vector of the origin ofBi Displacement transformation matrix Inertial-type matrix of equation of motion Energy External force acting on the link(i) The force that link (i) applies to (i + 1) at joint i+1 The force that link (i − 1) applies to link (i) at joint i Shaking force Gravitational acceleration vector Global coordinate frame, Base coordinate frame Gravitational vector of equation of motion Velocity coupling vector of equation of motion Local coordinate axes unit vectors Global coordinate axes unit vectors Mass moment matrix Pseudo-inertia matrix Identity matrix Jacobian Kinetic energy Length Angular moment vector, moment of moment Lagrangian Mass Position vector of oi from Ci Position vector of oi−1 from Ci External moment acting on the link (i) The moment that link (i) applies to (i + 1) at jointi + 1 The moment that link (i − 1) applies to link (i) at joint i

11.7 Key Symbols

P q Q

X, Y, Z Zi

Power Generalized coordinate Torque of an actuator, generalized nonpotential force Moment vector at a joint Position vectors, homogenous position vector, Global position of the mass center of a link The element i of r The element of row i and column j of a matrix Rotation transformation matrix sin Homogenous transformation matrix Translational velocity vector Potential energy Local coordinate axes Vector of unknown values in a set of linear equations Global coordinate axes Short notation of an equation

Greek α α α1, α2, α3 θ θ ij k ω1 , ω2 , ω3 ε θ θ ij k ω ω ω˜

Angular acceleration Angular acceleration vector Components of α Rotary joint angle θi + θj + θk Components of ω Small test number to terminate a procedure Rotary joint angle θi + θj + θk Angular velocity Angular velocity vector Skew symmetric matrix of the vector ω

Symbol [ ]−1 [ ]T ≡ (i) FBD tr

Inverse of the matrix [ ] Transpose of the matrix [ ] Equivalent Link number i Free body diagram Trace

Q r

ri rij R s T v V x, y, z x

677

678

11 Robot Dynamics

Fig. 11.27 A planar slider-crank mechanism

Exercises 1. Notation and symbols. Describe the meaning of these notations. a − F2

b − 0 F1 c − 1 F1

d − 2 M1 e − 2 Me1 f − B M

0 di j − 0 di g − m2 h − 0 n2 i −i−1

k − 0 ri

l − i−1 di

0 m − 0 L2 n − 0 I2 o −i−1 Li p − Ki

q − Vi

r − i−1 Ii

2. P R manipulator dynamics. Find the equations of motion for the planar polar manipulator shown in Fig. 5.14. Eliminate the joints’ constraint force and moment to derive the equations for the actuators’ force or moment. 3.  Even order recursive translational velocity. Find an equation to relate the velocity of link (i) to the velocity of link (i − 2), and the velocity of link (i) to the velocity of link (i + 2). 4.  Even order recursive angular velocity. Find an equation to relate the angular velocity of link (i) to the angular velocity of link (i −2), and the angular velocity of link (i) to the angular velocity of link (i + 2). 5.  Even order recursive translational acceleration. Find an equation to relate the acceleration of link (i) to the acceleration of link (i − 2), and the acceleration of link (i) to the acceleration of link (i + 2). 6.  Even order recursive angular acceleration. Find an equation to relate the angular acceleration of link (i) to the angular acceleration of link (i −2), and the angular acceleration of link (i) to the angular acceleration of link (i + 2). 7.  Acceleration in different frames. For the 2R planar manipulator shown in Fig. 11.6, find 01 a2 , 10 a2 , 02 a1 , 20 a1 , 20 a2 , and 01 a1 . 8. Slider-crank mechanism dynamics. A planar slider-crank mechanism is shown in Fig. 11.27. Set up the link coordinate frames, develop the Newton–Euler equations of motion, and find the driving moment at the base revolute joint. 9.  Global differential of a link momentum. In recursive Newton–Euler equations of motion, why we do not use the following Newton equation? i

F=

G

G d i d i F= m v = m i v˙ + i0 ωi × m i v dt dt

(11.608)

Exercises

679

Fig. 11.28 A 2 DOF Cartesian manipulator

Fig. 11.29 A 3R planar manipulator attached to a wall

10. A planar Cartesian manipulator. Determine the equations of motion of the planar Cartesian manipulator shown in Fig. 11.28. Hint: The coordinate frames are not based on DH rules. 11. 3R planar manipulator dynamics. A 3R planar manipulator is shown in Fig. 11.29. The manipulator is attached to a wall and therefore, g = g 0 ıˆ0 . (a) Find the Newton–Euler equations of motion for the manipulator. Do your calculations in the global frame and derive the dynamic force and moment at each joint. (b) Reduce the number of equations to three for moments at joints. (c) Substitute the vectorial quantities and calculate the moments in terms of geometry and angular variables of the manipulator. (d) 3R planar manipulator recursive dynamics. The manipulator shown in Fig. 11.29 is a 3R planar manipulator attached to a wall and therefore, g = −g 0 ıˆ0 . (a) Find the equations of motion for the manipulator utilizing the backward recursive Newton–Euler technique. (b)  Find the equations of motion for the manipulator utilizing the forward recursive Newton–Euler technique. 12. 3R planar manipulator Lagrange dynamics. Find the equations of motion for the 3R planar manipulator shown in Fig. 11.29 utilizing the Lagrange technique. The manipulator is attached to a wall and therefore, g = −g 0 ıˆ0 . 13. An articulated manipulator. Figure 11.30 illustrates an articulated manipulator with massless arms and two massive points m1 and m2 . (a) Follow the DH rules and complete the link coordinate frames. (b) Determine the DH transformation matrices. (c) Determine the equations of motion of the manipulator using Lagrange method.

680

11 Robot Dynamics

Fig. 11.30 An articulated manipulator

14. Polar planar manipulator dynamics. A polar planar manipulator with 2 DOF is shown in Fig. 5.14. (a) Determine the Newton–Euler equations of motion for the manipulator. (b) Reduce the number of equations to two, for moments at the base joint and force at the P joint. (c) Substitute the vectorial quantities and calculate the action force and moment in terms of geometry and angular variables of the manipulator. 15. Polar planar manipulator Lagrange dynamics. Find the equations of motion for the polar planar manipulator, shown in Fig. 5.14, utilizing the Lagrange technique. 16. Polar planar manipulator recursive dynamics. Figure 5.14 depicts a polar planar manipulator with 2 DOF . (a) Find the equations of motion for the manipulator utilizing the backward recursive Newton–Euler technique. (b)  Find the equations of motion for the manipulator utilizing the forward recursive Newton–Euler technique. 17.  Dynamics of a spherical manipulator. Figure 5.46 illustrates a spherical manipulator attached with a spherical wrist. Analyze the robot and derive the equations of motion for joints action force and moment. Assume g = −g 0 kˆ0 and the end-effector is carrying a mass m. 18.  Dynamics of an articulated manipulator. Figure 5.22 illustrates an articulated manipulator RRR. Use g = −g 0 kˆ0 and find the manipulator’s equations of motion. 19. A planar manipulator. Figure 11.31 illustrates a three DOF planar manipulator. Determine the equations of motion of the manipulator if the links are massless and there are two massive points m1 and m2 . 20.  Dynamics of a SCARA robot. Calculate the dynamic joints’ force system for the SCARA robot RRRP shown in Fig. 5.24 if g = −g 0 kˆ0 . 21.  Dynamics of an SRMS manipulator. Figure 5.24 shows a model of the Shuttle remote manipulator system (SRMS). (a) Derive the equations of motion for the SRMS and calculate the joints’ force system for g = 0. (b) Derive the equations of motion for the SRMS and calculate the joints’ force system for g = −g 0 kˆ0 . (c) Eliminate the constraint forces and reduce the number of equations equal to the number of action moments. (d) Assume the links are made of a uniform cylinder with radius r = .25 m and m = 12 kg/m. Use the characteristics indicated in Table 5.10 and find the equations of motion when the end-effector is holding a 24 kg mass. 22. Statics of a 3R planar manipulator. Figure 11.29 illustrates a 3R planar manipulator attached to a wall. Derive the static force and moment at each joint to keep the configuration of the manipulator if g = −g 0 ıˆ0 . 23. An RP R planar redundant manipulator. Figure 11.32 illustrates a 3 DOF planar manipulator with joint variables θ 1 , d2 , and θ 2 . Determine the equations of motion of the manipulator if the links are massless and there are two massive points m1 and m2 .

Exercises

681

Fig. 11.31 A planar manipulator

Fig. 11.32 A RP R planar redundant manipulator

24.  Recursive dynamics of an articulated manipulator. Figure 11.19 illustrates an articulated manipulator RRR. Use g = −g 0 kˆ0 and find the manipulator’s equations of motion (a) utilizing the backward recursive Newton–Euler technique. (b) utilizing the forward recursive Newton–Euler technique. 25.  Recursive dynamics of a SCARA robot. A SCARA robot RRRP is shown in Fig. 9.22. If g = −g 0 kˆ0 determine the dynamic equations of motion by (a) utilizing the backward recursive Newton–Euler technique. (b) utilizing the forward recursive Newton–Euler technique. 26.  Recursive dynamics of an SRMS manipulator. Figure 5.24 shows a model of the Shuttle remote manipulator system (SRMS). (a) Derive the equations of motion for the SRMS utilizing the backward recursive Newton–Euler technique for g = 0. (b) Derive the equations of motion for the SRMS utilizing the forward recursive Newton–Euler technique for g = 0. 27.  Work done by actuators. Consider a 2R planar manipulator moving on a given path. Assume that the endpoint of a 2R manipulator moves with constant speed v = 1 m/s from P1 to P2 , on a path made of two semi-circles as shown in Fig. 11.33. Calculate the work done by the actuators if l1 = l2 = 1 m and the manipulator is carrying a 12 kg mass. The center of the circles are at (0.75 m, 0.5 m) and (−0.75 m, 0.5 m). 28.  Lagrange dynamics of an articulated manipulator. Figure 11.19 illustrates an articulated manipulator RRR. Use g = −g 0 kˆ0 and find the manipulator’s equations of motion utilizing the Lagrange technique. 29.  Lagrange dynamics of a SCARA robot. A SCARA robot RRRP is shown in Fig. 9.22. If g = −g 0 kˆ0 determine the dynamic equations of motion by applying the Lagrange technique.

682

11 Robot Dynamics

Fig. 11.33 A 2R planar manipulator moving on a given path

Fig. 11.34 A 2R planar manipulator attached to a ceiling in static condition

30.  Lagrange dynamics of an SRMS manipulator. Figure 9.22 shows a model of the Shuttle remote manipulator system (SRMS). Derive the equations of motion for the SRMS utilizing the Lagrange technique for (a) g = 0 (b) g = −g 0 kˆ0 . 31. Statics of a 2R planar manipulator. Figure 11.34 illustrates a 2R planar manipulator attached to a ceiling. The links are uniform with the following characteristics. m1 = 24 kg g = −g 0 jˆ0

m2 = 18 kg

l1 = 1 m

l2 = 1 m

(11.609) (11.610)

There is a load Fe = −14g 0 jˆ0 N at the endpoint. Calculate the static moments Q1 and Q2 for θ 1 = 30 deg and θ 2 = 45 deg. 32. Statics of a 2R planar manipulator at a different base angle. In Exercise 31 keep θ 2 = 45 deg and calculate the static moments Q1 and Q2 as functions of θ 1 . Plot Q1 and Q2 versus θ 1 and find the configuration that minimizes Q1 , Q2 , Q1 + Q2 , and the potential energy V . 33.  Statics of an articulated manipulator. An articulated manipulator RRR is shown in Fig. 11.19. Find the static force and moment at joints for g = g 0 kˆ0 . The end-effector is carrying a 20 kg mass. Calculate the maximum base force moment. 34.  Statics of a SCARA robot. Calculate the static joints’ force system for the SCARA robot RRRP shown in Fig. 5.24 if g = −g 0 kˆ0 and the end-effector is carrying a 10 kg mass.

Exercises

683

Fig. 11.35 A planar polar manipulator with massive links

35.  Statics of a spherical manipulator. Figure 5.46 illustrates a spherical manipulator attached with a spherical wrist. Analyze the robot and calculate the static force system in joints for g = −g 0 kˆ0 if the end-effector is carrying a 12 kg mass. 36.  Statics of an SRMS manipulator. A model of the Shuttle remote manipulator system (SRMS) is shown in Fig. 9.22. Analyze the static configuration of the SRMS and calculate the joints’ force system for g = −g 0 kˆ0 . Assume the links are made of a uniform cylinder with radius r = 0.25 m and m = 12 kg/m. Use the characteristics indicated in Table 5.10 and find the maximum value of the base force system for a 24 kg mass held by the end-effector. The SRMS is supposed to work in a no-gravity field. 37. Polar manipulator with massive links. Figure 11.35 illustrates a planar polar manipulator with massive links. Derive the below equations of motion for Q as the torque of a motor at origin of B0 to turn link (1), and F for actuating force of a jack between origin of B1 and B2 to move link (2) respect to link (1). 2 m2 r¨ + m2 g sin θ − m2 (r + r2 ) θ˙ = F

m1 r12

(11.611)

+ m2 (r + r2 ) θ¨ + 2m2 (r + r2 ) r˙ θ˙ 2

+g (m1 r1 + m2 (r + r2 )) cos θ = Q

(11.612)

38. Project. 2R robot torque calculation. Consider a 2R manipulator as shown in Fig. 11.14, that its tip point is supposed to move on a line Y = f (X) from (1, 0.5) to (−1, 1.5) in one unit of time. l1 = l2 = 1 m

m0 = 1 kg

m12 = 1 kg

(11.613)

Assume the links of the robot are massless. A septic time function for X will move the end-effector from X (0) = 1 to X (1) = −1 with zero velocity, zero acceleration, zero jerk at both ends. X = 1 − 70t 4 + 168t 5 − 140t 6 + 40t 7

(11.614)

˙ X, ¨ Y , Y˙ , Y¨ as functions of time. (a) Determine and plot X, X, ˙ X, ¨ Y , Y˙ , Y¨ at the 101 points and store (b) Divide the total time of motion into 100 equal segments. Calculate X, X, them. (c) Using inverse kinematic analysis, determine, and store θ 1 , θ 2 at the 101 points. Plot the graphs of θ 1 , θ 2 as functions of time. (d) Using inverse velocity analysis, determine, and store θ˙ 1 , θ˙ 2 at the 101 points. Plot the graphs of θ˙ 1 , θ˙ 2 as functions of time.

684

11 Robot Dynamics

(e) Using inverse acceleration analysis, determine, and store θ¨ 1 , θ¨ 2 at the 101 points. Plot the graphs of θ¨ 1 , θ¨ 2 as functions of time. (f) Employing dynamic equations of motion (11.304) and (11.305), determine, and store Q0 , Q1 at the 101 points. Plot the graphs of Q0 , Q1 as functions of time. (g) Using the stored values of Q0 , Q1 and θ˙ 1 , θ˙ 2 determine, and store the power of each motor P1 , P2 at the 101 points. Plot the graphs of P1 , P2 as functions of time. (f) Using the stored values of Q0 , Q1 and θ 1 , θ 2 calculate the amount energy that each motor used E1 , E2 . Calculate the total consumed energy by the robot. 39.  Project. Ceiling and ground mounted 2R robot comparison. Figure 11.24 illustrates an ideal 2R planar manipulator mounted on a ceiling. The tip point will move on a line Y = f (X) from (1, −0.5) to (−1, −1.5) in one unit of time. l1 = l 2 = 1 m

m0 = 1 kg

m12 = 1 kg

(11.615)

Assume the links of the robot are massless and use a septic time function for X to move the tip point from X (0) = 1 to X (1) = −1 with zero velocity, zero acceleration, zero jerk at both ends. X = 1 − 70t 4 + 168t 5 − 140t 6 + 40t 7

(11.616)

(a) Calculate the time history of the torques and powers of the motors in this maneuver. Determine the total consumed energy. (b) Consider the same robot is mounted on the ground and the tip point is to move on a line Y = f (X) from (1, 0.5) to (−1, 1.5) in one unit of time. Use the same time function for X as (11.616) and calculate the time history of the torques and powers of the motors in this maneuver. Determine the total consumed energy. (c) Compare torques, powers, and consumed energy of the ceiling and ground mounted 2R robot, and make engineering conclusion and comment. 40.  Project. Inverse dynamics of 2R robot. Consider a 2R manipulator as shown in Fig. 11.14 with massless arms. Assume the robot to be at its rest position at which, θ 1 = θ 2 = 0. l1 = l2 = 1 m m0 = 1 kg m12 = 1 kg (11.617) Apply the following torques for 0 ≤ t ≤ 8 where t is in second. Q0 = 10 sin

π tN m 4

Q1 = 5 sin

π tN m 4

(11.618)

(a) Solve the equations of motion numerically and determine θ 1 , θ 2 . (b) Using forward kinematics, determine the path of motion of the tip point of the robot. (c) Employing numerical solution of the equations of motion, determine θ˙ 1 , θ˙ 2 , θ¨ 1 , θ¨ 2 and the velocity and acceleration of the tip point. (d) Having θ 1 , θ 2 from part (a), determine θ˙ 1 , θ˙ 2 , θ¨ 1 , θ¨ 2 by numerical differentiation and compare the result with part (c).

Part IV Control

Control is the science of desired motion. It relates the dynamics and kinematics of a robot to a prescribed motion. It includes optimization problems to determine forces so that the system will behave optimally. A typical example is the situation in which the initial and terminal configurations of a robot are given and the forces acting on the robot must be found to have the motion in minimum time. Path or trajectory planning is a part of control, in which we plan a path followed by the manipulator in a planned time profile. Paths can be planned in joint or Cartesian space. Joint path planning directly specifies the time evolution of the joint variables. However, Cartesian path specifies the position and orientation of the end frame. So, path includes attaining a desired target from an initial configuration. It may also include avoiding obstacles. Joint path planning is a relatively simple task because it does not involve inverse kinematics, but it is hard to digest the motion of the manipulator in Cartesian space. However, Cartesian coordinates make sense but need inverse kinematics calculation. In this part, we develop techniques to derive the required commands to control the robot’s task.

Path Planning

12

Path planning includes three tasks: (1) Defining a geometric curve for the end-effector between two points. (2) Defining a rotational motion between two orientations. (3) Defining a time function for variation of a coordinate between two given values. All of these three definitions are called path planning. Figure 12.1 illustrates a path of the tip point of a 2R manipulator between points P1 and P2 to avoid two obstacles.

Fig. 12.1 A path of the tip point of a 2R manipulator to avoid two obstacles

12.1

Cubic Path

A cubic function is the simplest polynomial to determine the time behavior of a variable, such as a joint variable, between two given values, rest-to-rest. A cubic path in joint space for the joint variable q(t) between two points q(t0 ) and q(tf ) is q(t) = a0 + a1 t + a2 t 2 + a3 t 3 where

   tf q0 + t0 qf 3t0 − tf tf2 t0 − 3tf t02 a0 =  q + q − 3 0  3 f   2 t0 tf t0 − tf t0 − tf t0 − tf

(12.1)



 2t0 + tf −6t0 tf  t0 + 2tf   a1 =  3 q 0 − q f +   2 tf q 0 +   2 t0 q f t0 − tf t0 − tf t0 − tf    3 t0 + tf  2t0 + tf  t0 + 2tf  a2 =  3 q 0 − q f −  2 q 0 −  2 qf t0 − tf tf − t0 tf − t0

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6_12

(12.2)

(12.3)

(12.4)

687

688

12 Path Planning

a3 =

  q0 + qf −2 q0 − qf +  3  2 t0 − tf t0 − tf

(12.5)

and q(t0 ) = q0

q(t ˙ 0 ) = q0

q(tf ) = qf

q(t ˙ f ) = qf

(12.6)

Proof A cubic polynomial has four coefficients. Therefore, it can satisfy the position and velocity constraints at the initial and final points. For simplicity, we call the value of the variable the position and the rate of the variable the speed. Assume the position and speed of a variable at the initial time t0 and at the final time tf are given as (12.6). Substituting the boundary conditions (12.6) in the position and speed equations of the variable q, q(t) = a0 + a1 t + a2 t 2 + a3 t 3

(12.7)

q(t) ˙ = a1 + 2a2 t + 3a3 t 2

(12.8)

generates four equations for the coefficients of the path. ⎡

1 t0 ⎢0 1 ⎢ ⎣ 1 tf 0 1

t02 2t0 tf2 2tf

⎤⎡ ⎤ ⎡ ⎤ t03 q0 a0 ⎢ a1 ⎥ ⎢ q0 ⎥ 3t02 ⎥ ⎥⎢ ⎥ = ⎢ ⎥ tf3 ⎦ ⎣ a2 ⎦ ⎣ qf ⎦ qf 3tf2 a3

(12.9)

Their solutions are given in (12.2)–(12.5). In case that t0 = 0, the coefficients simplify. a0 = q0

(12.10)

a1 = q0    3 qf − q0 − 2q0 + qf tf a2 = tf2    −2 qf − q0 + q0 + qf tf a3 = tf3

(12.11) (12.12)

(12.13)

It is also possible to employ a time shift and search for a cubic polynomial of (t − t0 ). q(t) = a0 + a1 (t − t0 ) + a2 (t − t0 )2 + a3 (t − t0 )3

(12.14)

Now, the boundary conditions (12.6) generate a set of equations ⎡

1 ⎢0 ⎢  ⎢ ⎣ 1 tf 0

⎤⎡ ⎤ ⎡ ⎤ 0 0 0 q0 a0 ⎥ ⎢ ⎥ ⎢ q ⎥ 1 0 0 a 1   2  3 ⎥ ⎥⎢ ⎥ = ⎢ 0 ⎥ − t0 tf − t0 tf − t0 ⎦ ⎣ a 2 ⎦ ⎣ q f ⎦    2 qf a3 1 2 tf − t0 3 tf − t0

(12.15)

12.1 Cubic Path

689

Fig. 12.2 Kinematics of a rest-to-rest cubic path

with the following solutions: ⎡ ⎤ ⎤ q0 a0 ⎥ ⎢ q0 ⎢ a1 ⎥ ⎢  ⎥ −2  ⎢ ⎥=⎢ ⎥ 3q0 − 3qf − 2t0 q0 − t0 qf + 2tf q0 + tf qf ⎥ ⎣ a 2 ⎦ ⎢ − tf − t0 ⎦ ⎣   −3 a3 tf − t0 2q0 − 2qf − t0 q0 − t0 qf + tf q0 + tf qf ⎡

A disadvantage of cubic paths is the acceleration jump at boundaries that introduces infinite jerks.

(12.16)



Example 381 Rest-to-rest cubic path. Here we analyze a rest-to-rest cubic path to look at the graphical variation of the variable and see how cubic paths works. Assume q(0) = 10 deg, q(1) = 45 deg, and q(0) ˙ = q(1) ˙ = 0. The set of equations and the coefficients of the cubic path will be ⎤⎡ ⎤ ⎡ ⎤ ⎡ 1000 a0 10 ⎢ 0 1 0 0 ⎥ ⎢ a1 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ (12.17) ⎣ 1 1 1 1 ⎦ ⎣ a2 ⎦ = ⎣ 45 ⎦ 0123 a3 0 a0 = 10

a1 = 0

a2 = 105

a3 = −70

(12.18)

which generate a cubic path for the variable q(t). q(t) = 10 + 105t 2 − 70t 3 deg

(12.19)

q(t) ˙ = 210t − 210t deg /s

(12.20)

q(t) ¨ = 210 − 420t deg /s2

(12.21)

2

The path information is shown in Fig. 12.2. Example 382 To-rest cubic path. A path may start from a nonzero speed value and end to rest as zero speed. Such path may be used as the last piece of a series of paths. ˙ ˙ Assume the angle of a revolute joint starts from θ(0) = 10 deg, θ(0) = 10 deg /s and ends at θ(2) = 45 deg, θ(2) = 0. The coefficients of a cubic path for this motion are a0 = 10

a1 = 10

a2 =

65 4

a3 =

−25 4

(12.22)

690

12 Path Planning

Fig. 12.3 Kinematics of a to-rest cubic path in joint space

The kinematics of the path are θ(t) = 10 + 10t + 16.25t 2 − 6.25t 3 deg

(12.23)

˙ θ(t) = 10 + 32.5t − 18.75t deg /s

(12.24)

¨ θ(t) = 32.5 − 37.5t deg /s2

(12.25)

2

and are shown graphically in Fig. 12.3. Example 383 Rest-to-rest path with a constant speed in the middle. The conditions on a path may be more complicated than only rest-to-rest. Here is an example of a path that needs to have a constant speed for a duration of time, as well as rest-to-rest conditions. Assume we need a rest-to-rest path with a constant given speed q˙ = q˙c for t1 < t < t2 , where t0 < t1 < t2 < tf . The required conditions to satisfy are q(t0 ) = q0

q(t ˙ 0 ) = q0

(12.26)

qc

t1 < t < t2

(12.27)

q(tf ) = qf

q(t ˙ f ) = qf

(12.28)

q(t) ˙ =

The path has three parts: (1) rest-to, (2) constant-speed, and (3) to-rest. We need an equation for the rest-to part of the motion to achieve the given speed. There are three boundary conditions for the first part of the path. The conditions are the initial position and speed and the final constant speed. Assuming t0 = 0, the conditions are q1 (0) = q0

q˙1 (0) = 0

q˙1 (t1 ) = qc

(12.29)

A quadratic path has three coefficients and may be utilized to satisfy the three initial conditions. q1 (t) = a0 + a1 t + a2 t 2

(12.30)

q˙1 (t) = a1 + 2a2 t

(12.31)

Employing the conditions generates the following equations: q0 = a0

0 = a1

qc = 2a2 t1

(12.32)

qc 2 t 2t1

0 < t < t1

(12.33)

Therefore, the rest-to path is q1 (t) = q0 +

12.1 Cubic Path

691

Given the specific constant speed, qc shows that the path in the middle part is q˙2 (t) = qc q2 (t) =

qc t

(12.34) + C1

t1 < t < t2

(12.35)

The constant of integration can be found by utilizing the position condition at t = t1 . q0 +

qc 2 t = qc t1 + C1 2t1 1 1 C1 = q0 − t1 qc 2

(12.36) (12.37)

There are four conditions for the to-rest part of the path. q3 (tf ) = qf

q˙3 (tf ) = 0

1 q3 (t2 ) = q2 (t2 ) = q2 = qc t2 + q0 − t1 qc 2  q˙3 (t2 ) = q˙2 (t2 ) = qc

(12.38) (12.39) (12.40)

Therefore, it can be calculated utilizing a cubic equation. q3 (t) = b0 + b1 t + b2 t 2 + b3 t 3

(12.41)

q˙3 (t) = b1 + 2b2 t + 3b3 t 2

(12.42)

t2 < t < tf

(12.43)

The conditions generate four equations ⎡

1 tf ⎢0 1 ⎢ ⎣ 1 t2 0 1

tf2 2tf t22 2t2

⎤⎡ ⎤ ⎡ ⎤ tf3 b0 qf ⎢ ⎥ ⎢ ⎥ 0 3tf2 ⎥ ⎥ ⎢ b1 ⎥ ⎢ ⎥ 3 ⎦ ⎣ b ⎦ = ⎣ q t + q − 1 t q ⎦ t2 2 0 c 2 2 1 c b3 qc 3t22

(12.44)

with the following solutions: b0 = −t2 tf2 +qf b1 = qc

tf3 − 3t2 tf2 qc + q 2 −2t2 tf + t22 + tf2 −t23 + tf3 − 3t2 tf2 + 3t22 tf

−t23 + 3t22 tf −t23 + tf3 − 3t2 tf2 + 3t22 tf 2t2 tf + tf2

−2t2 tf + t22 + tf2

−6t2 qf b2 = qc

−t23

+

tf3

+ 6q2 t2

(12.45)

tf −t23 + tf3 − 3t2 tf2 + 3t22 tf

tf − 3t2 tf2 + 3t22 tf

(12.46)

−t2 − 2tf −3t2 − 3tf + q2 3 2 2 −2t2 tf + t2 + tf −t2 + tf3 − 3t2 tf2 + 3t22 tf

+qf

−t23

3t2 + 3tf + − 3t2 tf2 + 3t22 tf tf3

(12.47)

692

12 Path Planning

Fig. 12.4 A piecewise rest-to-rest path with a constant velocity in the middle

b3 =

qc q2 +2 3 −2t2 tf + t22 + tf2 −t2 + tf3 − 3t2 tf2 + 3t22 tf −2

−t23

+

tf3

qf − 3t2 tf2 + 3t22 tf

(12.48)

A graph of the path for the following values is illustrated in Fig. 12.4. t1 = 0.4 s q0 = 0

12.2

t2 = 0.7 s qf = 60 deg

tf = 1 s qc = 50 deg /s

(12.49)

Polynomial Path

Polynomial paths are the easiest functions to satisfy multiple conditions. The number of required conditions determines the degree of the polynomial for q = q(t). In general, a polynomial path of degree n needs n + 1 conditions. q(t) = a0 + a1 t + a2 t 2 + · · · + an t n

(12.50)

The conditions may be of two types: positions at a series of points, so that the trajectory will pass through all specified points, or position, speed, acceleration, and jerk at two points, so that the smoothness of the path can be controlled. The problem of searching for the coefficients of a polynomial reduces to a set of linear algebraic equations that may be solved numerically. However, the path planning can be simplified by splitting the whole path into a series of segments and utilizing combinations of lower order polynomials for different segments of the path. The polynomials must then be joined together to satisfy all the required boundary conditions. Examples will illustrate how to define conditions and determine associated polynomial path functions. Example 384 Quintic path. To eliminate acceleration jump at both ends of a rest-to-rest path in quadratic path, we add the zero acceleration conditions at both ends. These new conditions require using a fifth degree path function. Forcing a variable to have specific position, speed, and acceleration at boundaries introduces six conditions: q(t0 ) = q0

q(t ˙ 0 ) = q0

q(tf ) = qf

q(t ˙ f ) = qf

q(t ¨ 0 ) = q0 q(t ¨ f ) = qf

(12.51)

q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5

(12.52)

A five degree polynomial can satisfy these conditions,

12.2 Polynomial Path

693

Fig. 12.5 A quintic rest-to-rest path

and generates a set of six equations. ⎡

1 t0 ⎢0 1 ⎢ ⎢0 0 ⎢ ⎢ 1 tf ⎢ ⎣0 1 0 0

t02 2t0 2 tf2 2tf 2

t03 3t02 6t0 tf3 3tf2 6tf

t04 4t03 12t02 tf4 4tf3 12tf2

⎤⎡ ⎤ ⎡ ⎤ t05 q0 a0 4 ⎥⎢  ⎥ ⎥ ⎢ 5t0 a ⎥ ⎢ 1 ⎥ ⎢ q0 ⎥ 3 ⎥⎢ ⎢ ⎥ 20t0 ⎥ ⎢ a2 ⎥ ⎥ = ⎢ q0 ⎥ 5 ⎥⎢ ⎥ ⎥ tf ⎥ ⎢ a 3 ⎥ ⎢ ⎢ qf ⎥  ⎦ 4 ⎦⎣ ⎦ ⎣ qf 5tf a4 a5 qf 20tf3

(12.53)

The set of equations is linear and can be solved by matrix inversion. As an example, let us consider a rest-to-rest path with no acceleration at the rest positions with the following conditions: q(0) = 10 deg

q(0) ˙ =0

q(0) ¨ =0

q(1) = 45 deg

q(1) ˙ =0

q(1) ¨ =0

(12.54)

The coefficients of the quintic function can be found by solving a set of equations. ⎡

⎤⎡ ⎤ ⎡ ⎤ 1000 0 0 a0 10 ⎢ 0 1 0 0 0 0 ⎥ ⎢ a1 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 0 0 2 0 0 0 ⎥ ⎢ a2 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 1 1 1 1 1 1 ⎥ ⎢ a3 ⎥ = ⎢ 45 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ 0 1 2 3 4 5 ⎦ ⎣ a4 ⎦ ⎣ 0 ⎦ 0 0 2 6 12 20 a5 0

(12.55)

⎡

⎤ ⎡ ⎤ 10 a0 ⎢ a1 ⎥ ⎢ 0 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ a2 ⎥ ⎢ 0 ⎥ ⎢ ⎥=⎢ ⎥ ⎢ a3 ⎥ ⎢ 350 ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ a4 ⎦ ⎣ −525 ⎦ a5 210

(12.56)

q(t) = 10 + 350t 3 − 525t 4 + 210t 5

(12.57)

The path equation is then equal to which is shown in Fig. 12.5.

694

12 Path Planning

Fig. 12.6 A jerk zero at start–stop path

Example 385 Septic path. To remove the nonzero jerk at both ends of quintic path, a jerk zero at a start–stop path is needed. A seven degree polynomial can make a rest-to-rest path with no acceleration and no jerk at boundaries. To make a path to start and stop with zero jerk, a seven degree polynomial and eight boundary conditions must be employed. q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5 + a6 t 6 + a7 t 7 ... q (0) = 0 q(0) = q0 q(0) ˙ =0 q(0) ¨ =0 ... q (1) = 0 q(1) = qf q(1) ˙ =0 q(1) ¨ =0

(12.58)

(12.59) Such a zero jerk start–stop path for q(0) = 10 deg and q(1) = 45 deg can be found by solving the following set of equations for the unknown coefficients a0 , a1 , · · · , a7 : ⎡

⎤⎡ ⎤ ⎡ ⎤ 1000 0 0 0 0 a0 10 ⎢ 0 1 0 0 0 0 0 0 ⎥ ⎢ a1 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 0 0 2 0 0 0 0 0 ⎥ ⎢ a2 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 0 0 0 6 0 0 0 0 ⎥ ⎢ a3 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 1 1 1 1 1 1 1 1 ⎥ ⎢ a4 ⎥ = ⎢ 45 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ 0 1 2 3 4 5 6 7 ⎥ ⎢ a5 ⎥ ⎢ 0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ 0 0 2 6 12 20 30 42 ⎦ ⎣ a6 ⎦ ⎣ 0 ⎦ 0 0 0 6 24 60 120 210 a7 0

(12.60)

The solutions make the required seventh degree polynomial. q(t) = 10 + 1225t 4 − 2940t 5 + 2450t 6 − 700t 7

(12.61)

A graph of this path is illustrated in Fig. 12.6. Figure 12.2 depicts the path of a rest-to-rest motion with no condition on acceleration and jerk. Figure 12.5 shows an improvement by forcing the motion to have zero accelerations at start and stop. In Fig. 12.6, the motion is forced to have zero acceleration and zero jerk at start and stop. Hence, it shows the smoothest start and stop. However, increasing the smoothness of the start and stop increases the peak value of acceleration. Example 386 Constant acceleration path. Considering acceleration to be proportional to force, minimization of the maximum acceleration could be optimal path planning for some applications. The minimum acceleration happens when it remains constant during the motion. Here is the path planning for constant acceleration. A rest-to-rest path with constant acceleration has two segments with positive and negative accelerations. Let us assume the absolute value of the positive and negative accelerations is given. |q(t)| ¨ = ac

(12.62)

12.2 Polynomial Path

695

For this symmetric assumption, the switching point will happen at the midway. The first half of the motion has a positive acceleration and a second degree polynomial is the simplest function to satisfy the initial conditions. 1 2 ac t + q0 2 1 0 < t < tf 2

q1 (t) =

q˙1 (t) = ac t

(12.63) (12.64)

The constants of integration of Eq. (12.62) or the coefficients of the path function are found based on the initial conditions. q1 (0) = q0

q˙1 (0) = 0

(12.65)

For the second half of the path, we start with a general second degree polynomial, 1 tf < t < tf 2

q2 (t) = a0 + a1 t + a2 t 2

(12.66)

and impose the following boundary conditions to determine the coefficients: q2 (tf ) = qf

a0 + a1 tf + a2 tf2 = qf

q˙2 (tf ) = 0  q1 ac 2



tf 2

 q˙1

tf 2

a1 + 2a2 tf = 0

 = q2

tf 2

+ q0 = a0 + a1 

= q˙2

tf 2

(12.68)



2

tf 2

(12.67)

(12.69) tf + a2 2

ac



tf 2

2 (12.70)

tf = a 1 + a 2 tf 2

There are four conditions that make four equations, and there are five unknowns, a0 , a1 , a2 , ac , and tf . Out of these parameters, we may choose either the time of travel tf or the value of acceleration ac , but not both. Let us assume the total time tf is set to generate four equations for the unknown parameters a0 , a1 , a2 , and ac . ⎡

1 ⎢0 ⎢ ⎣1 0

⎤⎡ ⎤ ⎡ ⎤ 0 tf tf2 a0 qf ⎢ a1 ⎥ ⎢ 0 ⎥ 1 2tf 0 ⎥ ⎥⎢ ⎥ = ⎢ ⎥ 1 t 1 t 2 − 18 tf2 ⎦ ⎣ a2 ⎦ ⎣ q0 ⎦ 2 f 4 f ac 0 1 tf − 12 tf

(12.71)

The equations will have the following solutions: ⎡

⎤ 2q0 − qf  ⎥ ⎡ ⎤ ⎢ 4  ⎢ q − q0 ⎥ a0 ⎢ tf f ⎥ ⎢ a1 ⎥ ⎢ ⎥ ⎢ ⎥=⎢ 2  ⎥ ⎣ a2 ⎦ ⎢ − 2 qf − q0 ⎥ ⎢ tf ⎥ ⎢ ⎥ ac  ⎦ ⎣ 4  qf − q0 tf2

(12.72)

Having a set of data as q0 = 10 deg

qf = 45 deg

tf = 1

(12.73)

696

12 Path Planning

Fig. 12.7 A constant acceleration path

provides us with a0 = −25

a1 = 140

a2 = −70

ac = 140

(12.74)

Figure 12.7 illustrates kinematics of constant acceleration path. Example 387 Point sequence path. If the whole path of motion is important, we may simulate the path with a series of points where the variable must be at particular times. Polynomial path planning is an easy way to determine the path. A path can be assigned via a series of points that the variable must attain at specific times. The points may also be located to approximate a trajectory. Consider an example path specified by four points q0 , q1 , q2 , and q3 , such that the points are reached at times t0 , t1 , t2 , and t3 , respectively. In addition to positions, we usually impose constraint on initial and final velocities and accelerations. q(t0 ) = q0

q(t ˙ 0) = 0

q(t ¨ 0) = 0

q(t1 ) = q1

q(t2 ) = q2

q(t3 ) = q3

q(t ˙ 3) = 0

q(t ¨ 3) = 0

(12.75)

There are eight conditions, and hence, a seven degree polynomial can be utilized to satisfy the conditions. q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 +a5 t 5 + a6 t 6 + a7 t 7

(12.76)

t0 = 0

(12.77)

The set of equations for the unknown coefficients is ⎡

1 t0 ⎢0 1 ⎢ ⎢0 0 ⎢ ⎢ 1 t1 ⎢ ⎢ 1 t2 ⎢ ⎢ 1 t3 ⎢ ⎣0 1 00

t02 2t0 2 t12 t22 t32 2t3 2

t03 3t02 6t0 t13 t23 t33 3t32 6t3

t04 4t03 12t02 t14 t24 t34 4t33 12t32

t05 5t04 20t03 t15 t25 t35 5t34 20t33

t06 6t05 30t04 t16 t26 t36 6t35 30t34

⎤⎡ ⎤ ⎡ ⎤ t07 a0 q0 ⎢ a1 ⎥ ⎢ 0 ⎥ 7t06 ⎥ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ 42t05 ⎥ ⎥ ⎢ a2 ⎥ ⎢ 0 ⎥ 7 ⎥⎢ ⎢ ⎥ t1 ⎥ ⎢ a 3 ⎥ ⎥ = ⎢ q1 ⎥ 7 ⎥⎢ ⎥ ⎥ t2 ⎥ ⎢ a 4 ⎥ ⎢ ⎢ q2 ⎥ 7 ⎥⎢ ⎥ ⎢ t3 ⎥ ⎢ a 5 ⎥ ⎢ q 3 ⎥ ⎥ 7t 6 ⎦ ⎣ a6 ⎦ ⎣ 0 ⎦ 3

42t35

a7

0

(12.78)

12.2 Polynomial Path

697

Fig. 12.8 A point sequence path

which can be simplified to

⎡

1 0 0 ⎢0 1 0 ⎢ ⎢0 0 2 ⎢ ⎢ 1 0.4 0.42 ⎢ ⎢ 1 0.7 0.72 ⎢ ⎢1 1 1 ⎢ ⎣0 1 2 0 0 2

0 0 0 0.43 0.73 1 3 6

0 0 0 0.44 0.74 1 4 12

0 0 0 0.45 0.75 1 5 20

0 0 0 0.46 0.76 1 6 30

⎤⎡ ⎤ ⎡ ⎤ 10 0 a0 ⎢ a1 ⎥ ⎢ 0 ⎥ 0 ⎥ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ 0 ⎥ ⎥ ⎢ a2 ⎥ ⎢ 0 ⎥ 7⎥⎢ ⎢ ⎥ 0.4 ⎥ ⎢ a3 ⎥ ⎥ = ⎢ 20 ⎥ 7⎥⎢ ⎥ ⎥ 0.7 ⎥ ⎢ a4 ⎥ ⎢ ⎢ 30 ⎥ ⎥ ⎢ ⎥ ⎢ 1 ⎥ ⎢ a5 ⎥ ⎢ 45 ⎥ ⎥ 7 ⎦ ⎣ a6 ⎦ ⎣ 0 ⎦ 0 42 a7

(12.79)

for the following example data: q(0) = 10 deg

q(0) ˙ =0

q(0) ¨ =0

q(1) ˙ =0

q(1) ¨ =0

q(0.4) = 20 deg q(0.7) = 30 deg q(1) = 45 deg The solution for the coefficients is

⎤ ⎡ ⎤ 10 a0 ⎢ a1 ⎥ ⎢ ⎥ 0 ⎢ ⎥ ⎢ ⎥ ⎢ a2 ⎥ ⎢ ⎥ 0 ⎢ ⎥ ⎢ ⎥ ⎢ a3 ⎥ ⎢ 1500.5 ⎥ ⎢ ⎥=⎢ ⎥ ⎢ a4 ⎥ ⎢ −7053 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ a5 ⎥ ⎢ 12891 ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ a6 ⎦ ⎣ −10380 ⎦ a7 3076.9

(12.80)

⎡

(12.81)

These coefficients generate a path as shown in Fig. 12.8. This method provides a continuous and differentiable function for the variable q. Continuity and differentiability of q = q(t) is an advantage to provide a continuous velocity, acceleration, and jerk. However, the number of equations increases by increasing the number of points, which needs larger data storage and increases the calculating time. Example 388 Splitting a path into a series of segments. When the number of path points is too many, we may split the whole path into smaller segments with a low polynomial function in each segment. Sawing the segments is the main art of this path planning method.

698

12 Path Planning

Instead of using a single high degree polynomial for the entire trajectory, we may prefer to split the trajectory into some segments and use a simpler function of low degree polynomials. Consider a path for the following boundary conditions: q(t0 ) = q0

q(t ˙ 0) = 0

q(t ¨ 0) = 0

q(t4 ) = q3

q(t ˙ 4) = 0

q(t ¨ 4) = 0

(12.82)

which must also pass through three middle points given below: q(t1 ) = q1 q(t2 ) = q2 q(t3 ) = q3

(12.83)

Let us split the entire path into four segments, namely q1 (t), q2 (t), q3 (t), and q4 (t). q1 (t) for q(t0 ) < q1 (t) < q(t1 ) and q2 (t) for q(t1 ) < q2 (t) < q(t2 ) and q3 (t) for q(t2 ) < q3 (t) < q(t3 ) and q4 (t) for q(t3 ) < q4 (t) < q(t4 ) and

t0 t1 t2 t3

0 kP > 0 (14.8) f = fc + kD e˙ + kP e The new control law uses a feedback command as shown in Fig. 14.3b. It is also possible to define a new control law only based on the error signal. f = −kD e˙ − kP e (14.9)

14.1 Open- and Closed-Loop Control

761

Fig. 14.3 Open-loop and closed-loop control algorithms for a linear oscillator

Employing this law, we can define a more compact feedback control algorithm and modify the equation of motion. mx¨ + (c + kD ) x˙ + (k + kP ) x = kD x˙ d + kP xd

(14.10)

The equation of the system can be summarized in a block diagram as shown in Fig. 14.3c. A general scheme of a feedback control system is that a signal from the output feeds back to be compared to the input. This feedback signal closes a loop and makes it reasonable to use the words feedback and close-loop. The principle of a closed-loop control is to detect any error between the actual output and the desired. As long as the error signal is not zero, the controller keeps changing the control command so that the error signal converges to zero. Example 422 Stability of a controlled system. How a linear control system is stable to make any error signals to approach zero? Here is the analysis of stability of a control system. Consider a linear mass-spring-damper oscillator as shown in Fig. 14.2 with a known equation of motion. m x¨ + c x˙ + k x = f

(14.11)

We define a control law based on the actual output f = −kD x˙ − kP x

kD > 0

kP > 0

(14.12)

and modify the equation of motion. mx¨ + (c + kD ) x˙ + (k + kP ) x = 0

(14.13)

Comparing with the open-loop equation (14.11), the closed-loop equation shows that the oscillator acts as a free vibrating system under the action of new stiffness k + kP and damping c + kD . Hence, the control law has changed the apparent stiffness and damping of the actual system. This example introduces the most basic application of control theory to improve the characteristics of a system and run the system to behave in a desired manner. A control system must be stable when the desired output of the system changes and also be able to eliminate the effect of small disturbances. Stability of a control system is defined as: The output must remain bounded for a given input or a bounded disturbance function. To investigate the stability of the system, we must solve the closed-loop differential equation (14.13) for its eigenvalues. The equation is linear and therefore, it has an exponential solution. x = eλt

(14.14)

762

14 Control Techniques

Substituting the solution into Eq. (14.13) provides the characteristic equation with two solutions. mλ2 + (c + kD ) λ + (k + kP ) = 0

c + kD (c + kD )2 − 4m (k + kP ) λ1,2 = − ± 2m 2m The nature of the solution (14.14) depends on λ1 and λ2 , and therefore on kD and kP . If (c + kD )2 < 4m (k + kP )

(14.15) (14.16)

(14.17)

then the roots of the characteristic equation are complex. λ = −a ± bi a=

4m (k + kP ) − (c + kD )2 b= 2m

c + kD 2m

(14.18)

(14.19) In this case, the solution of the equation of motion will be   x = Ce−ξ ωn t sin ωn 1 − ξ 2 t + ϕ

(14.20)

)

k + kP = a 2 + b2 m c + kD a ξ = √ =√ 2 m (k + kP ) a 2 + b2

ωn =

(14.21) (14.22)

The parameter ωn is called natural frequency, and ξ is the damping ratio of the system. The damping ratio controls the behavior of the system according to the following categories: 1. If ξ = 0, then the characteristic values are purely imaginary. λ1,2 = ±bi = ±i

1

4m (k + kP ) 2m

(14.23)

In this case, the system has no damping, and therefore, it oscillates with a constant amplitude around the equilibrium, x = 0, forever. 2. If 0 < ξ < 1, then the system is under-damped and it oscillates around the equilibrium with a decaying amplitude. The system is asymptotically stable in this case. 3. If ξ = 1, then the system is critically damped. A critically damped oscillator has the fastest return to the equilibrium in an unoscillatory manner. 4. If ξ > 1, then the system is over-damped and it slowly returns to the equilibrium in an unoscillatory manner. The characteristic values are real and the solution of an over-damped oscillator is x = A e λ 1 t + B e λ2 t

(λ1,2 ) ∈ R

(14.24)

Re(λ1,2 ) > 0

(14.25)

5. If ξ < 0, then the system is unstable because the solution would be x = A e λ1 t + B e λ2 t It shows a motion with an increasing amplitude.

14.1 Open- and Closed-Loop Control

763

As an example, consider a system with the following characteristic equation: λ2 + 6λ + 10 = 0 λ1,2 = −3 ± i

(14.26) (14.27)

  Solutions of this equation show a stable system because Re λ1,2 = −3 < 0. Characteristic equations are linear polynomials. Hence for high degree equations, it is possible to use numerical methods, such as Newton–Raphson, to find the solution and determine the stability of the system. Example 423  Complex roots. Complex roots make real response. Here is to show this fact. In case the characteristic equation has complex roots λ1,2 = a ± bi

(14.28)

eiθ = cos θ + i sin θ

(14.29)

we may employ the Euler formula and show that the solution can be written in a new form x = C1 eat (cos bt + i sin bt) + C2 eat (cos bt − i sin bt) = eat (A cos bt + B sin bt)

(14.30)

where, C1 and C2 are complex, and A and B are real numbers. A = C1 + C2

B = (C1 − C2 ) i

(14.31)

Example 424 Robot Control Algorithms. Here is a fundamental classification of control algorithms that are being employed in robotics. Robots are nonlinear dynamical systems, and there is no general method for designing a nonlinear controller to be suitable for every robot in every mission. However, there are a variety of alternative and complementary methods, each best applicable to particular class of robots in a particular mission. The most important control methods are as follows. Feedback Linearization or Computed Torque Control Technique In feedback linearization technique, we define a control law to obtain a linear differential equation for error command, and then use the linear control design techniques. The feedback linearization technique can be applied to robots successfully; however, it does not guarantee robustness according to parameter uncertainty or disturbances. This technique is a model-based control method, because the control law is designed based on a nominal model of the robot. Linear Control Technique The simplest technique for controlling robots is to design a linear controller based on linearization of the equations of motion about an operating point. The linearization technique locally determines the stability of the robot. Proportional, integral, and derivative, or any combination of them are the most practical linear control techniques.

Adaptive Control Technique Adaptive control is a technique for controlling uncertain or time varying robots. Adaptive control technique is more effective for low DOF robots. Robust and Adaptive Control Technique In the robust control method, the controller is designed based on the nominal model plus some uncertainty. Uncertainty can be in any parameter, such as the load carrying by the end-effector. For example, we develop a control technique to be effective for loads in a range of 1–10 kg. Gain-Scheduling Control Technique Gain-scheduling is a technique that tries to apply the linear control techniques to the nonlinear dynamics of robots. In gain-scheduling, we select a number of control points to cover the range of robot operation. Then at each control point, we make a linear time varying approximation to the robot dynamics and design a linear controller. The parameters of the controller are then interpolated or scheduled between control points.

764

14.2

14 Control Techniques

Computed Torque Control

Dynamics of a robot can be expressed in the form ˙ + G(q) Q = D(q) q¨ + H(q, q)

(14.32)

˙ t) is the torques applied at joints. Assume a desired path in joint space where q is the vector of joint variables, and Q(q, q, is given by a twice differentiable function q = qd (t) ∈ C 2 . Hence, the desired time history of joints’ position, velocity, and acceleration are known. We can control the robot to follow the desired path, by introducing a computed torque control law as below   ˙ + G(q) (14.33) Q = D(q) q¨ d − kD e˙ − kP e + H(q, q) where kD and kP are constant gain diagonal matrices and e is the error vector. e = q − qd

(14.34)

The control law is stable and applied as long as all the eigenvalues of the following matrix have negative real part: "

0 I [A] = −kP −kD

# (14.35)

Proof The required torque Qc to track a desired path qd (t) can directly be found by substituting the path function into the equations of motion. Qc = D(qd ) q¨ d + H(qd , q˙ d ) + G(qd ) (14.36) The calculated torques are called control inputs, and the control is based on the open-loop control law. In an open-loop control, we have the equations of motion for a robot and we need the required torques to move the robot on a given path. Open-loop control is a blind method because the current state of the robot is not used for calculating the inputs. Due to non-modeled parameters and also errors in adjustment, there is always a difference between the desired and actual paths. To make the robot’s actual path converge to the desired path, we must introduce a feedback control. Let us use the feedback signal of the actual path and apply the computed torque control law (14.33) to the robot. Substituting the control law in the equations of motion (14.32) yields e¨ + kD e˙ + kP e = 0 (14.37) This is a linear differential equation for the error variable between the actual and desired outputs. If the n × n gain matrices kD and kP are assumed to be diagonal, then we may rewrite the error equation in a matrix form. d dt

" # " #" # " # e e e 0 I = = [A] e˙ −kP −kD e˙ e˙

(14.38)

The linear differential equation (14.38) is asymptotically stable when all the eigenvalues of [A] have negative real part. The matrix kP has the role of natural frequency, and kD acts similar to damping. ⎡

ω21 ⎢ 0 kP = ⎢ ⎣ 0 0 ⎡

0 ω22 0 0

⎤ 0 0 0 0 ⎥ ⎥ ··· 0 ⎦ 0 ω2n

2ξ 1 ω1 0 ⎢ 0 2ξ 2 ω2 kD = ⎢ ⎣ 0 0 0 0

⎤ 0 0 0 0 ⎥ ⎥ ··· 0 ⎦ 0 2ξ n ωn

(14.39)

(14.40)

Because kD and kP are diagonal, we can adjust the gain matrices kD and kP to control the response speed of the robot at each joint independently. A simple choice for the matrices is to set ξ i = 0, i = 1, 2, · · · , n, and make each joint response equal to the response of a critically damped linear second-order system with natural frequency ωi .

14.2 Computed Torque Control

765

The computed torque control law (14.33) has two components as shown below.   ˙ + G(q) + D(q) −kD e˙ − kP e Q = D(q)q¨ d + H(q, q) 5 67 8 5 67 8 Qff Qf b

(14.41)

The first term, Qff , is the feedforward command, which is the required torques based on open-loop control law. When there is no error, the control input Qff makes the robot follow the desired path qd . The second term, Qf b , is the feedback command, which is the correction torques to reduce the errors in the path of the robot. Computed torque control is also called feedback linearization, which is an applied technique for robots’ nonlinear control design. To apply the feedback linearization technique, we develop a control law to eliminate all nonlinearities and reduce the problem to the linear second-order equation of error signal (14.37).  Example 425 Computed force control for an oscillator. To illustrate how computed force control should be applied on a dynamic system. Figure 14.2 depicts a linear mass-spring-damper oscillator under the action of a control force. m x¨ + c x˙ + k x = f

(14.42)

f = m (x¨d − kD e˙ − kP e) + c x˙ + k x

(14.43)

e = x − xd

(14.44)

Applying a computed force control law

makes a differential equation for the error signal e. e¨ + kD e˙ + kP e = 0

(14.45)

e = A e λ1 t + B e λ2 t  2 λ1,2 = −kD ± kD − 4kP

(14.46)

The solution of the error equation is

(14.47)

where A and B are functions of initial conditions, and λ1,2 are solutions of the characteristic equation. mλ2 + kD λ + kP = 0

(14.48)

The solution (14.46) is stable and e → 0 exponentially as t → ∞ if kD > 0. Example 426 Inverted pendulum. To keep an inverted pendulum to stay at its unstable upright equilibrium point, a torque Q must be applied at the base joint, calculated by feedback signals. Consider an inverted pendulum shown in Fig. 14.4. Its equation of motion is ml 2 θ¨ − mgl sin θ = Q

(14.49)

To control the pendulum and bring it from an initial angle θ = θ 0 to the vertical-up position, we may employ a feedback control law. Q = −kD θ˙ − kP θ − mgl sin θ (14.50) The parameters kD and kP are positive gains and are assumed constants. The control law (14.50) transforms the dynamics of the system, showing that the system behaves as a stable mass-spring-damper. ml 2 θ¨ + kD θ˙ + kP θ = 0

(14.51)

766

14 Control Techniques

Fig. 14.4 A controlled inverted pendulum

Fig. 14.5 A 2R planar manipulator with massive links

In case the desired position of the pendulum is at a nonzero angle, θ = θ d , we may employ a feedback control law based on the error e = θ − θ d as below, Q = ml 2 θ¨ d − kD e˙ − kP e − mgl sin θ (14.52) Substituting this control law in the equation of motion (14.49) shows that the dynamics of the controlled system is governed by an equation for the error signal. (14.53) ml 2 e¨ + kD e˙ + kP e = 0 Example 427 Control of a 2R planar manipulator. To illustrate how computed force control should be applied on a robotic system. A 2R planar manipulator is shown in Fig. 14.5 with dynamic equations given below. "

D11

#" # D11 D12 θ¨ 1 D21 D22 θ¨ 2 " #" # " # C11 C12 θ˙ 1 G1 + + C21 C22 θ˙ 2 G2   = m1 r12 + I1 + m2 l12 + l1 r2 cos θ 2 + r22 + I2 Q1 Q2

#

"

=

(14.54) (14.55)

D21 = D12 = m2 l1 r2 cos θ 2 + m2 r22 + I2

(14.56)

D22 =

(14.57)

m2 r22

+ I2

C11 = −m2 l1 r2 θ˙ 2 sin θ 2

(14.58)

C21 = −m2 l1 r2 (θ˙ 1 + θ˙ 2 ) sin θ 2

(14.59)

14.3 Linear Control Technique

767

C12 = m2 l1 r2 θ˙ 1 sin θ 2

(14.60)

C22 = 0

(14.61)

G1 = m1 gr1 cos θ 1 + m2 g (l1 cos θ 1 + r2 cos (θ 1 + θ 2 ))

(14.62)

G2 = m2 gr2 cos (θ 1 + θ 2 )

(14.63)

Let us write the equations of motion in the following form: ˙ q˙ + G(q) = Q D(q) q¨ + C(q, q)

(14.64)

and multiply both sides by D−1 to reform the equations of motion. q¨ + D−1 C q˙ + D−1 G = D−1 Q

(14.65)

To control the manipulator to follow a desired path q = qd (t), we apply the following control law: ˙ q˙ + G(q) Q = D(q) U + C(q, q)

(14.66)

U = q¨ d − 2k e˙ − k 2 e

(14.67)

e = q − qd .

(14.68)

where

The vector U is the controller input, e is the position error, and k is a positive constant gain number. Substituting the control law into the equation of motion shows that the error vector satisfies a linear second-order ordinary differential equation. e¨ + 2k e˙ + k 2 e = 0

(14.69)

Error will exponentially converge to zero for k > 0.

14.3

Linear Control Technique

Linearization of a robot’s equations of motion about an operating point while applying a linear control algorithm is a traditional and fundamental robot control technique. This technique works well in a vicinity of the operating point. Hence, it is only a locally stable method. The linear control techniques are proportional, integral, derivative, and any combination of them. The idea is to linearize the nonlinear equations of motion about some reference operating points to make a linear system, design a controller for the linear system, and then, apply the control. This technique will always result in a stable controller in some neighborhood of the operating point. However, the stable neighborhood may be quite small and hard to be determined. A proportional-integral-derivative (P I D) control algorithm employs a position error, derivative error, and integral error to develop a control law. Hence, a P I D control law has the following general form for the input command: Q = kP e + kI

t

e dt + kD e˙

(14.70)

0

where e = q − qd is the error signal, and kP , kI , and kD are positive constant gains associated to the proportional, integral, and derivative controllers. The control command Q is thus a sum of three terms: the P -term, which is proportional to error e, the I -term, which is proportional to the integral of the error, and the D-term, which is proportional to the derivative of the error.

768

14 Control Techniques

14.3.1 Proportional Control In case of proportional control, the P I D control law (14.70) reduces to Q = kP e + Qd

(14.71)

The variable Qd is the desired control command, which is called a bias or reset factor. When the error signal is zero, the control command is equal to the desired value. The proportional control has a drawback that results in a constant finite error at steady state condition.

14.3.2 Integral Control The main function of an integral control is to eliminate the steady state error and make the system follow the set point at steady state conditions. The integral controller leads to an increasing control command for a positive error, and a decreasing control command for a negative error. An integral controller is usually used with a proportional controller. The control law for a PI controller is - t e dt (14.72) Q = kP e + kI 0

14.3.3 Derivative Control The purpose of derivative control is to improve the closed-loop stability of a system. A derivative controller has a predicting action by extrapolating the error using a tangent to the error curve. A derivative controller is usually used with a proportional controller. The P D control law is Q = kP e + kD e˙ (14.73) Proof Any linear system behaves linearly if it is sufficiently near a reference operating point. Consider a nonlinear system q˙ = f(q, Q)

(14.74)

q˙ d = f(qd , Qc )

(14.75)

where qd is a solution generated by a specific input Qc .

Assume δq is a small change from the reference point qd because of a small change δQ from Q. q = qd + δq

(14.76)

Q = Qc + δQ

(14.77)

If the changes δq and δQ are assumed small at all times, then Eq. (14.74) can be approximated by its Taylor expansion and q be the solution of the following equation: ∂f ∂f q˙ = q+ Q (14.78) ∂qd ∂Qd     ∂f ∂f The partial derivative matrices ∂q and are evaluated at the reference point (qd , Qd ).  ∂Qd d Example 428  Linear control for a pendulum. How to linearize equation of motion of a nonlinear system around a set point and define a P I D control to keep the system as the set point. Figure 14.6 illustrates a controlled pendulum as a one-arm manipulator. The equation of motion for the arm is Q = I θ¨ + cθ˙ + mgl sin θ

(14.79)

14.3 Linear Control Technique

769

Fig. 14.6 A controlled pendulum as a one-arm manipulator

where I is the arm’s mass moment about the pivot joint and m is the mass of the arm. The joint has a viscous damping c, the distance between the pivot and mass center C, is l. Introducing a new set of variables θ˙ = x2

θ = x1

(14.80)

converts the equation of motion into state-space expression. x˙1 = x2

x˙2 =

Q − c x2 − mgl sin x1 I

(14.81)

The linearized form of these equations is "

Assume that the reference point is

x˙1 x˙2

#

"

0 1 = −mgl/I −c "

x xd = 1 x2

#

"

#"

π /2 = 0

# " #" # x1 0 0 0 + x2 0 1/I Q

(14.82)

# Qc = mgl

(14.83)

The coefficient matrices in Eq. (14.82) must then be evaluated at the reference point. We use a set of sample data c = 0.01 Ns/m

m = 1 kg l = 0.35 m I = 0.07 kg.m2 and find the partial derivatives.

" # ∂f 0 1 = −49.05 −0.01 ∂qd " # ∂f 0 0 = 0 14.286 ∂Qc

(14.84)

(14.85) (14.86)

Now we have a linear system and we may apply any control law that applies to linear systems. For instance, a P I D control law can control the arm around the reference point. -

t

Q = Qc − kD e˙ − kP e + kI

e dt

(14.87)

0

"

x − π /2 e = q − qd = 1 x2

# (14.88)

770

14 Control Techniques

Example 429 P D control. To show how a linear control law will be applied on general dynamics of robots. Consider a robot with dynamic equations as below. ˙ + G(q) Q = D(q) q¨ + H(q, q) ˙ q˙ + G(q) = D(q) q¨ + C(q, q)

(14.89)

Let us define a P D control law. Q = −kD e˙ − kP e e = q − qd

(14.90) (14.91)

Applying the P D control will produce the following control equation.   ˙ q˙ + G(q) + kD q˙ − q˙ d − kP (q − qd ) = 0 D(q) q¨ + C(q, q)

(14.92)

This control is ideal when qd is a constant vector associated with a specific configuration of a robot, and therefore q˙ d = 0. In this case the P D controller can make the configuration qd globally stable. In case of a path given by q = qd (t), we define a modified P D controller, ˙ q˙ d + G(q) − kD e˙ − kP e Q = D(q)q¨ d + C(q, q)

(14.93)

and reduce the closed-loop equation to an equation for the error signal e. ˙ + kD ) e˙ + kP e = 0 D(q)e¨ + (C(q, q)

(14.94)

The linearization of this equation about a control point q = qd = const provides a stable dynamics for the error signal. D(qd )e¨ + kD e˙ + kP e = 0

14.4

(14.95)

Sensing and Control

Position, velocity, acceleration, and force sensors are the most common sensors used in robotics. Consider the inverted pendulum shown in Fig. 14.4 as a one DOF manipulator with the following equation of motion: ml 2 θ¨ − cθ˙ − mgl sin θ = Q

(14.96)

From an open-loop control viewpoint, we need to provide a moment Qc (t) to force the manipulator to follow a desired path of motion θ d (t). Qc = ml 2 θ¨ d − cθ˙ d − mgl sin θ d (14.97) In robotics, we calculate Qc from the dynamics equation and dictate it to actuators. The manipulator will respond to the applied moment and will move accordingly. The equation of motion (14.96) is a model of the actual manipulator. However, there are many unmodeled phenomena that we cannot include them in our equation of motion or we cannot model them. Some are temperature, air pressure, exact gravitational acceleration, or even the physical parameters such as m and l that we assume to have good accuracy. So, applying a control command Qc will move the manipulator and provide a real value for θ, ˙ θ, ¨ which are not necessarily equal to θ d , θ˙ d , θ¨ d . Sensing is now important to measure the actual angle θ , angular velocity θ, θ˙ , and angular acceleration θ¨ to compare with θ d , θ˙ d , θ¨ d and make sure that the manipulator is following the desired path. This is the reason why the feedback control systems and the error signal e = θ − θ d were introduced. Robots can interact with the environment. Therefore, a robot needs two types of sensors: 1-sensing the robot’s internal parameters, which are called proprioceptors, and 2-sensing the robot’s environmental parameters, which are called exteroceptors. The most important interior parameters are position, velocity, acceleration, force, torque, and inertia.

14.4 Sensing and Control

771

14.4.1 Position Sensors Rotary Encoders In robotics, almost all kinds of actuators provide a rotary motion encoder. Then we may provide a rotation motion for a revolute joint, or a translation motion for a prismatic joint by using gears. So, it is ideally possible to sense the relative position of connected links by joints based on the angular position of the actuators. A possible error in position sensing is due to non-rigidity and backlash. The most common position sensor is a rotary encoder that can be optical, magnetic, or electrical. As an example, when the encoder shaft rotates, a disk counting a pattern of fine lines interrupting a light beam. A photodetector converts the light pulses into a countable binary waveform. The shaft angle is then determined by counting the number of pulses. Resolvers We may design an electronic device to provide a mathematical function of the joint variable. The mathematical function might be sine, cosine, exponential, or any combination of mathematical functions. The joint variable is then calculated indirectly by resolving the mathematical functions. Sine and cosine functions are more common. Potentiometers Using an electrical bridge, the potentiometers can provide an electric voltage proportional to the joint position. LVDT and RVDT LVDT/RVDT or a Linear/Rotary Variable Differential Transformer operates with two transformers sharing the same magnetic core. When the core moves, the output of one transformer increases while the other’s output decreases. The difference of the current is a measure of the core position.

14.4.2 Speed Sensors Tachometers A tachometer is a name for any velocity sensor. Tachometers usually provide an analog signal proportional to the angular velocity of a shaft. There are a vast amount of different designs for tachometers, using different physical characteristics such as magnetic field. Rotary Encoders Any rotary sensor can be equipped with a time measuring system and become an angular velocity sensor. The encoder counts the light pulses of a rotating disk and the angular velocity is then determined by time between pulses. Differentiating Devices Any kind of position sensor can be equipped with a digital differentiating device to become a speed sensor. The digital or numerical differentiating needs a simple processor. Numerical differentiating is generally an erroneous process. Integrating Devices The output signal of an accelerometer can be numerically integrated to provide a velocity signal. The digital or numerical integrating also needs a processor. Numerical differentiating is generally a smooth and reliable process.

14.4.3 Acceleration Sensors Acceleration sensors work based on Newton’s second law of motion. They sense the force that causes an acceleration of a known mass. There are many types of accelerometers. Stress-strain gage, capacitive, inductive, piezoelectric, and microaccelerometers are the most common. In any of these types, force causes a proportional displacement in an elastic material, such as deflection in a micro-cantilever beam, and the displacement is proportional to the acceleration. Accelerometers are the most common sensors with great accuracy and many applications include measurement of acceleration, angular acceleration, velocity, position, angular velocity, frequency, impulse, force, tilt, and orientation. Force and Torque Sensors Any concept and method that we use in sensing acceleration may also be used in force and torque sensing. We equip the wrists of a robot with at least three force sensors to measure the contact forces and moments with the environment. The wrist’s force sensors are important especially when the robot’s job is involved with touching unknown surfaces and objects.

772

14 Control Techniques

Proximity Sensors Proximity sensors are utilized to detect the existence of an object, field, or special material before interacting with it. Inductive, capacitive, hall effect, sonic, ultrasonic, and optical are the most common proximity sensors. The inductive sensors can sense the existence of a metallic object due to a change in inductance. The capacitive sensors can sense the existence of gas, liquid, or metals that cause a change in capacitance. Hall effective sensors work based on the interaction between the voltage in a semiconductor material and magnetic fields. These sensors can detect existence of magnetic fields and materials. Sonic, ultrasonic, and optical sensors work based on the reflection or modification in an emitted signal by objects.

14.5

Summary

In an open-loop control algorithm, we calculate the robot’s required torque commands Qc for a given joint path qd = q(t) based on the equations of motion Qc = D(qd ) q¨ d + H(qd , q˙ d ) + G(qd ). (14.98) However, there can be a difference between the actual joint variables and the desired values. The difference is called error e e = q − qd

(14.99)

e˙ = q˙ − q˙ d .

(14.100)

By measuring the error command, we may define a control law and calculate a new control command vector Q = Qc + kD e˙ + kP e

(14.101)

to compensate for the error. The parameters kP and kD are gain diagonal matrices.  constant    The control law compares the actual joint variables q, q˙ with the desired values qd , q˙ d and generates a command proportionally. Applying the new control command changes the dynamic equations of the robot to ˙ + G(q). Qc + kD e˙ + kP e = D(q) q¨ + H(q, q)

(14.102)

This is a closed-loop control algorithm, in which the control commands are calculated based on the difference between actual and desired variables. Computed torque control   ˙ + G(q) Q = D(q) q¨ d − kD e˙ − kP e + H(q, q) (14.103) is an applied closed-loop control law in robotics to make a robot follow a desired path.

14.6 Key Symbols

14.6

Key Symbols

a, b A A, B B c Ci e fc , fc f, f g G, B0 i I = [I ] J k kP kD l m q qd Q Qc Qf b Qff r ri t x, y, z X, Y, Z

Real and imaginary parts of a complex number Coefficient matrix Real coefficients Body coordinate frame Damping Complex coefficients Error, exponential function Actuator force control command Actual force command Gravitational acceleration Global coordinate frame, base coordinate frame Imaginary unit number Identity matrix, moment of inertia Jacobian Stiffness Proportional constant control gain Derivative constant control gain Length Mass Actual vector of joint variables Desired path of joint Actuators’ actual command Actuators’ control command Feedback command Feedforward command Position vectors, homogenous position vector The element i of r Time Local Cartesian coordinates Global Cartesian coordinates

Greek δ λ θ ωn ξ

Small increment of a parameter Characteristic value, eigenvalue Rotary joint angle Natural frequency Damping ratio

Symbol DOF R Re

Degree of freedom Real numbers set Real

773

774

14 Control Techniques

Exercises 1. Response of second-order systems. Solve the characteristic equations and determine the response of the following second-order systems at x(1), if they start from x(0) = 1, x(0) ˙ = 0: (a) x¨ + 2x˙ + 5x = 0 (14.104) (b) x¨ + 2x˙ + x = 0

(14.105)

x¨ + 4x˙ + x = 0

(14.106)

(b) 2. Modified P D control. Apply a modified P D control law f = −kP e − kd x˙

e = x − xd

(14.107)

to a second-order linear system mx¨ + cx˙ + kx = f

(14.108)

and reduce the system to a second-order equation in an error signal. me¨ + (c + kD )e˙ + (k + kP )e = kxD

(14.109)

Then, calculate the steady state error for a step input. x = xd = const

(14.110)

3. Modified P I D control. Apply a modified P D control law f = −kP e − kd x˙ − kI

t

edt

e = x − xd

(14.111)

0

to a second-order linear system mx¨ + cx˙ + kx = f

(14.112)

and reduce the system to a third-order equation in an error signal. ... m e + (c + kD )e¨ + (k + kP )e˙ + kI e = 0

(14.113)

Then, find the P I D gains such that the characteristic equation of the system simplifies to 

 λ2 + 2ξ ωn λ + ω2n (λ + β) = 0

(14.114)

4. Linearization. Linearize the given equations and determine the stability of the linearized set of equations. x˙1 = x22 + x1 cos x2

(14.115)

x˙2 = x2 + (1 + x1 + x2 )x1 + x1 sin x2

(14.116)

5. 2R manipulator control from the general robotic equations. Expand the control equations for a 2R planar manipulator using the following control law:   ˙ + G(q) Q = D(q) q¨ d − kD e˙ − kP e + H(q, q)

(14.117)

Exercises

775

Fig. 14.7 A mass-spring oscillator on a rough surface and attached to a wall

6. One-link manipulator control. A one-link manipulator is shown in Fig. 14.4. (a) Derive the equation of motion. (b) Determine a rest-to-rest joint path between θ(0) = 45 deg and θ(0) = −45 deg. (c) Solve the time optimal control of the manipulator and determine the torque Qc (t) for the following date: m = 1 kg

7.

8.

9.

10.

l = 1m

|Q| ≤ 120 Nm

(14.118)

(d) Now assume the mass is m = 1.01 kg and solve the equation of motion numerically by feeding the calculated torques Qc (t). Determine the position and velocity errors at the end of the motion. (e) Design a computed torque control law to compensate the error during the motion.  Mass-spring under friction control. Calculate the optimal control input f for the mass-spring system of Fig. 14.7. The optimal control command is limited |f (t)| ≤ 100 N, and moves the mass m = 1 kg rest-to-rest from x(0) = 0 to x(tf ) = 10 m. The mass is moving on a rough surface with coefficient μ = 0.5 and is attached to a wall by a linear spring with stiffness k = 5 N/m. Increase the stiffness %10, and design a computed torque control law to eliminate error during the motion.  2R manipulator control. (a) Solve Exercise 12.19 and calculate the optimal control inputs. (b) Increase the masses by 10%, and solve the dynamic equations numerically. (c) Determine the position and velocity error in Cartesian and joint spaces by applying the calculated optimal inputs. (d) Design a computed torque control law to eliminate error during the motion.  PR planar manipulator control. (a) Solve Exercise 13.13 and calculate the optimal control inputs. (b) Increase the gravitational acceleration by 10%, and solve the dynamic equations numerically. (c) Determine the position and velocity error in Cartesian and joint spaces by applying the calculated optimal inputs. (d) Design a computed torque control law to eliminate error during the motion. Sensing and measurement. Consider the one DOF manipulator in Eq. (14.96). To control the manipulator, we need to sense the actual angle θ, angular velocity θ˙ , and angular acceleration θ¨ and compare them with the desired values of θ d , θ˙ d , θ¨ d to make sure that the manipulator is following the desired path. Can we measure the actual moment Q, that the actuator is providing, and compare with the predicted value Qc instead? Does making Q equal to Qc guarantee that the manipulator does what it is supposed to do?

Global Frame Triple Rotation

A

In this appendix, the 12 combinations of triple rotation about global fixed axes are presented. ⎡

QZ,α

⎤ cos α − sin α 0 = ⎣ sin α cos α 0 ⎦ 0 0 1 ⎡

QY,β

QX,γ

1. QX,γ QY,β QZ,α

2. QY,γ QZ,β QX,α

3. QZ,γ QX,β QY,α

4. QZ,γ QY,β QX,α

5. QY,γ QX,β QZ,α

6. QX,γ QZ,β QY,α

cos β =⎣ 0 − sin β ⎡ 1 0 = ⎣ 0 cos γ 0 sin γ

⎤ 0 sin β 1 0 ⎦ 0 cos β ⎤ 0 − sin γ ⎦ cos γ

⎤ cα cβ −cβ sα sβ = ⎣ cγ sα + cα sβ sγ cαcγ − sα sβ sγ −cβ sγ ⎦ sα sγ − cα cγ sβ cαsγ + cγ sα sβ cβ cγ

(A.1)

(A.2)

(A.3)

⎡

⎤ cβ cγ sα sγ − cα cγ sβ cα sγ + cγ sα sβ ⎦ = ⎣ sβ cα cβ −cβ sα −cβ sγ cγ sα + cα sβ sγ cα cγ − sα sβ sγ

(A.4)

⎡

⎤ cα cγ − sα sβ sγ −cβ sγ cγ sα + cα sβ sγ = ⎣ cα sγ + cγ sα sβ cβ cγ sα sγ − cα cγ sβ ⎦ −cβ sα sβ cα cβ

(A.5)

⎡

⎤ cβ cγ −cα sγ + cγ sα sβ sα sγ + cα cγ sβ = ⎣ cβ sγ cα cγ + sα sβ sγ −cγ sα + cα sβ sγ ⎦ −sβ cβ sα cα cβ

(A.6)

⎡

⎤ cα cγ + sα sβ sγ −cγ sα + cα sβ sγ cβ sγ =⎣ cβ sα cα cβ −sβ ⎦ −cα sγ + cγ sα sβ sα sγ + cα cγ sβ cβ cγ

(A.7)

⎡

⎤ cα cβ −sβ cβ sα = ⎣ sα sγ + cα cγ sβ cβ cγ −cα sγ + cγ sα sβ ⎦ −cγ sα + cα sβ sγ cβ sγ cα cγ + sα sβ sγ

(A.8)

⎡

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

(A.9)

777

778

7. QX,γ QY,β QX,α

8. QY,γ QZ,β QY,α

9. QZ,γ QX,β QZ,α

10. QX,γ QZ,β QX,α

11. QY,γ QX,β QY,α

12. QZ,γ QY,β QZ,α

A Global Frame Triple Rotation

⎡

⎤ cβ sα sβ cα sβ = ⎣ sβ sγ cα cγ − cβ sα sγ −cγ sα − cα cβ sγ ⎦ −cγ sβ cα sγ + cβ cγ sα −sα sγ + cα cβ cγ ⎤ −sα sγ + cα cβ cγ −cγ sβ cα sγ + cβ cγ sα ⎦ =⎣ cα sβ cβ sα sβ −cγ sα − cα cβ sγ sβ sγ cα cγ − cβ sα sγ

(A.10)

⎡

⎤ cα cγ − cβ sα sγ −cγ sα − cα cβ sγ sβ sγ = ⎣ cα sγ + cβ cγ sα −sα sγ + cα cβ cγ −cγ sβ ⎦ sα sβ cα sβ cβ

(A.11)

⎡

⎤ cβ −cα sβ sα sβ = ⎣ cγ sβ −sα sγ + cα cβ cγ −cα sγ − cβ cγ sα ⎦ sβ sγ cγ sα + cα cβ sγ cα cγ − cβ sα sγ

(A.12)

⎡

⎤ cα cγ − cβ sα sγ sβ sγ cγ sα + cα cβ sγ ⎦ =⎣ sα sβ cβ −cα sβ −cα sγ − cβ cγ sα cγ sβ −sα sγ + cα cβ cγ

(A.13)

⎡

⎤ −sα sγ + cα cβ cγ −cα sγ − cβ cγ sα cγ sβ = ⎣ cγ sα + cα cβ sγ cα cγ − cβ sα sγ sβ sγ ⎦ −cα sβ sα sβ cβ

(A.14)

⎡

(A.15)

B

Local Frame Triple Rotation

In this appendix, the 12 combinations of triple rotation about local axes are presented. ⎡

Az,ϕ

⎤ cos ϕ sin ϕ 0 = ⎣ − sin ϕ cos ϕ 0 ⎦ 0 0 1

⎤ cos θ 0 − sin θ Ay,θ = ⎣ 0 1 0 ⎦ sin θ 0 cos θ ⎡ ⎤ 1 0 0 Ax,ψ = ⎣ 0 cos ψ sin ψ ⎦ 0 − sin ψ cos ψ

(B.1)

⎡

1. Ax,ψ Ay,θ Az,ϕ

2. Ay,ψ Az,θ Ax,ϕ

3. Az,ψ Ax,θ Ay,ϕ

4. Az,ψ Ay,θ Ax,ϕ

5. Ay,ψ Ax,θ Az,ϕ

6. Ax,ψ Az,θ Ay,ϕ

⎤ cθ cϕ cθ sϕ −sθ = ⎣ −cψ sϕ + cϕ sθ sψ cϕ cψ + sθ sϕ sψ cθ sψ ⎦ sϕ sψ + cϕ sθ cψ −cϕ sψ + sθ cψ sϕ cθ cψ

(B.2)

(B.3)

⎡

⎤ cθ cψ sϕ sψ + cϕ sθ cψ −cϕ sψ + sθ cψ sϕ ⎦ = ⎣ −sθ cθ cϕ cθ sϕ cθ sψ −cψ sϕ + cϕ sθ sψ cϕ cψ + sθ sϕ sψ

(B.4)

⎡

⎤ cϕ cψ + sθ sϕ sψ cθ sψ −cψ sϕ + cϕ sθ sψ = ⎣ −cϕ sψ + sθ cψ sϕ cθ cψ sϕ sψ + cϕ sθ cψ ⎦ cθ sϕ −sθ cθ cϕ

(B.5)

⎡

⎤ cθ cψ cϕ sψ + sθ cψ sϕ sϕ sψ − cϕ sθ cψ = ⎣ −cθ sψ cϕ cψ − sθ sϕ sψ cψ sϕ + cϕ sθ sψ ⎦ sθ −cθ sϕ cθ cϕ

(B.6)

⎡

⎤ cϕ cψ − sθ sϕvsψ cψ sϕ + cϕ sθ sψ −cθ sψ =⎣ −cθ sϕ cθ cϕ sθ ⎦ cϕ sψ + sθ cψ sϕ sϕ sψ − cϕ sθ cψ cθ cψ

(B.7)

⎡

⎤ cθ cϕ sθ −cθ sϕ = ⎣ sϕ sψ − cϕ sθ cψ cθ cψ cϕvsψ + sθ cψ sϕ ⎦ cψ sϕ + cϕ sθ sψ −cθ sψ cϕ cψ − sθ sϕ sψ

(B.8)

⎡

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

(B.9)

779

780

7. Ax,ψ Ay,θ Ax,ϕ

8. Ay,ψ Az,θ Ay,ϕ

9. Az,ψ Ax,θ Az,ϕ

10. Ax,ψ Az,θ Ax,ϕ

11. Ay,ψ Ax,θ Ay,ϕ

12. Az,ψ Ay,θ Az,ϕ

B Local Frame Triple Rotation

⎡

⎤ cθ sθ sϕ −cϕ sθ = ⎣ sθ sψ cϕ cψ − cθ sϕ sψ cψ sϕ + cθ cϕ sψ ⎦ sθ cψ −cϕ sψ − cθ cψ sϕ −sϕ sψ + cθ cϕ cψ ⎤ −sϕ sψ + cθ cϕ cψ sθ cψ −cϕ sψ − cθ cψ sϕ ⎦ =⎣ −cϕ sθ cθ sθ sϕ cψ sϕ + cθ cϕ sψ sθ sψ cϕ cψ − cθ sϕ sψ

(B.10)

⎡

⎤ cϕ cψ − cθ sϕ sψ cψ sϕ + cθ cϕ sψ sθ sψ = ⎣ −cϕ sψ − cθ cψ sϕ −sϕ sψ + cθ cϕ cψ sθ cψ ⎦ sθ sϕ −cϕ sθ cθ

(B.11)

⎡

⎤ cθ cϕ sθ sθ sϕ = ⎣ −sθ cψ −sϕ sψ + cθ cϕ cψ cϕ sψ + cθ cψ sϕ ⎦ sθ sψ −cψ sϕ − cθ cϕ sψ cϕ cψ − cθ sϕ sψ

(B.12)

⎡

⎤ cϕ cψ − cθ sϕ sψ sθ sψ −cψ sϕ − cθ cϕ sψ ⎦ =⎣ sθ sϕ cθ cϕ sθ cϕ sψ + cθ cψ sϕ −sθ cψ −sϕ sψ + cθ cϕ cψ

(B.13)

⎡

⎤ −sϕ sψ + cθ cϕ cψ cϕ sψ + cθ cψ sϕ −sθ cψ = ⎣ −cψ sϕ − cθ cϕ sψ cϕ cψ − cθ sϕ sψ sθ sψ ⎦ cϕ sθ sθ sϕ cθ

(B.14)

⎡

(B.15)

C

Principal Central Screws Triple Combination

In this appendix, the six combinations of triple principal central screws are presented. ˆ 1. sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) sˇ (hZ , α, K) ⎤ cαcβ −cβsα sβ γ pX + αpZ sβ ⎢ cγ sα + cαsβsγ cαcγ − sαsβsγ −cβsγ βpY cγ − αpZ cβsγ ⎥ ⎥ =⎢ ⎣ sαsγ − cαcγ sβ cαsγ + cγ sαsβ cβcγ βpY sγ + αpZ cβcγ ⎦ 0 0 0 1 ⎡

(C.1)

ˆ sˇ (hX , γ , Iˆ) 2. sˇ (hY , β, Jˆ) sˇ (hZ , α, K) ⎤ cαcβ sβsγ − cβcγ sα cγ sβ + cβsαsγ αpZ sβ + γ pX cαcβ ⎢ sα cαcγ −cαsγ βpY + γ pX sα ⎥ ⎥ =⎢ ⎣ −cαsβ cβsγ + cγ sαsβ cβcγ − sαsβsγ αpZ cβ − γ pX cαsβ ⎦ 0 0 0 1 ⎡

(C.2)

ˆ sˇ (hX , γ , Iˆ) sˇ (hY , β, Jˆ) 3. sˇ (hZ , α, K) ⎡

⎤ cαcβ − sαsβsγ −cγ sα cαsβ + cβsαsγ γ pX cα − βpY cγ sα ⎢ cβsα + cαsβsγ cαcγ sαsβ − cαcβsγ γ pX sα + βpY cαcγ ⎥ ⎥ =⎢ ⎣ −cγ sβ sγ cβcγ αpZ + βpY sγ ⎦ 0 0 0 1

(C.3)

ˆ sˇ (hY , β, Jˆ) sˇ (hX , γ , Iˆ) 4. sˇ (hZ , α, K) ⎡

⎤ cαcβ cαsβsγ − cγ sα sαsγ + cαcγ sβ γ pX cαcβ − βpY sα ⎢ cβsα cαcγ + sαsβsγ cγ sαsβ − cαsγ βpY cα + γ pX cβsα ⎥ ⎥ =⎢ ⎣ −sβ cβsγ cβcγ αpZ − γ pX sβ ⎦ 0 0 0 1

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

(C.4)

781

782

C Principal Central Screws Triple Combination

ˆ 5. sˇ (hY , β, Jˆ) sˇ (hX , γ , Iˆ) sˇ (hZ , α, K) ⎤ cαcβ + sαsβsγ cαsβsγ − cβsα cγ sβ γ pX cβ + αpZ cγ sβ ⎢ cγ sα cαcγ −sγ βpY − αpZ sγ ⎥ ⎥ =⎢ ⎣ cβsαsγ − cαsβ sαsβ + cαcβsγ cβcγ αpZ cβcγ − γ pX sβ ⎦ 0 0 0 1 ⎡

(C.5)

ˆ sˇ (hY , β, Jˆ) 6. sˇ (hX , γ , Iˆ) sˇ (hZ , α, K) ⎤ cαcβ −sα cαsβ γ pX − βpY sα ⎢ sβsγ + cβcγ sα cαcγ cγ sαsβ − cβsγ βpY cαcγ − αpZ sγ ⎥ ⎥ =⎢ ⎣ cβsαsγ − cγ sβ cαsγ cβcγ + sαsβsγ αpZ cγ + βpY cαsγ ⎦ 0 0 0 1 ⎡

(C.6)

Industrial Link DH Matrices

D

Most industrial robots are made by connected links that use one of the following joint configurations with their the associated Denavit–Hartenberg (DH ) homogenous transformation matrices. The angle in parenthesis is α i , the angle from zi−1 to zi about xi . The relative orientation of the axes zi−1 and zi is indicated by a parallel sign (), if their joint axes are parallel; or by an orthogonal sign (), if their joint axes are intersecting at a right angle; or by a perpendicular sign (⊥), if their joint axes are at a right angle with respect to their common normal. 1 2 3 4 5 6 7 8 9 10 11 12

R  R(0) R  R(180) R ⊥ R(90) R ⊥ R(−90) R  R(90) R  R(−90) P  R(0) P  R(180) P ⊥ R(90) P ⊥ R(−90) P  R(90) P  R(−90)

or or or or or or or or or or or or

R  P (0) R  P (180) R ⊥ P (90) R ⊥ P (−90) R  P (90) R  P (−90) P  P (0) P  P (180) P ⊥ P (90) P ⊥ P (−90) P  P (90) P  P (−90)

Denavit–Hartenberg homogenous transformation matrix i−1 Ti , ⎡

⎤ cos θ i − sin θ i cos α i sin θ i sin α i ai cos θ i ⎢ sin θ i cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 sin α i cos α i di ⎦ 0 0 0 1

(D.1)

depends on four parameters according to the following table. ai αi di θi

along xi about xi along zi−1 about zi−1

from zi−1 to zi from zi−1 to zi from xi−1 to xi from xi−1 to xi

Cases 1, 2. Links with RR or RP ai = const

α i = 0 or 180 deg

di = 0 θ i = variable

Figure D.1 illustrates a link RR(0), and Fig. D.2 illustrates a link RR(180). If the proximal joint of link (i) is revolute, the distal joint is either revolute or prismatic, and the joint axes at two ends are parallel, then α i = 0 or α i = 180 deg, ai is the

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

783

784

D Industrial Link DH Matrices

Fig. D.1 A link RR(0)

Fig. D.2 A link RR(180)

distance between the joint axes, and θ i is the only variable parameter. The joint distance di = const is the distance between the origin of Bi and Bi−1 along zi−1 . We usually set (xi , yi ) and (xi−1 , yi−1 ) coplanar to get di = 0. The xi and xi−1 -axes are parallel for a link RR at the rest position. Therefore, the transformation matrix i−1 Ti for such a link with α i = 0 known as RR(0) or RP(0) is R  R(0) or R  P (0) ⎡ ⎤ cos θ i − sin θ i 0 ai cos θ i ⎢ sin θ i cos θ i 0 ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 1 di ⎦ 0 0 0 1

(D.2)

while for a link with α i = 180 deg and RR(180) or RP(180) is R  R(180) or R  P (180) ⎡ ⎤ cos θ i sin θ i 0 ai cos θ i ⎢ sin θ i − cos θ i 0 ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 −1 di ⎦ 0 0 0 1

(D.3)

Cases 3, 4. Links with R⊥R or R⊥P ai = const di = 0

α i = 90 deg or − 90 deg θ i = variable

Figure D.3 illustrates a link R⊥R(90), and Fig. D.4 illustrates a link R⊥R(−90). If the proximal joint of link (i) is revolute, the distal joint is either revolute or prismatic, and if the joint axes at two ends are perpendicular, then α i = 90 deg or α i = −90 deg, ai is the distance between the joint axes on xi , and θ i is the only variable parameter. The joint distance di = const is the distance between the origin of Bi and Bi−1 along zi−1 . We usually set (xi , yi ) and (xi−1 , yi−1 ) coplanar to get di = 0.

D Industrial Link DH Matrices

785

Fig. D.3 A R⊥R(90) link

Fig. D.4 A R⊥R(−90) link

The R⊥R link is made by twisting the RR link 90 deg about its center line xi−1 -axis. The xi and xi−1 -axes are parallel for a link R⊥R at the rest position. Therefore, the transformation matrix i−1 Ti for such a link with α i = 90 deg known as R⊥R(90) or R⊥P(90) is R ⊥ R(90) or R ⊥ P (90) ⎡ ⎤ cos θ i 0 sin θ i ai cos θ i ⎢ sin θ i 0 − cos θ i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 0 0 0 1

(D.4)

while for a link with α i = −90 deg and R⊥R(−90) or R⊥P(−90) is R ⊥ R(−90) or R ⊥ P (−90) ⎡ ⎤ cos θ i 0 − sin θ i ai cos θ i ⎢ sin θ i 0 cos θ i ai sin θ i ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1

(D.5)

Cases 5, 6. Links with RR or RP ai = 0

α i = 90 deg or − 90 deg

di = 0

θ i = variable

Figure D.5 illustrates a link RR(90), and Fig. D.6 illustrates a link RR(−90). If the proximal joint of link (i) is revolute and the distal joint is either revolute or prismatic and the joint axes at two ends are intersecting orthogonal, then α i = 90 deg or α i = −90 deg, ai = 0. di = cte is the distance between the coordinate origin on zi−1 , and θ i is the only variable parameter. It is possible to have or assume di = 0 at the rest position. When di = 0, the xi and xi−1 -axes of a link RR are coincident

786

D Industrial Link DH Matrices

Fig. D.5 An RR(90) link

Fig. D.6 An RR(−90) link

and when di = 0 they are parallel. Therefore, the transformation matrix i−1 Ti for such a link with α i = 90 deg and RR(90) or RP(90) is R  R(90) or R  P (90) ⎡ ⎤ cos θ i 0 sin θ i 0 ⎢ sin θ i 0 − cos θ i 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 0 0 0 1

(D.6)

while for a link with α i = −90 deg and RR(−90) or RP(−90) is R  R(−90) or R  P (−90) ⎡ ⎤ cos θ i 0 − sin θ i 0 ⎢ sin θ i 0 cos θ i 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1

(D.7)

Cases 7, 8. Links with PR or PP ai = const

α i = 0 or 180 deg

di = variable

θi = 0

Figure D.7 illustrates a link PR(0), and Fig. D.8 illustrates a link PR(180). If the proximal joint of link (i) is prismatic, its distal joint is either revolute or prismatic, and the joint axes at two ends are parallel, then α i = 0 or α i = 180 deg, θ i = 0. ai = const is the distance between the joint axes on xi , and di is the only variable parameter. It is possible to have ai = 0. The transformation matrix i−1 Ti for a link with α i = 0 and PR(0) or PP(0) is

D Industrial Link DH Matrices

787

Fig. D.7 A PR(0) link

Fig. D.8 A PR(180) link

P  R(0) or P  P (0) ⎤ ⎡ 1 0 0 ai ⎢0 1 0 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 1 di ⎦ 000 1

(D.8)

while for a link with α i = 180 deg and PR(180) or PP(180) is P  R(180) or P  P (180) ⎡ ⎤ 1 0 0 ai ⎢ 0 −1 0 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 0 −1 di ⎦ 0 0 0 1

(D.9)

The origin of the Bi−1 -frame can arbitrarily be chosen at any point on the zi−1 -axis or parallel to the zi−1 -axis. A simple setup is to locate the origin oi of a prismatic joint at the previous origin oi−1 . This sets ai = 0 and furthermore sets the initial value of the joint variable di = 0, where di will vary when oi slides up and down parallel to the zi−1 -axis. Cases 9, 10. Links with P⊥R or P⊥P ai = const

α i = 90 or − 90 deg

di = variable

θi = 0

Figure D.9 illustrates a link P⊥R(90), and Fig. D.10 illustrates a link P⊥R(−90). If the proximal joint of link (i) is prismatic and its distal joint is either revolute or prismatic with orthogonal, then α i = 90 deg or α i = −90 deg, θ i = 0. ai = const is the distance between the joint axes on xi , and di is the only variable parameter. The transformation matrix i−1 Ti for a link with α i = 90 deg and P⊥R(90) or P⊥P(90) is

788

D Industrial Link DH Matrices

Fig. D.9 A P⊥R(90) link

Fig. D.10 A P⊥R(−90) link

P ⊥ R(90) or P ⊥ P (90) ⎡ ⎤ 1 0 0 ai ⎢ 0 0 −1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 00 0 1

(D.10)

while for a link with α i = −90 deg and P⊥R(−90) or P⊥P(−90) it is P ⊥ R(−90) or P ⊥ P (−90) ⎡ ⎤ 1 0 0 ai ⎢0 0 1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1

(D.11)

Cases 11, 12. Links with PR or PP. ai = 0

α i = 90 or − 90 deg

di = variable

θi = 0

Figure D.11 illustrates a link PR(90), and Fig. D.12 illustrates a link PR(−90). If the proximal joint of link (i) is prismatic and the distal joint is either revolute or prismatic and the joint axes at two ends are intersecting orthogonal, then α i = 90 deg or α i = −90 deg, θ i = 0, and ai = 0, and di is the only variable parameter. The xi -axis must be perpendicular to the plane of the zi−1 and zi -axes, and it is possible to have ai = 0. Therefore, the transformation matrix i−1 Ti for a link with α i = 90 deg and PR(90) or PP(90) is

D Industrial Link DH Matrices

789

Fig. D.11 A PR(90) link

Fig. D.12 A link PR(−90)

P  R(90) or P  P (90) ⎡ ⎤ 10 0 0 ⎢ 0 0 −1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 1 0 di ⎦ 00 0 1

(D.12)

while for a link with α i = −90 deg and PR(−90) or PP(−90) it is P  R(−90) or P  P (−90) ⎡ ⎤ 1 0 0 0 ⎢0 0 1 0 ⎥ i−1 ⎥ Ti = ⎢ ⎣ 0 −1 0 di ⎦ 0 0 0 1

(D.13)

E

Matrix Calculus

An m × n matrix A consists of mn numbers arranged in m rows and n columns. The element in row i and column j of the matrix A is denoted by aij . ⎤ ⎡ a11 a12 · · · a1n ⎢ a21 a22 · · · a2n ⎥   ⎥ A=⎢ (E.1) ⎣ · · · · · · · · · · · · ⎦ = aij am1 am2 · · · amn An m × 1 matrix is a column vector v of dimension m, and a 1 × n matrix is a row vector w of dimension n. An m × n matrix is called a square matrix if m = n. ⎡ ⎤ v11 ⎢ v21 ⎥ ⎥ v=⎢ (E.2) ⎣ ··· ⎦ vm1   w = wm1 wm2 · · · wmn (E.3)     Two matrices A = aij and B = bij are equal only if aij = bij . Diagonal Matrix   A diagonal matrix is a square matrix Λ = λij such that λij = 0 for i = j . The main diagonal of a square matrix are elements from top left to bottom right. ⎡ ⎤ λ11 0 · · · 0 ⎢ 0 λ22 · · · 0 ⎥ ⎥ Λ=⎢ (E.4) ⎣··· ··· ··· ··· ⎦ 0 0 · · · λnn A diagonal matrix may also be abbreviated as: Λ = {λ11 , λ22 , λ22 , · · · , λnn }

(E.5)

  A scalar matrix S = sij is a diagonal matrix whose diagonal elements are equal sii = s. ⎡

⎤ s 0 ··· 0 ⎢ 0 s ··· 0 ⎥ ⎥ S=⎢ ⎣··· ··· ··· ···⎦ 0 0 ··· s

(E.6)

If all elements of the diagonal of S are unity, the matrix is the identity matrix, or unit matrix, and shown by I. ⎡

⎤ 1 0 ··· 0 ⎢ 0 1 ··· 0 ⎥ ⎥ I=⎢ ⎣··· ··· ··· ···⎦ 0 0 ··· 1

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

(E.7)

791

792

E Matrix Calculus

A null matrix is a matrix of any shape in which every entry is zero. ⎡

⎤ 0 0 ··· 0 ⎢ 0 0 ··· 0 ⎥ ⎥ 0=⎢ ⎣··· ··· ··· ···⎦ 0 0 ··· 0

(E.8)

Trace   Trace of a square matrix A = aij is sum of its diagonal elements. tr A = a11 + a22 + · · · + ann tr [AB] = tr [BA]

(E.9) (E.10)

Matrix Arithmetic

A+B = B+A=C

(E.11)

cij = aij + bij

(E.12)

AB = C

cij =



aim bmj

(E.13)

m

AB = BA

(E.14)

[AB] C = A [BC] = D     aim bmn cj n dij = (aim bmn ) cj n = m,n

(E.15) (E.16)

m,n

[A + B] C = AC + BC = D    dij = aim cnj + bim cnj (aim + bim ) cnj = m

(E.17) (E.18)

m

Determinants The determinant of a 2 × 2 matrix A,

" A=

a11 a12 a21 a22

#

  = aij

(E.19)

with elements aij comprising real or complex numbers, or functions, denoted by |A| or by det A, and is defined by: det A = a11 a22 − a12 a21 The determinant of a 3 × 3 matrix A,

⎤ a11 a12 a13   A = ⎣ a21 a22 a23 ⎦ = aij a31 a32 a33

(E.20)

⎡

(E.21)

with elements aij comprising real or complex numbers, or functions, denoted by |A| or by det A, and is defined by: det A = a11 a22 a33 + a12 a23 a31 + a21 a32 a13 −a13 a22 a31 − a23 a32 a11 − a12 a21 a33

(E.22)

E Matrix Calculus

793

To calculate the determinant of an n × n matrix A, ⎡

a11 ⎢ a21 A=⎢ ⎣··· an1

a12 a22 ··· an2

⎤ · · · a1n   · · · a2n ⎥ ⎥ = aij ··· ··· ⎦ · · · ann

(E.23)

we need first to define the minor Mij associated with the element aij as determinant the order (n − 1) derived from by deletion of its ith row and j th column; and second, to define the cofactor Cij associated with the element aij as: Cij = (−1)i+j Mij

(E.24)

The determinant of A may be based on expansion of elements of a row or of a column of A. Expansion of det A by elements of the ith row is n  det A = aij Cij i = 1, 2, 3, · · · , n (E.25) j =1

and expansion of det A by elements of the j th column is det A =

n 

aij Cij

j = 1, 2, 3, · · · , n

(E.26)

i=1

The expansion method to determine the determinant is called the Laplace expansion method. As an example, let us calculate det A, where ⎡

⎤ −2 1 3 A = ⎣ 1 2 −1 ⎦ 4 −1 −2

(E.27)

|A| = C21 + 2C22 − C23

(E.28)

Expanding |A| by elements of its second row gives



1 3

= −1 C21 = (−1) −1 −2



2+2 −2 3 C22 = (−1) = −8 4 −2



2+3 −2 1 =2 C22 = (−1) 4 −1 2+1

(E.29) (E.30) (E.31)

Therefore, |A| = −1 + 2 (−8) − 2 = −19

(E.32)

Alternatively, expanding |A| by elements of its third column gives |A| = 3C13 − C23 − 2C33 C21 C22 C22



1 2

= −9 = (−1) 4 −1



2+3 −2 1 = (−1) =2 4 −1



3+3 −2 1 = (−1) = −5 1 2 1+3

(E.33) (E.34) (E.35) (E.36)

794

E Matrix Calculus

Therefore, |A| = 3 (−9) − 2 − 2 (−5) = −19 (E.37)     If A = aij , B = bij are two n × n matrices, then the following results are true for determinants of A and B. 1. If any two adjacent rows (or columns) of |A| are interchanged, the sign of the resulting determinant is changed. 2. If any two rows (or columns) of |A| are identical, then |A| = 0. 3. The value of a determinant is not changed if any multiple of a row (or column) is added to any other row (or column) of |A|. 4. |cA| = cn |A|, c ∈ N. 5. AT = |A|. 6. |A|

|B|

= |AB|. 7. A−1 = 1/ |A|. Transpose, Symmetric,  and  Skew Matrices   The matrix AT = aj i obtained from A = aij by changing rows to columns is the transpose of A.   A = aij

  AT = aj i

(E.38)

[A + B + · · · + C]T = AT + BT + · · · + CT

(E.39)

[AB · · · C] = C · · · B A T T    ain bnj = bni aj n = BT AT [AB]T =

(E.40)

T

T

T

T



n

(E.41)

n

A matrix A such that AT = A is symmetric matrix. A matrix A such that AT = −A is skew, skew symmetric, or alternating matrix. Inverse The inverse of a square matrix A is indicated by A−1 , A−1 =

adj A |A|

A−1 A = A A−1 = I

(E.42)

where adj A is the adjoint of the matrix A and Cij are cofactors associated to element aij of A.  T adj A = Cij [AB · · · C]−1 = C−1 · · · B−1 A−1  −1 −1 A =A  −1 T  T −1 A = A −1

[kA]

−1

−1

=k A

(E.43) (E.44) (E.45) (E.46) (E.47)

Non-square matrices of order m × n, m = n, do not have inverse. However, if A is m × n and the rank of A is equal to n, then A has a left inverse. An n × m matrix B such that BA = I. If A has rank m, then it has a right inverse. An n × m matrix B such that AB = I. Sherman–Morrison–Woodbury formula. 

−1  −1 T −1 A + vT w = A−1 − A−1 v I + vT A−1 w v A

(E.48)

E Matrix Calculus

795

Differentiation and  Integration   If A (t) = aij (t) , and B (t) = bij (t) are two n × n matrices, then " # d d A (t) = aij (t) dt dt -

t

"-

#

t

A (s) ds =

t0

(E.49)

aij (s) ds

(E.50)

t0

d d d [A (t) ± B (t)] = A (t) ± B (t) dt dt dt " # " # d d d A (t) B (t) + A (t) B (t) [A (t) B (t)] = dt dt dt " #T #T " d d d T T T B (t) A (t) + B (t) A (t) [A (t) B (t)] = dt dt dt If |A| = 0, then

  If A (x) = aj i (x) , then

" # d −1 d −1 A (t) = −A (t) A (t) A−1 (t) dt dt " # d T d T A (t) = −A (t) A (t) AT (t) dt dt n  daij d |A| = Cij dx dx i,j =1

(E.51) (E.52) (E.53)

(E.54) (E.55)

(E.56)

where Cij are cofactors associated to element aij of A. The Matrix Exponential If A is a square matrix, and z is any complex variable, then the matrix exponential exp (Az) is defined as: ∞

eAz = I + Az +

 Ak zk An zn A2 z2 + ··· + = 2! n! k! k=0

e0 = I eIz = I ez eA(z1 +z2 ) = eAz1 eAz2  −1 e−Az = eAz eAz eBz = e(A+B)z d k Az e = Ak eAz = eAz Ak dzk

(E.57)

(E.58) (E.59) (E.60)

F

Trigonometric Formula

Numeric Values and Functions 355 103993

113 33102 1 rad 57.296 deg 57◦ 17 44.8 π 3.14159265359 · · ·

(F.1) (F.2)

Trigonometric Functions θ is an acute angle of a right-angled triangle, and the hypotenuse h is the long side that connects the two acute angles. The side b adjacent to θ is the side of the triangle that connects θ to the right angle. The third side a is opposite to θ . h = hypotenuse sin θ =

a h

csc θ =

1 h = sin θ a

a = opposite

b = adj acent

a sin θ = b cos θ 1 h b sec θ = = cot θ = cos θ b a

cos θ =

b h

tan θ =

(F.3) (F.4) (F.5)

Definitions in Terms of Exponentials

cos z =

eiz + e−iz = cosh (iz) 2

(F.6)

sin z =

eiz − e−iz = −i sinh (iz) 2i

(F.7)

1 eiz − e−iz  = tanh z tan z =  iz −iz i i e +e

(F.8)

eiz = cos z + i sin z

(F.9)

e−iz = cos z − i sin z

(F.10)

(cos θ + i sin θ ) = cos nθ + i sin nθ

(F.11)

sin(α ± β) = sin α cos β ± cos α sin β

(F.12)

cos(α ± β) = cos α cos β ∓ sin α sin β

(F.13)

n

Angle Sum and Difference

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

797

798

F Trigonometric Formula

sinh(α ± β) = sinh α cosh β ± cosh α sinh β

(F.14)

cosh(α ± β) = cosh α cosh β ∓ sinh α sinh β

(F.15)

tan(α ± β) =

tan α ± tan β 1 ∓ tan α tan β

(F.16)

cot(α ± β) =

cot α cot β ∓ 1 cot β ± cot α

(F.17)

tanh α ± tanh β 1 ∓ tanh α tanh β

(F.18)

tanh(α ± β) = Periodicity

sin (α + 2nπ) = sin α

(F.19)

cos (α + 2nπ) = cos α

(F.20)

tan (α + 2nπ) = tan α

(F.21)

sin(−α) = − sin α

(F.22)

cos(−α) = cos α

(F.23)

tan(−α) = − tan α

(F.24)

Symmetry

Displacement π − α) = cos α 2 π cos( − α) = sin α 2 π tan( − α) = cot α 2 sin(

π − α) = csc α 2 π csc( − α) = sec α 2 π cot( − α) = tan α 2 sec(

(F.25) (F.26) (F.27) (F.28) (F.29) (F.30)

Multiple Angles sin(2α) = 2 sin α cos α =

2 tan α 1 + tan2 α

cos(2α) = 2 cos2 α − 1 = 1 − 2 sin2 α = cos2 α − sin2 α

(F.31) (F.32)

tan(2α) =

2 tan α 1 − tan2 α

(F.33)

cot(2α) =

cot2 α − 1 2 cot α

(F.34)

F Trigonometric Formula

799

sin(3α) = −4 sin3 α + 3 sin α

(F.35)

cos(3α) = 4 cos α − 3 cos α

(F.36)

− tan3 α + 3 tan α −3 tan2 α + 1

(F.37)

3

tan(3α) =

sin(4α) = −8 sin3 α cos α + 4 sin α cos α

(F.38)

cos(4α) = 8 cos4 α − 8 cos2 α + 1

(F.39)

tan(4α) =

−4 tan α + 4 tan α tan4 α − 6 tan2 α + 1 3

(F.40)

sin(5α) = 16 sin5 α − 20 sin3 α + 5 sin α

(F.41)

cos(5α) = 16 cos α − 20 cos α + 5 cos α

(F.42)

sin(nα) = 2 sin((n − 1)α) cos α − sin((n − 2)α)

(F.43)

cos(nα) = 2 cos((n − 1)α) cos α − cos((n − 2)α)

(F.44)

5

tan(nα) =

3

tan((n − 1)α) + tan α 1 − tan((n − 1)α) tan α

(F.45)

Half Angle ) 1 + cos α α cos = ± 2 2 ) α 1 − cos α sin = ± 2 2 ) α 1 − cos α sin α 1 − cos α tan = = =± 2 sin α 1 + cos α 1 + cos α sin α =

2 tan α2 1 + tan2

α 2

cos α =

1 − tan2 1 + tan2

α 2 α 2

(F.46) (F.47)

(F.48) (F.49) (F.50)

Powers of Functions

cos2 α + sin2 α = 1

(F.51)

cosh α − sinh α = 1

(F.52)

2

2

1 (1 − cos(2α)) 2 1 sin α cos α = sin(2α) 2 1 cos2 α = (1 + cos(2α)) 2 1 sin3 α = (3 sin(α) − sin(3α)) 4 1 sin2 α cos α = (cos α − 3 cos(3α)) 4 sin2 α =

(F.53) (F.54) (F.55) (F.56)

800

F Trigonometric Formula

sin α cos2 α = cos3 α =

1 (sin α + sin(3α)) 4

1 (cos(3α) + 3 cos α)) 4

1 (3 − 4 cos(2α) + cos(4α)) 8 1 sin3 α cos α = (2 sin(2α) − sin(4α)) 8 1 sin2 α cos2 α = (1 − cos(4α)) 8 1 sin α cos3 α = (2 sin(2α) + sin(4α)) 8 1 cos4 α = (3 + 4 cos(2α) + cos(4α)) 8 1 sin5 α = (10 sin α − 5 sin(3α) + sin(5α)) 16 1 sin4 α cos α = (2 cos α − 3 cos(3α) + cos(5α)) 16 1 sin3 α cos2 α = (2 sin α + sin(3α) − sin(5α)) 16 1 sin2 α cos3 α = (2 cos α − 3 cos(3α) − 5 cos(5α)) 16 1 sin α cos4 α = (2 sin α + 3 sin(3α) + sin(5α)) 16 1 cos5 α = (10 cos α + 5 cos(3α) + cos(5α)) 16 1 − cos(2α) tan2 α = 1 + cos(2α) sin4 α =

(F.57) (F.58) (F.59) (F.60) (F.61) (F.62) (F.63) (F.64) (F.65) (F.66) (F.67) (F.68) (F.69) (F.70)

Products of sin and cos 1 1 cos(α − β) + cos(α + β) 2 2 1 1 cos nθ cos θ = cos(n + 1)θ + cos(n − 1)θ 2 2 1 1 cos mθ cos nθ = cos(m + n)θ + cos(m − n)θ 2 2 cos α cos β =

1 1 cos(α − β) − cos(α + β) 2 2 1 1 sin α cos β = sin(α − β) + sin(α + β) 2 2 1 1 cos α sin β = sin(α + β) − sin(α − β) 2 2

sin α sin β =

(F.71) (F.72) (F.73)

(F.74) (F.75) (F.76)

sin(α + β) sin(α − β) = cos2 β − cos2 α = sin2 α − sin2 β

(F.77)

cos(α + β) cos(α − β) = cos2 β + sin2 α

(F.78)

F Trigonometric Formula

801

Sum of Functions α±β α±β cos 2 2 α−β α+β cos cos α + cos β = 2 cos 2 2 α+β α−β cos α − cos β = −2 sin sin 2 2 sin α ± sin β = 2 sin

α±β α±β cosh 2 2 α+β α−β cosh α + cosh β = 2 cosh cosh 2 2 α+β α−β cosh α − cosh β = −2 sinh sinh 2 2 sinh α ± sinh β = 2 sinh

(F.79) (F.80) (F.81)

(F.82) (F.83) (F.84)

tan α ± tan β =

sin(α ± β) cos α cos β

(F.85)

cot α ± cot β =

sin(β ± α) sin α sin β

(F.86)

tan α+β sin α + sin β 2 = sin α − sin β tan α−+β 2

(F.87)

−α + β sin α + sin β = cot cos α − cos β 2

(F.88)

sin α + sin β α+β = tan cos α + cos β 2

(F.89)

α−β sin α − sin β = tan cos α + cos β 2

(F.90)

sin2 α − sin2 β = sin(α + β) sin(α − β)

(F.91)

Trigonometric Relations

= cos β − cos α 2

2

(F.92)

cos2 α − cos2 β = − sin(α + β) sin(α − β)

(F.93)

cos2 α − sin2 β = cos(α + β) cos(α − β)

(F.94)

= cos2 β − sin2 α

(F.95)

cos x = y

(F.96)

Inverse of Trigonometric Functions sin x = y

tan x = y

arcsin y = (−1)k x + kπ

(F.97)

arccos y = ±x + 2kπ

(F.98)

802

F Trigonometric Formula

arcsin y = x + kπ −1 ≤ y −1 ≤ y −∞ ≤ y −∞ ≤ y

k = 0, 1, 2, 3, · · · π π ≤ 1 − ≤ arcsin y ≤ 2 2 ≤1 0 ≤ arccos y ≤ π π π ≤ ∞ − ≤ arctan y ≤ 2 2 ≤ ∞ 0 ≤ arccot y ≤ π

π 0 ≤ arccsc y ≤ 2 π y ≤ −1 − ≤ arccsc y ≤ 0 2 π 1 ≤ y 0 ≤ arcsec y ≤ 2 π y ≤ −1 ≤ arcsec y ≤ π 2

(F.99)

(F.100)

1≤y

(F.101)

Derivatives of Trigonometric Functions d sin θ = cos θ dθ d cos θ = − sin θ dθ d tan θ = 1 + tan2 θ = sec2 θ dθ d csc θ = − csc θ cot θ dθ d sec θ = sec θ tan θ dθ   d cot θ = − 1 + cot2 θ = − csc2 θ dθ

(F.102) (F.103) (F.104)

(F.105) (F.106) (F.107)

Integrals of Trigonometric Functions sin θ dθ = − cos θ + C

(F.108)

cos θ dθ = sin θ + C

(F.109)

tan θ dθ = − ln |cos θ| + C

(F.110)

-

-

-

-

 1 cos θ − 1 +C csc θ dθ = ln − 2 cos θ + 1 = − ln |csc θ + cot θ | + C  1 sin θ + 1 sec θ dθ = ln +C 2 sin θ − 1 = ln |sec θ + tan θ | + C cot θ dθ =

1 ln (2 − 2 cos 2θ ) + C = ln |sin θ| + C 2

(F.111)

(F.112) (F.113)

F Trigonometric Formula

803

cos2 x dx =

1 1 x − sin 2x 2 4

(F.114)

sin2 x dx =

1 1 x + sin 2x 2 4

(F.115)

-

cos mx cos nx dx = + -

sin (m + n) x 2 (m + n)

m2 = n2

sin mx sin nx dx = − -

sin (m + n) x 2 (m + n)

sin (m − n) x 2 (m − n)

sin (m − n) x 2 (m − n)

m2 = n2

sin mx cos nx dx = − − -

π

−π

-

cos (m − n) x 2 (m − n)

m2 = n2

(F.118) m, n ∈ N

(F.119)

sin mx sin nx dx = π δ mn m2 = n2

m, n ∈ N

(F.120)

m2 = n2 m, n ∈ N

(F.121)

−π

−π

(F.117)

cos mx cos nx dx = π δ mn m2 = n2

π

π

cos (m + n) x 2 (m + n)

(F.116)

sin mx cos nx dx = 0 -

x cos x dx = cos x + x sin x

(F.122)

x sin x dx = sin x − x cos x

(F.123)

  x 2 cos x dx = 2x cos x + x 2 − 2 sin x

(F.124)

  x 2 sin x dx = 2x sin x − x 2 − 2 cos x

(F.125)

-

cos mx cos nx dx = + -

sin (m + n) x 2 (m + n)

m2 = n2

sin mx sin nx dx = − -

sin (m + n) x 2 (m + n)

sin (m − n) x 2 (m − n)

sin (m − n) x 2 (m − n)

m2 = n2

sin mx cos nx dx = − − -

π

−π

cos (m + n) x 2 (m + n)

cos mx cos nx dx = π δ mn

(F.126)

(F.127)

cos (m − n) x 2 (m − n)

m2 = n2 m2 = n2

(F.128) m, n ∈ N

(F.129)

804

F Trigonometric Formula

-

π

−π π −π

sin mx sin nx dx = π δ mn sin mx cos nx dx = 0

m2 = n2 m2 = n2

m, n ∈ N m, n ∈ N

(F.130) (F.131)

x cos x dx = cos x + x sin x

(F.132)

x sin x dx = sin x − x cos x

(F.133)

  x 2 cos x dx = 2x cos x + x 2 − 2 sin x

(F.134)

  x 2 sin x dx = 2x sin x − x 2 − 2 cos x

(F.135)

-

G

Algebraic Formula

(a ± b)2 = a 2 ± 2ab + b2

(G.1)

(a ± b) = a ± 3a b + 3ab ± b 3

3

(a + b)n =

2

2

2

n   n k=0

k

a n−k bk

 n! n = k! (n − k)! k (a + b + c)2 = a 2 + b2 + c2 + 2 (ab + bc + ca)

(G.2)

(G.3)

(G.4) (G.5)

(a + b + c)3 = a 3 + b3 + c3 + 3a 2 (b + c) +3b2 (c + a) + 3c2 (a + c) + 6abc

(G.6)

a 2 − b2 = (a − b) (a + b)

(G.7)

a 2 + b2 = (a − ib) (a + ib)

(G.8)

  a 3 − b3 = (a − b) a 2 + ab + b2   a 2 + b2 = (a + b) a 2 − ab + b2

(G.9)

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

(G.10)

805

H

Unit Conversions

General Conversion Formulas

Na mb sc ≈ 4.448a × 0.3048b × lba ftb sc ≈ 4.448a × 0.0254b × lba inb sc lba ftb sc ≈ 0.2248a × 3.2808b × Na mb sc lba inb sc ≈ 0.2248a × 39.37b × Na mb sc Conversion Factors Acceleration 1 ft/s2 ≈ 0.3048 m/s2 1m/s2 ≈ 3.2808 ft/s2 Angle 1 deg ≈ 0.01745 rad 1 rad ≈ 57.307 deg Area

Damping

1 in2 ≈ 6.4516 cm2 1 ft2 ≈ 0.09290304 m2 1 acre ≈ 4046.86 m2 1 acre ≈ 0.4047 hectare

1 cm2 ≈ 0.155 in2 1 m2 ≈ 10.764 ft2 1 m2 ≈ 2.471 × 10−4 acre 1 hectare ≈ 2.471 acre

1Ns/m ≈ 6.85218 × 10−2 lbs/ft 1 lbs/ft ≈ 14.594Ns/m 1Ns/m ≈ 5.71015 × 10−3 lbs/ in 1 lbs/ in ≈ 175.13Ns/m

Energy and Heat 1 Btu ≈ 1055.056 J 1 cal ≈ 4.1868 J 1 kWh ≈ 3600 kJ 1 ftlbf ≈ 1.355818 J

1 J ≈ 9.4782 × 10−4 Btu 1 J ≈ 0.23885 cal 1 MJ ≈ 0.27778 kWh 1 J ≈ 0.737562 ftlbf

Force 1 lb ≈ 4.448222N 1N ≈ 0.22481 lb Fuel Consumption 1 l/100 km ≈ 235.214583 mi/ gal 1 mi/ gal ≈ 235.214583 l/100 km 1 l/100 km = 100 km/ l 1 km/ l = 100 l/100 km 1 mi/ gal ≈ 0.425144 km/ l 1 km/ l ≈ 2.352146 mi/ gal

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

807

808

H Unit Conversions

Length 1 in ≈ 25.4 mm 1 cm ≈ 0.3937 in 1 ft ≈ 30.48 cm 1m ≈ 3.28084 ft 1 mi ≈ 1.609347 km 1 km ≈ 0.62137 mi Mass

1 lb ≈ 0.45359 kg 1 kg ≈ 2.204623 lb 1 slug ≈ 14.5939 kg 1 kg ≈ 0.068522 slug 1 slug ≈ 32.174 lb 1 lb ≈ 0.03.1081 slug

Moment and Torque 1 lbft ≈ 1.35582Nm 1Nm ≈ 0.73746 lbft 1 lbin ≈ 8.85075Nm 1Nm ≈ 0.11298 lbin Mass Moment 1 lbft2 ≈ 0.04214 kg m2 1 kg m2 ≈ 23.73 lbft2 Power

Pressure and Stress

Stiffness

Temperature

1 Btu/h ≈ 0.2930711 W 1 hp ≈ 745.6999 W 1 hp ≈ 550 lb ft/s 1 lb ft/h ≈ 3.76616 × 10−4 W 1 lb ft/min ≈ 2.2597 × 10−2 W

1 W ≈ 3.4121 Btu/h 1 kW ≈ 1.341 hp 1 lb ft/s ≈ 1.8182 × 10−3 hp 1 W ≈ 2655.2 lb ft/h 1 W ≈ 44.254 lb ft/min

1 lb/ in2 ≈ 6894.757 Pa 1 MPa ≈ 145.04 lb/ in2 1 lb/ft2 ≈ 47.88 Pa 1 Pa ≈ 2.0886 × 10−2 lb/ft2 1 Pa ≈ 0.00001 atm 1 atm ≈ 101325 Pa 1 N/m ≈ 6.85218 × 10−2 lb/ft 1 lb/ft ≈ 14.594 N/m 1 N/m ≈ 5.71015 × 10−3 lb/in 1 lb/in ≈ 175.13 N/m ◦ ◦

C = (◦ F − 32)/1.8 F = 1.8 ◦ C + 32

Velocity 1 mi/h ≈ 1.60934 km/h 1 km/h ≈ 0.62137 mi/h 1 mi/h ≈ 0.44704 m/s 1m/s ≈ 2.2369 mi/h 1 ft/s ≈ 0.3048 m/s 1 m/s ≈ 3.2808 ft/s 1 ft/min ≈ 5.08 × 10−3 m/s 1 m/s ≈ 196.85 ft/min Volume

1 in3 ≈ 16.39 cm3 1ft3 ≈ 0.02831685m3 1 gal ≈ 3.785 l 1 gal ≈ 3785.41 cm3

1 cm3 ≈ 0.0061013 in3 1m3 ≈ 35.315ft3 1 l ≈ 0.2642 gal 1 l ≈ 1000 cm3

Bibliography

Front Matter Jazar, R. N. (2011). Advanced dynamics: Rigid body, multibody, and aero-space applications. New York: Wiley. Harithuddin, S., Trivailo M., & Jazar, R. N. (2015). On the Razi Acceleration. In: L. Dai, & R. Jazar (Eds.), Nonlinear approaches in engineering applications. New York: Springer.

Chapter 1: Introduction Asimov, I. (1942). Runaround, Astounding Science-Fiction. New York: Street & Smith Publications, Inc. Asimov, I. (1950a). I, Robot. New York: Gnome Press. Asimov, I. (1950b). Robots and Empire. New York: Doubleday. Aspragathos, N. A., & Dimitros, J. K. (1998). A comparative study of three methods for robot kinematics. IEEE Transaction on Systems, Man and Cybernetic-PART B: CYBERNETICS, 28(2), 115–145. ˇ Capek, K. (1994). Tales from two pockets, translated from the Czech and with an introduction by Norma Comrada. North Haven, CT: Catbird Press. Chernousko, F. L., Bolotnik, N. N., & Gradetsky, V. G. (1994). Manipulation Robots: Dynamics, Control, and Optimization. Boca Raton, Florida: CRC Press. Denavit, J., & Hartenberg, R. S. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, 22(2), 215–221. Dugas, R. (1995). A History of Mechanics (English translation), Switzerland, Editions du Griffon. New York: Central Book Co. Erdman, A. G. (1993). Modern Kinematics: Developed in the Last Forty Years. New York: Wiley. Fahimi, F. (2009). Autonomous Robots: Modeling, Path Planning, and Control. New York: Springer. Fosdick, R. & Fried, E. (2016). The Mechanics of Ribbons and Mö bius Bands. New York: Springer. Harithuddin, S., Trivailo M., & Jazar, R. N. (2015). On the Razi Acceleration. In: L. Dai, & R. Jazar (Eds.), Nonlinear Approaches in Engineering Applications. New York: Springer. Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London: Oxford University Press. Husbands, P., Holland, O., & Wheeler M. (Eds.) (2008). The Mechanical Mind in History. Cambridge: The MIT Press. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aero-space Applications. New York: Wiley. Jazar, R. N. (2012). Derivative and coordinate frames. Journal of Nonlinear Engineering, 1(1), 25–34. Jazar, R. N. (2017). Vehicle Dynamics: Theory and Application (3rd ed.). New York: Springer. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Milne, E. A. (1948). Vectorial Mechanics. London: Methuen & Co. LTD. Niku, S. B. (2020). Introduction to Robotics: Analysis, Systems, Applications. New York: Wiley. Rosheim, M. E. (1994). Robot Evolution: The Development of Anthrobotics. New York: Wiley. Shahinpoor, M. (1987). A Robot Engineering Textbook. New York: Harper and Row Publishers. Tsai, L. W. (1999). Robot Analysis. New York: Wiley. Veit, S. (1992). Whatever happened to … personal robots?. The Computer Shopper, 12(11), 794–795.

Chapter 2: Rotation Kinematics Buss, S. R. (2003). 3-D Computer Graphics: A Mathematical Introduction with OpenGL. New York: Cambridge University. Cheng, H., & Gupta, K. C. (1989). A historical note on finite rotations. Journal of Applied Mechanics, 56, 139–145. Coe, C. J. (1934). Displacement of a rigid body. American Mathematical Monthly, 41(4), 242–253. Denavit, J., & Hartenberg, R. S. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, 22(2), 215–221.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

809

810

Bibliography

Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London: Oxford University. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications. New York: Wiley. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Mason, M. T. (2001). Mechanics of Robotic Manipulation. Cambridge, MA: MIT Press. Murray, R. M., Li, Z., & Sastry, S. S. S. (1994). A Mathematical Introduction to Robotic Manipulation. Boca Raton: CRC Press. Nikravesh, P. (1988). Computer-Aided Analysis of Mechanical Systems. New Jersey: Prentice Hall. Niku, S. B. (2020). Introduction to Robotics: Analysis, Systems, Applications. New York: Wiley. Paul, B. (1963). On the composition of finite rotations. American Mathematical Monthly, 70(8), 859–862. Paul, R. P. (1981). Robot Manipulators: Mathematics, Programming, and Control. Cambridge, MA: MIT Press. Rimrott, F. P. J. (1989). Introductory Attitude Dynamics. New York: Springer. Rosenberg, R. M. (1977). Analytical Dynamics of Discrete Systems. New York: Plenum Publishing Co. Schaub, H., & Junkins, J. L. (2003). Analytical Mechanics of Space Systems. Reston, Virginia: AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and Mechanisms Design. New York: Wiley. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Tsai, L. W. (1999). Robot Analysis. New York: Wiley.

Chapter 3: Orientation Kinematics Buss, S. R. (2003). 3-D Computer Graphics: A Mathematical Introduction with OpenGL. New York: Cambridge University Press. Denavit, J., & Hartenberg, R. S. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, 22(2), 215–221. Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London U.K: Oxford University Press. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications. New York: Wiley. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Mason, M. T. (2001). Mechanics of Robotic Manipulation. Cambridge, MA: MIT Press. Murray, R. M., Li, Z., & Sastry, S. S. S. (1994). A Mathematical Introduction to Robotic Manipulation. Boca Raton, Florida: CRC Press. Nikravesh, P. (1988). Computer-Aided Analysis of Mechanical Systems. New Jersey: Prentice Hall. Paul, B. (1963). On the composition of finite rotations. American Mathematical Monthly, 70(8), 859–862. Paul, R. P. (1981). Robot Manipulators: Mathematics, Programming, and Control. Cambridge, MA: MIT Press. Rimrott, F. P. J. (1989). Introductory Attitude Dynamics. New York: Springer. Stanley, W. S. (1978). Quaternion from Rotation Matrix. AIAA Journal of Guidance and Control, I(3), 223–224. Wittenburg, J. (2016). Kinematics: Theory and Applications. Berlin Heidelberg: Springer. Rosenberg, R. M. (1977). Analytical Dynamics of Discrete Systems. New York: Plenum Publishing Co. Schaub, H., & Junkins, J. L. (2003). Analytical Mechanics of Space Systems. Reston, Virginia: AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and Mechanisms Design. New York: Wiley. Tsai, L. W. (1999). Robot Analysis. New York: Wiley. Wittenburg, J., & Lilov, L. (2003). Decomposition of a finite rotation into three rotations about given axes. Multibody System Dynamics, 9, 353–375.

Chapter 4: Motion Kinematics Ball, R. S. (1900). A Treatise on the Theory of Screws. New York: Cambridge University Press. Bottema, O., & Roth, B. (1979). Theoretical Kinematics. The Netherlands: North-Holland Publication, Amsterdam. Chasles, M. (1830). Notes on the general properties of a system of 2 identical bodies randomly located in space; and on the finite or infinitesimal motion of a free solid body. Bulletin des Sciences Mathematiques, Astronomiques, Physiques et Chimiques, 14, 321–326. Chernousko, F. L., Bolotnik, N. N., & Gradetsky, V. G. (1994). Manipulation Robots: Dynamics, Control, and Optimization. Boca Raton, Florida: CRC press. Davidson, J. K., & Hunt, K. H. (2004). Robots and Screw Theory: Applications of Kinematics and Statics to Robotics. New York: Oxford University Press. Denavit, J., & Hartenberg, R. S. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics,22(2), 215–221. Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London: Oxford University Press. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications. New York: Wiley. Jazar, R. N. (2013). Advanced Vibrations: A Modern Approach. New York: Springer. Jazar, R. N. (2017). Vehicle Dynamics. New York: Springer. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer.

Bibliography

811

Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Mason, M. T. (2001). Mechanics of Robotic Manipulation. Cambridge, Massachusetts: MIT Press. Mozzi, G. (1763). Discorso matematico sopra il rotamento momentaneo dei corpi. In Stamperia di Donato Campo, Napoli. Murray, R. M., Li, Z., & Sastry, S. S. S. (1994). A Mathematical Introduction to Robotic Manipulation. Boca Raton, Florida: CRC Press. Niku, S. B. (2020). Introduction to Robotics: Analysis, Systems, Applications. New York: Wiley. Plücker, J. (1866). Fundamental views regarding mechanics. Philosophical Transactions, 156, 361–380. Schaub, H., & Junkins, J. L. (2003). Analytical Mechanics of Space Systems. Reston, Virginia: AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc. Schilling, R. J. (1990). Fundamentals of Robotics: Analysis and Control. New Jersey: Prentice Hall. Selig, J. M. (2005). Geometric Fundamentals of Robotics (2nd ed.). New York: Springer. Sourin, A. (2021). Making Images with Mathematics. Cham, Switzerland: Springer. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and Mechanisms Design. New York: Wiley.

Chapter 5: Forward Kinematics Asada, H., & Slotine, J. J. E. (1986). Robot Analysis and Control. New York: Wiley. Ball, R. S. (1900). A Treatise on the Theory of Screws. USA: Cambridge University Press. Bernhardt, R., & Albright, S. L. (2001). Robot Calibration. New York: Springer. Bottema, O., & Roth, B. (1979). Theoretical Kinematics. The Netherlands: North-Holland Publication, Amsterdam. Davidson, J. K., & Hunt, K. H. (2004). Robots and Screw Theory: Applications of Kinematics and Statics to Robotics. New York: Oxford University Press. Denavit, J., & Hartenberg, R. S. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, 22(2), 215–221. Fahimi, F. (2009). Autonomous Robots: Modeling, Path Planning, and Control. New York: Springer. Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London: Oxford University Press. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications. New York: Wiley. Jazar, R. N. (2013). Advanced Vibrations: A Modern Approach. New York: Springer. Jazar, R. N. (2017). Vehicle Dynamics. New York: Springer. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Mason, M. T. (2001). Mechanics of Robotic Manipulation. Cambridge, MA: MIT Press. Paul, R. P. (1981). Robot Manipulators: Mathematics, Programming, and Control. Cambridge, MA: MIT Press. Pieper, D. (1968). The Kinematics of Manipulators under Computer Control, Ph.D. thesis. Stanford: Stanford University. Schilling, R. J. (1990). Fundamentals of Robotics: Analysis and Control. New Jersey: Prentice Hall. Schroer, K., Albright, S. L., & Grethlein, M. (1997). Complete, minimal and model-continuous kinematic models for robot calibration. Robotics and Computer-Integrated Manufacturing, 13(1), 73–85. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and Mechanisms Design. New York: Wiley. Tsai, L. W. (1999). Robot Analysis. New York: Wiley. Wang, K., & Lien, T. (1988). Structure, design and kinematics of robot manipulators. Robotica, 6, 299–306. Zhuang, H., Roth, Z. S., & Hamano, F. (1992). A complete, minimal and model-continuous kinematic model for robot manipulators. IEEE Transactions on Robotics and Automation, 8(4), 451–463.

Chapter 6: Inverse Kinematics Asada, H., & Slotine, J. J. E. (1986). Robot Analysis and Control. New York: Wiley. Fahimi, F. (2009). Autonomous Robots: Modeling, Path Planning, and Control. New York: Springer. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications. New York: Wiley. Jazar, R. N. (2013). Advanced Vibrations: A Modern Approach. New York: Springer. Jazar, R. N. (2017). Vehicle Dynamics. New York: Springer. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Paul, R. P. (1981). Robot Manipulators: Mathematics, Programming, and Control. Cambridge, Massachusetts: MIT Press. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Tsai, L. W. (1999). Robot Analysis. New York: Wiley. Wang, K., & Lien, T. (1988). Structure, design and kinematics of robot manipulators. Robotica, 6, 299–306.

812

Bibliography

Chapter 7: Angular Velocity Bottema, O., & Roth, B. (1979). Theoretical Kinematics. Amsterdam, The Netherlands: North-Holland Publications. Geradin, M., & Cardonna, A. (1987). Kinematics and dynamics of rigid and flexible mechanisms using finite elements and quaternion algebra. Computational Mechanics, 4(2), 115–135. Hunt, K. H. (1978). Kinematic Geometry of Mechanisms. London: Oxford University Press. Jazar, R. N. (2011). Advanced Dynamics: Rigid Body, Multibody, and Aero-space Applications. New York: Wiley. Jazar, R. N. (2019). Advanced Vehicle Dynamics. New York: Springer. Jazar, R. N. (2020). Approximation Methods in Science and Engineering. New York: Springer. Jazar, R. N. (2021). Perturbation Methods in Science and Engineering. New York: Springer. Mason, M. T. (2001). Mechanics of Robotic Manipulation. Cambridge, MA: MIT Press. Schaub, H., & Junkins, J. L. (2003). Analytical Mechanics of Space Systems. Reston, Virginia: AIAA Educational Series, American Institute of Aeronautics and Astronautics, Inc. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and Mechanisms Design. New York: Wiley. Tsai, L. W. (1999). Robot Analysis. New York: Wiley.

Chapter 8: Velocity Kinematics Carnahan, B., Luther, H. A., & Wilkes, J. O. (1969). Applied numerical methods. New York: Wiley. Eich-Soellner, E., & Führer, C. (1998). Numerical methods in multibody dynamics. B.G. Teubner Stuttgart. Gerald, C. F., & Wheatley, P. O. (1999). Applied numerical analysis (6th ed.). New York: Addison Wesley. Hunt, K. H. (1978), Kinematic geometry of mechanisms. Oxford University Press. Jazar, Reza N. (2011). Advanced dynamics: Rigid body, multibody, and aero-space applications. New York: Wiley. Kane, T. R., Likins, P. W., & Levinson, D. A. (1983). Spacecraft dynamics. New York: McGraw-Hill. Kane, T. R., & Levinson, D. A. (1980), Dynamics: Theory and applications. New York: McGraw-Hill. Mason, M. T. (2001). Mechanics of robotic manipulation. Cambridge, MA: MIT Press. Nikravesh, P. (1988). Computer-aided analysis of mechanical systems. New Jersey: Prentice Hall. Rimrott, F. P. J. (1989). Introductory attitude dynamics. New York: Springer. Schilling, R. J. (1990). Fundamentals of robotics: Analysis and control. New Jersey: Prentice-Hall. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot modeling and control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978), Kinematics and mechanisms design. New York: Wiley. Talman, R. (2000). Geometric mechanics. New York: Wiley. Tsai, L. W. (1999). Robot analysis. New York: Wiley.

Chapter 9: Acceleration Kinematics Jazar, Reza N. (2011). Advanced dynamics: Rigid body, multibody, and aero-space applications. New York: Wiley. Mason, M. T. (2001). Mechanics of robotic manipulation. Cambridge, MA: MIT Press. Nikravesh, P.. (1988). Computer-aided analysis of mechanical systems. New Jersey: Prentice Hall. Rimrott, F. P. J. (1989). Introductory attitude dynamics. New York: Springer. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot modeling and control. New York: Wiley. Suh, C. H., & Radcliff, C. W., (1978). Kinematics and Mechanisms Design, New York: John Wiley & Sons. Tsai, L. W. (1999). Robot analysis. New York: Wiley.

Chapter 10: Applied Dynamics Goldstein, H., Poole, C., & Safko, J. (2002). Classical mechanics (3rd ed.). New York: Addison Wesley. Jazar, R. N. (2011). Advanced dynamics: Rigid body, multibody, and aero-space applications. New York: Wiley. Jazar, R. N. (2013). Advanced vibrations: A modern approach. New York: Springer. MacMillan, W. D. (1936). Dynamics of rigid bodies. New York: McGraw-Hill. Meirovitch, L. (1970). Methods of analytical dynamics. New York: McGraw-Hill. Nikravesh, P. (1988). Computer-aided analysis of mechanical systems. New Jersey: Prentice Hall. Rimrott, F. P. J. (1989). Introductory attitude dynamics. New York: Springer. Rosenberg, R. M. (1977). Analytical dynamics of discrete systems. New York: Plenum Publishing. Schaub, H., & Junkins, J. L. (2003). Analytical mechanics of space systems. Reston, VA: AIAA Educational Series, American Institute of Aeronautics and Astronautics. Thomson, W. T. (1961). Introduction to space dynamics. New York: Wiley. Tsai, L. W. (1999). Robot analysis. New York: Wiley. Wittacker, E. T. (1947). A treatise on the analytical dynamics of particles and rigid bodies (4th ed.). New York: Cambridge University Press.

Bibliography

813

Chapter 11: Robot Dynamics Brady, M., Hollerbach, J. M., Johnson, T. L., Lozano-Prez, T., & Mason, M. T. (1983). Robot motion: Planning and control. Cambridge, MA: MIT Press. Jazar, Reza N. (2011). Advanced dynamics: Rigid body, multibody, and aero-space applications. New York: Wiley. Jazar, Reza N. (2013). Advanced vibrations: A modern approach. New York: Springer. Kelly, R., Santibáñez, V., & Loría A., (2005). Control of robot manipulators in joint space. Leipzig, Germany: Springer. Murray, R. M., Li, Z., & Sastry, S. S. S. (1994). A mathematical introduction to robotic manipulation. Boca Raton, FL: CRC Press. Nikravesh, P. (1988). Computer-aided analysis of mechanical systems. New Jersey: Prentice Hall. Niku, S. B. (2020). Introduction to robotics: Analysis, systems, applications. New York: Wiley. Paul, R. P. (1981). Robot manipulators: Mathematics, programming, and control. Cambridge, MA: MIT Press. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot modeling and control. New York: Wiley. Suh, C. H., & Radcliff, C. W. (1978). Kinematics and mechanisms design. New York: Wiley. Tsai, L. W. (1999). Robot analysis. New York: Wiley.

Chapter 12: Path Planning Asada, H., & Slotine, J. J. E. (1986). Robot analysis and control. New York: Wiley. Biagiotti, L., & Melchiorri, C. (2008). Trajectory planning for automatic machines and robots. Berlin Heidelberg : Springer. Fahimi, F. (2009), Autonomous robots: Modeling, path planning, and control. New York: Springer. Murray, R. M., Li, Z., & Sastry, S. S. S. (1994). A mathematical introduction to robotic manipulation. Boca Raton, FL: CRC Press. Niku, S. B. (2020). Introduction to robotics: Analysis, systems, applications. New York: Wiley. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2006). Robot modeling and control. New York: Wiley.

Chapter 13: Time Optimal Control Ailon, A., & Langholz, G. (1985). On the existence of time optimal control of mechanical manipulators. Journal of Optimization Theory and Applications, 46(1), 1–21. Bahrami, M., & Jazar, R. N. (1991). Optimal control of robotic manipulators: Optimization algorithm. Amirkabir Journal, 5(18) (in Persian). Bahrami M., & Jazar, R. N. (1992). Optimal control of robotic manipulators: Application. Amirkabir Journal, 5 (19) (in Persian). Bassein, R. (1989). An optimization problem. American Mathematical Monthly, 96(8), 721–725. Bobrow, J. E., Dobowsky, S., & Gibson, J. S. (1985). Time optimal control of robotic manipulators along specified paths. The International Journal of Robotics Research, 4(3), 495–499. Courant, R., & Robbins, H. (1941). What is mathematics? London: Oxford University Press. Fahimi, F. (2009). Autonomous robots: Modeling, path planning, and control. New York: Springer. Fotouhi, C. R., & Szyszkowski W. (1998). An algorithm for time optimal control problems. Transaction of the ASME, Journal of Dynamic Systems, Measurements, and Control, 120, 414–418. Fu, K. S., Gonzales, R. C., & Lee, C. S. G. (1987). Robotics, control, sensing, vision and intelligence. New York: McGraw-Hill. Gamkrelidze, R. V. (1958). The theory of time optimal processes in linear systems (Vol. 22, pp. 449–474). Izvestiya Akademii Nauk SSSR. Garg, D. P. (1990). The new time optimal motion control of robotic manipulators. The Franklin Institute, 327(5), 785–804. Hart P. E., Nilson N. J., & Raphael B. (1968). A formal basis for heuristic determination of minimum cost path. IEEE Transaction Man, System & Cybernetics, 4, 100–107. Kahn, M. E., & Roth B. (1971). The near minimum time control of open loop articulated kinematic chain. Transaction of the ASME, Journal of Dynamic Systems, Measurements, and Control, 93, 164–172. Kim B. K., & Shin K. G. (1985). Suboptimal control of industrial manipulators with weighted minimum time-fuel criterion. IEEE Transactions on Automatic Control, 30(1), 1–10. Krotov, V. F. (1996). Global methods in optimal control theory. New York: Marcel Decker. Golnaraghi, F., & Kuo, B. C. (2009). Automatic control systems. New York: Wiley. Lee, E. B., & Markus, L. (1961). Optimal control for nonlinear processes. Archive for Rational Mechanics and Analysis, 8, 36–58. Lee, H. W. J., Teo, K. L., Rehbock, V., & Jennings, L. S. (1997). Control parameterization enhancing technique for time optimal control problems. Dynamic Systems and Applications, 6, 243–262. Lewis, F. L., & Syrmos, V. L. (1995). Optimal control. New York: Wiley. Meier E. B., & Bryson A. E. (1990). Efficient algorithm for time optimal control of a two-link manipulator. Journal of Guidance, Control and Dynamics, 13(5), 859–866. Mita T., Hyon, S. H., & Nam, T. K. (2001). Analytical time optimal control solution for a two link planar acrobat with initial angular momentum. IEEE Transactions on Robotics and Automation, 17(3),361–366. Nakamura, Y. (1991). Advanced robotics: Redundancy and optimization. New York: Addison Wesley. Nakhaie Jazar G., & Naghshinehpour A. 2005. Time optimal control algorithm for multi-body dynamical systems. IMechE Part K: Journal of Multi-Body Dynamics, 219(3), 225–236. Pinch E. R. (1993). Optimal control and the calculus of variations. New York: Oxford University Press. Pontryagin, L. S., Boltyanskii, V. G., Gamkrelidze, R. V., & Mishchenko, E. F. (1962). The mathematical theory of optimal processes. New York: Wiley.

814

Bibliography

Roxin, E. (1962). The existence of optimal controls. Michigan Mathematical Journal, 9, 109–119. Shin K. G., & McKay N. D., (1985), Minimum time control of a robotic manipulator with geometric path constraints, IEEE Transaction Automatic Control, 30(6), 531–541. Shin K. G., & McKay N. D. (1986). Selection of near minimum time geometric paths for robotic manipulators. IEEE Transaction Automatic Control, 31(6), 501–511. Skowronski, J. M. (1986). Control dynamics of robotic manipulator. London, GB: Academic Press. Slotine, J. J. E., & Yang, H. S. (1989). Improving the efficiency of time optimal path following algorithms. IEEE Transactions on Robotics and Automation, 5(1), 118–124. Spong, M. W., Thorp, J. S., & Kleinwaks, J., M. (1986). The control of robot manipulators with bounded input. IEEE Journal of Automatic Control, 31(6), 483–490. Sundar, S., & Shiller, Z. (1996). A generalized sufficient condition for time optimal control. Transaction of the ASME, Journal of Dynamic Systems, Measurements, and Control, 118(2), 393–396. Takegaki, M., & Arimoto, S. (1981). A new feedback method for dynamic control of manipulators. Transaction of the ASME, Journal of Dynamic Systems, Measurements, and Control, 102, 119–125. Vincent T. L., & Grantham W. J. (1997). Nonlinear and Optimal Control Systems. New York: Wiley.

Chapter 14: Control Techniques Åström, K. J., & Hägglund, T. (1995). PID controllers (2nd ed.). ***Research Triangle Park, North Carolina: Instrument Society of America. Fahimi, F. (2009). Autonomous robots: Modeling, path planning, and control. New York: Springer. Fu, K. S., Gonzales, R. C., & Lee, C. S. G. (1987). Robotics, control, sensing, vision and intelligence. New York: McGraw-Hill. Golnaraghi, F., & Kuo, B. C. (2009). Automatic control systems. New York: Wiley. Jamshidi, M. (2008). Systems of systems engineering: Principles and applications. New York: Wiley. Jeffrey, A., & Dai, H. H. (2008). Handbook of mathematical formulas and integrals. Burlington, MA: Academic Press. Kelly, R., Santibáñez, V., & Loría A., (2005). Control of robot manipulators in joint space. Leipzig: Springer. Lewis, F. L., & Syrmos, V. L., (1995), Optimal control. New York: Wiley. Malek-Zavarei, M., & Jamshidi, M. (1986). Linear control systems: A computer aided approach. London: Pergamon Press. Paul, R. P. (1981). Robot manipulators: Mathematics, programming, and control. Cambridge, MA: MIT Press. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot modeling and control. New York: Wiley. Takegaki, M., & Arimoto, S. (1981). A new feedback method for dynamic control of manipulators. Journal of Dynamic Systems, Measurements, and Control, 102, 119–125. Vincent T. L., & Grantham W. J. (1997). Nonlinear and optimal control systems. New York: Wiley.

Index

A Acceleration angular, 489, 491, 501, 507, 508 bias vector, 521 body point, 384, 508, 510 centripetal, 500, 504, 508, 510 constant parabola, 705 constant path, 694 Coriolis, 510, 536, 563 definition, 27 discontinuous path, 700 discrete equation, 740, 748 end-effector, 499 forward kinematics, 518, 519 gravitational, 634, 660, 669 inverse kinematics, 520 jump, 689 matrix, 490, 510, 516, 533 mixed, 538, 540 mixed Coriolis, 543 mixed double, 543 Razi, 543, 552 recursive, 526, 528, 609 rotational transformation, 490, 499 sensors, 771 simple, 534 tangential, 500, 504, 508, 510, 536, 543 transformation, 494, 541 transformation matrix, 510 Accessory, 260 Active transformation, 79 Actuator, 7, 12 force and torque, 610, 632, 673 optimal torque, 750, 751 torque equation, 617, 642, 747 Admissible path, 735 Algorithm floating-time, 739, 746 inverse kinematics, 343 LU factorization, 451 LU solution, 451 Newton-Raphson, 464 Angle Euler, 59 nutation, 59 precession, 59 spin, 59 Angular acceleration, 489, 491, 508 B-expression, 502 end-effector, 499 Euler parameters, 501, 507 matrix, 490

principal, 502 quaternions, 507 recursive, 533 relative, 494, 500 rotational transformation, 490, 492 technical point, 504 vector, 490 Angular jerk, 506 Angular momentum, 569–571 2 link manipulator, 574 Angular velocity, 64, 66, 67, 97, 361, 364, 372, 491, 495, 502 alternative definition, 384, 386 combination, 364 coordinate transformation, 372 decomposition, 370 elements of matrix, 376 Euler frequencies, 370 Euler parameters, 375 instantaneous, 363 instantaneous axis, 361, 364, 372, 495 matrix, 362 principal matrix, 369 quaternions, 374 rate, 361 recursive, 418, 527 rom B-frame, 363 rom G-frame, 363 rotation matrix, 371 skew symmetric, 363 vector, 361 Anthropomorphic hand, 7 Arm, 2 Articulated arm, 9, 252, 254 manipulator, 9, 252, 254, 325, 433 robot, 252 Articulated manipulator equations of motion, 647 inverse kinematics, 319, 322, 323, 331 inverse velocity, 447 Jacobian matrix, 428, 472 left shoulder configuration, 336 right shoulder configuration, 336 Asimov, Isaac, 1 Associativity property, 76 Atan2 function, 317 Attitude angle, 58 Automorphism, 123 property, 122 Auxiliary frame, 285 Avicenna, 278 Axis-angle

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 R. N. Jazar, Theory of Applied Robotics, https://doi.org/10.1007/978-3-030-93220-6

815

816 matrix, 91 rotation, 91 transformation, 91 Axis-angle rotation, 91, 94–96, 110, 112, 114, 127

B bac-cab rule, 19, 147 Bang-bang control, 731 Bank angle, 58 Bar, 2 Bernoulli, Jacob, 738 Bernoulli, Johann, 738 Binary link, 228 Block diagram, 760 Body frame, 14 Bong, 28 Brachistochrone, 737, 745 Bryant angles, 68

C Camera inspection, 260, 263 SSRMS, 260 vision, 263 Capek, Karel, 1 Cardan angles, 68 frequencies, 68 Cardano, Geronimo, 68 Cartesian angular velocity, 66 coordinate space, 247 coordinate system, 20 end-effector position, 441 end-effector velocity, 442 manipulator, 9, 11 orthogonality condition, 20 path, 705 Cartesian manipulator equations of motion, 650 Catapults, 597 Central difference, 743 Centrifugal moments, 576 Centroid, 391 Chasles, Michel, 186 Chasles theorem, 187 China, 597 Christoffel Elwin Bruno, 658 Christoffel operator, 658 Christoffel symbol, 658 Classification industrial links, 236 Closure property, 75 Co-state variable, 732 Common normal, 226 Compound link, 2 Condition orthogonality, 20 Configuration path, 27 Control adaptive, 763 admissible, 733 bang-bang, 731, 733

Index characteristic equation, 762 closed-loop, 759 command, 759 computed force, 765 computed torque, 763, 764 derivative, 768 desired path, 759 directional control system, 263 error, 759 feedback, 760 feedback command, 765 feedback linearization, 763, 765 feedforward command, 765 gain, 760 gain-scheduling, 763 input, 764 integral, 768 linear, 763, 767 minimum time, 731 modified PD, 770 open-loop, 759, 764 path points, 707 PD, 770 proportional, 768 robots, 12 sensing, 770 stability of linear, 761 time-optimal, 738, 741, 746, 747, 751 time-optimal description, 747 time-optimal path, 745 Controller, 8 Control unit, 8 Coordinate cylindrical, 174 non-Cartesian, 589 non-orthogonal, 136 parabolic, 589 spherical, 175, 396 system, 21 Coordinate frame, 14, 16, 21 dummy, 278 extra, 278 global, 21 intermediate, 278 jump, 278 local, 21 main, 240, 278 neshin, 277 origin, 16 orthogonal, 16, 20 orthogonality condition, 20 Sina, 240, 278 spare, 278 takht, 277 temporary, 278 Coordinate space Cartesian, 247 joint variable, 247 Coordinate system, 14, 21 Coriolis acceleration, 498, 510 effect, 563 force, 563 Coriolis, Gaspard, 563 Couple, 558 Crackle, 28

Index Critically-damped, 762 Cyclic interchanging, 14, 19 Cycloid, 738

D Damping ratio, 762 Dead frame, 269 Decoupling technique, 313 inverse kinematics, 313 inverse orientation, 314 inverse position, 314 Denavit-Hartenberg, 35 construction steps, 231 industrial links, 236 linkage, 245 mechanism, 245 method, 225, 228, 293 non-classical, 242 nonstandard method, 245, 341 notation, 225 parameters, 225, 401, 405, 416, 528, 668 proof of equation, 233, 234 shortcomings, 232 spherical robot, 244 Stanford arm, 230 3R PUMA robot, 229 3R planar manipulator, 229 transformation, 232, 235–239, 242, 289, 783–788 2R planar manipulator, 235 Derivative coordinate frames, 376 mixed, 377, 534 mixed double, 543 mixed second, 542 simple, 376, 534 transformation mixed, 387, 388 transformation formula, 383, 541 Deviation moments, 576 Devol, George, 2 Differential transformation matrix, 402 Differential equation angular velocity, 383 first-order vectorial kinematic, 383 rotation matrix, 383 vectorial kinematic, 383 Differential manifold, 76 Differentiating B-derivative, 376, 379, 381, 538 coordinate frame, 376 G-derivative, 376, 384 second, 386 second derivative, 534 transformation formula, 383 Direct dynamics, 555 Direct kinematics, 247 Directed line, 202 Direction cosines, 53 Directional control system, 104, 263 cosine, 18, 25, 72, 73, 127, 243 Directional cosine, 17 Directional line, 16 Directions principal, 578

817 Displacement, 23 Distal end, 225, 669 Dynamics, 13, 526, 555, 609 actuator’s force and torque, 632 backward Newton-Euler, 626 direct, 13, 555 forward Newton-Euler, 627 4 bar linkage, 624 global Newton-Euler, 609 inverse, 13, 555 Lagrange, 632 motion, 557 Newton-Euler, 609 Newtonian, 563 one-link manipulator, 611 recursive Newton-Euler, 609, 626 robots, 609 2R planar manipulator, 616–618, 628, 637

E Earth effect of rotation, 563 kinetic energy, 589 moving vehicle, 498 revolution, 589 rotating, 498 rotation, 589 rotation effect, 498 Eigenvalue rotation matrix, 98, 142 Eigenvector rotation matrix, 98, 142 End-effector, 7 acceleration, 518 angular acceleration, 499 angular velocity, 440 articulated robot, 325 configuration vector, 470, 518 configuration velocity, 518 force, 628 frame, 231 inverse kinematics, 313 kinematics, 288 link, 225 orientation, 328, 441 path, 708, 712 position kinematics, 247 position vector, 435 rotation, 720 SCARA position, 172 SCARA robot, 256 space station manipulator, 257 speed vector, 419, 423 spherical robot, 292 time optimal control, 731 velocity, 431, 442 Energy Earth kinetic, 589 kinetic, 559, 572 kinetic rotational, 568 link’s kinetic, 633, 660 link’s potential, 634 mechanical, 589 potential, 591 robot kinetic, 633, 660

818 robot potential, 634, 660 Engelberger, Joseph, 2 Equation the most important, 19 Euclidean space, 126 Euler angles, 22, 62–64, 128 integrability, 67 coordinate frame, 66 equation of motion, 567, 571, 611, 627 frequencies, 64, 66, 370 inverse matrix, 62 -Lexell-Rodriguez formula, 93 parameters, 109, 110, 112, 113, 116, 117, 119, 130, 374, 375 rotation matrix, 62 rotation theorem, 102 theorem, 56, 102, 109 Euler angles, 59, 338 Euler-Chasles theorem, 187 Euler equation body frame, 571 Eulerian viewpoint, 391 Eulerian wrist, 105 Euler-Lagrange equation of motion, 735, 736 Euler, Leonhard, 56, 93, 130, 383 Euler theorem, 153 Exponent -1, 47 Exteroceptors sensor, 770

F Feedback control, 760 Final rotation formula, 100, 101 First variation, 736 Fixed frame, 14, 21 Fleming, Sir John Ambrose, 15 Floating bar, 266 Floating time, 739 analytic calculation, 744 backward path, 741 convergence, 743 forward path, 741 method, 738 multi DOF algorithm, 746 multiple switching, 751 1 DOF algorithm, 739 path planning, 745 robot control, 746 Force, 557, 558 action, 610 actuator, 632 body, 557 conservative, 591 contact, 557 Coriolis, 563 driven, 610 driving, 610 external, 557 function, 563 generalized, 586, 591, 635 gravitational vector, 635 internal, 557 moment of, 558 potential, 591 potential field, 588

Index reaction, 610 resultant, 557 sensors, 771 shaking, 626 total, 557 Force system, 557, 558 equivalent, 558 Formula derivative transformation, 383, 387, 388, 541–543 derivative transport, 502 geometric transformation, 359 mixed-derivative, 388 relative acceleration, 493, 494, 500 relative angular velocity, 365 Rodriguez, 132, 364 Forward kinematics, 36, 247 4 bar linkage dynamics, 624 4R planar manipulator statics, 669 Frame base, 231 central, 565 final, 231 fixed, 21 global, 21 goal, 231 moving, 21 neshin, 277 principal, 568, 571, 577, 578 reference, 20 special, 231 station, 231 takht, 277 tool, 231 transformation, 21 world, 231 wrist, 231 Function Heaviside, 322

G Galilei, 738 Gauss, Johann Carl Friedrich, 130 Generalized coordinate, 584, 586, 588, 592 force, 586, 588, 590, 591, 593, 595, 632 inverse Jacobian, 467 Globalframe, 14 Grassmann, Hermann, 205 Grassmannian, 205 Gripper, 7 Gripper axis, 269, 270 Gripper frame, 269 Gripper wall, 270 Group properties, 75

H Hamilton, Sir William Rowan, 120 Hamilton, William, 130 Hamiltonian, 731, 732 Hand, 7 Hayati-Roberts method, 298 Heading angle, 58 Heaviside, Oliver, 322 Heaviside function, 322

Index Helix, 28, 182 Hermann Grassmann, 205 Hodograph, 27 Home configuration, 228 Homogenous combined transformation, 167 coordinate, 154, 159 direction, 159 general transformation, 159, 166 inverse transformation, 162, 164, 165, 168 position vector, 154 reverse transformation, 162 scale factor, 154 transformation, 153–155, 157, 158, 162, 164

I Identity matrix, 198 Identity property, 75 Industrial robot classification, 236 Inner automorphism, 122 Inspection camera, 260, 263 Integrability, 67 Inverse function symbol, 47 trigonometric functions, 45, 46 Inverse dynamics, 555 Inverse kinematics, 36, 313 articulated manipulator, 319, 322, 323, 331 comparison of techniques, 347 decoupling technique, 313 Euler angles, 338 Euler angles matrix, 340 existence, 347 general formulas, 328 inverse transformation technique, 329 iterative algorithm, 343 iterative technique, 343 multiple solutions, 315 Newton-Raphson method, 343, 346 nonstandard DH, 341 numerical solution, 323 Pieper technique, 331 spherical robot, 334 techniques, 347 2R planar manipulator, 315 uniqueness, 347 Inverse orientation, 314 Inverse position, 314 Inverse property, 76 Inverse symbol, 47 Inverse velocity, 442 Inverted pendulum, 765 Iteration technique, 346 Iterative technique, 343

J Jacobi identity, 19 Jacobian, 343 analytical, 441, 442 angular, 441 displacement matrix, 420, 423 elements, 440 generating rate vector, 533

819 generating vector, 429–431, 469, 533 geometrical, 441 inverse, 344, 467 of link, 633 matrix, 344, 346, 348, 419, 420, 423, 427, 429, 431, 433, 437, 438, 442, 446, 463, 466, 469, 471, 472, 518, 519, 523, 644 polar manipulator, 423, 524 rotational matrix, 420, 423 spherical wrist, 446 2R manipulator, 424 Jacobian matrix, 419, 420, 423 displacement, 420 rotational, 420–422 systematic method, 422, 423 Jeeq, 28 Jerk, 506 angular, 505 body point, 506 definition, 27 global, 506 matrix, 516 rotational transformation, 505, 506 transformation, 516, 517 transformation matrix, 516 zero path, 694 Johns Hopkins beast, 2 Joint, 3 acceleration vector, 518 active, 4 angle, 227 axis, 3 coordinate, 3 cylindrical, 296 degree of freedom, 4 distance, 227 elbow, 229 free, 4 inactive, 4 orthogonal, 9 parallel, 9 parameters, 228, 289 passive, 4 path, 708 perpendicular, 9 prismatic, 3 revolute, 3 rotary, 3 shoulder, 229 speed vector, 419, 431 spherical, 269 translatory, 3 variable, 3 waist, 229 Joint variable angle, 3 coordinate space, 247 distance, 3 Jolt, 28 Jounce, 28

K Kinematic length, 227 Kinematic operation, 285 Kinematic pair, 3 Kinematic surgery, 285

820 Kinematics, 13, 35 acceleration, 489, 491 assembling, 276, 277 direct, 247 forward, 13, 36, 225, 247 forward acceleration, 518 forward velocity, 419 inverse, 13, 36, 313, 329 inverse acceleration, 520 inverse velocity, 442 motion, 149 numerical methods, 448 operation, 285 orientation, 91 rigid body, 149 rotation, 37 surgery, 285 velocity, 415 Kinetic energy, 559 Earth, 589 link, 660 parabolic coordinate, 590 rigid body, 572 robot, 633, 660 rotational body, 568 Kronecker’s delta, 73, 116, 568

L Lagrange dynamics, 632 equation, 659 equation of motion, 584, 591 mechanics, 591 multiplier, 738 Lagrangean, 591, 661 robot, 661 Lagrangean viewpoint, 391 Lagrange formula, 19 Lagrange multiplier, 738 Larz, 28 Law cosine, 33 of motion, 558 motion second, 565 robotics, 1 second of motion, 558, 559 third of motion, 558, 559 Levi-Civita density, 116 Lexell, Anders Johan, 93 Lie group, 76 Linear space, 24 Line of action, 22 Link, 2 angular velocity, 417 binary, 228 class 11 and 12, 238, 788 class 1 and 2, 236, 783 class 3 and 4, 237, 784 class 5 and 6, 237, 785 class 7 and 8, 238, 786 class 9 and 10, 238, 787 classification, 236 compound, 2 end-effector, 225 Euler equation, 627

Index kinetic energy, 633 length, 227 Newton-Euler dynamics, 609 offset, 227 parameters, 228, 289 recursive acceleration, 526, 528 recursive Newton-Euler dynamics, 626 recursive velocity, 527, 528 rotational acceleration, 526 translational acceleration, 526 translational velocity, 417 twist, 227 velocity, 415 Living frame, 269 Local frame, 14 Location vector, 184, 185 Louis Poinsot, 564 LU factorization method, 449, 452, 453, 456, 460 matrix inversion, 458 pivoting, 453 uniqueness of solution, 456

M Main coordinate frame, 240, 278 Mainframe, 7 Manganic, 597 Manipulator anthropomorphic, 9 articulated, 9, 229 Cartesian, 9 cylindrical, 9 definition, 5 elbow, 9 inertia matrix, 634 one-link, 592 one-link control, 768 one-link dynamics, 611 planar polar, 636 PUMA, 229 SCARA, 9 space station, 257 spherical, 9 3R planar, 250 transformation matrix, 325 2R planar, 593, 642 Manjanic, 597 Manjaniq, 597 Mass center, 558, 560, 565 Mass moment matrix, 576 principal, 578 Matrix displacement Jacobian, 420, 423 Jacobian, 420 orthogonality condition, 72 rotational Jacobian, 420, 423 skew symmetric, 92, 110, 137 Measure number, 16 Mechanics Newtonian, 563 Mechanism slider-crank, 245 3D slider-crank, 266 Method Hayati-Roberts, 298

Index non Denavit-Hartenberg, 293 parametrically continuous convention, 298 Minimum time control, 731 Mixed derivative, 377, 388 Mobius strip, 15 Mobius surface, 15 Modular manipulator, 302, 304–307 Moment, 557, 558 action, 610 driven, 610 driving, 610 external, 571 reaction, 610 resultant, 557, 571 total, 557 Moment of inertia, 576 about a line, 583 about a plane, 583 about a point, 583 characteristic equation, 582 eigenvalues, 578 elements, 576 Huygens-Steiner theorem, 578 matrix, 576 parallel-axes theorem, 576–578 polar, 576 principal, 577 principal axes, 568 principal invariants, 582 product, 576 rigid body, 568, 570 rotated-axes theorem, 576–578 Moment of momentum, 558 Momentum, 558 angular, 558, 569–571 linear, 558 translational, 558 Motion, 13 Moving frame, 14 Mozzi, Giulio, 186 Multibody articulated manipulator, 180 directional control system, 263 order free rotation, 102 order free transformation, 176 spherical robot, 244 spherical wrist, 280 3D slider-crank mechanism, 266

N Natural frequency, 762 Negative triad, 14 Neshin frame, 277, 279, 282 Newton equation of motion, 558, 559, 566, 584 Newton equation body frame, 566 definition, 537 global frame, 565 Lagrange form, 586 rotating frame, 563 Newton-Euler backward equations, 626 equation of motion, 610, 627 forward equations, 626, 627

821 global equations, 609 recursive equations, 626 Newton-Raphson method, 343 Non-standard Denavit-Hartenberg method, 245 Non Denavit-Hartenberg methods, 293 Norm-infinity, 458 Norm of a vector, 24 Numerical methods, 448 analytic inversion, 460 Cayley–Hamilton inversion, 462 condition number, 457 consistent solution, 456 ill-conditioned, 456 inconsistent solution, 456 Jacobian matrix, 469 LU factorization, 449 LU factorization with pivoting, 453 matrix inversion, 458 Newton-Raphson, 463, 466 nonlinear equations, 463 norm of a matrix, 458 pivot element, 453 undetermined solution, 456 uniqueness of solution, 456 well-conditioned, 456 Nutation, 59

O Object manipulation, 173 Obstacle free path, 716 Onager, 597 Opposite triad, 14 Optimal control, 731 a linear system, 733 description, 747 first variation, 736 Hamiltonian, 732, 735 Lagrange equation, 735 objective function, 731, 735 performance index, 735 second variation, 736 switching point, 733 Order-free theorem, 176 Origin, 16 Orthogonal exes, 226 Orthogonality condition, 20, 72 Orthogonal matrix, 71 Orthogonal transformation, 71 Orthogonal triad, 14 Over-damped, 762

P Parallel exes, 226 Passive transformation, 79 Path Brachistochrone, 745 Cartesian, 705 configuration, 27 constant acceleration, 694 constant angular acceleration, 722 control points, 707 cubic, 687

822 cycloid, 704 harmonic, 703 higher polynomial, 692 jerk zero, 694 joint space, 708 non-polynomial, 702 planning, 687, 705 point sequence, 696 quintic, 692 rest-to-rest, 689, 690 rotational, 720 segment method, 697, 699, 700 spline, 701 splitting method, 697, 699–701 to-rest, 689 Pendulum control, 765 inverted, 765, 770 linear control, 768 oscillating, 587 simple, 495, 586 spherical, 572, 574, 592 Permutation symbol, 116 Perpendicular exes, 226 Persia, 278, 543 Persian, 278, 597 Phase plane, 734 Physical quantity vectorial, 22 Physical quantity scalaric, 24 vectorial, 23 Pieper technique, 331 Pitch angle, 58 Pivot element, 453 Pivoting method, 453 Plücker angle, 208 axis coordinate, 205 classification coordinate, 206 coordinate, 202, 204, 206, 207 distance, 208 line, 202 line arrangement, 204 line coordinate, 202, 204, 205, 208, 211–213, 293 method, 202 moment, 208 ray coordinate, 204, 205 reciprocal product, 208 screw, 212 vector, 202 virtual product, 208 Plücker, Julius, 204 Plücker coordinate, 207 general case, 206 line at infinity, 206 line through origin, 206 Poinsot, Louis, 564 Poinsot theorem, 187 Point at infinity, 159 Polar manipulator inverse acceleration, 524 Jacobian, 423 Pole, 192, 193, 391 Pontryagin principle, 732

Index Pop, 28 Positioning, 13 Position sensors, 771 Position vector, 16, 23 Positive triad, 14 Potential energy, 559, 591 field, 559 force, 591 function, 559 Potential energy robot, 634, 660 Precession, 59 Prince of physicians, 278 Principal angular acceleration, 502, 503 angular velocity, 502 Principal directions, 578 Principle conservation of energy, 559 superposition, 564 Prismatic velocity coefficient matrix, 402 velocity transformation, 402 Problem composition, 132 decomposition, 132 open, 57 Proprioceptors sensor, 770 Proximal end, 225, 669

Q Quaternion automorphism, 122 Quaternions, 117, 129 addition, 118 composition rotation, 122 flag form, 117, 123 inverse rotation, 120 matrix, 130 multiplication, 118 rotation, 119 unit, 130

R Rabota, 1 Razi, Zakariya , 543 Razi acceleration, 543 Redundancy, 251 Reference frame, 14 Resolved rates, 442 Rest position, 228, 247 Revolute velocity coefficient matrix, 402 velocity transformation, 402 Right hand rule, 14 Rigid body acceleration, 508, 526 angular momentum, 569, 570 angular velocity, 97 Euler equation, 571 kinematics, 149 kinetic energy, 572 moment of inertia, 568, 570

Index motion, 149–151 motion classification, 195 motion composition, 152 non-central rotation, 169, 170, 195 off-center rotation, 169, 170, 195 principal rotation matrix, 581 rotational kinetics, 567 steady rotation, 574 translational kinetics, 565 velocity, 388, 389 Rigid body theorem, 153 Robot application, 13 articulated, 9, 252, 254, 282, 325, 433, 438 Cartesian, 11 classification, 8 closed-loop, 9 control, 12, 13 control algorithms, 763 cylindrical, 9, 307 dynamics, 13, 526, 609, 635, 642 end-effector path, 712 equation of motion, 662 fixed sequence, 8 forward kinematics, 247, 292 gravitational vector, 635 hybrid, 9 inertia matrix, 634 intelligent, 8 kinematics, 13 kinetic energy, 633, 660 Lagrangean, 634, 658 Lagrange dynamics, 632, 659 Lagrange equation, 635 link classification, 291 manual handling, 8 modified PD control, 770 Newton-Euler dynamics, 609 numerical control, 8 open-loop, 9 parallel, 9 PD control, 770 playback, 8 potential energy, 634, 660 recursive Newton-Euler dynamics, 626 rest position, 226, 228, 252, 254, 283 SCARA, 172, 255 serial, 9 spherical, 9, 230, 286, 292, 334, 432 state equation, 734 statics, 667 time-optimal control, 734, 746 variable sequence, 8 velocity coupling vector, 635 Robotic geometry, 9 history, 2 laws, 1 Robotics Institute of America, 1 Rodriguez rotation formula, 93, 95, 111–115, 121, 127, 150, 185, 191, 196, 201, 364, 372, 720 vector, 115, 132, 133 Rodriguez rotation matrix, 116 Rodriguez, Benjamin, 130 Rodriguez, Olinde, 93

823 Roll angle, 58 Roll-pitch-yaw body rotation matrix, 58 bovy angles, 58 frequency, 58 global angles, 50, 123 global rotation matrix, 50, 123 Rotation, 35 about global axes, 37, 43, 45, 46 about local axes, 52, 55, 56 acceleration transformation, 490, 492 axis-angle, 91, 94–96, 110, 112, 114, 127, 372 composition, 132 decomposition, 132 eigenvalue, 98, 142 eigenvector, 98, 142 exponential form, 114 final formula, 100 general, 71 infinitesimal, 113 instantaneous center, 391 local versus global, 69 matrix, 22, 127 order free, 102 pole, 391 quaternion, 119 representation, 91 Rodriguez formula, 94, 372 Rodriguez matrix, 116 stanley method, 117 Taylor expansion, 130 triple global axes, 45, 46 x-matrix, 38, 52 y-matrix, 38, 52 z-matrix, 38, 52 Rotational jerk, 506 Rotational path, 720 Rotation kinematics, 37 Rotations problems, 126 Rotation theorem, 102 Rotator, 94, 124, 126 Rule bac-cab, 19 relative angular acceleration, 493 relative angular velocity, 493 right-hand, 14 Runaround, 1

S Scalar, 24 equal, 24 equivalent, 24 Scale, 16 SCARA manipulator, 9 robot, 172, 255 Screw, 182, 186, 195 axis, 182, 392 central, 183, 185, 188, 198, 202, 212, 228, 289, 291, 293 combination, 200, 201 coordinate, 182 decomposition, 201, 202 exponential, 200 forward kinematics, 289

824 general, 199 instantaneous, 213 intersection, 293 inverse, 197–199, 201 left-handed, 182 link classification, 291 location vector, 182, 184 motion, 182, 188, 228, 289, 392 parameters, 182, 193 pitch, 182 Plücker coordinate, 212 principal, 186, 201, 202 reverse central, 183 right-handed, 14, 182 special case, 192 transformation, 185, 194 twist, 182 twist angle, 182 twist axis, 182 Screw motion, 182 Second derivative, 386 Second variation, 736 Sensor acceleration, 771 position, 771 rotary, 771 velocity, 771 Sensors, 8 Shaking force, 626 Sheth method, 293 Sheth notation, 293 Shuttle arm, 2 Simple derivative, 376 Sina, Abu Ali, 278 Sina coordinate frame, 240, 278 Sina frame, 104, 240, 241, 263, 266, 267, 278, 285 Singular configuration, 347 Singularity, 298 Snap, 28 Sooz, 28 Space station arm, 257 Spherical pendulum, 572, 574 wrist, 280 Spherical coordinate, 175 Spherical joint, 269 Spherical manipulator, 283–285 Spherical robot, 244, 286 Spherical wrist, 105, 269, 280–282, 286, 446 dead frame, 269 forearm, 269 gripper frame, 269 hand, 269 Jacobian, 446 living frame, 269 tilt, 269 tool frame, 269 turn, 269 twist, 269 Spin, 59 Spin angle, 58 Spinor, 94, 124, 126 Spline, 701 SRMS, 257 SSRMS, 257, 260

Index Camera, 260 dimensions, 257 kinematic parameters, 257 Stanley method, 117 Stark effect, 590 Switching point, 733, 741 Symbols, xi System of particles motion equation, 560

T Takht frame, 180, 277, 279, 283, 332 Tetrad, 15 Theorem Euler, 102 Euler-Chasles, 187 Euler rigid body, 153 Euler rotation, 102 Huygens-Steiner, 578 order free rotations, 103 order free transformations, 176 parallel-axes, 576, 578 Poinsot, 187 rotated-axes, 576 3R planar manipulator DH transformation matrix, 229 forward kinematics, 250 Tilt vector, 274 Time derivative, 376 Time optimal control, 731 Tool frame, 269 Top, 64 Torque, 558 Transformation, 35 active and passive, 79 general, 71 geometric, 504 homogenous, 153, 154 jerk, 516 kinematic, 504 order free, 176 orthogonal, 71 rotational acceleration, 504 rotational jerk, 505, 506 rotational velocity, 504 Transformation matrix derivative, 400 differential, 402, 403 elements, 73 velocity, 392 Translation, 35 Trebuchet, 240, 595 Triad, 14 coordinate frame, 14 left-handed, 14 negative, 14 positive, 14 right-handed, 14 Trigonometric equation, 318, 319 first type, 318, 319 Trigonometric functions inverse, 45, 46 Turn vector, 274 Twist vector, 274 2R planar manipulator

Index acceleration analysis, 512 assembling, 278 control, 766 DH transformation matrix, 235 dynamics, 593, 666 elbow down, 316 elbow up, 316 equations of motion, 595 forward acceleration, 519 ideal, 593 inverse acceleration, 522 inverse kinematics, 315, 316, 344, 464 inverse velocity, 443–445 Jacobian, 424 Jacobian matrix, 427 joint 2 acceleration, 509 joint forces, 623 joint path, 709 kinematic motion, 317 kinematics, 512 kinetic energy, 594 Lagrangean, 594 Lagrange dynamics, 642, 663 line path, 710 Newton-Euler dynamics, 616–618, 637 potential energy, 594 recursive dynamics, 628 singular configuration, 445 time-optimal control, 747 velocity analysis, 397 with massive joints, 617, 618, 637 with massive links, 663

U Under-damped, 762 Unimates, 2 Unimation, 2 Unit system, xi Unit vectors, 16

V Variable scalar , 26 Variation first, 736 second, 736 Vecface, 23 Vecfree, 23 Veclane, 23 Vecline, 23 Vecpoface, 23 Vecpoint, 23 Vecpolane, 23 Vecpoline, 23 Vecporee, 23 Vector absolute value, 16 addition, 24 associative property, 24 axis, 22 bounded, 23 characteristics, 22 commutative property, 24 comparable, 23

825 components, 16 decomposed expression, 16, 26 decomposition, 16, 26, 136 definition, 22 derivative, 26 direction, 22, 23 end-effector speed, 419 end point, 22 free, 23 function, 26 gravitational force, 635, 659 inverse element property, 24 joint speed, 419 length, 16, 22 line, 23 line of action, 22 magnitude, 16 modulus, 16 natural expression, 16, 26 null element property, 24 physical quantity, 22 plane, 23 point, 23 point-free, 23 point-line, 23 point-plane, 23 position, 16 requirements, 22 sliding, 23 space, 24 start point, 22 surface, 23 tilt, 274 turn, 274 twist, 274 types, 22 variable, 25 variable direction, 25 variable length, 25 vecface, 23 vecfree, 23 vecline, 23 vecpoface, 23 vecpoint, 23 vecpolane, 23 vecpoline, 23 vecporee, 23 velocity coupling, 635, 659 Vector space, 25 Vector triple product, 19 Velocity coefficient matrix, 402 definition, 27 discrete equation, 740, 748 inverse transformation, 394 matrix, 516 multiple frames, 390 operator matrix, 400 prismatic transformation, 402 revolute angular matrix, 405 revolute transformation, 402 rigid body, 388 sensors, 771 simple, 534 transformation matrix, 392–395, 400 Velocity kinematics, 415

826 forward, 415 inverse, 415 Vision camera, 263

W Walter William Grey, 2 Work, 559, 561 virtual, 586 Work-energy principle, 559 Working space, 255 Workspace, 12 dexterous, 12 reachable, 12 World War II, 2 Wrench, 564 Wrist, 12, 13, 271 classification, 270 dead frame, 269 decoupling kinematics, 314 design, 276

Index Eulerian, 272, 273 forward kinematics, 269, 271 frame, 231 kinematics assembly, 282 live frame, 269 Pitch-Yaw-Roll, 272, 275 point, 6, 269, 328 position vector, 327 possible types, 272 Roll-Pitch-Roll, 270–273 Roll-Pitch-Yaw, 272, 274 spherical, 6, 230, 244, 269, 272, 274, 286, 438 transformation matrix, 271, 282, 325 Wrist point, 263

Y Yaw angle, 58

Z Zero velocity point, 391