2103-530Industrial Robots 1 Phongsaen Pitakwatchara January 4, 2014 To those who work hard in robotics Contents Preface 4 1 5 2 Introduction 1.1 Background 1.2 The Mechanics and Control of Manipulators 1.3 Course Overview 2.2 2.3 2.4 2.5 6 . . . . . . . . . . . . 7 . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 Spatial Descriptions and Transformations 2.1 Descriptions: Positions, Orientations, and Frames 10 . . . . . . . . . 11 2.1.1 Description of a position . . . . . . . . . . . . . . . . . . . 11 2.1.2 Description of an orientation . . . . . . . . . . . . . . . . . 13 2.1.3 Description of a frame 17 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.2.1 Mappings: Changing the Description Mapping involving translated frame . . . . . . . . . . . . . 18 2.2.2 Mapping involving rotated frame . . . . . . . . . . . . . . 18 2.2.3 Mapping involving general frames . . . . . . . . . . . . . . 20 Operators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.3.1 Translational Operators . . . . . . . . . . . . . . . . . . . 22 2.3.2 Rotational Operators . . . . . . . . . . . . . . . . . . . . . 23 2.3.3 Transformation Operators 24 . . . . . . . . . . . . . . . . . . Transformation Arithmetics and Equations . . . . . . . . . . . . . 27 2.4.1 Compound Transformation: Multiplication Operator . . . 27 2.4.2 Inversion Operator . . . . . . . . . . . . . . . . . . . . . . 28 2.4.3 Transform Equations . . . . . . . . . . . . . . . . . . . . . Other Representation of the Orientation . . . . . . . . . . . . . . 29 34 2.5.1 Euler Angles Representation . . . . . . . . . . . . . . . . . 34 2.5.2 Fixed Angles Representation . . . . . . . . . . . . . . . . . 37 2.5.3 Angle-Axis Representation . . . . . . . . . . . . . . . . . . 40 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 Problems 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Manipulator Kinematics 45 3.1 Link Description . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 3.2 Denavit-Hartenberg Convention . . . . . . . . . . . . . . . . . . . 50 Chulalongkorn University Phongsaen PITAKWATCHARA . 100 Trajectory Generation for Two Path Points . . . .1 Problems 81 Velocity Propagation . Joint Space. .3 Velocity of the Robot . . . . .1. . . . . .2 Geometric Approach . . . . . . 82 5. . . . . . . . . . . 103 6. .2. . . . . . . . . . . . . . . . . . .2 Relative Angular Velocity of the Objects . . . . . . . . . . . . . . . . . . . .1 65 66 4. . . . . .2. . . . . . . . 110 Cartesian Space Schemes . . . . . . 72 . . . Problems 4 . . . . . . . . . . . . . . 100 .3 Singularities . . . . . . 90 5. . 53 59 61 63 4. . . . . . . . . . . .CONTENTS 3. . . . . . . . . . . . . . . . . . . . .1. . . . . . . . . . . . . . . . . . 108 . . .1 Cubic Polynomial Function 6. . . . . . . . .2. . 83 5. .3 Linear Function with Parabolic Blends . . . .1 Solvability . . . 83 5. . . . .1 6. . . . . .3 99 . and Cartesian Space . . . . . . . . . . . . . . .2. . . . . . .2 Higher-Order Polynomial Function . . 96 . . . . . .1 Cubic Polynomial Function 101 6. . . . .1 Existence of Solution . . . . . . . . . .3 Manipulator Kinematics 3. . . 101 6. . . . . . . . . . . . . 64 4. . . . . . . . . . . . . . . . . . . . . . . 67 4. . . . . . . . . . .2. . . . . . . .2. . . . . . 103 . . . . . . . . . . . . . . .4 Actuator Space. . . . . . .1 Path Description 6. 64 4. . .1 Relative Linear Velocity of Any Two Points . . . . . . . . . . . . . . . . . . . . . . . . Algebraic vs. . . . . . . . . . . . . . . . . . . . . . .1. . . . . . . Inverse Manipulator Kinematics 4. .3 . . . . . . . . . . . . . . . . . . .2 5 3 . . .2. . . . Algebraic Approach . . 85 5. . . . . . . . . . . . . . . . . .1. . .2. . . . . . .2. . . . . . . . . . . . . . . . . . . . . . 80 Examples Jacobians 5. . . . . . . . . . 82 5. . . . . . . . . . . . .1. . . . . . . .4 Static Force Problems 6 . . . . . . . . . . . . .1. . . . . . 70 . . . . . . . . . . . . . . . . 115 Problems . . 109 6. . .1. . . . . . .2. . . .3 Linear Function with Parabolic Blends . . . . . . . . . . . . . .2. . . 108 130 7 Manipulator-Mechanism Design 131 8 Introduction to Hybrid Force-Position Control 133 Appendix: Robotics Toolbox Chulalongkorn University 135 Phongsaen PITAKWATCHARA . .2 Joint Space Schemes 6. . . 6. . . . . 98 Trajectory Generation 6. . . . . . . . . . . . .2 Jacobian Matrix . . . . . .2. . . . . .2 Quintic Polynomial Function . . . . . . . . . . . . . Trajectory Generation for a Path with Via Points . . . . . . . . .1. . . . . . . . . . Geometric Approaches . . . . . . . . . . . . .2 6. . . . . . . .2 Multiple Solutions 4. . . . .2. . . . . . . . . . For this introductory course. and introductory to control of the manipulator. trajectory generation. Lastly. This one-semester course introduces fundamental topics in robotics science emphasizing in its mechanical dynamics.com. Audiences of this course are the fourth year stand- ing undergraduate students as well as graduate students who are interested in robotics and never have taken any course in robotics before. the course takes the serial manipulator as an examplar and studies the mathematical modeling of it. Although I have worked my best to prepare and revise the lecture note. I will be very grateful for any notice or comment which will help me improving the material. Therefore. I hope this lecture note will be useful to the students and readers for their studies and careers. Chulalongkorn University Phongsaen Pitakwatchara April 2013 Chulalongkorn University Phongsaen PITAKWATCHARA . mechanism design.Preface This is the lecture note for the course 2103-530 Industrial Robots 1 taught at Chulalongkorn University. The continuing course 2103-630 Industrial Robots 2 will use these basic knowledge in studying dynamics and control of it. Please send them to phongsaen@gmail. there might be some uncaught errors or some poorly explained issues. In particular. topics to be covered are the kinematics. Chapter 1 Introduction Chulalongkorn University Phongsaen PITAKWATCHARA . 1 Background 6 Figure 1. the robot is de ned as A robot is a reprogrammable. the topics to be pursued shall be limited to the mechanical aspects of the serial type manipulators. The International Federation of Robotics (IFR) has estimated that. Nevertheless. Robotics is an applied science that emerges from the utilization of knowledge in many disciplines together for analyzing and designing the robots. for example. Such fundamentals sciences are. however. However. industrial robots still possess the largest share in the market. parts.1.1.000 as shown in Fig. 1. or specialized devices through variable programmed motion for the performance of a variety of tasks. the number of the industrial robots will be raised to 1. tools. This de nition re ects the characteristics of the industrial robots in 1980's that have main usage in the assembly lines.120. mechanical engineering which will be useful for the design and analysis of the mechanisms that can produce the desired motion. computer engineering that addresses the e cient algorithms and program development.1: Bar chart showing the increasing number of the industrial robots used worldwide. by the end of 2013. 1. the robots has undergone substantial development such that the above de nition cannot cover all today's existed robots. In this introductory course. electrical engineering concerns with the electronics and circuits in controlling the robots. multifunctional manipulator designed to move materials. Chulalongkorn University Phongsaen PITAKWATCHARA . Few examples are humanoid robots or the insect robots.1 Background According to the Robot Institute of America (RIA). driving the robot to perform the desired tasks. this section oers a short overview about regulating the robot at large. most actuators employed in the robotic system today are motors. it gives the robot more freedom to move. By the current technology limitation. Robot is the physical structure which performs the task corresponding to the peripheral information and the control processing. Often they are equipped with the reduction and transmission mechanisms. Sensor is the components that acquires many information about the robot system itself and the environment in real time. is the wheel mechanism. 1. These information will be further processed and transferred to the control unit. The simplest mechanism.1. Novel actuators are made of synthesized polymer-form materials and have their usage in bio-inspired robotic systems. These are then sent to the actuators. Position and velocity information from the encoder and the tachometer are relatively simple compare to the image data retrieving from the camera. which is the mechanism that enables the motion of the robot from one place to another place. It receives the raw information from various sensors. The problem is to control the robot end eector such that its motion tracks the given desired Chulalongkorn University Phongsaen PITAKWATCHARA . 3. Another part of the structure is the manipulation part which is mainly responsible for the task execution. 2. Processing and control unit has the role which is comparable to the human brain.2. 4. Relationship between these four components are depicted in Fig. also being the most widely employed. 1.2 The Mechanics and Control of Manipulators Before studying in details the fundametals of the mechanics of the robot. The rst one is the locomotion. Other common kinds of the actuators are uid-power driven actuators such as hydraulics and pneumatics. Common structures of this part yield the appearance of the hand and the arm. Although the legged mechanism is much more complicated. They are fused and processed to appropriate forms as the commands for the control unit in calculating the corresponding control signals.2 The Mechanics and Control of Manipulators 7 There are essentially four main components comprising the robotic system. The structure may be divided into two main parts. Information from dierent kinds of sensors may possess dierent levels of complexity. Actuator is the part which causes the robot motion and makes it perform the desired task. for example. Boundaries for these unit might be physically vague in some systems due to the integration of them to produce more e cient robotic systems. which are 1. The dynamic equations of the robot are the set of nonlinear dierential equations governing the relationship between the applied force and torque and the resulting motion of the robot. Especially. It starts with chapter 2 where various methods in describing the position and orientation in three dimensional space are provided. as a starting it is necessary to rst describe the desired three dimensional motion of the end eector by means of some suitable representations. Chulalongkorn University Phongsaen PITAKWATCHARA . which is not appropriate for such calculation directly. 1.2: Robot components. However. When the desired trajectory has been transformed into the joint space already. known as the inverse kinematics calculation.1.3 Course Overview 8 Figure 1. This chapter serves as the important basis for the mechanics study of the robot. the motion of the robot end eector happens in the three dimensional space. dynamic equations of the robot in joint space may be used to calculate the necessary joint torque and force. Related operations such as the rigid body translation and rotation are considered as well. the given desired trajectory is usually described in the robot task space.3 Course Overview 2103-530: Industrial Robots 1 is considered an introductory course in robotics where the study is con ned to the mechanical aspects of the serial type manipulators. involving both the position and the orientation. This can be xed by transforming the desired end eector trajectory to the equivalent desired robot joint motion. These joint torques may further be used as the reference signals of the controller through the actuators. called the computed torque control. These required force and torque may be calculated via solving the robot's equation of motion. the appropriate force and torque must be provided to the robot typically at the joints. This dynamics may be derived from the kinematics of the robot and its inertial parameters. Generally. trajectory. To achieve the goal of controlling the motion of the end eector. the three dimensional orientation is far more complicated than the degenerated inplane orientation. This trajectory may be generated such that it causes the grasped object to be moved to a new desired position. Therefore. Once the study of robot kinematics is completed. an excellent instantly found to be useful in the applications. Basically. Finally.1. the relationship between the end eector velocity and the joint velocity is linear. Chapter 6 treats such topic on the account of the kinematical aspect solely. The path may be planned either in the joint space. is explained in Chapter 3 based on the modi ed Denavit-Hartenberg convention. By the duality.3 Course Overview 9 Chapter 3 and 4 are closely related. generation of the desired end eector trajectory may be possible. Chapter 5 extends the kinematical analysis of the robot to the velocity level. commonly by specifying its end eector posture. the appendix provides a concise explanation of the Robotics Toolbox based on MatLAB tool in analysis and design of the robot system. which is controlled by the Jacobian matrix. or more naturally in the Cartesian space. The process of calculating the corre- sponding robot posture (position and orientation) from the speci ed robot's joint variables. called the forward kinematics. The material may ® [6]. Chapter 4 is the reverse problem. Jacobian matrix also regulates the static relationship between the force acting at the end eector and the torque applied at the joint. The next two chapters 7 and 8 provides a glimpse on the design of the robot and the introductory to the hybrid force/position control. Rank de cient of this matrix signals the robot is in singular condition. It is related to calculating the corresponding robot's joint variables from the given robot posture. Chulalongkorn University Phongsaen PITAKWATCHARA . Chapter 2 Spatial Descriptions and Transformations Chulalongkorn University Phongsaen PITAKWATCHARA . p¯ = px py p z T . to describe or express them. Often.2. its coordinates {x y z} may be used explicitly. on the left superscript denotes the frame in which this vector is described. Figure 2. some other dierent ways of describing the orientation will be investigated.1 that A Chulalongkorn University p¯ 6= B p¯. coordinate systems have to be de ned. the position vector column vector of the dimension A where the letter A p¯ may be expressed by a 3 × 1.2. and Frames 2. The most commonly used one is the Cartesian coordinate system consisting of the rectangular frame. Orientations. and the coordinates.1 Descriptions: Positions. a point is commonly presented in several dierent frames. 2. It may be rather intuitive in planar motion for which the position is described by the position vector and the orientation by the angle.3. Orientations.1 shows a point coordinate frames {1} and P represented in two dierent {2}.4. The description can be viewed from dierent angles. explained in section 2. Several transformations may be combined to produce a new. Working with the general robot inevitably needs to analyze the motion in the three dimensional space.1 Descriptions: Positions. although vector quantities are inherently independent of the coordinates.2 and 2. 2. Phongsaen PITAKWATCHARA . the origin. and the coordinates x−y−z depicting the distances of the point with respect to the origin along the coordinate axes of the frame.1 Description of a position The fundamentals of kinematical analysis is the means to describe or express the position and orientation. If the frame has not been named. the origin of the frame. and Frames 11 In this chapter. methods of describing the position and the orientation of the objects in three dimensional space will be discussed. more generally. It is proven to be a useful tool in the analysis of the robot kinematics. In the last section. which leads naturally to the rotation matrix and. This can be achieved through the notion of the frame. here the kinematical problems are treated in a formal way for the three dimensional case. which leads to the other two interpretations of this result: the mapping and the operator. It is obvious from Fig. usually more complicated. First of all. In Cartesian coordinate system.1. Therefore. compound transformation described in section 2. the homogeneous transformation matrix introduced in subsection 2.3. The coordinate system consists of the frame. 20) Chulalongkorn University Phongsaen PITAKWATCHARA .1 Descriptions: Positions.2: Example 2. 12 ([1]. ([1]. Orientations.1. 19) Figure 2.1: A position vector and two Cartesian coordinate systems. pp.2. pp. and Frames Figure 2. Orientation of the Cartesian body- xed frame may be represented by the 3×3 matrix. By this where θx0 x Chulalongkorn University Phongsaen PITAKWATCHARA . T A o¯B = 10 6 . B p¯. o¯A = From the example.2 4. Mathematically.2. for which they add up to provide the original unit vector. Orientation of the body- xed frame may be described with respect to another frame called the reference frame.8 . which needs not be xed. which can be determined are the position vector of the origin of position vector of the origin of in a similar manner. B o¯A 6= A o¯B unless the coordinate axes of the frames are parallel and opposite to each other. Write the column vector for the following position vector: A p¯.1) 0 is the angle between the axes x and x. In order to describe the three dimensional orientation of an object.1 Refer to Fig. ˆi0 = cos θx0 xˆi + cos θx0 y ˆj + cos θx0 z kˆ ˆj 0 = cos θy0 xˆi + cos θy0 y ˆj + cos θy0 z kˆ ˆ kˆ0 = cos θz0 xˆi + cos θz0 y ˆj + cos θz0 z k. a frame called the body- xed frame is attached to the object rst.3 illustrates the body- xed frame {x0 y 0 z 0 } and the reference frame {x y z}.4 . 2.2 Description of an orientation There are many ways to describe an orientation in three dimensional space. Solution spect to the origin of of P ([1]. A o¯B .4 10. Figure 2. {A} = {x y z} is the {B} = {x0 y 0 z 0 } is the body- xed frame. etc.1 Descriptions: Positions. Prob.1. (2.4 showing two frames. The projections are its components along the {A}. Also cos θx0 x is the directional cosine of the vector ˆ i0 along the direction of the x-axis. 2. B o¯A . position vector may be expressed as T p¯ = 4 5 . Consider Fig. Orientations.1) By the direct measurement of the distance of point {A} and {B} P with re- along their coordinate axes. the clearest and widely used method of such will be explained. it can be said that in general. A B o¯A and A o¯B {A} described in {B} and the {A}. B {B} described in T 4.2. and Frames 13 Example 2. 2. The origins of both frames may be placed at the same location for the sake of clari cation. The orientation of the frame is then the same as the orientation of the body. A unit vector reference frame and along the coordinate axes of onto the coordinate axes of coordinate axes of {B} may be written in {A} by projecting the vector {A}. T B p¯ = 4. 2. In this section. 22) Figure 2. 19) Chulalongkorn University Phongsaen PITAKWATCHARA . pp.1 Descriptions: Positions.2. ([1]. pp. Orientations.4: Projection of a unit vector along the body- xed coordinate axis onto the reference frame.3: A body- xed frame {x0 y 0 z 0 } 14 a xed to the object. and Frames Figure 2. ([1]. 2. cos θM N = ¯ N ¯ M (2. each row is seen as the directional cosine vectors of the coordinate axes of the xed frame described in the moving frame. and Frames 15 means. which is very useful for typically long expressions in robotics. Hence. cθx0 z cθy0 z cθz0 z Aˆ kB Aˆ jB (2. it may be written as A BR With the symmetry of {A} Chulalongkorn University and Bˆ iA T T Bˆ = j A T Bˆ kA {B}. Orientations. Commonly adopted notation in robotics for this matrix is to write the directional cosine vectors of the coordinate axes of the moving frame. Other relevant notations are sθ and tθ for sin θ and tan θ. the rotation matrix of {B} with is A BR = Aˆ iB 0x 0x 0x cθ cθ cθ x y z = cθx0 y cθy0 y cθz0 y . It is convenient to express the above equations in a single compact unit with the help of the matrix.3) the rotation matrix in Eq. respect to {A} According to Eq. Using the fact that the directional cosine of arbitrary two vectors may be obtained by computing the scalar product of their unit vectors.2) The name of the frame to describe the orientation is subscripted and the name of the frame which this orientation is measured with respect to is superscripted. kˆ0 · kˆ (2.4) If the rotation matrix is read row-wise instead. the rotation matrix of {A} with respect to Phongsaen PITAKWATCHARA .2 becomes ˆi0 · ˆi A ˆi0 · ˆj BR = ˆi0 · kˆ ˆj 0 · ˆi ˆj 0 · ˆj ˆj 0 · kˆ kˆ0 · ˆi kˆ0 · ˆj . 2. in a column-wise manner. In addition. elements of the rotation matrix may be expressed in a dierent way.1. represented in the xed frame. This matrix is called a rotation matrix. . respectively. Eq. with the expression of ¯ ¯ N M · . Namely. the equation also shows the shorthand notation of cos θ as cθ.1 describes the orientation of the moving frame {B} and therefore the orientation of the object. 2.2.1 Descriptions: Positions. Moreover. identity matrix.6) In summary. From Fig. A which is obviously the transpose of B R. √ − 23 0 − 21 A 0 1 0√ BR = 1 0 − 23 2 and √ 1 − 23 0 2 B 0 1 0√ . The y -axis is pointing out of the page. {B} which is rotated relative to {A} about common y -axis by −150 .5 shows ◦ Solution the the Equation 2.5. (2.1 Descriptions: Positions. applying Eq.4 reveals the structure of the rotation matrix as the collection of the dot product of the unit vectors along the coordinate axes. Determine A B rotation matrix B R and A R.4 for this problem. (2. Orientations. it can be shown that if the physical frame is the right-handed coordinate frame. it may be concluded that R−1 = RT . 2. since T A A BR BR where I3 is the 3×3 Aˆ iB T T Aˆ = j B T Aˆ kB AˆiB Aˆ jB Aˆ kB = I3 . 2. determinant of the rotation matrix is equal to +1. Then.2. AR = − 12 0 − 23 Cross-checking with the property in Eq. This nice property holds because the rotation matrix is an orthonormal matrix.5.2 Fig. 2. B AR = T A BR . Particularly.5) Furthermore. Chulalongkorn University Phongsaen PITAKWATCHARA . 2. these dot products may be formed visually. and Frames {B} 16 may be expressed as B AR = Bˆ iA Bˆ jA Bˆ kA Bˆ iA T T T Bˆ = j A T Bˆ kA . the result is veri ed. calculation of the inverse of the rotation matrix is simply its transpose. Example 2. 2. For an object.1.2. frame where the latter coordinates.1. there is a need in expressing the same quantity in terms of several reference coordinate systems.5: Example 2. Phongsaen PITAKWATCHARA . B R and o denotes the position vector of the origin of {B} written in {A} In Fig. In this section. which is the description of a frame.2 4×4 homogeneous transformation matrix. mapping does not change the quantity Chulalongkorn University or changing Note that the per se . only the representation is changed.1 and 2.2 Mappings: Changing the Description 17 Figure 2. A {B} may be described with respect to {A} by A ¯B . From section 2. As depicted in Fig 2. In the next section. 2. a formal method in p¯ may be expressed mapping the description from one frame to another frame is explained. {B} = A B R. a position may be described by the position vector and an orientation may be described by three vectors. a position vector in either {A} or {B}. A o¯B . Conceptually.2.1. the homogeneous transform is introduced where a frame can be described by the 2.3 Description of a frame Position and orientation are needed to completely specify the posture.2. or the 3×3 3×1 3×1 directional rotation matrix. its posture may be described by attaching a body- xed frame onto it. Mappings: Changing the Description Often.6.1. Then the object's posture may be determined by specifying the position of the frame's origin and the orientation of the frame itself. as B p¯ = p x0 p y 0 p z 0 T . 2. the components of p ¯ may Chulalongkorn University Phongsaen PITAKWATCHARA .2. 2.2 Mappings: Changing the Description 18 Figure 2.6: Two frames positioned and oriented dierently by 2. The vector may be expressed in A or in {B} p¯ = px py p z {A} T p¯ and two rotated frames as . note that the components of any vector are just the projections of that vector onto the unit A vector along the coordinate axes.2.7 which illustrates a point {B}. (2. To determine the mapping between the two rotated frames.1 o¯B A and B R. As a consequence. A p¯ =B p¯ + A o¯B . for which their relationships are governed by the following equation.2 Mapping involving rotated frame Now consider Fig.8 which illustrates a free vector {A} and {B}. 2. where A o¯B de nes the mapping.7) This equation may be viewed as the (translational) mapping of the vector from {B} to {A}. Mapping involving translated frame Consider Fig.2. A A position of the point P P and two parallel frames {A} and may be described in either frame. 24) {A} Figure 2. pp.8: A free vector p ¯ may be expressed in two rotated frames A B R de nes the mapping.2 Mappings: Changing the Description 19 Figure 2. {B}. ([4]. pp. 25) Chulalongkorn University and {A} or {B}. ([4]. Phongsaen PITAKWATCHARA .2.7: Position of a point P described in two parallel frames A o¯B de nes the mapping. 8) Hence this equation may be viewed as the (rotational) mapping of the vector A from {B} to {A}. From section 2. which packs the above equation as the homogeneous mapping A B ¯ P¯ = A BT P . BR p (2. (2. In particular.2.2. Phongsaen PITAKWATCHARA .9) Rotation and translation may be treated as an integral unit of general mapping A B T . let consider Fig.10) where the quantities are expressed in the homogeneous four dimensional space.3 Mapping involving general frames Lastly. The description of the vector p¯ may be changed from {B} to by B R and o {A} by the following steps.2 Mappings: Changing the Description 20 be calculated as px = py = Bˆ iA · B p¯ Bˆ jA · B p¯ pz = Bˆ kA · B p¯. ?? which involves a vector p¯ and two frames {A} and {B} arranged in an arbitrary manner.1.2. Chulalongkorn University A p¯ 1 = A BR 0 A o¯B 1 B p¯ 1 . 2. A p¯ = I p¯ + A o¯B . the vectors p ¯ represented in {B} and {I} obey the following relationship. I B p¯ = A ¯. First introduce an intermediate frame {I} which has the same orientation as {A} but its origin is coincident with the origin of {B}.2.2 of the rotational mapping. Relative posture of these frames are speci ed A A ¯B . the change of description may proceeds to {A} readily by the transla- {B} to tional mapping in section 2. The equations may then be arranged in the matrix-vector form as A B p¯ = A ¯. Collectively the general mapping of the vector from {A} may be formu- lated as A B p¯ = A ¯ + A o¯B . where B R de nes the mapping. BR p (2. BR p Next. 11) homogeneous transformation matrix.1. 1 0 0 0 0 0 1 −1 B AT = 0 −1 0 3 0 0 0 1 Phongsaen PITAKWATCHARA . described in {A} is same point in {B}. To determine the unknown B p¯. the frames {A} and {B}. {A} and {B}.3 The origin of {B} is located relative to the origin of {A} by 3 yA -axis and 1 unit along zA -axis. Solution Figure 2. determine the position vector for the point P . Example 2.9: A vector p ¯ may be expressed in arbitrary two frames A of which its relative posture is described by B T . Referring back to section 2.3. The orientation of {B} is generated {A} by 90◦ about the x-axis. the homogeneous transformation matrix can then be used as an description of the frame. and A the position vector p ¯ and B p¯ as the problem stated. Note that the vector in the homogeneous space is the vector in the usual three dimensional space appended with the fourth element of A BT is called the = A BR A 0 o¯B 1 1.2 Mappings: Changing the Description 21 Figure 2.2.10 may be applied as units along from the rotation of B of which 0 2 A¯ P = 2 1 Chulalongkorn University A¯ P¯ = B AT P . which collects the information of both the relative position and orientation in the same place. the homogeneous mapping Eq. The matrix (2. 2. If the position vector of the T 0 2 2 .10 shows the point P . 11 which shows a frame and two points P and Q. In fact.10: Example 2. Chulalongkorn University (2.3 Operators Robot motion in general may be imagined as the simultaneous translation and rotation of the connecting rigid links altogether.12) Phongsaen PITAKWATCHARA .1 Translational Operators Consider Fig. 0 1 B ¯ P = 1 . serves for this objective. 2. of which its result is the newly translated point Q. for this problem. 2. 2. This operator in constructed from translating the original point action may be written vectorially as q¯ = T rans (¯ o) p¯ = p¯ + o¯.3 Operators 22 Figure 2. basic tools acting as the operators to move the object. on the other view. 1 as can be veri ed directly from the gure.2. are needed. Point Q is P along the direction and distance dictated by the vector o ¯. In other words.3.3. a point P is operated by the translational operator. For the purpose of describing these actions mathematically. the description of frame. which is just a collection of in nitely many points. By simple matrix multiplication. 3 Operators 23 Figure 2. the resulting vector corresponds to the new point which occurs by the translating action of o¯.7. 2. which its result is the newly translated point T rans (¯ o). BR q Phongsaen PITAKWATCHARA . 2. the vector q¯ may be represented in A q¯ as well by A Chulalongkorn University B q¯ = A ¯.12 which illustrates two frames {A} = {x y z} and {B} = {x0 y 0 z 0 }.3. For the mapping.12 to Eq. Point Q is in this case constructed from rotating the original point frame rotated from {A} P around the origin. or by the opposite motion of the representing frame. the resulting vector represents the same point in the new frame {A} that is translated by −¯ o. the intimate roles of the mapping and the operator may be understood.2. components of the vector of the rotated point described in the rotating frame point P {B} must be the same as those of the original represented in the initial frame B Q {A}. That is q¯ = A p¯. of Q. For the operator.11: A point P is operated by the translational operator. According to this setup.2 Rotational Operators Now consider Fig. It may be said that the relative position may be obtained by the motion of the point. 2. Comparing Eq. along with the point Frame {B} is the body- xed Q. From the mapping point of view. 2. and two points P and Q. comparing Eq. Therefore. Point rotational operator the two vectors describing these points are related by q¯ = R¯ p + o¯. 2. 2. 2. Similar to the translational case.3. its result is the newly rotated point Q Rot (R). Since every quantity is described in the same frame. Recapitulating.3 Transformation Operators Lastly.12: A point 24 P is operated by the rotational operator. For the mapping. then translating the intermediate point along the direction and distance dictated by the translational operator T rans (¯ o). the resulting vector represents the same point in the new frame {A} that is −1 rotated by R . or by the opposite motion of the representing frame. it may be said that the relative position may be obtained by the motion of the point. 29) Combining both relations.13 which involves a frame and two points P and Q. of which around the origin. the resulting vector corresponds to the new point which occurs by the rotating action of R. Q is constructed from rotating the original point P about the origin by the Rot (R). ([1]. 2. notations used for the frame in the operator equation may be dropped. the relationship between the rotated vectors may be written as q¯ = Rot (R) p¯ = R¯ p.14) Phongsaen PITAKWATCHARA . Chulalongkorn University (2.2.8. the intimate roles of the mapping and the operator may again be understood.3 Operators Figure 2. For the operator.13) P to point Q is rotated to the current frame in the same {B}. where R is the rotation matrix that rotates point direction as the initial frame {A} (2.13 to Eq. pp. let consider Fig. the resulting vector represents the −1 same point in the new frame {A} that is transformed by T . Fig. 2.14 shows the point and the resulting point Chulalongkorn University Q. of B which its result is the newly rotated and translated point Q. Interpret this operation as the equivalent vector mapping between two frames. T . Locate the position of the resulting point. x- units. or by the opposite motion of the representing frame. these successive operations may be combined using the homogeneous transformation matrix T as ¯ = T P¯ . T is viewed as the general operator which rotates and translates point P to point Q in the same manner as the initial frame {A} is transformed to the current frame {B}.3 Operators 25 is operated by the general transformation operator. Equation 2.10 and Eq. in turn. For the operator. Q (2. 2. q¯ 1 = R o¯ 0 1 p¯ 1 .4 A point is located by the position vector p ¯ = 3 7 0 ◦ The point is rotated about the origin by 30 and then translated along the and y -axis by 10 and 5 P T .2. the resulting vector corresponds to the new point which is generated from the general motion operator T.13: A point P Eectively. Position of this point may be determined by the Phongsaen PITAKWATCHARA .10. 2. In other words. the transformation matrix described in the same frame. Note again that all quantities are Here.15 are in fact the same equations interpreted dierently. Example 2. Speci cally. Note that q ¯ = A p¯.15) where the vectors are expressed in the homogeneous four dimensional space. Solution P According to the problem statement. the relative posture may be obtained by the motion of the point. Figure 2. Namely for the mapping in Eq. 1 0 0 1 which combines the explained rotational and translational operator.562 .098 12. direct measurement for this simple example or it may be carried out formally through the following homogeneous transformation matrix. = 0 1 Figure 2. 0 1 expressed in the moving frame to the one represented in the xed frame Chulalongkorn University {A} {B} is mapped by the homogeneous mapping Phongsaen PITAKWATCHARA . Hence. c30◦ −s30◦ s30◦ c30◦ A¯ B ¯ P = P =A T B 0 0 0 0 Position vector of the point P 0 10 3 7 0 5 1 0 0 1 0 1 9.4 (viewed as transformation operator).e.562 = .098 12. position of the transformed point Q may be calculated as c30◦ −s30◦ ◦ c30◦ ¯ = T P¯ = s30 Q 0 0 0 0 3 0 10 7 0 5 1 0 0 1 0 1 9.15 presents the above equation from the mapping point of view.2.3 Operators 26 Figure 2.14: Example 2. c30◦ −s30◦ s30◦ c30◦ T = 0 0 0 0 0 10 0 5 . i. C From section 2. {B}. This matrix.16 where there are three frames: B {A}.1 Compound Transformation: Multiplication Operator Mapping between two frames {A} and {B} can be generalized to the mapping through several frames naturally.4 (viewed as general mapping). Accordingly. Succesive transformations A B T and C T are known. it is shown how these operations are related to the physical mapping or transformation of frames. It is desired to map the vector p ¯ represented in {C} to the one described in {A}. which is the same as the homogeneous transformation operator matrix moving point P to Q. p ¯ may be mapped to the intermediate frame as B p¯ by B Chulalongkorn University C ¯ P¯ = B CT P . and {C}.4 Transformation Arithmetics and Equations 27 Figure 2. Consider Fig.2. Phongsaen PITAKWATCHARA . many frames are involved. 2.4. 2. 2.3.2. there is a need to determine the result of mapping or transforming through several frames. In this section. as will be seen in chapter 3. As a consequence. is the inverse of the matrix used to transform {B} to {A}.15: Example 2.4 Transformation Arithmetics and Equations For the analysis of a manipulator. A B T . there are two operations for the set of homogeneous transformation matrices: the multiplication and the inversion. xx) Similarly.16) is obvious. A CT B =A B T C T. one need not perform the general inverse matrix calculation due to the special structure of the matrix. the compound mapping may be formulated. ([4].6. 2. Nevertheless. A CT = A B B RC R A B ¯C BR o 0 1 + A o¯B . pp. Cancellation of the subscript and the superscript of the intermediate frame {B} acts as a good mnemonic of straightforwardly writing the compound mapping equation. A B C ¯ P¯ = A BT C T P . Explicit multiplication of the detailed transformation matriA ces leads to the formula of C T .2 Inversion Operator As shown in Eq. This does not extend to the homogeneous transformation matrix. 2.16: Several transformation matrix may be combined to yield the compound transformation. as explained earlier. (2.4 Transformation Arithmetics and Equations 28 Figure 2.17) Compound transformation or operator may be formulated readily by acknowledging it as just another interpretation of the mapping. from which the following compound mapping. Combining the above mappings. (2. the mapped B p¯ may be furthered mapped to the xed frame as A A p¯ by B ¯ P¯ = A BT P . the inverse of the rotation matrix can be calculated simply by taking the transpose. Chulalongkorn University Phongsaen PITAKWATCHARA .4.2. ??. See Fig.19) Phongsaen PITAKWATCHARA . {U } and {D} as the reference Then. This is achieved through the compound transformation along the intermediate frames conventionally attached to the robot linkages. the mapping must be applied. for example. more primitive.18 displays the set of frames of which the transformation relative to their consecutive frames are assumed available rst. AT or by U DT D −1 = UB T B . Mathematically.4. 2. −1 A BT =B A T. and the working table base frame.17. 2. It should be noted that there are several variations to derive the transform equations. = UB T B CT C T (2. the robot base frame.4 Transformation Arithmetics and Equations 29 Physical meaning of the inverse operation of the transformation matrix is to exchange the role of two frames. to establish this relationship.3 1 Transform Equations A typical situation in the manipulator analysis is to determine the motion. the transformation of the end eector with respect to some reference frame. one may formulate the transform equation from the successive frame transformation matrices by walking through the loop path of the frames. More details are given in the next chapter. (2. In other words. In Fig. they may be equate to obtain the transform equation as U D −1 AT A T Chulalongkorn University D −1 . B From the de nition of the homogeneous transformation matrix.18) BT 0 2.2. As an example. The arrow joining the origin of two frames indicates their relative representation. B A TA o¯A = −B ¯B = −A o¯B . Fig. the compound transformation between these two frames may be calculated either by U DT −1 = UA T D . one may choose and the target frames respectively rst. Generally. CT C T Consequently. AR o BR A Substituting this relation into the transformation matrix. AT = 0 Since B 1 o¯A is the negative of A o¯B but expressed with respect to the dierent frames. the inverse of B T may be computed as A T A TA −1 R − R o ¯ B A B = B . A T may be written as B B o¯A B AR . or. Other scenario might entail the transformation between the end eector and the manipulated object via the camera frame. and then the rotation of 40◦ about the moving y -axis. That Solution {B} is c (−30◦ ) −s (−30◦ ) 0 = s (−30◦ ) c (−30◦ ) 0 . Relative position {B}. and E T .−30◦ ◦ Similarly. B From Fig. E T may be determined by rst observing that ◦ {S} is oriented relative to {S} with the rotation of −30 about z -axis.17: Transformation between the end eector and the manipulated object. Determine the homogeneous transformation matrix E T . the working table frame {S}. {E} is oriented relative to {S} with rst the rotation of −20 about {S} z -axis.5 Figure 2. From these results. pp. O T . and the camera frame {C} are as shown B S C in the gure. ([4].2. C C B E E S T . Compare the results and orientation of the base frame frame {O}. matrix manipulation may be performed on the above equation to yield D CT U −1U B =D A T AT B T C T. 2. xx) D If the actual requirement is to determine the transformation C T . the object {E}.4 Transformation Arithmetics and Equations 30 Figure 2. the end eector frame with the direct observation.19. calculate O T . 0 0 1 S BR = Rz. S T . corresponding to the relative posture of the object relative to the end eector frame. O T . Example 2. and O T .19 depicts the robot grasping an object. The resulting Chulalongkorn University Phongsaen PITAKWATCHARA . 5. pp. pp. ([4].4 Transformation Arithmetics and Equations 31 Figure 2. xx) Figure 2.2.18: Frame diagram to formulate the frame transform equation.19: Example 2. 64) Chulalongkorn University Phongsaen PITAKWATCHARA . ([1]. Next.−20◦ Ry. S OT = Rz.19 to be S o¯E/B = 3 −0. = 0.7544 −0.5 0.1116 0. Phongsaen PITAKWATCHARA .40◦ Consequently.7660 Moreover.30◦ Rz.1736 0.7660 1.5 S −0.5 0.6428 0 0.6330 0. brings the orientation of {O}.8938 B ET = −0.−150◦ 0 −0.1330 0. Putting the orientation and position information together.2 1 T .8938 1.9848 0.7544 −0.4 Transformation Arithmetics and Equations 32 transformation would be c (−20◦ ) −s (−20◦ ) 0 c (40◦ ) 0 s (40◦ ) .2. rst note that C Chulalongkorn University o¯O = 0. by 2.25 0 0 0 1 C To determine O T .45 0 0 0 1 is determined. posture of the object will now be determined relative to the table corner.8660 0.2 0. 0 1 0 = s (−20◦ ) c (−20◦ ) 0 ◦ ◦ 0 0 1 −s (40 ) 0 c (40 ) S ER = Rz. As a result. = 1 0 0 1 0.5 o¯O −0.9481 0.45 T .40◦ 0. the orientation of {E} relative to {B} may be obtained by the following compound transformation: B ER = SB R−1SE R = Rz.9848 0.5 0 0.1330 −0. the relative position of the origin of {E} to {B} is seen from Fig.8660 0 0.1736 0. 2.25 T . Relative position of the origins is readily observed to be S Rotation about {S} z -axis o¯O = by −150◦ 0.6330 2.9481 0.−20◦ Ry. the transformation matrix 0.45 which must further be represented in B o¯E = SB R−1S o¯E/B = {B} T .6428 0 0.1116 .7 1.5 . 2198 −0.5 0.2 0.7660 0.5 0. S T may be observed directly from Fig. Phongsaen PITAKWATCHARA .8660 −0.6040 1.6040 .7198 −0.25 0 0 0 1 = B E C E T C T OT Chulalongkorn University . As a result.3 −0.2 C .5 C .8660 −0.5 0.4 Transformation Arithmetics and Equations 33 Relative orientation may be determined by representing the unit vectors of the x-y -z -axes of {O} in {C}.9397 0. OR = 0 0 −1 Therefore. Position of the origin of {E} with respect to obvious from Fig.19.8660 −0.2198 −0.9397 0.2620 −0.7198 −0. 2.7 −1 0 0 0.8 .3420 −0. T = E 0. B OT C −1C =B ET ET OT −0.3420 −0.8660 0 C 0. However.5 0 1.6428 0 −0. 0.6428 0 −0.25 0 0 0 1 {C} is T C o¯E = −0. = 0. 0.5 0 . 0.8660 0 4.1108 = 0 0 1 1.3239 −0.5 0 0. OT = 0 0 −1 1 0 0 0 1 C Similarly.8 0 0 0 1 The remaining transformation matrices may be obtained by formulating the transform equations and substituting the above results.7660 Hence.19 as 0 −1 0 0. T = S 0 0 −1 1.3 1.7 C . the relative orientation should be calculated indirectly via C ER = C S S RE R 0 −1 0 c (−20◦ ) −s (−20◦ ) 0 c (40◦ ) 0 s (40◦ ) 0 s (−20◦ ) c (−20◦ ) 0 0 1 0 = −1 0 ◦ ◦ 0 0 −1 0 0 1 −s (40 ) 0 c (40 ) 0.2.2620 −0. 2.5 0.8660 0 0. 5 −1C =C ET OT −0. For this case. there are totally six inherent constraints among nine elements of the rotation matrix. Consequently. Due to the constraints of three mutually perpendicular unit vectors along the rectangular coordinate axes.7660 0. There are several choices. Let Commonly used axes are the ZY X .7419 0 0 0 1 E C C T OT Other Representation of the Orientation Section 2. this representation must also be speci ed with the sequence of three axes about which the rotations occur: totally of 3×2×2 = 12 possibilities. only three parameters can describe arbitrary three dimensional orientation completely.7198 −0.5 Other Representation of the Orientation Figure 2. These three angles of the basic rotations about the axes of the moving frames are called Euler angles.2 presented a rotation matrix to describe the three dimensional orientation. −0. or moving. (α.5868 −0. the orientation is constructed from three successive rotations as Chulalongkorn University Phongsaen PITAKWATCHARA .9397 0 −0.4924 0.7660 −0.1 Euler Angles Representation This method chooses three independent parameters for describing the orientation to be three consecutive angles of the basic rotations around the axes of the current.4924 0. = 0.6040 −0. 34) −1C S −1 =C ET OT OT 0. 2.4132 0. Successive rotations must not occur about the same axis.6661 . γ) be the Euler angles around the ZY X axes as shown in Fig.6428 1.1. additional three dierent ways of describing the orientation shall be studied. the ZY Z . β.7660 0.1271 0. Therefore. frames. however.3420 0.1954 −0.20: Orientation description by the 34 ZY X Euler angles.5. pp.20.6428 1. 2.3583 0 0 0 1 E ST = E C O C T OT S T E OT = = 2.2620 −0. and ZXZ .2. ([1].0252 .2198 0.6428 0 −0. in fact! Here. 20 determines the corresponding rotation matrix of the ler angles (α. rotation is constructed from the rotation about x2 -axis of the current frame with the angle γ . 2. Hence the equivalent rotation matrix is the description of {x3 y3 z3 } {x0 y0 z0 } {x3 y3 z3 } R relative to {x y z } {x0 y0 z0 } {x y z } of which its detailed mappings are {x y z } = {x01 y01 z01 } R{x12 y12 z12 } R{x23 y23 z23 } R = Rz. β. β.20) ZY X Eu- The opposite problem is to determine the Euler angles out of the given rotation matrix. one can conclude that the common factor r32 and r33 must not be zero simultaneously too. The remaining angles will also be dierent for each cβ = ± case. −sβ cβsγ cβcγ Equation 2. eventually.20.β Rx.21) γ = atan2 (r32 . making the original frame rotated to {x1 y1 z1 }. if a rotation matrix r11 r12 r13 R = r21 r22 r23 .20. Initially. r33 ) . First rotation occurs from the rotation about z0 -axis with the angle α. First. In particular. See Fig. assume that at the same time.5 Other Representation of the Orientation 35 {x0 y0 z0 }. (2. γ) will be calculated. −sβ = r31 6= ±1 and p 2 1 − r31 6= 0. This turns the frame to be coincident with {x3 y3 z3 }. 2. the angle β has two possible values depending on whether cβ > 0 or cβ < 0. the Euler angles are determined as α = atan2 (r21 . Hence.2.γ cαcβ cαsβsγ − sαcγ cαsβcγ + sαsγ = sαcβ sαsβsγ + cαcγ sαsβcγ − cαsγ . and the last.α Ry. By comparing the given matrix elements with the expressions in Eq. in the following the equivalent Euler angles (α. γ). cβ 6= 0 and the member Moreover. Chulalongkorn University Phongsaen PITAKWATCHARA . the rotated frame coincides with calculated by recognizing each sub-rotation as the description of the resulting frame relative to the previous one. β. If cβ > 0. Second rotation occurs from the rotation about y1 -axis of the current frame with the angle β that makes the frame rotated to {x2 y2 z2 }. since any row or column of the rotation matrix is a directional unit vector. γ) may be follow. The equivalent rotation matrix of the ZY X Euler angles (α. r31 r32 r33 ZY X r11 and r21 are not zero describing an arbitrary rotation is provided. 1 − r31 (2. r11 ) q 2 β = atan2 −r31 . The third. 20 reduces to 0 −sα+γ −cα+γ R = 0 cα+γ −sα+γ . 2. Therefore. −r11 ) q 2 β = atan2 −r31 . r13 ) = atan2 (−r12 . Therefore. −r33 ) . Eq. In this case. 1 0 0 Consequently. there are in nitely many values of Euler angles that lead to this same orientation. −r13 ) = atan2 (−r12 .21 and 2.2. there are two ways in achieving the speci ed orientation around the particular sequence of rotation axes. 2 Chulalongkorn University Phongsaen PITAKWATCHARA . the values of α and β cannot be deduced.20 reduces 0 −sα−γ cα−γ R = 0 cα−γ sα−γ . This is called the representational singularity of Euler angles. the values of the Euler angles will become π 2 α − γ = atan2 (−r12 . For the case when the values of both and r33 r11 and r21 be zero simultaneously. r22 ) . It happens when the rst and the third sub-rotation occur about the same physical axis of rotation. For the ZY X Euler angles.22) γ = atan2 (−r32 . if to (2. 2. it implies sβ = 1 and cβ = 0. In this case.5 Other Representation of the Orientation While if cβ < 0. the representational singularity will π happen when β = ± . Eq. r32 will be zero as well. β = − However. Eq.22 cannot be used to calculate the Euler angles because the function atan2 (·) is not de ned when both arguments are zero. (2. for which the axis z0 and x2 will line up. 36 the Euler angles will be dierent: α = atan2 (−r21 . r22 ) . it implies sβ = −1 and cβ = 0. it can be said that r31 = −sβ = ±1. Imposing the constraint of unit vector for each row or column.24) In other words.23) r31 = −1. − 1 − r31 (2. If r31 = 1. the values of the Euler angles will become π 2 α + γ = atan2 (−r12 . 1 0 0 Consequently. 2. β = For these latter cases. 2.5 Other Representation of the Orientation 37 Example 2.6 An orientation is constructed from the following sub-rotations. z -axis by 90◦ . Then follow by the rotation about the y -axis of the current frame by −180◦ . Finally, rotate about the current x-axis by −90◦ . Describe this orientation with the ZY X Euler angles. Firstly, rotate about the Solution The speci ed orientation occurs from the sub-rotations about the body- xed frame. Hence the resultant rotation matrix may be calculated by post-multiplying them in order; R = Rz,90◦ Ry,−180◦ Rx,−90◦ c (90◦ ) −s (90◦ ) 0 c (−180◦ ) 0 s (−180◦ ) 0 1 0 = s (90◦ ) c (90◦ ) 0 0 0 1 −s (−180◦ ) 0 c (−180◦ ) 1 0 0 × 0 c (−90◦ ) −s (−90◦ ) 0 s (−90◦ ) c (−90◦ ) 0 0 −1 = −1 0 0 . 0 1 0 This rotation matrix can also be represented by the ZY X Euler angles where the values are determined by Eq. 2.21 and Eq. 2.22 as α = atan2 (−1, 0) = −90◦ β = atan2 (0, 1) = 0◦ γ = atan2 (1, 0) = 90◦ , or α = atan2 (1, 0) = 90◦ β = atan2 (0, −1) = 180◦ γ = atan2 (−1, 0) = −90◦ . This means that the speci ed orientation may also be constructed from the sub◦ ◦ ◦ rotations in the same order, but with dierent angles of −90 , 0 , and 90 . 2.5.2 Fixed Angles Representation Three independent parameters for describing the orientation may be the three consecutive angles of the basic rotations about the axes of the xed, or reference, frames. Again, successive rotations must not occur about the same axis, however. Similarly, this representation must also be speci ed with the sequence of three axes about which the rotations occur. Hence it has totally 12 possibilities. These three angles of the basic rotations about the axes of the xed frame are called Chulalongkorn University Phongsaen PITAKWATCHARA 2.5 Other Representation of the Orientation Figure 2.21: Orientation description by the xed angles. Commonly used axes are the 38 XY Z XY Z and xed angles. ([1], pp. 38) ZY X . As a historical note, xed angles description of the orientation has its root from the roll-pitch-yaw angles used to represent the orientation of the vehicle in nautical and aeronautical science. Let (ψ, θ, φ) be the xed angles around the XY Z axes as shown in Fig. 2.21. For this case, the orientation is constructed from three successive rotations as {x0 y0 z0 }. First rotation occurs ψ , making the original frame rotated to {x1 y1 z1 }. Second rotation occurs from the rotation about y0 -axis of the xed frame by the angle θ that makes the frame rotated to {x2 y2 z2 }. The third, and the last, rotation is constructed from the rotation about z0 -axis of the xed frame with the angle φ. This turns the frame to be coincident with {x3 y3 z3 }, eventually. See Fig. 2.21. The equivalent rotation matrix of the XY Z xed angles (ψ, θ, φ) may be follow. Initially, the rotated frame coincides with from the rotation about x0 -axis with the angle calculated by realizing each sub-rotation as the operator which further rotates the resulting frame from the previous rotation. Hence the equivalent rotation matrix will be the operator that rotates {x0 y0 z0 } {x3 y3 z3 } to of which its detailed sub-rotations are R = Rz,φ Ry,θ Rx,ψ cφcθ cφsθsψ − sφcψ cφsθcψ + sφsψ = sφcθ sφsθsψ + cφcψ sφsθcψ − cφsψ . −sθ cθsψ cθcψ (2.25) Equation 2.25 determines the corresponding rotation matrix of the angles (ψ, θ, φ). XY Z xed The opposite problem is to determine the xed angles out of the given rotation matrix. This can be done in a similar manner to the previous Euler angles description. Hence only the results shall be mentioned. The XY Z xed angles description for the speci ed rotation matrix may be calculated as follow. For the case when the values of Chulalongkorn University r11 and r21 , or r32 and r33 , are not zero Phongsaen PITAKWATCHARA 2.5 Other Representation of the Orientation 39 simultaneously, there are two possible sets of the angles, i.e. ψ = atan2 (r32 , r33 ) q 2 θ = atan2 −r31 , 1 − r31 (2.26) φ = atan2 (r21 , r11 ) , when cθ > 0, and ψ = atan2 (−r32 , −r33 ) q 2 θ = atan2 −r31 , − 1 − r31 (2.27) φ = atan2 (−r21 , −r11 ) , when cθ < 0. The following cases cause the representational singularity of xed angles, which happen when the resulting rotation can be constructed by merely two, or one, sub-rotations. For the XY Z xed angles, the singularity will happen π when θ = ± . The x-axis of the resultant frame will always direct along the 2 z -axis of the reference frame. This indicates that the resulting rotation can be constructed from just two consecutive rotations, i.e. the rst rotation about y0 -axis by ± π2 and follow by the appropriate rotation about z0 -axis. π When θ = ± , cθ = 0 and sθ = ±1. For the case r31 = 1, there are in nitely 2 many xed angles which can describe this same orientation. Their values are as follow; π 2 φ + ψ = atan2 (−r12 , −r13 ) = atan2 (−r12 , r22 ) . θ = − If r31 = −1 (2.28) instead, the angles are changed to π 2 φ − ψ = atan2 (−r12 , r13 ) = atan2 (−r12 , r22 ) . θ = Example 2.7 Describe the rotation in Ex. 2.6 with the Solution XY Z (2.29) xed angles. Referring to the rotation matrix 0 0 −1 R = −1 0 0 . 0 1 0 This rotation matrix may be represented by the XY Z xed angles where the values are determined by Eq. 2.26 and Eq. 2.27 as ψ = atan2 (1, 0) = 90◦ θ = atan2 (0, 1) = 0◦ φ = atan2 (−1, 0) = −90◦ , Chulalongkorn University Phongsaen PITAKWATCHARA The description is kˆ = [kx ky kz ]T be a unit vector along the axis of rotation written in the ˆ corresponds to the direction of reference frame {x y z}. called the angle-axis representation.22: Orientation description by the angle-axis representation θ. actually this must happen around the untouched the rotation. This realization is supported by the Euler's theorm of rotation. a subordinate of the Chasles' theorem. after θ about kˆ. sub-rotations is equivalent to the rotation of Chulalongkorn University θ kˆ Combination of these three about kˆ directly. the rotation by θ about kˆ can be represented by the rotation matrix. 41) or ψ = atan2 (−1. which is now coincident with the However.3 Angle-Axis Representation Arbitrary orientation may be described in the most natural way by specifying the angle and the axis of rotation.2. −1) = 180◦ φ = atan2 (1. Consider Fig. the current must be redirected back to the original direction.22. 0) = −90◦ θ = atan2 (0. Therefore. Let θ be the angle of the ˆ. Phongsaen PITAKWATCHARA . ([1]. Then the rotation of z -axis. kˆ . pp. Hence the representation is commonly written as the pair of rotation about k the angle and the directional unit vector of the rotation: θ. 2. is executed.5 Other Representation of the Orientation 40 Figure 2. Such rotation may be realized indirectly by rst redirecting the rotation axis kˆ to align with the z -axis. ˆ k . 2. Mathematically. Let Of course.5. kˆ or θkˆ. Positive direction of k positive rotation which obeys the right-hand rule. 0) = 90◦ . 5 Other Representation of the Orientation 41 {x0 y0 kˆ} {xyz} Rk. it is desirable to determine the angle and axis for which such rotation will yield the same orientation. 2.32. calculated as −1 θ = cos Chulalongkorn University The angle of rotation r11 + r22 + r33 − 1 2 θ hence may be .θ ˆ kx2 vθ + cθ kx ky vθ − kz sθ kx kz vθ + ky sθ ky2 vθ + cθ ky kz vθ − kx sθ .31 to obtain Rk.30 and 2.22. r11 r12 r13 R = r21 r22 r23 . ˆ = 0 0 ˆ R · Rz. (2.32) is used to make the matrix more compact.2. From Fig.33) Phongsaen PITAKWATCHARA . Similar to other rotation descriptions. = kx ky vθ + kz sθ kx kz vθ − ky sθ ky kz vθ + kx sθ kz2 vθ + cθ The function vθ = versθ = 1 − cθ (2.and z -axis of the reference frame = Rz.θ ·{xyz} {x y k } (2. 2.θ R. where kˆ is recognized as a unit vector.β .30) where the sub-rotations are referred to the reference frame and so the post{xyz} multiplication implies.31) α and β may be written explicitly as ky sin α = p 2 kx + ky2 kx cos α = p 2 kx + ky2 q sin β = kx2 + ky2 cos β = kz . the inverse problem shall be analyzed. Further. r31 r32 r33 is speci ed. If the rotation matrix. By observing the matrix elements in Eq. summing the diagonal elements will give the following equality. These expressions are substituted into Eq. {xyz} R {x0 y0 kˆ} y .α · Ry. R may be computed from the post{x0 y0 kˆ} multiplication of the sub-rotations about the with the angle β and α. the trigonometric function of (2. vθ + 3cθ = r11 + r22 + r33 . 2. 90◦ Ry. determination of the axis of rotation from Eq. the angle of rotation is −1 θ = cos 2π 1 =± . There are two possible rotations for this method of orientation description to fail. Therefore the angle-axis representation for a particular rotation is not unique. Indeed. If θ=0 2π . 3 − 21 − 34 4 There are two possible direct rotations which yield the desired orientation. or the rotation about the axis 2 3 h √ √ iT − √13 −1 1−2 3 1+2 3 by the angle of −120◦ would work.34) It can be veri ed that the axis of rotation associated to each angle is the negative of each other. kˆ and −θ. r32 − r23 1 r13 − r31 . This corresponds to the physics that the axis of rotation can be chosen to point in any direction since there is really no rotation! Example 2.8 A rotation is generated by a rotation of z -axis. or where the rotation matrix becomes the identity matrix.5 Other Representation of the Orientation 42 There are two possible answers which are the negative of each other.−60◦ √ 3 1 − 0 − 2 2 √ √ = 23 − 43 √41 . kˆ = 2sθ r21 − r12 (2. Chulalongkorn University Phongsaen PITAKWATCHARA . followed ◦ rotation of −60 about ◦ 30 about the current y -axis. −kˆ .2.30◦ Rx.33. Therefore there are two possible rotations. Using Eq. there are two solutions: θ. and by the nal current x-axis. From the given orientation description. The axis of rotation kˆ can be retrieved by subtracting the appropriate o-diagonal elements. 2. − 2 3 These angles have the corresponding axes of kˆ = 1 2 sin ± 2π 3 −1 √ 1− 3 2√ 1+ 3 2 = ± √1 3 −1 √ 1− 3 2√ 1+ 3 2 . 2. the rotation matrix may be calculated as R = Rz. How such rotation be created by a single by a rotation of about the 90◦ Solution rotation.34 will fail. As a result. Either the rotation about the h √ √ iT 1− 1+ 3 3 1 ◦ axis √ −1 2 by the angle of 120 . in general. φ) of r11 r12 r13 R = r21 r22 r23 . and then the rotation is rotated about the by the rotation about the y -axis of the reference frame. −50 . and 100◦ respectively. Determine the A are the description of the free relationship between these two frames. the letter is rotated about the axis k write the letter at the new location. 6. −80 ) representation into the equivalent of XY X - xed the ZY X -Euler angles angles representation. A letter `A' is shown in Fig. −50◦ .2. Determine the resulting rotations are nal vector. Vector [3 4 − 2]T about the x-axis of the reference frame.9925]T vector p ¯ in {A} and {B}. −50 . and ends with the rotation about y -axis of the ◦ ◦ ◦ reference frame. p¯ = [3 4]T and B p¯ = [0. θ. 5. 2. The corresponding angles executed are 20 . 200 .2730 − 4. A rotation is constructed from the following sub-rotations. Determine the equivalent ZY Z - xed angles (ψ. The angles of these successive 30◦ . Find the equivalent single rotation about the proper axis. followed z -axis of the current frame. 4. the rotation about the x- axis of the reference frame. r31 r32 r33 the rotation matrix Discuss the representational singularity problem in this case. 3. Does the letter has the same size and shape? Chulalongkorn University Phongsaen PITAKWATCHARA . If ˆ = [−2 3 − 7]T by the angle of 190◦ .5 Other Representation of the Orientation 43 Problems 1. initially the rotation occurs about the rotation about the z -axis x-axis of the reference frame.23 along with the coordinates of its edges. then follows by the of the current frame. Transform ◦ ◦ the orientational ◦ (30 . 2. ◦ and −80 respectively. 24 showing a robot set up 1 m from the table. It is a xed with {x2 y2 z2 }.23: A letter `A'. Determine the transformations relating each of these frames to the base frame {x0 y0 z0 }. Determine the position of the box seen from the camera. Figure 2. Chulalongkorn University Phongsaen PITAKWATCHARA . located by stalled above the table corner opposite to {x1 y1 z1 } by 2 m. Consider Fig.2.24: A robot grasping an object.5 Other Representation of the Orientation 44 Figure 2. A camera. The table dimension is 1 m square of its top and 1 m in height. {x1 y1 z1 }. with which is assigned the frame placed at the center of the table. is {x3 y3 z3 }. by the transformation calculation and by direct observation. 7. is in- A small cube. 2. Chapter 3 Manipulator Kinematics Chulalongkorn University Phongsaen PITAKWATCHARA . the frames are attached to the linkages of the robot according to the Denavit-Hartenberg convention introduced in section 3. 8) Method of describing the position and the orientation by the homogeneous transformation matrix in chapter 2 will be applied to analyze the kinematics of the serial manipulator. leading to the relationship between the actuator parameters and the robot posture. As depicted in Fig. Also.2. 3.3.1 Link Description Serial robot or manipulator may be think of as the mechanism constructed from the links connecting together with the joints sequentially from the base to the end eector. pp. serial robot employs the open chain(s) structure. the joints used in the robot are the simple joints such as the revolute or the prismatic joint. especially its end eector. Normally. ([1]. in terms of the joint variables shall be studied: Chulalongkorn University Phongsaen PITAKWATCHARA . Consequently. Successive transformation of them can then be performed so that the posture of the end-eector frame may be determined in terms of the robot joint parameters through the evaluation of the robot transformation equation in section 3. Hence the robot posture is determined by the joint parameters.1. Speci cally. the robot can be moved by the actuators which are often installed at the robot joints.1: Conceptual schematic diagram of the open chain structured serial manipulator.3. The last section explains a practical problem of mapping between the joint and the actuator variables. in the following.1 Link Description 46 Figure 3. the relationship of the robot posture. 3. the geometric approach works well to the robots with simple structure only. ([1]. . As a preparation to the forward kinematics analysis. notations for describing the Chulalongkorn University Phongsaen PITAKWATCHARA . . 68) a basis to the robot motion. q2 . A set of equations then shall be set up and some manipulation B should be performed to deduce the elements of E T in terms of the joint variables. a variation of the DH convention as used in [4] will be adopted. The analysis may be carried out straightforwardly by observing the geometry of the robot at hand.1 Link Description 47 Figure 3. pp.2: Open chain structure of the serial manipulator. the forward kinematics problem is to determine the homogeB neous transformation matrix E T as a function of the joint variables (q1 . The procedure in setting up these frames follows what is known as the Denavit-Hartenberg (DH) convention. .3. This is known as the From the bird's eye view. In this lecture note. qn ). if {E} and {B} forward kinematics analysis. . Denavit and Hartenberg [7] recognized this problem in 1950's and proposed the formal method of the forward kinematics analysis by introducing the frames attached to the robot linkages. are a xed to the end eector and the base of the robot. Unfortunately. connects link the end eector. Phongsaen PITAKWATCHARA . it is ready to start According to the above enumeration scheme.. called joint #1. kinematic analysis of the robot can be performed recursively from the base to the end eector.3. 3. i. Also.3. # (i + 1). . Note that no matter how the robot moves. joint #i will join link analyzing the kinematics of the serial robot. as link #n where there exists the end eector. B As in subsection 2. 0 1 n−1 n =B 0 T 1 T (q1 ) 2 T (q2 ) · · · n T (qn ) E T. According to the serial robot structure with the simple joints and these enumeration schemes. The rst joint.4. In addition to the frame {x0 y0 z0 } xated to the base. {xi−1 yi−1 zi−1 } at any instant. re ecting the progressive direction of the analysis procedure from the base to the end eector.3 depicts the robot in Fig. the matrix E T which describes the posture of the end eector relative to the base may be determined by forming the compound transformation from the multiplication of the sub-transformations of the intermediate frames between {B} and B ET {E}. By the repeating structure of each joint connecting two linkages. This analysis is hence called the forward kinematics analysis. the joint variable will be the rotated angle of the linkage θi . totally there are n #1 to the base. sequen- joints connecting the linkages in the serial topology. actuation of joint #i causes the motion of link #i. by the serial structure of the robot.2 equipped with these auxiliary frames. usually denoted by {xn yn zn } attached to the last link assigned with the the end eector indicate its posture. The analysis starts by attaching the frame {xi yi zi } onto the link #i.1 Link Description 48 robot linkages and joints will be explained rst. Counting outward to joints. Figure 3. . In case of the prismatic joint. the posture of depends solely on the joint variable qi .2 depicts a schematic diagram of the open chain structured serial robot which mechanically consists of tially to the n + 1 links counting from the robot base. any point on the linkage will always be described by the xed coordinates in its moving frame. (3. in addition to the frame #n where the end eector is installed. it will be the displacement di . there might be a speci c base frame or the xed {B}. The equation presumes no relative motion between are a xed to the last link Chulalongkorn University {n} and {E}. Figure 3. With these notations.e. it is frame {E} = {xe ye ze } next to {xn yn zn } to reference frame. {xi yi zi } relative to If the joint is of revo- lute type. # (i − 1) to link #i. Linkages of Joints of n link #0. #n.1) both frames #n. . This is captured by the i−1 transformation i T (qi ). Similarly. 3: Serial manipulator and the associated frames for kinematical analysis. pp.3.1 Link Description 49 Figure 3. ([1]. 69) Chulalongkorn University Phongsaen PITAKWATCHARA . 3. In this course. ([1].2 Denavit-Hartenberg Convention 50 Figure 3. 70) 3. (n + 1)-linkages starting from the link #0 for the base. will connect link #i Chulalongkorn University to link #1 Joint #i which joins the link # (i − 1). They are The x-axis of a frame must be intersecting with the z -axis of the next frame. Enumerate the cessively 2. the number of indepedent variables used to describe the transformation between two successive frames is reduced from six to four only. it is quite common to adopt the Denavit-Hartenberg (DH) convention to construct and attach the frames in a systematic manner. In the following. Enumerate the n-joints starting from the joint #1 to the base. the steps of attaching the frames to the serial robot with the simple joints only according to the DH convention will be explained.2 Denavit-Hartenberg Convention In general. 1. Phongsaen PITAKWATCHARA . The x-axis of a frame must be perpendicular to the z -axis of the next frame. link frames may be a xed to the robot linkages arbitrarily.4: Frames and their parameters according to the DH convention. succounting to the link #n where the end eector is situated. successively counting toward the end eector joint. pp. notations for the DH convention used in [4] are adopted. As a result. In case the robot possesses the compound joint. Nevertheless. The DH convention has two constraints in formulating the frames. it will be decomposed as the serial connection of the simple joint rst. {E} at any convenient location on the end n should be the one that makes E T as simplest as possible. Lastly. {n} 9. Positive direction is selected such that it points toward the end eector joint. the xi -axis should be chosen such i−1 with the xi−1 -axis. It is the the mathematical twist angle between the joint axes of It is the angle measured from Joint displacement di Joint angle θi of the link is the distance measured from is the angle measured from Chulalongkorn University zi−1 xi−1 to to zi around xi−1 -axis. De ne the frame which moves along with link structed in a way that zi -axis #i. It should be chosen so that it coincides at the robot home position After nishing the frame installation to the robot. In case when the zi and zi+1 -axis intersect. the DH parameters shall then be determined. This would simplify i T . De ne the base frame nient location. {E} However. and that it intersects xn -axis can be chosen arbitrarily. if possible. 7.3. They are used to describe the relative posture between the successive frames. The xi -axis is oriented in the direction from the zi to 6. xi -axis is chosen to coincide with the common normal line between zi and zi+1 -axes. {i} 5. the DH parameters involved are Link length ai−1 is the mathematical length distance from zi−1 to zi along xi−1 -axis. as possible. Link twist αi−1 is the link # (i − 1). 4. 10. install {0} a xed to the base. The frame is con- coincides the joint axis #i. De ne the end eector frame eector. {1} is {E}. For example. To transform {i − 1} to {i}. In case when zi zi+1 -axis are parallel. 8. The origin of with it is the intersecting point of the xi and zi -axes. the xi -axis will be perpendicular to the plane formed by these two axes. {B} should be the one that makes 0 T as simplest 3. The axis is oriented in the direction where the rotation or the translation is de ned positive. Phongsaen PITAKWATCHARA . Direction of the should be such parallel to {i} 11. xi−1 to xi along zi -axis. The the zi+1 -axis. n n−1 that n T and E T be less complicated.2 Denavit-Hartenberg Convention 51 {B} at the base for the reference frame at any conveB However. # (i − 1). xi around zi -axis. However. Translate {i − 1} along the xi−1 -axis by ai−1 .ai−1 Rotxi−1 . which is the homogeneous transformation matrix describing the posture of i−1 i T {i} relative to {i − 1}. 1 0 0 1 This corresponds to the operator T rxi−1 . i T will be a function of ai−1 . αi−1 . With the notations adopted in this course. (3.di Rotzi . This corresponds to the operator 1 0 0 0 cαi−1 −sαi−1 = 0 sαi−1 cαi−1 0 0 0 0 0 . di .ai−1 1 0 = 0 0 2. 1. Accordingly. 0 1 0 0 0 ai−1 0 0 . the equivalent operator.di 1 0 = 0 0 0 1 0 0 0 0 .θi cθi −sθi sθi cθi = 0 0 0 0 0 0 1 0 0 0 .θi cθi −sθi 0 ai−1 cαi−1 sθi cαi−1 cθi −sαi−1 −di sαi−1 = sαi−1 sθi sαi−1 cθi cαi−1 di cαi−1 0 0 0 1 Chulalongkorn University .4 displays the frames attached to the linkages and their relevant DH i−1 parameters. {i} may be generated from {i − 1} in the following manner. responds to the operator T rzi . This cor- zi -axis by θi .3. 0 1 Because all the rotations and translations happen with respect to the current frame.2) Phongsaen PITAKWATCHARA . and θi .2 Denavit-Hartenberg Convention 52 Figure 3. di 1 0 0 1 0 4.αi−1 T rzi . 0 1 3. may be calculated by = T rxi−1 . Rotate the resulting frame around the (unchanged) This corresponds to the operator Rotzi .αi−1 by di . Translate the resulting frame along the (resulting) zi -axis Rotxi−1 . Rotate the resulting frame around the (unchanged) xi−1 -axis by αi−1 . 5. i. . Solution Following the guideline explained in section 3. the table would have (n + 2) rows. . . . . . table 3.3 Manipulator Kinematics In this section. transformation contains the parameters used to transform 3. the results are as follow. Note that most of the parameter's values are zero.1: Format of the table of DH parameters. . the forward kinematics analysis of two robots are performed. i 0 1 ai−1 ab a0 αi−1 αb α0 di db d1 . it is advisable to create the table of DH parameters to assist the systematic calculation of the transformation matrices. di . . θi . 1 B 0 0T = 0 0 Chulalongkorn University 0 1 0 0 0 0 1 0 0 0 h 1 c1 −s1 0 s 1 c1 1T = 0 0 0 0 0 0 1 0 0 0 0 1 Phongsaen PITAKWATCHARA .3. Row of the order #i {i − 1} to {i}. Totally. For general robot analysis. 3. . .5. i ai−1 αi−1 . . Joint variables will be donoted by the star-mark. Specifically.2 is the table of DH parameters for the robot. while the last row will be the ∗ ones from {n} to {E}. . . . 3. it will be chosen such that the frames' origin be coincident and the joint oset distance be zero.2 to determine each sub-transformation. . For the cases where there are options to de ne the frame.1. . the associated frames according to DH convention are attached to the robot as shown in Fig. n E an−1 ae αn−1 αe dn de θi θb θ1∗ . 3. . . . The rst example is simple while the second one is from the well known PUMA 560 industrial robot. θn∗ θe B This transformation matrix can then be used to determine E T in Eq. 3. depicted in Table 3. The rst row will be the transformational parameters from {B} to {0}.1. . Therefore the homogeneous transformation matrices will be simpli ed. d∗i . the table has 5 As columns indicating the order of the sub- #i and the DH parameters ai−1 . .1 Articulated Arm : Perform the forward kinematics analysis of the articulated robot depicted in Fig. . θi ∗ for the revolute and di for the prismatic joint. αi−1 . θi . .3 Manipulator Kinematics 53 Table 3. . . Applying Eq. Example 3.2.e. i 0 1 2 3 E Chulalongkorn University ai−1 0 0 0 l1 l2 αi−1 0 0 π/2 0 0 di h 0 0 0 0 θi 0 θ1∗ θ2∗ θ3∗ 0 Phongsaen PITAKWATCHARA .1.3 Manipulator Kinematics 54 Figure 3.5: Example 3.3. Table 3.1.2: Table of DH parameters for the articulated robot in example 3. 2 PUMA 560 Robot : Perform the forward kinematics analysis of the six degrees of freedom PUMA 560 robot depicted in Fig. 3.3: Table of DH parameters for the PUMA 560 robot in example 3. 77) Solution Schematic diagram of the robot geometry is illustrated in Fig. table 3. 3. Now.3. 3. B ET = B 0 1 2 3 0 T1 T2 T3 TE T c1 c23 −c1 s23 s1 c1 (l1 c2 + l2 c23 ) s1 c23 −s1 s23 −c1 s1 (l1 c2 + l2 c23 ) . pp. In this gure. 3.1.7 where the frames of the upper arm portion are attached according to the procedure outlined in section 3. it is straightforward to apply Eq. may be determined by Eq.6. = s23 c23 0 h + l1 s2 + l2 s23 0 0 0 1 Example 3.8. i 0 1 2 3 4 5 6 E ai−1 0 0 0 a2 a3 0 0 0 αi−1 di 0 H 0 0 −π/2 0 0 d3 −π/2 d4 π/2 0 −π/2 0 0 e c2 −s2 0 0 0 0 −1 0 1 2T = s2 c2 0 0 0 0 0 1 1 3 0 ET = 0 0 θi 0 θ1∗ θ2∗ θ3∗ θ4∗ θ5∗ θ6∗ 0 2 3T 0 1 0 0 0 0 1 0 c3 −s3 s 3 c3 = 0 0 0 0 l2 0 . ([4]. The frames of the forearm portion are depicted in Fig. l1 0 0 1 {E} with respect representing the forward kinematics of the robot.3 Manipulator Kinematics 55 Table 3.2. 0 1 0 0 1 0 Consequently. The base and the end eector frames are introduced for generalizing the result.2 to determine each of the link Chulalongkorn University Phongsaen PITAKWATCHARA . According to the selected frames. the robot is in the posture that makes all joint angles equal to zero.2. 3. the homogeneous transformation matrix of to {B}.3 contains the DH parameters of the robot. ([4]. 78) Chulalongkorn University Phongsaen PITAKWATCHARA .3.6: PUMA 560 by Unimation Incorporate. pp.3 Manipulator Kinematics 56 Figure 3. Figure 3.3.3 Manipulator Kinematics 57 Figure 3.8: Frame assignments for the forearm of the PUMA 560.7: Frame assignments for the upper arm of the PUMA 560. Chulalongkorn University Phongsaen PITAKWATCHARA . 3.3. 0 0 0 1 {E} with respect representing the forward kinematics of the robot.1. ET = 0 0 1 e 0 0 0 1 Consequently. B ET = = B 0 1 2 3 4 5 6 0 T1 T2 T3 T4 T5 T6 TE T B 3 4 3 T4 TE T r11 r12 r13 px r21 r22 r23 py = r31 r32 r33 pz . s5 c6 −s5 s6 c5 ec5 0 0 0 1 Phongsaen PITAKWATCHARA . the homogeneous transformation matrix of to {B}.3 Manipulator Kinematics 58 transformation: 1 B 0 0T = 0 0 0 1 0 0 0 0 0 0 1 H 0 1 c2 −s2 0 0 0 0 1 0 1 T = 2 −s2 −c2 0 0 0 0 0 1 c4 −s4 0 a3 0 0 1 d4 3 4T = −s4 −c4 0 0 0 0 0 1 c6 −s6 0 0 0 0 1 0 5 6T = −s6 −c6 0 0 0 0 0 1 c1 −s1 s 0 1 c1 1T = 0 0 0 0 0 0 1 0 c3 −s3 0 a2 s 3 c3 0 0 2 T = 3 0 0 1 d3 0 0 0 1 c5 −s5 0 0 0 0 −1 0 4 5T = s 5 c5 0 0 0 0 0 1 1 0 0 0 0 1 0 0 6 . 0 0 0 1 where 3 4T = c1 c23 −c1 s23 −s1 s B 1 c23 −s1 s23 c1 3T = −s23 −c23 0 0 0 0 c4 −s4 0 a3 0 0 1 d4 4 T = E −s4 −c4 0 0 0 0 0 1 Chulalongkorn University a2 c 1 c 2 − d 3 s 1 a2 s 1 c 2 + d 3 c 1 H − a2 s 2 1 c5 c6 −c5 s6 −s5 −es5 s6 c6 0 0 . may be determined by Eq. 3) joint space. pp. Joint Space.4 Actuator Space. Joint Space. Actuator Space.9: Mapping between three spaces. they are grouped as the joint vector : q¯ = [θ1 · · · θi dj · · · θn ]T . 94) and r11 r21 r31 r12 r22 r32 r13 r23 r33 px py pz 3. Usually.3. and Cartesian Space 59 Figure 3. except for the direct drive robot.4 = = = = = = = = = = = = c1 [c23 (c4 c5 c6 − s4 s6 ) − s23 s5 c6 ] + s1 (s4 c5 c6 + c4 s6 ) s1 [c23 (c4 c5 c6 − s4 s6 ) − s23 s5 c6 ] − c1 (s4 c5 c6 + c4 s6 ) −s23 (c4 c5 c6 − s4 s6 ) − c23 s5 c6 c1 [−c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 ] + s1 (c4 c6 − s4 c5 s6 ) s1 [−c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 ] − c1 (c4 c6 − s4 c5 s6 ) s23 (c4 c5 s6 + s4 c6 ) + c23 s5 s6 −c1 (c23 c4 s5 + s23 c5 ) − s1 s4 s5 −s1 (c23 c4 s5 + s23 c5 ) + c1 s4 s5 s23 c4 s5 − c23 c5 c1 (a2 c2 + a3 c23 − d4 s23 ) − d3 s1 − e [c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5 ] s1 (a2 c2 + a3 c23 − d4 s23 ) + d3 c1 − e [s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 ] H − a2 s2 − a3 s23 − d4 c23 − e (−s23 c4 s5 + c23 c5 ) . ([1]. and Cartesian Space Results from the forward kinematics analysis shows that the robot posture will be completely determined through the joint variables. In practice. there must have intermediate mech- Chulalongkorn University Phongsaen PITAKWATCHARA . the joint varaibles are not regulated by the actuator variables. The space of all possible joint vectors is called the (3. For convenience. g.5) ¯e = X ¯ e (¯ X q) . T = T (¯ q) (3. actuator variables may be written as the actuator vector Likewise. 3. and the space of all possible actuator vectors is called the (3.4 Actuator Space. the Cartesian coordinate system and the position vector.9. tasks for the robot will determine the robot end eector motion: position and orientation. the p¯ = [p1 · · · pi · · · pm ]T . In the previous and this chapter. which depends on the joint variables. Rotation in three dimensional space. Mathematically. e. and Cartesian Space 60 anisms which drive the robot joints from the actuator motion. Generally. X The space of all possible three dimensional positions and orientations is called the operational space. Additional mapping of the actuator space to the joint space q¯ = q¯ (¯ p) (3.7) is necessary to calculate the joint motion from the actuators where the encoders are assembled to and the low level feedback control happens. is more complicated since it is not the vector quantity. task space. Methods in chapter 2 may be used to describe the end eector posture. or sometimes Cartesian space. Position can be described readily via. Joint Space. Chulalongkorn University Phongsaen PITAKWATCHARA . 0 0 0 1 or the 6-tuples of the Cartesian position vector and the Euler angles or the xed angles for its orientation ¯ e = [px py pz ψ θ φ]T . are studied.. nevertheless. is depicted in Fig. they are viewed as the mapping from the joint space to the operational space. several methods of describing the posture of the robot end eector. both forward and backward.3. (3. Conceptual pic- ture of the mapping between these three spaces.6) or This mapping falls into the nonlinear conformal mapping and normally is so complex that it may not be written as a usual function explicitly. such as the homogeneous transformation matrix r11 r12 r13 px r21 r22 r23 py T = r31 r32 r33 pz .4) actuator space. Joint Space.11. Consider a 3R non-planar arm in Fig. Do the problem using the geometric approach and the formal frame setup approach under the Denavit-Hartenberg notation.10: Schematic diagram of a planar revolute-prismatic (RP) joint robot arm.10. Set up the necessary frames and derive its forward kinematics and workspace.4 Actuator Space. and Cartesian Space 61 Problems 1. Analyze the forward kinematics of the planar robot arm illustrated in Fig. Figure 3. 3. Analyze for its workspace as well.11: Schematic diagram of a 3R non-planar arm. 3. Chulalongkorn University Phongsaen PITAKWATCHARA . 2. Figure 3. Figure 3.12 depicts the kinematic diagram of a 3-DOF non-orthogonal wrist. 3.3. The rst and second joint type are of revolute and prismatic. respectively. Assign the necessary frames to the mechanism and determine its forward kinematics and workspace. 12: Schematic diagram of a 3-DOF non-orthogonal wrist. Chulalongkorn University Phongsaen PITAKWATCHARA . Figure 3. 4.3.13. Analyze its workspace as well.13: Schematic diagram of a SCARA robot arm. 3. Joint Space. Determine the forward kinematics of the SCARA robot as shown in Fig.4 Actuator Space. and Cartesian Space 62 Figure 3. Chapter 4 Inverse Manipulator Kinematics Chulalongkorn University Phongsaen PITAKWATCHARA . 2. the number of kinematically independent robot joints must not be less than the number of degrees of freedom (DOF) for specifying an Chulalongkorn University Phongsaen PITAKWATCHARA . However. For the serial manipulator. the inverse kinematics analyzes for the function of joint variables in terms of the given robot posture.1 Solvability The essence of the inverse kinematics problem is to solve for the joint angles q¯ = [θ1 · · · θi dj · · · θn ]T provided the robot posture is given.1) Physically. two methods of the algebraic and the geometric approaches in solving the problem will be discussed in section 4. Particularly. Indeed only three independent equations may be formulated for a particular orientation. (4. 4.1. If this is speci ed by the homogeneous B transformation matrix E T . Combined with the position vector. there is no book-keeping procedure for the inverse kinematics analysis in general. the inverse kinematics analysis may be viewed as the problem of solving a set of the following nonlinear equations: r11 = r11 (¯ q) r21 = r21 (¯ q) r31 = r31 (¯ q) r12 = r12 (¯ q) r22 = r22 (¯ q) r32 = r32 (¯ q) r13 = r13 (¯ q) r23 = r23 (¯ q) r33 = r33 (¯ q) px = px (¯ q) py = py (¯ q) pz = pz (¯ q) . twelve equations can be formulated. Hence. unfortunately.1 Existence of Solution The rst question before starting to look for the corresponding joint angles is that does such solution really exist. Inverse kinematics of the manipulators in chapter 3 will nally be considered. unfortunately due to the nonlinearity of the trigonometric functions which are not bijective. the inverse problem is more di cult.1. the issue of problem solvability will be addressed rst. 4.1 Solvability 64 Manipulator inverse kinematics problem is the converse problem of the forward kinematics in the previous chapter. it is known that those nine elements for the rotation matrix are dependent. In section 4. Therefore solving the inverse kinematics is a much more di cult problem and it typically does not have a book-keeping procedure to follow.4. Then. there will be totally merely six equations that may be used to solve for the joint angles. This is called the existence of the solution. For the solution q¯ to always exist. The necessary and su cient condition of the existence of the solution is the speci ed posture must physically be a posture in the robot workspace. These equations heavily involve with the trigonometric functions which are nonlinear and not bijective. it should be designed in a manner that there are minimally six independent joints available in the workspace. 4. Fig. there will be in nitely many robot postures according to a bunch of possible orientation of the end eector by the third joint angle rotation. there will be two robot postures which satis es the requirement. 115) arbitrary position and orientation in the robot workspace. For the robot to perform general task in three dimensional space. 4. If the robot end eector position and orientation are speci ed.2. the existence of solution depends on the joint limits and the obstacles as well.2. Consider another example of a planar three DOF robot in Fig. it is thus necessary to check the inverse kinematics solution against these issues before the execution. a chance that there are multiple solutions is possible because the trigonometric functions are not the injective function. This is in agreement with two solutions from the algebraic approach that will be explained in the next section.1 depicts a two DOF planar robot. See the left gure of Fig. ([1]. Its workspace is de ned to be the set of all reachable end-eector positions.2 Multiple Solutions If there exists joint angles q¯ for a particular robot posture.1. The number of solutions for a speci c posture depends on the number of Chulalongkorn University Phongsaen PITAKWATCHARA .1: The desired end eector position P¯ can be reached in a multiple ways. Solution of the joint angles from the inverse problem of T (¯ q) may not be realizable since it is out of the joint range or it causes the collision of the robot arm and the obstacle. As an example. However. pp. The gure illustrates two robot postures which correspond to the speci ed end eector position. then for a given robot end eector position.4. 4. 4. the robot should possess at any instant at least three independent joints. Moreover. if the end eector orientation causes no eect to the robot operation as if the workspace is the set of merely the end eector positions. If task is con ned to be in a particular plane. In pracetice.1 Solvability 65 Figure 4. The workspace is de ned as the set of all end eector positions and orientations. there might have no solution for a certain postures. This will be shown in analyzing the inverse kinematics of the PUMA 560. On the other hand. ([1]. Worse yet. the more the nite number of solutions will be. Unfortunately in many q¯ must be written using the implicit functions or with Chulalongkorn University Phongsaen PITAKWATCHARA cases. pp. 4. there will be in nitely many inverse kinematics solutions. Moreover. there are as many as 16 solutions of q¯. Generally. 116) independent robot joints.2: Inverse kinematics solution of a three DOF planar robot when the position and orientation (left) or only the position (right) of the end eector are speci ed. the number of solutions depends on the kinematical structure of the robot as well. it is often used in the simulation of the complex robot systems that are not possible to determine the closed form inverse kinematics solution. the more the nite number of solutions will be.2 Algebraic vs.4. the more the number of nonzero link length ai or joint displacement di . Geometric Approaches 66 Figure 4. Hence the time spent cannot be predicted. the more the number of the joint twist αi or the joint ◦ ◦ angle θi which are not 0 or 90 . the method basically relies on recursive evaluation. Consequently. Solution of the joint vector q¯ will be called the closed form solution if it can be written as the explicit function of the given parameters describing the robot posture. This method. Practically. Moreover. Geometric Approaches There is no general way to solve the arbitrary nonlinear algebraic equations except the numerical method. the expression of . has a drawback of providing a single solution which might not be the required one. nevertheless. If the latter is greater than the number of required DOF for the task space.2 Algebraic vs. there is no guarantee whether the algorithm will converge to a solution. if it is less than the number of required DOF. Similarly. the numerical method for solving the inverse kinematics is rarely used in real time control problem. For a six DOF revolute joint robot. the robot is often designed with a simple structure that possesses the closed form inverse kinematics solution. However. there are only six independent equations which implies no more than six unknowns (possibly may not be the joint variables) are constrained. arbitrary posture of the robot end eector may be expressed by the general homogeneous transformation matrix: r11 r12 r13 px r21 r22 r23 py H= r31 r32 r33 pz . after the appropriate set of equations are formed. Nevertheless. Twelve algebraic equations can be formed. twelve equations may be formulated. θ2 . the frames are set up and the homogeneous transformation matrix between the end eector and the base frame. Three DOF Planar Robot : Example 4. ([1].1 Algebraic Approach This approach compares the speci ed position and orientation with the robot forward kinematics expressions and solves for q¯. It may further be managed to write such variable as the explicit function of the speci ed posture. the next step is to solve this system of nonlinear equations. 0 0 0 1 Corresponding joint angles θ1 . If the homogeneous transforma- tion representation is employed. the forward kinematics must be analyzed rst. Geometric Approaches 67 the composite functions to reduce the complexity of the expression.1) Solution As a prerequisite.2. They will bring about the conditions on the parameters. and θ3 may be determined from the fact that 0 they equate E T to the speci ed H . 4.3. 4. Commonly.4. the equations are manipulated to eliminate several scalar variables so that only single variable is left in the equation. especially the trigonometric identities.2 Algebraic vs. Prob. Any technique or methodology. Issues pertaining to the existence of and mutiple solutions need to be addressed. as mentioned in section 4. However. Use the algebraic approach. 4. 1 0 0 1 Also. {E} and {0}. There are two main approaches in solving the inverse kinematics: the algebraic and the geometric approaches. Following the DH convention studied in chapter 3. Chulalongkorn University Phongsaen PITAKWATCHARA . may be employed to solve the equations for the joint variables.1 Perform the inverse kinematics analy- sis of a three DOF planar robot as shown in Fig. may be determined as c123 −s123 0 s123 c123 ET = 0 0 0 0 0 l1 c1 + l2 c12 + l3 c123 0 l1 s1 + l2 s12 + l3 s123 .1. ([1]. Geometric Approaches 68 Figure 4.2 Algebraic vs. The remaining equations are related to the joint variables. 75) Among them. does not obey to these requirements. in the expression of px and py . the explicit conditions are r13 = 0 r23 = 0 Therefore. They are r11 r12 r21 r22 px py = = = = = = c123 −s123 s123 c123 l1 c1 + l2 c12 + l3 c123 l1 s1 + l2 s12 + l3 s123 . if the given H r31 = 0 r32 = 0 r33 = 1 pz = 0. in turn. Also Replace (1. 1) and (2. 2). It may then be further concluded that the element equal.4. 2) of they may be rewritten as px − l3 r11 = l1 c1 + l2 c12 py − l3 r21 = l1 s1 + l2 s12 . 1) must be the negative of the element (1.1. c123 and s123 by r11 and r21 . Chulalongkorn University Phongsaen PITAKWATCHARA . H must be the element (2. the speci ed posture is out of the robot workspace and hence there is simply no solution. pp.3: Example 4. After some manipulation. 1]. Hence there are two possible solutions of θ2 and they are θ2 = atan2 (s2 . l1 + l2 c2 ) . θ1 = atan2 (y.2 Algebraic vs. for each value of θ2 back into θ2 . If the speci ed distance is out of the range. The equation implies the speci ed posture of the end eector must induce the distance from the rst to the third joint that is no less than the dierence of the length of the rst two links. In conclusion.4. Let us introduce x = px − l3 r11 y = py − l3 r21 . 2l1 l2 Since range of the cosine function is the closed interval [−1. θ1 may thus be determined by substituting the already known the equations of px and py . c2 ) . the condition for the existence of the solution is (l1 − l2 )2 ≤ x2 + y 2 ≤ (l1 + l2 )2 . r11 ) − θ1 − θ2 . there are two sets of the joint angles of the three DOF planar robot corresponding to a speci ed reachable posture. the sine of θ2 will be q s2 = ± 1 − c22 . the corresponding θ3 may be determined simply from θ3 = atan2 (r21 . x) − atan2 (l2 s2 . for each set of θ1 and θ2 . for conciseness of the development. such distance must not be greater than the sum of the length of the rst two links as well. Chulalongkorn University Phongsaen PITAKWATCHARA . If this condition is satis ed. Furthermore. x = (l1 + l2 c2 ) c1 − l2 s2 s1 y = l2 s2 c1 + (l1 + l2 c2 ) s1 . θ1 may be solved. That is. Lastly. the cosine of c2 = θ2 is simply x2 + y 2 − l12 − l22 . Geometric Approaches 69 Squaring each equation and summing them together lead to (px − l3 r11 )2 + (py − l3 r21 )2 = l12 + l22 + 2l1 l2 c2 . it simply cannot be reached. Immediately. This equation has only one unknown of θ2 which is inside the cosine function. the relationship is derived by noticing the sum of the vectors alongside of a polygon must yield a null vector. it might be useful to project the robot plane rst. ([1].2 Geometric Approach The geometric approach. 4. however.2 Algebraic vs. From the gure. 121) 4. 4. That is −−→ −−→ −−→ OB = OE − BE.4. The polygon may not be con ned to lie on a plane. ([1]. will develop the kinematical equations from the structural geometric relationship of the robot. Usually. if a straight line connecting the rst and the third joint is drawn. a geometric relationship can be determined from the triangle OAB .4. Particularly.4.2 Three DOF Planar Robot : Perform the inverse kinematics anal- ysis of a three DOF planar robot as shown in Fig.2. on the contrary.4: Example 4.4 displays the geometry of the robot pertinent to the following analysis. the vector equation may be represented in Chulalongkorn University {XY } Phongsaen PITAKWATCHARA .2.2) Solution Figure 4. From the geometry in Fig. Note that only the joint variable θ2 is involved and thus the equation may be used to determine its value directly. Geometric Approaches 70 Figure 4. Geometrical analysis of the projected polygon in the plane will then be performed. One may also set up the equations by projecting the polygon onto a plane which is orthogonal to the joint axis that de nes the unknown joint variable. if the joint variable links onto the xi -yi θi is to be solved for. Prob. −−→ OB which represents a side of the triangle OAB may be determined by sub- tracting the vector associated with the third link from the position vector of the end eector. 4. Now use the geometric approach instead. pp. Example 4. 2). 1].2 Algebraic vs. θ2 is solvable if and only if the distance from the rst to the third joint determined from the speci ed end eector posture must not be longer than (l1 + l2 ). If the above condition is satis ed. Since the rotation matrix of the end eector frame must represent the rotation in the XY plane in accordance with the robot geometry. Law of cosine may be applied to the triangle in determining θ2 OAB for setting up the relation as x2 + y 2 = l12 + l22 − 2l1 l2 cos (π − θ2 ) = l12 + l22 + 2l1 l2 c2 . which would be equal when the robot fully folds its arm back. Additionally. must be equal to the cosine. Geometric Approaches as −−→ OB = px py 71 − l3 c123 l3 s123 . x and y x y = px − l3 r11 py − l3 r21 . (1. Because the −−→ θ1 + θ2 + θ3 .4. numerical value of the right hand side term must lie in the closed interval [−1. and cosine of the rotated angle of the end eector. OB may be expressed in terms of the given rotated angle is obviously position and orientation as −−→ OB = where. sine. Physical interpretation of this inequality may be understood from the underlying geometry in Fig. Hence c2 = x2 + y 2 − l12 − l22 . elements r31 = 0 r32 = 0 (1. the distance must not be shorter than |l1 − l2 |. represent the coordinates of the third joint in the base frame.4. 1). the elements of the speci ed homogeneous transformation matrix must conform to the conditions: r13 = 0 r23 = 0 Moreover. 2) r33 = 1 pz = 0. In the same manner. minus sine. Phongsaen PITAKWATCHARA . which would be equal when the robot fully stretches out its arm so the rst and the second links line up. (2. physically. 1). (l1 − l2 )2 ≤ x2 + y 2 ≤ (l1 + l2 )2 . 4. the cosine function θ2 = acos Chulalongkorn University θ2 may be determined from the inverse of x2 + y 2 − l12 − l22 2l1 l2 . and (2. leading to the condition for the existence of the solution. 2l1 l2 which corresponds to the result using the algebraic approach. π] corresponding to the is for the case when the rst and the second link are in the posture shown by the dotted lines in Fig. from Fig. θ2 < 0 Lastly. they yield the same angle as can be viewed from dierent triangles. From Fig. θ1 = α − β. r11 ) − θ1 − θ2 .4. θ2 ≥ 0 α + β.4. the second link the lower arm. is limited to the interval Another solution of θ2 [0.4. Chulalongkorn University Phongsaen PITAKWATCHARA . However it will be the sum of α and β associated with θ20 for 0 0 mirrored triangle OA B . two solutions of the second joint angles will make the arm be in the the elbow up θ1 pose for θ elbow down pose for θ2 and 20 . This is the continuation of their forward kinematics analysis. angle α is readily determined as will be the dierence of the angles α and the the α = atan2 (y. That is. β = acos for which the value is limited to x2 + y 2 + l12 − l22 p 2l1 x2 + y 2 [0.3 Examples 72 θ2 for which the value of geometry of the triangle. 4. 4. 4. Angle β is an angle of the triangle OAB and hence may be determined from the cosine law. π] ! by the physical geometry of the triangle.4. Referring to Fig. β associated with θ2 for triangle OAB . Hence. 4. Therefore the second solution may be readily determined since the triangle 0 0 and OA B are the mirror images of each other about the line OB : OAB θ20 = −θ2 .3 Examples This section provides more examples of analyzing the inverse kinematics of two robots in the previous chapter. 4. θ3 may thus be determined as θ3 = φ − θ1 − θ2 = atan2 (r21 . if the rst link is thought of as a human upper arm. Note that although the expression of θ1 obtained from the geometric and the algebraic approach are dierent. φ and from the constraint of the workspace onto the valid homogeneous transformation matrix as explained above. and the second joint as the elbow joint. since the angle the end eector oriented in plane is equal to θ1 + θ2 + θ3 .4. x) . 3) Solution If the rst joint does not exist. it can be concluded that θ1 = atan2 (py . Phongsaen PITAKWATCHARA . px ) + π. it cannot achieve arbitrary speci ed posture in three dimensions. px ) . ([1]. as the arm can then ip back to reach P as well. To apply the inverse kinematics analysis of the planar arm in this problem.5: Projection of the articulated arm onto x 1 -y 1 plane and the related parameters. Since the robot has just three DOF. one may specify only the desired end eector position P¯ = [px py pz ]T represented in the base frame. 125) Example 4. 4. ([1]. 4. pp. Thus one may apply the analysis result of the three DOF planar robot by setting the length of the third link to zero. the second and the third link images will be a single line T T 0 0 px p y origin to the projected end eector point which is parallel to connecting the as depicted in Fig. Prob. Therefore. 3. rstly the end eector position must be expressed in and lower arm lie in θ1 = atan2 (py . Moreover. {x1 z1 } Chulalongkorn University P¯ = p T p2x + p2y pz − h . 4.5. when the arm faces its back to P.6 for the case when is chosen. θ1 = atan2 (py . Hence. If the robot is projected onto the x 1 -y 1 plane xb -yb . px ) x1 -z1 {x1 y1 z1 }.3 Examples 73 Figure 4. Since the upper plane as illustrated in Fig.5.3 Articulated Arm : Perform the inverse kinematics analysis of the articulated robot illustrated in Fig. the robot arm is degenerated to merely a two DOF planar robot.4. p2x + p2y − atan2 (l2 s3 . this must be equal to l1 c2 + l2 c23 l1 s2 + l2 s23 T . l1 + l2 c3 ) Chulalongkorn University Phongsaen PITAKWATCHARA . px ) + π is chosen instead. P = − p2x + p2y pz − h If in Hence the corresponding c3 s3 θ2 and θ3 will be p2x + p2y + p2z + h2 − 2hpz − l12 − l22 = 2l1 l2 q = ± 1 − c23 θ3 = atan2 (s3 . inverse kinematics result of the previous example may now be applied as follow.3 Examples 74 Figure 4. c3 ) q θ2 = atan2 pz − h.4. 125) Geometrically.6. the end eector position expressed {x1 z1 } will be p T {x1 z1 } ¯ . pp. c3 s3 p2x + p2y + p2z + h2 − 2hpz − l12 − l22 = 2l1 l2 q = ± 1 − c23 θ3 = atan2 (s3 .6: Projection of the articulated arm onto x1 -z1 plane. l1 + l2 c3 ) There are two solutions for θ2 and θ3 according to the elbow up or elbow down con guration. ([1]. θ1 = atan2 (py . − p2x + p2y − atan2 (l2 s3 . c3 ) q θ2 = atan2 pz − h. 4. as observed in Fig. Consequently. which degenerates the three dimensional robot to a planar robot. Chulalongkorn University θ1 may be solved Phongsaen PITAKWATCHARA . From example 3. 3. the robot will face towards the position and the arm be either in the elbow up or elbow down. Solution Since the robot has full six DOF. however. For the other two solutions.2.6.3 Examples 75 In summary.4 PUMA 560 Robot : Perform the inverse kinematics analysis of the six DOF PUMA 560 robot illustrated in Fig. If r11 r12 r13 px r21 r22 r23 py B ET = r31 r32 r33 pz 0 0 0 1 is given. the inverse kinematics problem is to determine the solution of the robot six joint angles.4. both the desired position and orientation of the end eector may be speci ed arbitrarily. the arm postures are not changed. θ1 to θ6 . mathematically the articulated arm yields four solutions. The arm posture will be in a manner that. for the rst two solutions. r33 pz − H − er33 0 1 px 1 0 py 0 1 pz 0 0 1 0 0 0 0 0 0 1 −e 0 1 One may need to look for simple relations that contain few unknowns at at time so it may be capable of solving for the analytical solutions. B ET 0 6 =B 0 T6 TE T. 0 6T = −1 B 6 −1 B 0T ET ET 1 0 0 0 1 0 = 0 0 1 0 0 0 r11 r12 r21 r22 = r31 r32 0 0 0 r11 r12 r13 0 r21 r22 r23 −H r31 r32 r33 1 0 0 0 r13 px − er13 r23 py − er23 . for a speci ed end eector position. Example 4. Looking at the nal posture. the robot will turn its back to the position and the second joint angle will be rotated greater than 90◦ so the arm still can reach the position which is located at its back. Thus. 4) and (3. the equality of (1. The equation may be rewritten so the left hand side appears Chulalongkorn University Phongsaen PITAKWATCHARA . (py − er23 ) c1 − (px − er13 ) s1 = d3 . Sum those three together and rearrange the equation into the following form 2 = 2 2 (px − er13 ) + (py − er23 ) + (pz − H − er33 ) − 2a2 Hence two solutions of θ3 a22 − a23 − a3 c 3 − d 4 s 3 − d24 = K. 4) element on the right hand side is the least complex one. r12 r13 px − er13 r22 r23 py − er23 r32 r33 pz − H − er33 0 0 1 a3 c23 − d4 s23 + a2 c2 d3 . d3 . 4) elements on both sides require that a3 c23 − d4 s23 + a2 c2 = (px − er13 ) c1 + (py − er23 ) s1 −d4 c23 − a3 s23 − a2 s2 = pz − H − er33 . q 2 2 θ3 = atan2 (−d4 . There are two possible solutions of θ1 . i. py − er23 ) q 2 2 p2x + p2y − d23 + e2 (r13 + r23 ) − 2e (px r13 + py r23 ). ±atan2 Further.3 Examples 76 rst by rewriting the above statement as −1 0 0 T 1 6T c1 s 1 −s1 c1 0 0 0 0 0 0 1 0 0 r11 0 r21 0 r31 1 0 ∗ ∗ ∗ ∗ ∗ ∗ = ∗ ∗ ∗ 0 0 0 = 1 6 T. −d4 c23 − a3 s23 − a2 s2 1 Multiplying the subtransformation matrices in example 3.2. where now θ1 is treated as the known variable. which may be expressed as θ1 = atan2 (− (px − er13 ) . d23 may be solved in a similar manner as of θ1 .e. Consider the governing 0 equation of 6 T again. d3 ) ± atan2 a3 + d 4 − K 2 . the (2. which fortunately is independent of other unknowns. Square the above two equations and the rst one as well.4. The next unknown that should be looked for is θ2 . K . The same technique may be applied here as follow.2. The remaining unknowns of θ4 . 3 Assume the value of the (2. 3) element of 6 T is neither one or minus one. It is seen that the value of θ23 depends on θ1 and θ3 . π . [(px − er13 ) c1 + (py − er23 ) s1 ]2 + (pz − H − er33 )2 s23 = c23 Thus. 4) elements since they contain as unknown. c5 = − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) q s5 = ± 1 − (r13 c1 s23 + r23 s1 s23 + r33 c23 )2 . (a2 c3 + a3 ) [(px − er13 ) c1 + (py − er23 ) s1 ] + (a2 s3 − d4 ) (pz − H − er33 )) .1 and 2. 4) = 36 T. −1 0 0 3T 6T c1 c23 s1 c23 −s23 −a2 c3 −c1 s23 −s1 s23 −c23 a2 s3 −s1 c1 0 −d3 0 0 0 1 c4 c5 c6 − s 4 s 6 s 5 c6 = −s4 c5 c6 − c4 s6 0 Setting up the equations from the only θ2 (1. θ5 . Inverse kinematics problem of this kind has already been addressed in subsection 2.4. each of which has two solutions. s 4 c5 s 6 − c4 c6 s4 s5 0 0 0 1 and (2. Chulalongkorn University Phongsaen PITAKWATCHARA . θ23 = atan2 (− (a2 c3 + a3 ) (pz − H − er33 ) + (a2 s3 − d4 ) [(px − er13 ) c1 + (py − er23 ) s1 ] .3 Examples as a function of 77 θ2 . r11 r12 r13 px − er13 r21 r22 r23 py − er23 r31 r32 r33 pz − H − er33 0 0 0 1 −c4 c5 s6 − s4 c6 −c4 s5 a3 −s5 s6 c5 d4 . and θ6 may now be solved straightfor3 wardly if one recognize that the rotation matrix in 6 T is of the same form as the corresponding rotation matrices of the Euler angles or the xed angles descriptions.5. Hence there are four possible combinations and so four corresponding solutions of θ2 = θ23 − θ3 . The equations may be solved simultaneously for s23 and c23 as − (a2 c3 + a3 ) (pz − H − er33 ) + (a2 s3 − d4 ) [(px − er13 ) c1 + (py − er23 ) s1 ] [(px − er13 ) c1 + (py − er23 ) s1 ]2 + (pz − H − er33 )2 (a2 c3 + a3 ) [(px − er13 ) c1 + (py − er23 ) s1 ] + (a2 s3 − d4 ) (pz − H − er33 ) = .5. One then can set up the following relations. That is c5 6= ±1 or θ5 6= 0. (px − er13 ) c1 c23 + (py − er23 ) s1 c23 − (pz − H − er33 ) s23 − a2 c3 = a3 − (px − er13 ) c1 s23 − (py − er23 ) s1 s23 − (pz − H − er33 ) c23 + a2 s3 = d4 . In particular. θ4 = atan2 (r13 s1 − r23 c1 . There are two solutions for the latter three joints for the wrist to ip up/down. if s5 = − 1 − (r13 c1 s23 + r23 s1 s23 + r33 c23 )2 is chosen. the solution will stay in the same branch. and θ3 .2) of the roll-pitchroll wrist. The rst three joints have four choices corresponding to the left/right arm and elbow up/down solutions as illustrated in Fig. − (r11 c1 s23 + r21 s1 s23 + r31 c23 )) . which will happen when the wrist outstretches so the joint axes of and θ6 θ4 line up. q Instead. In other words. θ2 . The solution to choose for will. r11 c1 s23 + r21 s1 s23 + r31 c23 ) .1 and 2. when − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) = −1. In summary.5. some of them may not be possible because they will make the arm jammed with the environment. However. if of θ5 . θ5 = 0. Only their sum would know.f. θ5 = 0 θ4 − θ6 = atan2 (−r11 s1 + r21 c1 . the dierence of the fourth and the sixth joint angles yields the end eector rolling angle. r12 s1 − r22 c1 ) . 4.4. one would never know what are exactly the contributing angles from the fourth and the sixth joints to the resultant rolling angle of the end eector. be the one of which the joint angles are closest to the present values. r12 s1 − r22 c1 ) . r13 c1 c23 + r23 s1 c23 − r33 s23 ) θ6 = atan2 (− (r12 c1 s23 + r22 s1 s23 + r32 c23 ) . Specif- θ4 = atan2 (−r13 s1 + r23 c1 . − (r13 c1 c23 + r23 s1 c23 − r33 s23 )) θ6 = atan2 (r12 c1 s23 + r22 s1 s23 + r32 c23 . there are two possible solutions θ5 : q 2 θ5 = atan2 ± 1 − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) . This choice results in smooth motion of the robot since it will not run into the singular con guration unnecessarily. When possible the representational con guration in subsection 2. Similarly. However. − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) . if the robot is not in one of its singular con guration. In this con guration.5. then The remaining variables of ically. θ5 = 0 θ4 + θ6 = atan2 (r11 s1 − r21 c1 . For this con guration. θ4 and θ6 depends on the selected value q s5 = + 1 − (r13 c1 s23 + r23 s1 s23 + r33 c23 )2 is chosen. In other words. − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) = 1. This is the `physical' singular con guration (c. there will be eight solutions of the joint angles which can achieve the speci ed end eector posture. θ5 = π .3 Examples 78 According to the selected values of of θ1 . This is another physical singular con guration of the wrist corresponding to the time when the wrist fully folds back. Mathematically.7. it will not be to solve for θ4 and θ6 . in general. Chulalongkorn University Phongsaen PITAKWATCHARA . 7: Four solutions for the rst three joints of PUMA 560. pp. ([4].3 Examples 79 Figure 4. 118) Chulalongkorn University Phongsaen PITAKWATCHARA .4. 8: Schematic diagram of a 2-DOF revolute joint planar arm. 2 of Chapter 3. 3 of Chapter 3 from its forward kinematics result. Perform the inverse kinematics analysis of a simple 2-DOF planar robot in Fig. Determine the inverse kinematics of the SCARA arm depicted in Prob. 2. Chulalongkorn University Phongsaen PITAKWATCHARA . Determine the inverse kinematics of the 3-DOF non-orthogonal wrist in Prob. 4 of Chapter 3 by your convenient method. Determine the inverse kinematics of the robot arm in Prob.4. 3. Problems 1. 4.3 Examples 80 Figure 4.8 by both the algebraic and geometric approach. 4. Chapter 5 Jacobians Chulalongkorn University Phongsaen PITAKWATCHARA . ˆj . as discussed in section 5. with the angular velocity to ω ¯. the time rate of change of these vectors are due solely to their changes in direction. methods are available to develop these quantities. 5. From the gure. dt Substitute these relations into the above relative velocity equation. It may be compactly expressed by the geometric Jacobian matrix. depicted in Fig. After some manipulation. 5. dˆ i=ω ¯ × ˆi dt dˆ j=ω ¯ × ˆj dt dˆ ˆ k=ω ¯ × k. the velocity of A may be written as v¯A = v¯B + ω ¯ × p¯A/B + v¯rel . dˆ dˆ dˆ ˆ ˆ ˆ x i + y j + z k + x˙ i + y˙ j + z˙ k .1.1. which has its origin at B and is rotating used to describe the position vector of A relative space. Since p¯A/B describes the motion of the relative velocity of A with respect to d v¯A − v¯B = p¯A/B = dt Because ˆi.2. dt dt dt are unit vectors.1 Velocity Propagation Velocity kinematics analysis of the robot is the determination of the linear and angular velocity at various points of the end eector or the linkages. for this introductory course. Several In this course.1. Motion of travels with B. Since Jacobian matrix is an important matrix in robot analysis. The result is a linear mapping from the joint velocity space to the end eector spatial velocity space. The velocity propagation approach will be studied to derive the velocity kinematics in section 5.3 and 5. Indeed. Chulalongkorn University (5.1) B. for which it can be constructed on-the- y.1 Velocity Propagation 82 This chapter analyzes the robot kinematics at the velocity level to understand how the robot will be moved by the motion of its joints. is B: ˆ p¯A/B = xˆi + yˆj + z k. two usages of it for the singularity analysis and the static force analysis will be studied in section 5. 5.1 Relative Linear Velocity of Any Two Points Consider any two points A and B moving independently in three dimensional A may be observed from the observer who {xyz}.2) Phongsaen PITAKWATCHARA . the method of velocity propagation taught in the introductory to dynamics course will be adopted. and kˆ A relative to (5.4.5. its time dierentiation is B. 173) is the velocity of A A seen by the observer. it may be said that the angular velocity of angular velocity of D C may be determined from the once their relative velocity is known: ω ¯C = ω ¯D + ω ¯ C/D . Let ω ¯C and ω ¯D be the angular velocity of two objects C and D. the relative velocity formula developed earlier will be applied to determine the velocity of the robot. 5. the dierence of the angular velocity of the relative angular velocity of C with respect to D. called may be calculated simply by ω ¯ C/D = ω ¯C − ω ¯D . C and D.1.3 (5. Thus. pp.5. ([1].1: Relevant frames and position vectors for the relative motion analysis of arbitrary two points where A v¯rel = x˙ ˆi + y˙ ˆj + z˙ kˆ says that the velocity of and B.1. 5. Since the angular velocity is a free vector. Chulalongkorn University Consider link#(i − 1) and link#i for Phongsaen PITAKWATCHARA .1 Velocity Propagation 83 Figure 5. This equation may be determined from the velocity of B once their relative velocity is known.3) Velocity of the Robot In this subsection.2 Relative Angular Velocity of the Objects Angular velocity of an object describes the time rate of change of its rotation. 5. {i} relative to Consequently. However. v¯i = v¯i−1 + ω ¯ i−1 × p¯i/i−1 + d˙i kˆi . the angular velocity of of {i − 1} {i} may be (5. from Eq. {i} with respect Substituting such value into Eq. ([1].8) (5. v¯i = v¯i−1 + ω ¯ i−1 × p¯i/i−1 + v¯rel . the linear velocity of and angular velocity of {i} may be determined from the linear {i − 1}. . pp. {i} and Substituting this value into Eq.5. 5.5) If joint#i is the prismatic joint.2. . 5.4. As a result.4) determined from the angular velocity as ω ¯i = ω ¯ i−1 + ω ¯ i/i−1 .2. n] of a serial link manipulator. Chulalongkorn University (5. Frames {i − 1} and {i} established after DH convention and the pertaining position vectors are depicted in Fig. Applying this Instead. it will cause the rotation of to {i − 1} by the relative angular velocity ω ¯ i/i−1 = θ˙i kˆi . (5. the prismatic actuation causes the motion of the origin of to the origin of {i − 1} by the relative linear velocity value to Eq. the rotating actuation causes no motion of the origin of the origin of {i − 1}. ω ¯i = ω ¯ i−1 . .2: Link frames and the relevant position vectors. Recalling Eq. 174) i ∈ [1. . 2.6) {i} with respect v¯rel = d˙i kˆi . In dual. there will be no relative rotation of {i − 1}. v¯rel = ¯0. 5.4. ω ¯i = ω ¯ i−1 + θ˙i kˆi . (5.5. ω ¯ i/i−1 = ¯0.1 Velocity Propagation 84 Figure 5.5. 5. (5.9) Phongsaen PITAKWATCHARA . Hence.7) If joint#i is the revolute joint. v¯i = v¯i−1 + ω ¯ i−1 × p¯i/i−1 . 5. (5.8.10 using the recursive substitution of Eqs. 5. Linear velocity of the end eector may be determined in the same vein.6.8. ([1].10) be the set of the revolute and prismatic joint enumerations. 5. 176) 5.3: In uence of the prismatic joint actutation onto the end eector linear velocity. With the recursive substitution of Eqs. respectively. 5. From Eq.8 into Eq. In other words. the following relative velocity equations are used to determine the velocity of from the velocity of {n}: ω ¯E = ω ¯n v¯E = v¯n + ω ¯ n × p¯E/n . and 5.7. it is seen that the velocity of the end eector when the serial robot possesses only the simple joints may be computed from the developed relative velocity equations 5. linear velocity of the end eector may be computed by v¯E = X d˙t kˆt + t∈nt on the condition v¯0 = ¯0 and X θ˙r kˆr × p¯E/r . and 5.7.11.6 and 5.12) r∈nr ω ¯ 0 = ¯0. In addition.6. 5. Thus the end eector angular velocity may be expressed compactly as ω ¯E = X θ˙r kˆr . 5.9. the angular velocity of the end eector is the sum of the angular velocity of all revolute joints. 5.10. motion from the prismatic Chulalongkorn University Phongsaen PITAKWATCHARA . 5. 5. 5. Note the base is assumed to be immobile.11) r∈nr The equation is developed from Eq.9 successively.2 Jacobian Matrix 85 Figure 5. Let nr and nt {E} (5.5. (5.2 Jacobian Matrix From the previous section. pp. Hence both equations may be written as the multiplication variables of the matrix and the column vector of the joint velocity as v¯E ω ¯E = J (¯ q ) q¯˙ = Jv (¯ q) Jω (¯ q) q¯˙. However. The matrix J (¯ q) This equation indicates the linear mapping by the matrix of the joint velocity q¯˙ to the current robot pose is the (geometric) Jacobian matrix describing the contribution of the each joint motion to the robot end eector motion.12 that each joint term P velocity in uences the end eector velocity indepedently. ([1].5.2 Jacobian Matrix 86 Figure 5. consisting of d˙t and ˙θr . Their contributions to the linear velocity are expressed by the ˙ ˆ ¯E/r in Eq. 177) joints do not contribute to the end eector angular velocity. 5. it can be observed from Eqs. corresponding to the terms in Eq. (5.12. motion from both types of the joints have an aect on the linear velocity of the end eector.12. 5.12. r∈nr θr kr × p Furthermore. pp. in the expressions. Figure 5. as shown in Eq. 5.4: In uence of the revolute joint actutation onto the end eector linear velocity.4 shows the linear motion caused by the revolute joint.11 and 5.11 and 5. The terms kˆi and p¯E/i in Eqs. On the other hand. motion of the prismatic joints contribute to the end eector linear velocity directly as the sum of the linear velocity of all prismatic joints.12 are the functions of the joint q¯ solely corresponding to the explicit separation of q¯˙.3 illustrates the linear motion of the end eector caused by the prismatic joint actuation while Fig. 5. This can be imagined that at a time there is only one joint being actuated while the others are freezed. 5. 5. the end eector motion caused by the revolute joints depends on the position vector p¯E/r from the joint to the end eector as well.13) J (¯ q ) from the space T T v¯E ω ¯ ET the space of the end eector velocity when corresponds to the joint variables q ¯. Speci cally. Chulalongkorn University The matrix is related to the partial Phongsaen PITAKWATCHARA . .12 and th Jω (¯ q ) from Eq. the Jacobian matrix may be decomposed into two submatrices as shown in Eq. its value is determined by the third column of the rotation B matrix i R.14) If it is described in the reference frame. the submatrix Jω (¯ q) governs the relationship between the joint velocity vector and the end eector angular velocity. A suggestion on the way is therefore to complete the manipulator kinematics analysis before deriving the robot Jacobian matrix. 3.5. ˆi × p¯E/i k th . denoted Ji . 3 0 kˆ1 = 0 1 Chulalongkorn University s1 kˆ2 = −c1 0 s1 kˆ3 = −c1 .1. 5. The submatrix Jv (¯ q ) dictates the relationship between the joint velocity vector and the end eector linear velocity. All relevant parameters may be computed from the manipulator kinematics analysis in Ex. n] may be expressed as Ji = The term kˆi Jvi Jωi ˆi k ith − prismatic joint ¯0 . and frame.12. 0 Phongsaen PITAKWATCHARA . = . . The i -column of J (¯ q ). It is determined by subtracting B B ¯E/i is thus expressed in the the embedded position vectors in E T and i T when p base frame. i − revolute joint kˆi is the unit vector along the ith -joint axis. If the Jacobian matrix is chosen to be expressed in the base ˆ1 . In particular. 3.2 Jacobian Matrix 87 derivatives of the multivariable functions.11. Example 5.1 Articulated Arm : Determine the Jacobian matrix of the articu- lated robot in Fig. J (¯ q) = kˆ1 × (¯ pE − p¯1 ) kˆ2 × (¯ pE − p¯2 ) kˆ3 × (¯ pE − p¯3 ) ˆ ˆ k1 k2 kˆ3 . Since all three joints of the robot are the revolute type. 5. 2. the analysis may be applied to determine the linear velocity of arbitrary point on the robot and the angular velocity of any link. and kˆ3 is the third column vector of 0 R. Also.13. the unit vectors k 1 2 0 R respectively.5. Jv (¯ q ) may be determined from Eq.11 and 5. . 0 R. when i ∈ [1. Solution The Jacobian matrix linking the end eector velocity to the joint velocity may be determined straightforwardly by Eq. 5. (5. 5. The other term p ¯E/i denotes the displacement vector pointing the origin of {E} with respect to the origin of {i}. . Similarly. From Eqs.14. kˆ2 . which is originated from the idea of a great German mathematician named Carl Gustav Jacob Jacobi on the topic of Jacobian determinant. 5. and E T is Their values are 0 p¯1 = 0 0 0 p¯2 = 0 0 p¯1 . the homogeneous transformation of every frame described with respect to {3} may be computed. c1 (l1 c2 + l2 c23 ) p¯E = s1 (l1 c2 + l2 c23 ) . For this purpose. Determine the Jacobian matrix of the PUMA 560 robot in Fig. this choice will simplify the singularity analysis as done in Ex. 5. the forward kinematics performed in Ex. l1 s2 + l2 s23 Substitute these values into the Jacobian matrix and perform the evaluation. T T T v¯O6 ω ¯O The Jacobian matrix which calculates the velocity of the wrist 6 center on the last link will be expressed in {3} to reduce the complexity.6. Using the compound transformation. one gets J (θ1 . 0 0 0 0 Position vector in 1 T . 3 T . p¯2 . 0 −s23 −a2 c3 0 −c23 a2 s3 1 0 −d3 0 0 1 s3 0 −a2 c3 c 3 0 a2 s 3 0 1 −d3 0 0 1 c4 −s4 0 a3 0 0 1 d4 3 4T = −s4 −c4 0 0 0 0 0 1 c23 3 3 0 −s23 1 T =0 T1 T = 0 0 c3 3 3 1 −s3 2 T =1 T2 T = 0 0 3 3T = I4×4 Chulalongkorn University Phongsaen PITAKWATCHARA .2 Jacobian Matrix Note that kˆ2 = kˆ3 88 re ecting the second and the third joint axis pointing in the same direction. In addition. it is more convenient to formulate the Jacobian matrix of the wrist center rather than the Jacobian matrix of the end eector. θ3 ) = Example 5.2 −s1 (l1 c2 + l2 c23 ) −c1 (l1 s2 + l2 s23 ) −l2 c1 s23 c1 (l1 c2 + l2 c23 ) −s1 (l1 s2 + l2 s23 ) −l2 s1 s23 0 l1 c2 + l2 c23 l2 c23 0 s1 s1 0 −c1 −c1 1 0 0 PUMA 560 Robot : . 2 T .4.5. θ2 . l1 c1 c2 p¯3 = l1 s1 c2 l1 s2 and p¯E expressed in {0}. p¯3 . Solution For this six DOF robot where the last three joints constitute the spherical wrist.2 must be represented in {3}. 3. 3. −a2 c3 p¯1 = p¯2 = a2 s3 −d3 p¯3 = ¯03 p¯4 = p¯5 = p¯6 = p¯O6 a3 = d4 . p¯2 . kˆ5 = 0 c4 s4 s5 0 3 3 3 is the position vector in 1 T . . . p¯1 . . . 2 R. . Phongsaen PITAKWATCHARA . 6 R in order. Lastly. . . . k6 is the third column of 1 R. . . p¯6 In particular. .2 Jacobian Matrix 89 c4 c5 −c4 s5 s4 a3 s5 c5 0 d4 4 3 3 T = T T = 4 5 5 −s4 c5 s4 s5 c4 0 0 0 0 1 −s4 s6 + c4 c5 c6 −s4 c6 − c4 c5 s6 −c4 s5 a3 s5 c6 −s5 s6 c5 d4 5 3 3 . 0 Substituting these parameters into the form of the Jacobian matrix above. Speci cally from the above homogeneous transformation matrices. . position vectors p¯O6 = in order. . . .5.14 and since all joints are the revolute type. of {6}. 6 T p¯6 since the wrist center always coincides with the origin Similarly. 5. the unit vectors k 3 3 3 ˆ ˆ k2 . kˆ6 ˆ1 . . 2 T . Since the Jacobian matrix is chosen to be expressed in {3}. the Jacobian matrix of the PUMA 560 will have the form of kˆ1 × (¯ pO6 − p¯1 ) kˆ1 kˆ4 × (¯ pO6 − p¯4 ) kˆ4 J (¯ q) = kˆ2 × (¯ pO6 − p¯2 ) kˆ2 kˆ5 × (¯ pO6 − p¯5 ) kˆ5 kˆ3 × (¯ pO6 − p¯3 ) kˆ3 kˆ6 × (¯ pO6 − p¯6 ) . one obtain J (¯ q) = −d3 c23 a2 s3 − d4 −d4 d3 s23 a2 c 3 + a3 a3 a2 c2 + a3 c23 − d4 s23 0 0 −s23 0 0 −c23 0 0 0 1 1 Chulalongkorn University 0 0 0 0 0 0 0 0 0 0 s4 −c4 s5 1 0 c5 0 c4 s 4 s 5 . 6 T =5 T6 T = −c4 s6 − s4 c5 c6 −c4 c6 + s4 c5 s6 s4 s5 0 0 0 0 1 According to Eq. −s23 0 ˆ ˆ ˆ k1 = −c23 k2 = k3 = 0 0 1 0 s4 −c4 s5 kˆ4 = 1 kˆ6 = c5 . Chulalongkorn University In other Phongsaen PITAKWATCHARA . it may be stated that for the end ector to be capable of executing arbitrary motion. = J1 q˙1 + J2 q˙2 + · · · + Jn q˙n . Since the Jacobian matrix is the function of the joint variables q¯.13. At the singularity.5: Workspace-interior singularity of the articulated arm when the wrist center lies on the rst joint axis. The robot is said to be at the con guration when rank J singular is less than its maximally possible value. pp. its value and hence its rank will be changed accordingly. For J ∈ R . ([1]. (5.3 Singularities As explained in the previous section. n).5. or T T v¯E ω ¯ ET ∈ R6 . 5.15) Because the end eector velocity is the six dimensional real vector space. the robot cannot achieve arbitrary motion. the number of linearly indepedent columns of Jacobian matrix must be equal to six. This is a case where the singularity locations are known a-priori. 1. rank J ≤ min (6. Awareness of the singularity of the robot is important for the following reasons. Therefore. the rank of 6×n its Jacobian matrix must be equal to six. 196) 5. the Jacobian matrix J (¯ q) is the linear mapping from the space of the joint velocity to the space of the end eector velocity as depicted in Eq. for the end eector to be able to move arbitrarily. combination of the columns v¯E ω ¯E Ji The equation may be rewritten as the linear of the Jacobian matrix. From the linear algebra.3 Singularities 90 Figure 5. the rank of a matrix will be equal to the number of maximally linearly independent rows or columns of that matrix. 197) Chulalongkorn University Phongsaen PITAKWATCHARA .6: Workspace-interior singularity of the articulated arm when the fourth and sixth joint axes of the wrist aligns.5. pp. ([1].3 Singularities 91 Figure 5. It may happen at any location in the workspace. this type of singularity is not a problem since their locations are predetermined. 2. Such behaviors cause the problem in controlling the robot at or near the singularities. singularities may be classi ed according to its location into two types as 1.5. Besides. Workspace-boundary Singularity is the singularity which happens when the robot fully outstretches or fully folds back their linkages. The system becomes unstable eventually. 4. 5. Otherwise. For example. even the robot end eector moves with low speed. See Fig. making the response not perform as expected. the applied end eector force might be very large. the singularity may be prevented by designing the end eector trajectory not to get close to the rst joint axis. Thus it is necessary to analyze the robot singularity throughout the workspace rst. In practice. Chulalongkorn University Phongsaen PITAKWATCHARA . Also. With these con gurations. Corresponding posture of some interior singularity con gurations brings the information of where they happen.5. At the singularity. 3. it might not be possible to identify where the interior singularity con guration will happen. Workspace-interior Singularity is the singularity which happens when the end eector is not at the workspace boundary. the bounded operational space velocity of the end eector may require the unbounded joint space velocity. There may be in nitely many solutions of the robot inverse kinematics at the singularity. in the neighborhood of the singularity. it is known that the robot will run into the singularity when the wrist center of the articulated arm lies on the rst joint axis. However. Figure 5. in the neighborhood of the singularity. the robot is unable to move further in such direction. At the singularity. Typically. the computed control signals will be impractically large. even the robot is supplied with small joint torque. This type of singularity is often caused by the parallelism of two or more robot joint axes.3 Singularities 92 words. Thus. it may cause a problem of non-smooth trajectory when the robot passes through the singular point. 2.6 displays the articulated robot sin- gularity when the fourth and sixth joint axes of the spherical wrist aligns. its joint velocity might be very large. the robot trajectory will be planned to stay far enough from the workspace boundary and this singularity can be avoided. Since the end eector is still be inside the workspace. the bounded joint torque vector may be associated with the unbounded force acting at the end eector. there are some directions in which the end eector cannot translate or rotate. Additionally. s3 = 0 and/or l1 c2 + l2 c23 = 0. may be performed simply by checking its determinant. 5. 5. However. Solution In Ex. Speci cally. if the speci ed task involves with the translational motion of the end eector only.16) Analyze the singularity of the articulated robot in Fig.7 The other case of l1 c2 + l2 c23 = 0 will occur when the end eector lies on the rst joint axis for which this singularity is of the interior singularity type. θ2 . the robot singularity may be determined from the following condition of det Jv (¯ q ) = −l1 l2 s3 (l1 c2 + l2 c23 ) = 0. the Jacobian matrix of the robot is derived. the robot will be at the singular con guration if and only if det J (¯ q ) = 0. of the Jacobian matrix is happening when the number of the operational DOF and the number of the joints are equal.3 Singularities 93 Precise location of the end eector when this singularity occurs is not known until the inverse kinematics analysis is performed.8. 3. Chulalongkorn University Phongsaen PITAKWATCHARA .3 Articulated Arm : (5. J (θ1 . the Jacobian matrix may be degenerated into the sub-Jacobian matrix of the linear velocity portion. The arm will be fully outstretch or fully fold back accordingly. See Fig. 0 l1 c2 + l2 c23 l2 c23 Consequently. The end eector will not be able to move in the direction perpendicular to the plane formed by the links l1 and l2 . in which the determinant is not de ned. there are in nitely many solutions of the robot inverse kinematics at this con guration. Value of the rst joint can be chosen arbitrarily as it does not cause any translation of the end eector. Example 5. θ3 ) = c1 (l1 c2 + l2 c23 ) −s1 (l1 s2 + l2 s23 ) −l2 s1 s23 . as shown in Fig. θ2 . 6×3. Determination of the robot singularity for the special case where the dimension n × n. π . That is −s1 (l1 c2 + l2 c23 ) −c1 (l1 s2 + l2 s23 ) −l2 c1 s23 Jv (θ1 . These con gurations are the boundary singularity type. θ3 ) = −s1 (l1 c2 + l2 c23 ) −c1 (l1 s2 + l2 s23 ) −l2 c1 s23 c1 (l1 c2 + l2 c23 ) −s1 (l1 s2 + l2 s23 ) −l2 s1 s23 0 l1 c2 + l2 c23 l2 c23 0 s1 s1 0 −c1 −c1 1 0 0 Dimension of the Jacobian matrix is . 5. This makes the links l1 and l2 This equation will be satis ed when s3 = 0 will be true when θ3 = 0 or lie in the same straight line.5.1. Moreover.5. 8: Singularity of the articulated arm when the end eector lies on the rst joint axis. ([1].3 Singularities 94 Figure 5.7: Singularity of the articulated arm when the arm fully outstretches or folds back. 200) Chulalongkorn University Phongsaen PITAKWATCHARA . Figure 5.5. pp. In summary. Condition when the singularity of the arm. Arrangement of the linkages resemble the one shown in Fig. and the wrist center point. Considering the robot kinematical structure in Figs. The second case will hold if the wrist center lies on the rst joint axis. joint axis#3.8. which is perpendicular to the plane formed by joint axis#5 and joint axis#6. the rst case will hold if the robot arm stretches out or folds back the link a2 and d4 in a way that one can draw a line which passes joint axis#2. 3. will happen is that det J11 = a2 (a3 s3 + d4 c3 ) (a2 c2 + a3 c23 − d4 s23 ) = 0. Thus the roll-pitch-roll wrist will be unable to rotate in the yaw direction. 5. The robot posture is similar to the one illustrated in Fig. driven by the rst three joints. condition when the singularity of the wrist. the singularity analysis may be decomposed into two subproblems of the arm and the wrist singularity. When θ5 = 0 or π . it makes joint axis#4 and joint axis#6 aligned. Chulalongkorn University Phongsaen PITAKWATCHARA .2. The wrist center lies on the rst joint axis. If this happens.3 Singularities Example 5.5. The matrix is a lower-block triangular matrix.4 95 PUMA 560 Robot : Determine the singularity of the PUMA 560 robot in Fig. this condition will holds. Hence.5. will happen is that det J22 = −s5 = 0. joint axis#3. Physically. The wrist fully stretches out or folds back. 5. making it unable to move perpendicular to the plane formed by the link a2 and d4 . Solution The Jacobian matrix of PUMA 560 is derived in Ex.6. the conditions which brings the PUMA 560 to one of its singular point are The link a2 and d4 stretch out or fold back in a way that there is a line passing through joint axis#2.7 and 3. the point will not be able to move along that line.6. coincident with the situation when they are controlled separately. driven by the last three joints. It is shown again here J (¯ q) = −d3 c23 a2 s3 − d4 −d4 d3 s23 a2 c3 + a3 a3 a2 c2 + a3 c23 − d4 s23 0 0 −s23 0 0 −c23 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 s4 −c4 s5 1 0 c5 0 c4 s 4 s 5 ¯ = J11 03×3 J21 J22 for convenience. This will occur when a3 s 3 + d 4 c 3 = 0 or a2 c2 + a3 c23 − d4 s23 = 0. 3. and the wrist center. 5. On the other hand. pp. Chulalongkorn University Phongsaen PITAKWATCHARA . From the principle. 5.4 Static Force Figure 5. 5. the last two cases of singularities bring about in nitely many inverse kinematic solutions.5. In addition.9 showing the ideal serial manipulator of which the friction may be omitted. 5. 232) Locations of the wrist center for the rst two cases are known priori and thus can be prevented.13. Consider Fig.2. it can be shown that when the robot is in equilibrium. recall the statement of the principle of the virtual work : The virtual work acting by the external active forces to the ideal mechanism under the equilibrium condition will be equal to zero. the remaining external active forces acting on the robot will be merely the actuator torques at the joints and the force at the end eector. Therefore. if it is legitimate to neglect the gravity force or it has been compensated separately.9: 96 Relationship of the force acting onto the environment at the end eector and the actuator torques applied at the robot joints. it is shown that the geometric Jacobian matrix governs the relationship between the joint velocity and the end eector velocity as summarized in Eq.4 Static Force In section 5. ([1]. the Jacobian matrix also dictates the relationship between the force acting at the end eector and the supplied torque at the joints. Moreover. it must be checked whether s5 ≈ 0 at every sampling before proceeding. Nevertheless. the last case of singularity can happen everywhere inside the workspace. However. external active forces τ ¯ and F¯ acting on the robot that F¯ = When the f¯ and (5. (5. Since the principle of the virtual work must hold for arbitrary virtual displacement. the application of the principle of the virtual work when the robot is in equilibrium yields ¯ = 0. Chulalongkorn University Phongsaen PITAKWATCHARA .20) It can be stated that the mapping from the end eector reaction force to the robot joint torque is determined by the transpose of the Jacobian matrix. τ¯ = τ1 τ2 · · · τn T .18) is in equi- δ q¯ from T at (¯ ω dt)T librium.19.4 Static Force 97 Actuator torques may be grouped and written as the joint torque vector in n R . Correspondingly. the relationship between the end eector force and the joint torque at the equilibrium may be derived by rst drawing the free-body-diagram of all participating linkages and elements. generally it is composed of the force R6 as the couple µ ¯ which may be written as a vector in T T T ¯T f x f y f z µx µy µz = f¯ µ . After eliminating the internal forces and grouping the equations altogether. 5. Virtual work is the work done by the force on the system according to the virtual displacement. If it is decided that the work done by the environment on the system is positive and the work done by the system on the environment is negative.17) For the force acting onto the environment at the end eector. Of course. τ¯T δ q¯ − F¯ T δ X With the robot kinematic constraints of Eq. one may conclude that τ¯ = J T F¯ . the above equation may be written as τ¯T − F¯ T J δ q¯ = 0. (5.19) are the virtual displacements because these displacement are guaran- teed to satisfy only the kinematic constraints. the relationship between the external forces acting at the end eector and the joints may be attained. ¯ δ q¯ and δ X (5. Then the general equilibrium equations of X f¯ = ¯0 X µ ¯ = ¯0 may be applied to develop the relationship between the forces acting on various parts of the robot.5. assume the joints are displaced by an arbitrary deformation the equilibria. the displacement ¯ = δX δ¯ x T the end eector admissible to the kinematic constraints is related to the joint displacement by ¯ = J (¯ δX q ) δ q¯. 1 of Chapter 3 expressed in the base frame. A schematic diagram of the 2-DOF long-reach robot arm is shown in Fig. Which description is simpler? Analyze for the singularities. Derive the Jacobian of the robot arm in Prob. Figure 5. At the depicted posture. The rst link weighs total lengths are 1.G. 2 of Chapter 3 expressed in the base and in the end eector frame. 4. let the C. Determine the Jacobian of the robot wrist in Prob. 3. Chulalongkorn University Phongsaen PITAKWATCHARA .10. Determine the Jacobian of the robot wrist in Prob.10: Schematic diagram of a 2-DOF long-reach arm. Their 4 m. determine the required torque and force of both actuators to maintain the con guration. Which description is simpler? Analyze for the singularities.5. of the links be at their mid-point. 5. 5. 4 of Chapter 3 expressed in the base frame and in the end eector frame. Derive the Jacobian of the manipulator in Prob. Identify. if any. respectively.5 m and m forward. ◦ where the rst link is rotated by 110 and the second link is traveled by 3.5 50 kg and the second link 10 kg. 3 of Chapter 3 expressed in the base frame and in the end eector frame.4 Static Force 98 Problems 1. For simplicity. Determine a set of joint angles which bring the robot at its workspace boundary and workspace interior singularities. 2. the singularity of this robot. Chapter 6 Trajectory Generation Chulalongkorn University Phongsaen PITAKWATCHARA . Accordingly. In this chapter. Nevertheless. By this implementation. it could be the trajectory of the joint or the end eector motion. For the robot control point of view.6. Beware the time spent during the same segment of each joint must be the same for the end eector to actually meet the speci ed via points. rstly the joint angles which cause the end eector to be at the speci ed set of postures must be computed. 6. Therefore the user might need to specify a sequence of desired postures. the next two sections describe each method in details. common users will provide a set of points or postures through which the end eector should pass. e. velocity. called via points. 6. points along the trajectory will be computed at a certain rate depending on the control loop update rate.1 explains some concepts and related terms. Usually.g. for each joint. the joint space schemes generate the robot trajectory at the joint level. Hence Ti and Tf are speci ed.2 Joint Space Schemes With the given path points. Section 6. and acceleration of the object motion. how the end eector move depends on the generated trajectory. This can be achieved by the inverse kinematics calculation.1 Path Description The term trajectory generally refers to a time history of position. During the way. named after in which space the trajectory are planned. Hence. The simplest speci cation would be to give the initial and nal posture the end eector must be. one must have the desired motion in mind.1 Path Description 100 In order to command the robot to move. Collection of these path points to include both the initial and nal points as points may be called well. There are two main approaches in generating the trajectory. Homogeneous transformation description might be used. the joint space schemes yield the desired end ector Chulalongkorn University Phongsaen PITAKWATCHARA . It may be necessary that the end eector be in a certain posture with some velocity and/or at a speci ed time during the motion to accomplish some tasks. between the initial and nal ones. Consequently. several methods to generate the trajectory for the end eector to follow will be studied. Typically they must pass or go through certain points with some particular velocity. They are the joint space scheme and the Cartesian space scheme. from this speci cation it must be the computer program that create the trajectory. Then. it requires more than merely where the end eector should be nally. The trajectory or path will have to satisfy a set of requirements. the assembly task. Often. a smooth function is determined that passes through the stream of discrete points of the joint angles. the time instant 6. inverse kinematics calculation yielding the θ (t) θf . at the two end points. First and foremost. Another important issue is there will be no singularity problem if the trajectory is constructed in the joint space. there are many such functions and so the trajectories which satisfy the given conditions as depicted in Fig. the user must not give the via point coincident with the singularity point.6. Usually. 6. θi = a0 + a1 ti + a2 t2i + a3 t3i θf = a0 + a1 tf + a2 t2f + a3 t3f Chulalongkorn University Phongsaen PITAKWATCHARA .1. both position and velocity are speci ed.2. (6. Singularity is the problem in which the inverse kinematics function fails at certain points in the workspace.1) From these requirements.1 Trajectory Generation for Two Path Points The problem here is to move the end eector from its initial to nal pose by the speci ed time interval.1 ti and tf . at corresponding joint angles will be performed rst. As a result.2 Joint Space Schemes 101 pose at the via points. Cubic Polynomial Function However. Of course. This will shorten the length of each segment of the joint trajectory.2. Computational cost is much less compared to the Cartesian space schemes mainly because the inverse kinematics will be performed at the via points only. typical methods used to generate the trajectory given the via points under some constraints are considered.1. Then. In the following. 6. θ (ti ) = θi θ˙ (ti ) = ωi θ (tf ) = θf θ˙ (tf ) = ωf . θi and will be determined. The joint space schemes have some advantages. segments of the end eector trajectory will be shorten as well so that each of them joining the adjacent via points may be described by a linear interpolating function. one may use the cubic polynomial function θ (t) = a0 + a1 t + a2 t2 + a3 t3 (6. The simplest case of two path points will be considered rst. Shape of the motion during the via points at joint level is described by the functions used to t the via points. The drawback may be resolved naturally by simply introducing more constrained via points during the desired path. Typically.2) as the trajectory. However the shape of the corresponding motion of the end eector is not put in the design requirement and hence might turn out to be arbitrarily complex. it is easier to generate the trajectory in joint space. Substituting the position constraints into the equation. a smooth function for each joint which passes through its initial and nal joint angles. simpli ed analytical expressions may be determined as a0 = θi a1 = 0 3 a2 = 2 (θf − θi ) tf 2 a3 = − 3 (θf − θi ) . (6.3) The above four equations may then be used to solve for the coe cients of the cubic polynomial function. ([4]. where the velocity equation is obtained by taking the time derivative of Eq.1: Several possible trajectories for a single joint which satisfy the ending joint values. For the special case of zero initial and nal velocity and setting ti = 0. Chulalongkorn University Phongsaen PITAKWATCHARA .6. the velocity conditions call for ωi = a1 + 2a2 ti + 3a3 t2i ωf = a1 + 2a2 tf + 3a3 t2f . 6. 6. 231) must hold.2: ω (t) = a1 + 2a2 t + 3a3 t2 .4 will be a parabola maximally/minimally at the midpoint and the acceleration will be a linearly decreasing/increasing function having zero value at the midpoint.4) It should be mentioned that since the trajectory function is cubic.2 Joint Space Schemes 102 Figure 6. the velocity function according to the coe cients in Eq. tf (6. pp. Analogously. 238) 6. In other words. which leads to an impulsive acceleration that can deteriorate the tracking result.2: Linear trajectory connecting the intial and nal joint angles.1.6.2 Quintic Polynomial Function If additional constraints on acceleration of θ¨ (ti ) = αi θ¨ (tf ) = αf (6.5) are speci ed.1. the linear segment trajectory is blended with the parabolic function. 6. it will make the velocity to be discontinuous at the end points.7) (6.6) is necessary.8) However the analytical expressions will be more complicated. A typical linear function with parabolic blends trajectory and its velocity and acceleration pro le are illustrated in Fig. Coe cients of the equations may be solved in the same manner with the velocity and acceleration pro le of ω (t) = a1 + 2a2 t + 3a3 t2 + 4a4 t3 + 5a5 t4 α (t) = 2a2 + 6a3 t + 12a4 t2 + 20a5 t3 . To x this problem. ([4]. 6. During the blending period. However. a parabolic function is introduced at the ends to smoothen the path with continuous position and velocity.3 Linear Function with Parabolic Blends Yet another way to generate a function for the trajectory that passes through the given end points is to use the linear segment function as shown in Fig. (6. pp.2.2.3.2 Joint Space Schemes 103 Figure 6. a fth-order or quintic polynomial function θ (t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5 (6.2. 6. the Chulalongkorn University Phongsaen PITAKWATCHARA . 2 where α is the constant acceleration. Without loss of generalization. tb < t ≤ tf − tb . Thus. duration. (6. typically one rst needs to specify the initial and nal joint angles. a special case of zero initial and nal velocity and setting Let θ˙i ti = 0 be the symmetric blending time. To determine θ (tb ). 2 Chulalongkorn University tb < t ≤ tf − tb . and the time of the motion. during the rst blend.9) equality of the velocity at the end of the rst blend and at the beginning of the constant velocity. (6. the following equation θ tf 2 = θ (tb ) + V tf − tb 2 = θi + θf 2 holds due to the symmetry of the trajectory. tf − ti .6. tb and θ˙f . In the linear segment portion. the motion is thus described by the linear function of θ (t) = θ (tb ) + V (t − tb ) . ([3].11) resulting in the linear segement trajectory of θ (t) = θi + θf − V tf + V t.3: Linear function with parabolic blends trajectory and its velocity and acceleration pro le. θi and θf .2 Joint Space Schemes 104 Figure 6. θ (tb ) = θi + θf − V tf + V tb . To construct such trajectory. will be considered. the motion is described by the parabolic function of 1 θ (t) = θi + αt2 . the according velocities. Hence. αtb = V. segment requires the relationship of the following parameters. pp.10) During the linear segment portion. 195) associated velocity will be changed linearly with a constant acceleration.12) Phongsaen PITAKWATCHARA . 2 (6. V. At time (6. 0 ≤ t ≤ tb t = tb . the motion will be executed with a constant velocity. 14) or the motion is not possible. the relationship between the blending time and the acceleration is governed by the quadratic αt2b − αtf tb + θf − θi = 0. 2 2 2 Applying the boundary angle condition θf at time tf .13) tf . (6. 6. 6.10 and 6.2 Joint Space Schemes 105 Moreover. Consequently. the following condition 2 α2 t2f − 4α (θf − θi ) α < tf must hold. The blending time may also be expressed as a function of the acceleration.10. This implies the desired constant acceleration value α≥ 4 (θf − θi ) t2f (6. Thus the blending time may be written as a function of the speci ed end conditions and the constant velocity: tb = Furthermore. the trajectory of Eq. Finally.11 into the above θ (t) may be expressed as 1 1 θi + θf − V (tf − tb ) θ (t) = θf − αt2f + αtf t − αt2 − . V θf − θi θf − θi <V ≤2 tf tf (6. Rewrite Eq. 2 Substituting the expression of α and θ (tb ) equation and performing some reduction. the motion is again described by the parabolic function of 1 θ (t) = (θf − θ (tb )) + V (t − tf + tb ) − α (t − tf + tb )2 .15) tf . from Eq.13 by making use of Eq.6. 6. since (6. tf − tb < t ≤ tf . θi + θf − V (tf − tb ) =0 2 Chulalongkorn University Phongsaen PITAKWATCHARA .16) must be satis ed. it can be shown that since q 0≤ . 6. the blend time may be determined by tb = tf − 2 q α2 t2f − 4α (θf − θi ) 2α 0 < tb ≤ Analogously.9 and 6. the desired constant velocity value must be in 2 0 < tb ≤ between θi − θf + V tf .12 must blend at tb . during the last blend. 1 A single-link robot with a rotary joint is motionless at It is desired to move the joint in a smooth manner to θ = 75◦ in 3 θ = 15◦ .4 and 6. the blending time may now be computed from Eq 6. 1 Therefore. Thus.5 respectively. 1<t≤2 θ (t) = 2 −60 + 90t − 15t . 6.3660 θ (t) = −105 + 120t − 20t2 .6340 < t ≤ 2.3590t. As in the rst case. Solution The blending time according to the given velocity may be calcu- lated from Eq. 6.13 as tb = 15 − 75 + 30 × 3 = 1 sec. 0 ≤ t ≤ 0. Chulalongkorn University Phongsaen PITAKWATCHARA . seconds.6. V = 40 × 0.3590◦ /sec. What will be the new trajectory if ◦ 2 the acceleration of the parbolic blends is speci ed to be 40 /sec instead.6340 sec. 2. velocity.12. Determine the linear function with parabolic blends trajectory if the velocity of ◦ the linear segment is chosen to be 30 /sec. 2 × 40 Velocity during the linear segment must obey Eq. For the case where the acceleration is speci ed.9. and 6. 2 2 tf − tb < t ≤ tf . Thus. 0. 2 < t ≤ 3. and acceleration for both cases are displayed in Fig. the trajectory in the last segment may be described by 1 1 θ (t) = θf − αt2f + αtf t − αt2 .6340 = 25. 6. 6.10. Plots of the angle. 6. the linear function with parabolic blends for the second case may be determined. (6.9615 + 25. Thus. α= 30 = 30◦ /sec2 . the linear function with parabolic blends for the rst case may be determined from Eqs. 30t. 30 Acceleration during the parabolic blends must obey Eq.15 as 3 tb = − 2 p 402 × 32 − 4 × 40 × (75 − 15) = 0.6340 15 + 20t2 . 6.17) Example 6. 6.10.2 Joint Space Schemes 106 may be concluded.3660 < t ≤ 3.17. 0≤t≤1 15 + 15t2 . 2 Joint Space Schemes 107 Figure 6.6. Figure 6.5: Linear function with parabolic blends trajectory and its velocity and acceleration pro le for the second case.4: Linear function with parabolic blends trajectory and its velocity and acceleration pro le for the rst case. Chulalongkorn University Phongsaen PITAKWATCHARA . 1 Cubic Polynomial Function If the desired velocities of the joints at the via points are speci ed. Solving the above four equations for the coe cients.2. Therefore the generated trajectory is expected to pass through these via points as well. the path generator system should have the capability to Chulalongkorn University Phongsaen PITAKWATCHARA . the nal time tf ti = 0. i. the following constraints θ (ti ) = θi θ˙ (ti ) = ωi must be satis ed.2. applying the velocity constraints to the velocity equation of the trajectory in Eq. 6.1. extensions of the previous derivations are necessary.2 108 Trajectory Generation for a Path with Via Points As explained in section 6. Therefore. At a particular segment. In general. the above constraints may be substituted to obtain θi = a0 θf = a0 + a1 tf + a2 t2f + a3 t3f . tf tf (6.2. Therefore. 6. one then have a0 = θi a1 = ωi 2 1 3 a2 = 2 (θf − θi ) − ωi − ωf tf tf tf 2 1 a3 = − 3 (θf − θi ) + 2 (ωf + ωi ) . where for each via point. it is required that the end eector passes through the via points without stopping.1 may be applied immediately. 6.2 Joint Space Schemes 6.2. θ (tf ) = θf θ˙ (tf ) = ωf (6.19) In practice. This implies is the time duration spent in that segment.6.2. specifying the velocity at each via point may not be intuitive to the user. the user may need to specify a sequence of path points rather than merely two end points. the initial time is reset to zero. it is simple to construct segments of cubic polynomial trajectory which connect two neighboring via points. the solutions in subsection 6.18) Using the cubic polynomial function of Eq.3. If the end eector is to come to rest during each visited via point. the following equations ω i = a1 ωf = a1 + 2a2 tf + 3a3 t2f may be formed.e. Similarly. As a result.2 Higher-Order Polynomial Function If additional constraints on acceleration at the via points are speci ed.2 Joint Space Schemes 109 Figure 6. The following simple heuristic algorithm may be used in selecting the velocity. Another approach for this issue is not to pay attention to the speci c velocity values at the via points. pp. the initial and nal velocity and acceleration of ω2 . Consider the path points of a robot joint in Fig. t1 . θ1 . the zero velocity is chosen. and α2 at ω0 . Fortunately. If the sign does not change. These velocities are then to be used as the velocity constraints at the via point as before.1.6. In addtion. If the slope of the line changes sign at the via point.2. ([4]. work of using the quintic polynomial in subsection 6. In a similar vein to the cubic polynomial function.6.6: Heuristic decision of assigning the velocity at the via points for the cubic polynomial function trajectory. the velocity at the via point is selected to be the average of the two slopes. Imagine the via points connected with straight line segments.2.2. and θ2 time t0 . A drastically dierent approach in trajectory generation for the path points is to solve for a single high-order polynomial function which passes through all points with possibly additional velocity and acceleration constraints. a higherorder polynomial function is needed since it has more room to satisfy all of them.2 for generating a path joining two end points may be extended for the general case of given path points. and t2 . the trajectory for a segment will depend on the adjacent ones as well. motion of a joint is required to pass through θ0 . 6. the equations arising from applying these via point constraints may be represented in the matrix-vector equation. Hence the constrained equations to be formulated Chulalongkorn University Phongsaen PITAKWATCHARA . 6. For example. α0 . Order of the polynomial depends on the total number of the constraints to be satis ed. the new constraints that both the velocity and acceleration be continuous are employed. are speci ed. 235) compute the appropriate via point velocity autonomously. Rather. Tri-diagonal form of the matrix expedites the computation of the coe cients of the cubic polynomial trajectories. and its velocity and acceleration. According to Fig. straight lines are constructed to join the via points rst. 6. consider three neighboring via points Time duration of the segment connecting points j j and k . 242) are θ0 θ (t0 ) θ1 = θ (t1 ) θ2 = θ (t2 ) ω0 = θ˙ (t0 ) ω2 = θ˙ (t2 ) α0 = θ¨ (t0 ) α2 = θ¨ (t2 ) . 6.3 Linear Function with Parabolic Blends Linear function with parabolic blends for two end points may be modi ed to serve for the case when the path points are speci ed.2. θk . tjk . Qualitatively. pp. of which its coe cients may be solved uniquely. k . and time duration of are not speci ed. Then parabolic blends are employed at each via point to round the path. This introduces a large system of linear equations to be solved.7: Multisegment linear function with parabolic blends trajectory for the speci ed path points and time durations. A big disadvantage will be seen when many via points and/or constraints are speci ed.2 Joint Space Schemes 110 Figure 6. Eectively. Figure 6.6. Phongsaen PITAKWATCHARA . The determinate polynomial function which satis es these constraints is a sixth order polynomial θ (t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5 + a6 t6 . is speci ed. θ¨k and θ˙jk . Corresponding constant acceleration and velocity are denoted as Chulalongkorn University and and ever. θl .2. tk . time duration during the blend region at point the linear segment between points θj . the resulting blended path will not pass through the via points. is intrinsically continuous at the via points. An advantage of this approach is that the trajectory. It only comes close to the point.7. ([4]. Resolution to this issue will be discussed later.7 depicts the smooth trajectory which ts the via points. tdjk . Howk . 2 Joint Space Schemes 111 If the magnitude of the acceleration to use at each via point .6. . . ¨ . . θ k . . and the linear velocities at each segment of the connecting trajectory. Refer to Fig. the linear times. 6. Their streams of discrete values will be computed at the path update rate and fed as a reference input to the robot controller. the following computation may be used to determine the blend times. is speci ed. These parameters will be used to generate the trajectory and its derivatives. For the rst segment.7. the blend time t1 may be solved by recognizing the velocity at the end of the blending portion must be equal to that during the linear segment. . θ¨1 = sgn (θ2 − θ1 ) θ¨1 θ¨1 t1 = θ2 − θ1 td12 − 12 t1 s t1 = td12 − t2d12 − (6. the linear velocity of the rst segment follows immediately.20) 2 (θ2 − θ1 ) .21) Hence.22) For the interior via points. td12 − 12 t1 (6. θk − θj θ˙jk = tdjk θl − θk θ˙kl = tdkl . the computation of trajectory parameters is performed with the following equations. θ¨1 (6. θ2 − θ1 θ˙12 = . . . . ¨ ˙ ˙ θk = sgn θkl − θjk . θ¨k . 26) Note from Fig.25) (6.24) θ˙kl − θ˙jk θ¨k 1 1 = tdjk − tj − tk . Two end points of the entire path are exceptional where the full blend region must be counted. (6. t12 . 2 2 tk = tjk (6.7 that only half of the blend region of each via point enter the segment. 6. can be computed after the blend time of the second point Chulalongkorn University t2 is known. That Phongsaen PITAKWATCHARA .23) (6. Another point is that the linear time of the rst segment. 6.2 Joint Space Schemes 112 is θ3 − θ2 θ˙23 = td23 . . . . θ¨2 = sgn θ˙23 − θ˙12 . θ¨2 . t2 = θ˙23 − θ˙12 θ¨2 1 t12 = td12 − t1 − t2 . 2 (6. Finally. assuming the zero ending velocity.26.23 to 6.27) Intermediate segments are thus successively calculated using Eqs. 6. . the last segment connecting θn−1 and θn is reached. Its related parameters may be computed from matching the velocity of the linear segment with the starting velocity of the blending portion. . . . θ¨n = sgn (θn−1 − θn ) . θ¨n . 24.2 The trajectory of a particular joint is speci ed as follow. Calculate all segment velocities. 75. (6. 6.31) is determined from Eqs. the default acceleration of all blendings. and linear times.29) θn − θn−1 td(n−1)n − 21 tn 1 = td(n−1)n − tn−1 − tn . 90.30) t(n−1)n (6. Path points in degrees are 3 10. 2 θ˙(n−1)n = (6. 50◦ /sec2 is to be used for seconds.25 and 6. The computation may be performed straightforwardly following Chulalongkorn University Phongsaen PITAKWATCHARA . Example 6. 1. Time spent during the segments are 2. 60. blend times. For simplicity.28) θn − θn−1 = 0 − θ¨n tn td(n−1)n − 12 tn s tn = td(n−1)n − where tn−1 t2d{n−1}n + 2 (θn − θn−1 ) θ¨n (6. Generate the trajectory and its velocity and acceleration at a rate of Solution 1 kHz. 5858 − 0.007) θ (t) = 79.993) 60.649 2. Particularly.649) − 25 (t − 2.0862 (t − 3. θ¨1 = sgn (60 − 10) × 50 = 50◦ /sec2 r 2 (60 − 10) = 0.0142 − 0.2893◦ /sec θ˙12 = 2 − 0.5 × 0.585 < t ≤ 1.993 < t ≤ 2.1017 = 2.7950 + 29.2099 + 30 (t − 2.2893) × 50 = 50◦ /sec2 30 − 29.5474 sec.585) 2 59.7017 sec t3 = −50 t23 = 1 − 0.4071 sec θ¨4 = sgn (90 − 75) × 50 = 50◦ /sec2 r 2 (75 − 90) t4 = 3 − 32 + = 0.2893 t2 = = 0.993 1.0142 sec 50 t12 = 2 − 0.898 5.898 < t ≤ 6.5 × 0.5 × 0. the blending process causes the trajectory not passing through the via points unless the robot come to a stop. one merely insert the repeated via point in the path points speci cation.0862 − 30) × 50 = −50◦ /sec2 −5. 6. If a pause is acceptable.351) 75.351 < t ≤ 5.0142 = 1.0862 − 30 = 0.0862◦ /sec 3 − 0.649)2 88.898)2 0 ≤ t ≤ 0.649 < t ≤ 3.585 0.7017 − 0.5 × 0. 10 + 25t2 18.2098 − 5.4699 + 30 (t − 2.351 3.001 sec are shown in Fig. and acceleration generated with the time step of 0.6.993) + 25 (t − 1.0862 (t − 5.2893 (t − 1.898) + 25 (t − 5. From these parameters.5556 + 29. Chulalongkorn University Phongsaen PITAKWATCHARA .2 Joint Space Schemes 113 a series of the above formulas.5858 sec t1 = 2 − 22 − 50 60 − 10 = 29.8.1017 θ¨3 = sgn (−5. the trajectory generated by segments of the linear function with parabolic blends may be written explicitly as follow. the path can come pretty close to the points if high blending acceleration is employed.2893 (t − 0. velocity.5 × 0.5 × 0. As mentioned earlier.7017 = 0.007 2.1017 sec 50 75 − 90 θ˙34 = = −5.2553 − 5. Plots of the trajectory.007 < t ≤ 2. Nevertheless.6420 sec t34 = 3 − 0.5858 90 − 60 θ˙23 = = 30◦ /sec 1 θ¨2 = sgn (30 − 29. pp. Figure 6.6.8: Trajectory generated by segments of the linear function with parabolic blend and its velocity and acceleration pro le for the speci ed path points. 245) Chulalongkorn University Phongsaen PITAKWATCHARA . time duration.9: Use of pseudo via points to create a trajectory which passes through the original via with speci ed velocity. ([4]. and the blending acceleration. The `o' marks indicate the segment transition point.2 Joint Space Schemes 114 Figure 6. 3 Cartesian Space Schemes Cartesian space schemes generate the trajectory of the robot end eector directly in the Cartesian space or the task space. B θ. circular.3 Cartesian Space Schemes 115 Sometimes it is required that the motion passes through the via point without stopping. As a result. the Cartesian space one has a drawback of heavy inverse kinematics computation and its relates at every update time to bring the generated Cartesian coordinates and their derivatives to the joint space angles and their associates.32) {E} with respect to {B}. it is readily seen that each coordinate must change linearly between two end values. a scheme to generate a straight line end eector translational motion connecting two points in the workspace and simultaneously provide the smooth rotational transition between the speci ed orientations will be studied. the designer may also impose the velocity used to pass through that point. Furthermore. the generated trajectory in task space may pass through some robot singular points incidentally. one might want to apply the same interpolation scheme to all coordinates Chulalongkorn University Phongsaen PITAKWATCHARA . In the following. This can be achieved by replacing that via point with two pseudo via points. See Fig. it is natural and makes more sense to generate the trajectory by this scheme since the path points are visually speci ed in the task space. Consequently. 6. Several Cartesian space schemes for the trajectory generation have been proposed. such as the straight line. kˆ ¯E = X p¯. for simplicity in the calcu- lation. However. By this method. any desired path. The rst step of this scheme is to describe the speci ed task space path points using the 6-tuples consisting of the position vector. θkx θky this notion does not work for the rotational motion since T θkz -tuple is not a vector. The term through point might be named for a path point through which the manipulator is forced to pass exactly. or sinusoidal motion during the via points may be established.9. Opposite to the joint space schemes. linear functions with parabolic blends may be applied to each coordinate of the position vector of the path points to generate segments of the end eector trajectory. for the position and the . Placement of the pseudo via points are such that the original via point lies in the linear segment of the path connecting them. px py pz θkx θky θkz dictates the position and the orientation of the speci ed T (6. one on each side of the original. angle/axis representation. Nonethelesss. Namely for each path point. for the orientation. to construct a straight line trajectory connecting two consecutive via points. resulting in the failure of the mappings to the joint space. From the path planner viewpoint. 6.6. Concerning of the translational motion. 400 400 p¯3 = 500 600 500 p¯6 = 500 . the angle/axis representation should be chosen such that θ2 (kx ) − θ1 (kx ) 2 1 θ2 (ky ) − θ1 (ky ) 2 1 θ2 (kz ) − θ1 (kz ) 2 1 is minimized. Example 6.3 Cartesian Space Schemes 116 of the tuple.e. 500 400 p¯4 = 500 . the end eector is required to pass through the sequence of path points (in mm) with respect to the origin of 500 p¯1 = 500 . i. Consequently. S may be troublesome to the trajectory generation between two orientations 1 R S and 2 R. Rather than specifying the blending acceleration. ˆ ˆ θ. 500 Chulalongkorn University {B}: 500 p¯2 = 500 . the rotational motion is not obtained by the rotation about the unique axis. This might cause the motion be unnatural.34) (6. With this selection. k . The required acceleration should be checked that it does not exceed the maximum value or the blending period might be prolonged. to meet this requirement. 600 400 p¯5 = 600 . k = θ + 2πn. at the interior via points with the assigned tk .3 As a part of the particular task execution of PUMA 560 manipulator. Mathematically. the blending time should be assigned and the acceleration be computed from the relationship. the blending and the linear time spent during the same segment of each coordinate must be the same to ensure that the robot motion will follow a straight line in space (during the linear segments). the amount of rotational motion will be minimized. Similar to the path generation in the joint space. Note an issue of non-unique angle/axis representation. the calculation steps of the linear function with parabolic blends trajectory should be modi ed.33) (6.35) should be evaluated instead. As a result.6. 500 Phongsaen PITAKWATCHARA . a set of the following equations θk − θj θ˙jk = tdjk θl − θk θ˙kl = tdkl θ˙kl − θ˙jk θ¨k = tk 1 1 tjk = tdjk − tj − tk 2 2 (6. Typically. is introduced.5. e = 100 mm. the time used for each segment may be chosen as t1 = 1. Planned trajectory should bring the end eector from the rst to the second via point by 5 seconds.3. t40 = 1. d4 = 433. Generate the trajectory using the simple Cartesian straight line motion scheme at a rate of 1 kHz. Solution The given path points must be modi ed to ful ll the require- 3 seconds at the second via point. t12 = 3. Since the robot must stay immobile for a repeated via point of the second one.1. point cation is at the fourth point where the robot is required to pass through exactly. t340 = 1. a3 = 20. . H = 700. Finally. t220 = 2. points 4 and 00 4 . 0 A strategy is to replace this via point with two pseudo via points. 20 . The time used for each blending is and acceleration as well. of which their values are determined from the constraint on through speed. However. the robot must pass through the fourth point precisely with the speed of 30 mm/s. for which the new points and 4-5.10 display the plot of the conceptual path points and the generated trajectory versus time of this problem. the motion to the goal point during the last segment should be completed 1 second. t20 3 = 3. t20 = 1 t3 = 1. t5 = 1. Dimensions of the PUMA arm are such that a2 = 431. . t40 400 = 3.1.6. Another modi - ments. d3 = 150. Investigate the velocity in 5 seconds. Speci cally. the modi ed path points might be designed Chulalongkorn University Phongsaen PITAKWATCHARA . . t6 = 1. t56 = 3. seconds.5. Accordingly. Then it is required to stop at the second via for Time spent during each next three segments should be 4 3 seconds. t2 = 1. 40 and 400 are assigned at the half time between point 3-4 respectively. Figure 6. 0 −1 0 . t400 5 = 1.3 Cartesian Space Schemes 117 Corresponding orientations are speci ed by the rotation matrix of in {E} described {B}: 0 0 1 R1 = 0 −1 0 1 0 0 1 0 0 R3 = 0 −1 0 0 0 −1 1 0 0 R5 = 0 0 1 0 −1 0 √ √ 1/ 2 0 1/ 2 0√ R2 = 0√ −1 1/ 2 0 −1/ 2 1 0√ 0√ R4 = 0 −1/√2 1/ √2 0 −1/ 2 −1/ 2 0 0 1 R6 = −1 0 0 . .8. t400 = 1. Chulalongkorn University one get the blending acceleration of the Phongsaen PITAKWATCHARA .2. for the rst segment 1-2. They are selected to yield the minimum amount of rotational motion.10: Conceptual path points and the trajectory vs.2.7 . 0 0 θkˆ = π θkˆ = π θkˆ = π 0√ . Other choices of points 500 p¯20 = 500 . Apin section 6.9239 1/ 2 .3827 1/ 2 1 −1 −1 0 . θ t1 (td12 − 12 t1 ) plying this to the 6-tuple representation. 600 400 p¯400 = 540. In particular.3827 0. θkˆ = − 5π θkˆ = − 5π 4 4 3 40 400 0 0 0 √ −1/√ 3 −1 ˆ = − 4π 1/ 3 .6. 600 400 p¯5 = 600 . 520 500 p¯2 = 500 . 2 20 1 0.3 Cartesian Space Schemes 118 Figure 6. θ k θkˆ = − 3π 2 3 √ 5 6 0 −1/ 3 With these speci cations. 500 400 p¯40 = 459. 0 . time of Ex.9239 0. 0 θkˆ = π 0 . 480 for the position. 500 p¯1 = 500 . the equivalent angle/axis representation is used to generate the trajectory. computation of the parameters of the linear segment parabolic blends trajectory may be performed with the same formulas developed θ2 −θ1 ¨1 = .3. 6. as follow.3. √ 0. 400 40 and 400 400 p¯3 = 500 600 500 p¯6 = 500 500 may be used. For the orienta- tion.3 . 222 0 0 600−500 1(5−0.5×1) √ .3 Cartesian Space Schemes 119 rst via point p¨¯1 = and 0 = mm/s2 . 0 22.6. 0.9239−1/ 2)π ( d2 θkˆ . . 1(5−0.5×1) . = 0 √ dt2 . . 0.2265 1(5−0.3827−1/ 2)π ( 1 0.5×1) 1-2 Linear velocity of the segment and . 0 −0.1513 = rad/s2 . ˆ d θk . . . dt . . 0 22. is needed. To calculate the acceleration at point θ −θ θ˙220 = 2t 0 0 2 . = 12 0 0 p¯˙12 = is then calculated from 600−500 5−0.2265 5−0. td12 − 12 t1 0 = mm/s.222 √ (0.3827−1/ 2)π −0. d22 the linear velocity of the segment .5×1 θ2 −θ1 .9239−1/ 2)π 0. Hence. 0 √ 0 (0.1513 = rad/s.5×1 2.5×1 5−0. d θkˆ . . . dt . . 222 1 . θ t2 p¨¯2 = and 0 = mm/s2 . p¯˙220 θ˙12 = 0 = 0 mm/s. 0 −22. robot. Consequently.222 0 0 0−22. 220 0 = 0 rad/s 0 as would be expected since the original point 2 is repeated and this pauses the ¨2 = θ˙220 −θ˙12 may be evaluated. 0 2-20 . d2 θkˆ . . . = dt2 . . t 0 20 .2265) 1 −0.1513 = rad/s2 . 2 0−0. Hence. to calculate the acceleration at point θ3 −θ20 0 segment 2 -3. 0 Phongsaen PITAKWATCHARA .2265 Similarly. 0 0.1513 1 0 0−(−0. the linear velocity of the d2 3 p¯˙20 3 = Chulalongkorn University 400−500 4 0 0 −25 = 0 mm/s. is needed. θ˙20 3 = . 6. .3 Cartesian Space Schemes and Consequently. ˆ d θk . . . dt . . 3006 0 −0. 0 −0. 120 (1−0.9239)π 4 = 0.0598 = rad/s. 0 0 and . t20 θ¨20 = −25 1 −25 p¨¯20 = 0 = 0 mm/s2 .3827π 4 20 3 θ˙20 3 −θ˙220 may be evaluated. ˆ d θk . . . = dt2 . . segment 3-4 .3006 1 2 0. . 0 −0. 0 2 0. is needed.7−500 2 520−600 2 p¯˙340 = and Consequently. θ˙340 = 4 t 0 3. Hence. the linear velocity of the d34 459.0598 1 0 −0. to calculate the acceleration at point θ 0 −θ3 0 .3006 Similarly.0598 = rad/s2 . ˆ d θk . . . dt . . θ¨3 = = 0.3927 = rad/s.15 1 −40 1 . t3 p¨¯3 = 0−(−25) 1 −20. 0 0 0 0 θ˙340 −θ˙20 3 may be evaluated. ˆ d θk . . . = dt2 . . 7 4 480−520 4 0 = 20.15 mm/s. 2 0 = −20. θ˙40 400 = 4 .3006 Similarly.15 mm/s2 .3927−0.3−459.3006) 1 0. 0 0. −40 0. is needed. the linear velocity of the d4 4 p¯˙40 400 = Chulalongkorn University 0 540. to calculate the acceleration at point θ 00 −θ40 0 00 segment 4 -4 . −10 Phongsaen PITAKWATCHARA .3329 = rad/s2 . −40 (5/4−1)π 2 340 and 0 3 25 = −20. t 0 00 40 . Hence.0598 1 0 −(−0.15 mm/s. 121 .3 Cartesian Space Schemes and Consequently.6. ˆ d θk . . . dt . . 0 0 . 40 400 θ˙40 400 −θ˙340 may be evaluated.15) 1 −10−(−40) 1 p¨¯40 = and 0 0 = 0 = 0 rad/s.15−(−20. t40 θ¨40 = 0 20. d2 θkˆ . . . = dt2 . . 30 −0.3 mm/s2 .3927 1 0 0 4 0 = 40. Hence. 0 0 Similarly.3927 = rad/s2 . . segment 4 -5. θ˙400 5 = td400 5 p¯˙400 5 = and Consequently. 0 0−0. is needed. to calculate the acceleration at point θ5 −θ400 00 . d θkˆ . . . dt . . θ¨400 = = the linear velocity of the 0 = 29.3927 = rad/s. 0 0 θ˙400 5 −θ˙40 400 may be evaluated. −40 (3/2−5/4)π 2 0 0 400 5 0.85 mm/s. t400 p¨¯400 = . ˆ d θk . . . dt2 . . 1 d56 − 2 t6 5.3 2 400−480 2 and 0 400 . the linear velocity of the segment p¯˙56 = Chulalongkorn University 500−400 5−0.7 mm/s2 .222 Phongsaen PITAKWATCHARA .5×1 22.222 mm/s.5×1 500−600 5−0. −30 0. to calculate the acceleration at point 5 5-6. is needed.3927 = rad/s2 .222 = −22. θ˙56 = t θ6 −θ .5×1 500−400 5−0.85−20. 0 0 Similarly.3927 1 = 400 0 0 0 = 9. 2 600−540. Hence. 0 29.15 1 −40−(−10) 1 0. 22. 3 Cartesian Space Schemes and .6. d θkˆ . . . dt . . 85 1 22. θ¨5 = = 56 √ 3−3/2)π 5−0.5374 rad/s.5×1 (4/3 −0.5×1 √ 4π/3 3 5−0.222 1 −22. 0. t5 p¨¯5 = and 122 22.5×1 √ −4π/3 3 5−0.5098 = −0. Consequently.5374 θ˙56 −θ˙400 5 may be evaluated.222−(−40) 1 .222−29. d θkˆ . . . = dt2 . . the acceleration at point 6 −0. ¨6 = − θ6 −θ51 . 500−400 −22.5×1) and √ .5374 is determined from matching the velocity of the linear segment and the initial velocity entering the last blend. 62.5×1) −22.222 mm/s . with θ t6 (td56 − 2 t6 ) That is.5098−0. 500−400 − 1(5−0.5×1) = 22.222 500−600 2 p¨¯6 = − 1(5−0.9025 = −0. 0.222 − 1(5−0.5374 1 Finally.222 −0.5374 rad/s2 .222 = −52. 2 5 22.072 mm/s2 .5374 1 0.3927 1 −0. (4/3 3−3/2)π − . 1(5−0.5×1) d θkˆ . √ . 5×1) dt2 . = − −4π/3 3 1(5−0. . −0.5×1) 2 0. the trajectory generated by segments of the linear function with parabolic blends for each element of the end eector Chulalongkorn University 6-tuples may Phongsaen PITAKWATCHARA .5374 With these parameters.5098 = 0.5374 rad/s2 . √ 4π/3 3 6 − 1(5−0. 5 < t ≤ 14.5 < t ≤ 12.222 (t − 20) 400 + 22.5 5.5 < t ≤ 7.5 18.5 19.5 < t ≤ 11.5 < t ≤ 18.5)2 400 400 400 400 400 400 + 11.5 < t ≤ 19.5 < t ≤ 5.5)2 500 − 20.5 13.5 (t − 7.111 (t − 24)2 500 500 500 500 500 500 500 − 10.5 19.5 5.5 < t ≤ 24 24 < t ≤ 25.5 11.5 < t ≤ 5.5 18.3 + 29.3 Cartesian Space Schemes 123 be written explicitly as follow.5 13.5 < t ≤ 24 24 < t ≤ 25.5 < t ≤ 17.5 < t ≤ 13.5)2 400 + 22.5 17.5 17.5)2 500 − 25 (t − 8) 500 − 25 (t − 8) + 12.5 < t ≤ 12.15 (t − 14) 459.5 14.5 < t ≤ 13.15 (t − 12) + 20.5 < t ≤ 20.111 (t − 24)2 Chulalongkorn University 0≤t≤1 1 < t ≤ 4.5 < t ≤ 20.5 < t ≤ 18.5 (t − 11.5 8.7 + 20. 0≤t≤1 1 < t ≤ 4.15 (t − 12) 500 − 20.5 < t ≤ 7.5 12.5 20.5 7.5 < t ≤ 11.222 (t − 20) + 11.3 + 29.5 < t ≤ 14. px (t) = py (t) = 500 500 500 500 500 − 12.5)2 600 − 22.85 (t − 17.036 (t − 19.5 4.85 (t − 18) 540.5 < t ≤ 8.5 11.111 (t − 19.5 < t ≤ 8.5)2 540.7 + 20.85 (t − 18) − 26.222 (t − 20) 600 − 22.5 7.5 < t ≤ 19.5)2 459.5 14. Phongsaen PITAKWATCHARA .075 (t − 11.5 12.5 8.5 < t ≤ 17.5 20.5 4.222 (t − 20) − 11.15 (t − 14) + 4.15 (t − 13.6. 5)2 5π 4 5π 4 5π 4 5π 4 3π 2 3π 2 + 0.5 17.19635 (t − 13.5 12.222 (t − 0.5)2 0.5)2 + 0.5 < t ≤ 5.111 (t − 4.5)2 π + 0.0598 (t − 8) + 0.5 13.5 < t ≤ 11.5)2 400 + 22.5)2 600 − 40 (t − 12) 600 − 40 (t − 12) + 15 (t − 13.3927 (t − 18) − 0.0598 (t − 8) 0.5 18.1513 (t − 0.5 < t ≤ 24 24 < t ≤ 25.5)2 600 600 600 600 − 20 (t − 11. + 0.5 14.5 5.3927 (t − 12) π + 0.5 13.5 14.5 < t ≤ 8.5 < t ≤ 11. Phongsaen PITAKWATCHARA .5 17.1513 (t − 0.5 19.5 < t ≤ 5.222 (t − 20) 400 + 22.5) − 0.5) 500 + 22.9239π + 0.111 (t − 19.5)2 0.5 < t ≤ 20.5 < t ≤ 13.07565t2 + 0.5 4.5) + 0.5 < t ≤ 14.5)2 520 − 10 (t − 14) 520 − 10 (t − 14) − 15 (t − 17.5 20.5 4.5 < t ≤ 18.111t2 500 + 22.5098 (t − 20) + 0.5)2 480 − 40 (t − 18) 480 − 40 (t − 18) + 31.5 11.222 (t − 20) − 11.9239π + 0.5098 (t − 20) − 0.5 5.5 7.5 < t ≤ 7.45125 (t − 19.5) − 11.6.07565 (t − 4.16645 (t − 11.111 (t − 24)2 π √ 2 π √ 2 π √ 2 0≤t≤1 1 < t ≤ 4.5 < t ≤ 12.5 11.5 < t ≤ 18.5 20.9239π + 0.5 < t ≤ 19.5 < t ≤ 20.0299 (t − 7.2549 (t − 24)2 Chulalongkorn University 0≤t≤1 1 < t ≤ 4.3927 (t − 18) + 0.5 < t ≤ 12.5 < t ≤ 7.19635 (t − 17.5 19.5 < t ≤ 8.9239π 0.5 12.3 Cartesian Space Schemes pz (t) = θkx (t) = 124 500 + 11.5 < t ≤ 13.5 < t ≤ 17.222 (t − 0.5)2 − 0.5 < t ≤ 14.5 8.5 < t ≤ 24 24 < t ≤ 25.5 7.5 < t ≤ 19.5 8.3927 (t − 12) − 0.5 < t ≤ 17.5 18. 5 < t ≤ 17.5 < t ≤ 24 24 < t ≤ 25.5 18.2265 (t − 0.5 < t ≤ 19.5 7.5374 (t − 20) + 0.5 < t ≤ 14.5 5. the equivalent homogeneous transformation matrix B B ¯ representation.3006 (t − 8) + 0.5 12.5 < t ≤ 8.3827π 0.5)2 0. − 0.5)2 0 0 0 0 0 0.5 14.5 < t ≤ 11.5)2 −0. of XE is computed.2687 (t − 24)2 π √ 2 π √ 2 π √ 2 0≤t≤1 1 < t ≤ 4.5374 (t − 20) − 0.5 < t ≤ 19.5 11.5 < t ≤ 14.5 < t ≤ 11.5 7.3006 (t − 8) 0.1503 (t − 11.5 < t ≤ 5.6.5)2 0.5 4.5 17. 560 inverse kinematics closed form solution of Ex.5 < t ≤ 24 24 < t ≤ 25.5 14.5 < t ≤ 13.4.5 8.5 < t ≤ 12.3827π − 0.5 5.3827π − 0.5 < t ≤ 20. the branch of Chulalongkorn University Phongsaen PITAKWATCHARA .5 < t ≤ 7.5) − 0.5 < t ≤ 20.3827π − 0.5) + 0.5 < t ≤ 7.5 8.5 19. 4.3 Cartesian Space Schemes θky (t) = θkz (t) = 125 0 0 0 0 0 0 0 0 0 0 0 0 −0.5 13.5 < t ≤ 5.5374 (t − 20) 0.5 < t ≤ 8.5 < t ≤ 17.5 < t ≤ 12.2687 (t − 19. Then the corresponding joint angles may be determined by substituting the matrix element values into the PUMA of 1 kHz.11325 (t − 4.11325t2 − 0.5 11.5 4.5 < t ≤ 18.5 17.1503 (t − 7.5 20.5 13.5374 (t − 20) −0.5 19.5 < t ≤ 18.5 < t ≤ 13. These results of the end eector trajectory functions are evaluated at a rate At a speci c time t.2687 (t − 24)2 0≤t≤1 1 < t ≤ 4.2687 (t − 19.5 18.5)2 0.5 12.2265 (t − 0. E T .5 20. Speci cally. d3 . Plots of the trajectory.3 Cartesian Space Schemes 126 the solution where θ1 = atan2 (− (px − er13 ) . The nonlinear kinematic mapping is a smooth mapping and so should not be blamed for. θ6 = atan2 (r12 c1 s23 + r22 s1 s23 + r32 c23 .16. 6. (a2 c3 + a3 ) [(px − er13 ) c1 + (py − er23 ) s1 ] + (a2 s3 − d4 ) (pz − H − er33 )) . Another point is there are large impulsive accelerations.6. Chulalongkorn University Phongsaen PITAKWATCHARA . d3 ) − atan2 a23 + d24 − K 2 .11-6. − (r13 c1 c23 + r23 s1 c23 − r33 s23 )) . +atan2 + r23 p2x + p2y − d23 + e2 (r13 (px − er13 )2 + (py − er23 )2 + (pz − H − er33 )2 − a22 − a23 − d23 − d24 K = . θ2 = θ23 − θ3 . − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) . velocity. This is because the nonlinear inverse kinematic mapping has distorted the path shape. The velocity and acceleration are computed numerically by the backward Euler dierentiation formula. 2a2 q θ3 = atan2 (−d4 . and acceleration of the joint angles are shown in Figs. θ4 = atan2 (−r13 s1 + r23 c1 . It should be mentioned that the computed joint space trajectory does not have the shape of the linear function B ¯ with parabolic blends as planned in the task space through the elements of X E. − (r11 c1 s23 + r21 s1 s23 + r31 c23 )) . It is caused by the signi cant digit rounding o of the adjacent trajectory functions at the task space level. K . py − er23 ) q 2 2 ) − 2e (px r13 + py r23 ). q 2 θ5 = atan2 1 − (r13 c1 s23 + r23 s1 s23 + r33 c23 ) . is selected. θ23 = atan2 (− (a2 c3 + a3 ) (pz − H − er33 ) + (a2 s3 − d4 ) [(px − er13 ) c1 + (py − er23 ) s1 ] . Figure 6. Chulalongkorn University Phongsaen PITAKWATCHARA .3 Cartesian Space Schemes Figure 6. time spent during the segments. The `o' marks indicate the segment transition point. and the blending time.11: Joint 127 θ1 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points.12: Joint θ2 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points.6. time spent during the segments. and the blending time. 6.3 Cartesian Space Schemes Figure 6. and the blending time. time spent during the segments. Chulalongkorn University Phongsaen PITAKWATCHARA . and the blending time.14: Joint θ4 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points. time spent during the segments. Figure 6.13: Joint 128 θ3 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points. 3 Cartesian Space Schemes Figure 6.6. and the blending time.15: Joint 129 θ5 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points. time spent during the segments. time spent during the segments.16: Joint θ6 trajectory and its velocity and acceleration pro le generated by segments of the linear function with parabolic blend in the task space for the speci ed path points. and the blending time. Chulalongkorn University Phongsaen PITAKWATCHARA . Figure 6. For this joint. θ3 = 40◦ .3 Cartesian Space Schemes 130 Problems 1. 2. and acceleration of θ . and t3 for a two-segment LSPB. and acceleration by stacking them vertically.6. Indicate signi cant points in each graph. 40◦ to 130◦ . Time spent for each blend is chosen to be 1 second. θ1 = 5◦ . Chulalongkorn University Phongsaen PITAKWATCHARA . θ˙23 . θ2 = 15◦ . Determine the equations of the joint angle for every interval. rest to rest. Design the trajectory using the LSPB scheme. Sketch plots of position. velocity. Calculate θ˙12 . Assume that td12 = td23 = 1 second and that ◦ 2 the default acceleration to use during blends is 80 /second . A single revolute joint robot is to be rotated from in 6 seconds. t2 . velocity. t1 . Sketch the pro les of the joint angle. Chapter 7 Manipulator-Mechanism Design Chulalongkorn University Phongsaen PITAKWATCHARA . 132 blah Chulalongkorn University Phongsaen PITAKWATCHARA . Chapter 8 Introduction to Hybrid Force-Position Control Chulalongkorn University Phongsaen PITAKWATCHARA . 134 blah Chulalongkorn University Phongsaen PITAKWATCHARA . Appendix: Robotics Toolbox Chulalongkorn University Phongsaen PITAKWATCHARA . 136 blah balh Chulalongkorn University Phongsaen PITAKWATCHARA . Robotics: Modelling. Denavit and R. Pearson Prentice Hall. Bangkok. New Jersy. Third edition. L. [5] B. 215-221. MIT Press. New York. Vol. Vidyasagar. ASME Journal of Applied Mechanics. Pitakwatchara. Chulalongkorn University Phongsaen PITAKWATCHARA . 22. I.Bibliography Fundamentals of Robotics: Mechanics of the Serial Manipulators. S. and Control. Hutchinson. [4] J. Paul. [6] P. L. Craig. 3. pp. 2013. Villani. S. J. IEEE Robotics and Au- [7] J. Robot Manipulators: Mathematics. Hartenberg. Spong. No. [2] R. Cambridge. and M. W. Robot Modeling and Con- Introduction to Robotics: Mechanics and Control. London. tomation Magazine. P. 2009. 2006. and G. Vol. Siciliano. Springer-Verlag. 1996. 24-32. 1955. 1. A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices. trol. 2005. 1982. pp. Oriolo. Corke. (in Thai) [1] P. John Wiley & Sons. Sciavicco. [3] M. Planning. Programming and Control. A Robotic Toolbox for MatLAB ®. Chulalongkorn University Press.