UFDC Home  Search all Groups  UF Institutional Repository  UF Institutional Repository  UF Theses & Dissertations  Vendor Digitized Files   Help 
Material Information
Subjects
Notes
Record Information

Full Text 
/ REPEATABLE JOINT DISPLACEMENT GENERATION FOR SERIAL REDUNDANT ROBOTIC SYSTEMS BY YONG SOEB CHUNG A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 1992 ACKNOWLEDGEMENTS The author wishes to express his sincere appreciation to his committee chairman, Dr. Joseph Duffy, for his encouragement and guidance. He is also grateful to the committee members, Dr. Carl D. Crane, Dr. Gary K. Matthew, Dr. Ralph G. Selfridge, Dr. Neil L. White, who showed great interest. He also would like to thank his fellow students in the Center for Intelligent Machines and Robotics (CIMAR) for sharing their knowledge. Also special thanks go to Dr. Michael W. Griffis, postdoctoral associate, for his guidance. Finally, the author extends his deepest appreciation to his family for their financial and moral support. TABLE OF CONTENTS Page ACKNOWLEDGEMENTS..................................... ii ABSTRACT ............................................... v CHAPTERS 1 INTRODUCTION ....... ......... ........................ 1 2 ANALYSIS OF SERIAL REDUNDANT MANIPULATORS ........ 4 2.1 Introduction to Plicker Line Coordinate ..................... 4 2.2 Development of the New Kinematic Control Strategy ........... 7 2.3 Relationship of New Control Strategy to Previous Control Strategies .. 15 2.4 Discussion of Repeatability Based Upon the Lie Bracket Condition .. 17 3 REPEATABILITY OF PLANAR SERIAL REDUNDANT MANIPULATORS ........................................ 22 3.1 Planar 3R Manipulator ................................. 22 3.1.1 Derivation of Inverse Kinematic Equation ................. 22 3.1.2 Numerical Verification and Simulation of Repeatability ....... 30 3.2 Planar PRR Manipulator ................................ 41 3.2.1 Derivation of Inverse Kinematic Equation ................. 41 3.2.2 Numerical Verification and Simulation of Repeatability ....... 41 3.3 Planar 4R Manipulator ................................. 48 3.3.1 Derivation of Inverse Kinematic Equation ................. 48 3.3.2 Numerical Verification and Simulation of Repeatability ....... 51 3.3.3 Implementation ............................... ..... 52 4 REPEATABILITY OF A SPATIAL 7R MANIPULATOR .......... 57 4.1 Notation ............................................ 57 4.2 Mechanism Dimensions of 7R Manipulator ................... 57 4.3 Specification of Position and Orientation .................... 61 4.4 Derivation of Forward Kinematic Equation .................. 64 4.5 Derivation of Jacobian Matrix ............................. 68 4.6 Derivation of Inverse Kinematic Equation .................... 72 4.7 Numerical Verification and Simulation of Repeatability .......... 75 5 CONCLUSION .......................................... 80 APPENDICES A DERIVATION OF INVERSE KINEMATIC EQUATION OF PRR MANIPULATOR .................................. 81 B DERIVATION OF INVERSE KINEMATIC EQUATION OF 4R MANIPULATOR .................................... 87 C DERIVATION OF FORWARD KINEMATIC EQUATION OF 7R MANIPULATOR (SSRMS) ............................ 94 D DERIVATION OF JACOBIAN MATRIX OF 7R MANIPULATOR .................................... 100 E DERIVATION OF DERIVATIVE JACOBIAN MATRIX OF 7R MANIPULATOR .................................... 111 REFERENCES ............................................. 123 SUPPLEMENTARY BIBLIOGRAPHY ........................... 125 BIOGRAPHICAL SKETCH .................................... 129 Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy REPEATABLE JOINT DISPLACEMENT GENERATION FOR SERIAL REDUNDANT ROBOTIC SYSTEMS By Yong Soeb Chung December 1992 Chairman: Dr. Joseph Duffy Major Department: Mechanical Engineering This thesis presents a novel, practical, and theoretically sound kinematic control strategy for serial redundant manipulators. This strategy yields repeatability in the joint space of a serial redundant manipulator whose end effector undergoes some general cyclic type motion. This is accomplished by deriving a new inverse kinematic equation that is based on springs being theoretically or conceptually located in the joints of the manipulator (torsional springs for revolute joints, translational springs for prismatic joints). Previous researchers have also derived an inverse kinematic equation for serial redundant manipulators. However, to the author's knowledge, the new strategy is the first to include the free angles of torsional springs and the free lengths of translational springs. This is important because it ensures the repeatability in the joint space of a serial redundant manipulator whose end effector undergoes a cyclic type motion. Numerical verification for repeatability is done in terms of Lie bracket condition. Choices for the free angle and torsional stiffness of a joint (or the free length and translational stiffness) are made based upon the mechanical limits of the joint. For instance, the free angle of a joint is that angle which is midway between joint limits. Joint stiffnesses are chosen so that the most dexterous joint is the most pliable, and so that the least dexterous joint is the stiffest. This strategy helps to keep the joints of the manipulator away from their respective joint limits. CHAPTER 1 INTRODUCTION There is an increased interest in using manipulators with seven or more degrees of freedom because of the dexterity limitation of current six degree of freedom manipulators. For example, the limited ranges of motion of the joints and torque limits on motors restrict the movement of a six degree of freedom manipulator. Because of these considerations, it is desirable for a general purpose robotic manipulator to have more than six degrees of freedom. Redundant manipulators have more degrees of freedom than is required for a task. For example, a seven degree of freedom manipulator is redundant for the task of positioning and orienting the end effector (three degrees of freedom are required for position and three are required for orientation). The extra degrees) of freedom of a redundant manipulator may be used to satisfy many supplementary criteria such as keeping the joint variables within their physical limitations, providing greater dexterity, placing the joint torques close to the midpoint of joint torque limits, and minimizing kinetic energy, singularity avoidance, obstacle avoidance, etc. In most work on serial redundant manipulators, the forward velocity equations are frequently written in a Jacobian form, and the MoorePenrose pseudoinverse of the Jacobian is used to give the minimum Euclidean norm solution [1]. The use of 2 this procedure gives a solution, subject to the condition of minimum norm, in cases where all the joints are either revolute or prismatic, or, indeed, for the case of robots where the joints are all of the same type. However, the use of the MoorePenrose pseudoinverse has much more serious consequences when the serial redundant manipulator consists of a mix of joint types. If there are both revolute and prismatic joints present, for example, minimizing the Euclidean norm lacks formal meaning, and the result so obtained is heavily dependent on the unit of length used to describe the robot structure. Indeed, it is also possible to arrive at other solutions by superposing other allowable motions of the manipulator that do not entail end effector motion of any kind in addition to the MoorePenrose pseudoinverse solution [2]. Whitney [3] applied the weighted pseudoinverse solution to the inverse kinematic problem for redundant manipulators. This differs from the MoorePenrose solution in that the sum of squares of actuator velocities is a weighted sum (a,68: + a26eo + ... + a, e,, where a1, a2, ..., a, are positive numbers chosen by the user). The MoorePenrose pseudoinverse control or the weighted pseudoinverse control may drive the robot into an undesired manipulator configuration. Using these techniques, the manipulator configurations themselves are not reproduced when an end effector performs a cyclic motion (the joint displacements are not cyclic). Several researchers have identified the repeatability problem of redundant manipulators [1,4,5]. This thesis describes a general pseudoinverse solution to the inverse kinematic problem for redundant manipulators. This solution is one that determines a suitable 3 set of actuator velocities (or increments) which minimize a general quadratic function: a,,6E + a,6e2 + ... + a682 + a26eo,62 + a2626e3 + ... + a(,1),~6 .e,. The coefficients aN, a, are not constants (they change as the manipulator moves). In order to specify a,, ai (or [A] from the matrix expression, 6ae [A] S6), this work bases its foundation on a pseudoelastic potential energy function. In addition to actuator stiffnesses (torsional or translational), this new function also includes free angles of torsional springs and/or free lengths of translational springs. This formulation yields a control strategy that facilitates repeatability in the joint space of a manipulator whose end effector undergoes a cyclic type motion. CHAPTER 2 ANALYSIS OF SERIAL REDUNDANT MANIPULATORS This chapter contains a theoretical development and discussion of the new control strategy. 2.1 Introduction to Pliicker Line Coordinate Because lines (screws) will be used in subsequent sections, a review of line coordinates is presented here. Two distinct points, 1(x,,y,,z,) and r(x2,y2,z2), determine a line as shown in Fig. 2 1. The vector S can be expressed in the form S = Li + Mj + Nk, (2.1) where L = x2x,, M = y,yl, N = z2z, (2.2) are defined as the direction ratios. The direction ratios (L,M,N) are related to the distance between the two points by L2 + M2 + N2 = IS2. (2.3) Let r represent a vector to any arbitrary point on the line. The vector (rrE) will be parallel to S and therefore the equation of the line may be written as (rr) x S = 0. (2.4) This equation can be expressed in the form 5 rx S = S, (2.5) where S= 1 x S, (2.6) is the moment of the line about the origin and is origin dependent. The vectors (S;S) which must satisfy the orthogonality condition SSo = 0, (2.7) are the Pliicker coordinates of the line [6,7]. (S;&) are homogeneous coordinates since from eq. (2.5) the coordinates (kS;k%) where k is a nonzero scalar determine the same line. The vectors (S;S,) may be written in terms of their components as (L,M,N;P,Q,R). 21: Development of Plucker Line Coordinates 6 A wrench on a screw will be represented as a general "ray" that is assigned an R6 vector w = [f: .] where f is a force vector in the direction of the wrench and m0 is a moment vector referenced to the origin. Ray coordinates for screws (direction vector first) are assigned lower case labels. This ordering of screw coordinates is based on Plicker's definition of a ray, which is a line formed from the join of two points. However, a twist on a screw will be represented using two different formats, one based on the ray, a = [Sr; 6S], and the other based on a general "axis" that is assigned an R6 vector, D = [6; SR]. Axis coordinates for screws (direction vector last) are assigned upper case labels. Plicker defined the axis to be dual to the ray, and axis is thus a line dually formed from the meet of two planes. For a twist, 61 and S6 both represent the direction of an infinitesimal rotation, and Sr and SR both represent an infinitesimal displacement of the origin. Therefore, the axis and ray representations of the same twist are related by [f 1 [03 13 ] ]R aJ oz I[ I' 6Sr 13 03 . which can be more conveniently written as d = [A]D, (2.8) where [A] is the symmetric 6 x 6 matrix above. Since [A]1' = [A], a reversal in the ordering of this same transformation yields D = [A]a. (2.9) The transformation described by eq. (2.8) and eq. (2.9) is an example of a more 7 general transformation, defined as a correlation, that maps an axis to a ray (or a ray to an axis). It is important to distinguish this transformation from a collineation, which maps a ray to a ray (or an axis to an axis). 2.2 Development of the New Kinematic Control Strategy We consider an n degree of freedom manipulator illustrated in Fig. (22), with joint coordinates e,, i = 1, 2,..., n, and a task described by m task coordinates rj ,j = 1, 2,..., m ( m 5 n ). Let the kinematic transformation from the joint space to the task space be given by r = f(e), (2.10) where 9 = [el, e2, e and I = [r,, r2, rJf are the joint and task coordinate vectors respectively. The superscript T denotes the transpose. Eq. (2.10) will be assumed to be continuous and differentiable up to the second order in the entire task space. Differentiating eq. (2.10) with respect to time yields = J(_)_ (2.11) where t = dr/dt E R" (mdimensional Euclidean space), e = do/dt e R", and J(9) = af(_)/3a e R" (the set of all m x n real matrices). The matrix J(a) is called the Jacobian. From eq. (2.11), we see that the Jacobian is simply a linear transformation that maps the joint velocity in R" into the task velocity in R". The end effector is required to undergo an infinitesimal twist relative to ground. Then an alternative expression of eq. (2.11) for the forward instantaneous motion can be written in the form 8 $5 k S $N k3 $21 Fig. 22 : Kinematic Model of nJointed Serial Manipulator A Bn D = z 6eiS,, (2.12) where D is the axis coordinates of the twist, line $i is assigned axis coordinates Si, and where S6e is an infinitesimal rotation of the i'h revolute axis. This can be more conveniently written as D = [J] e, (2.13) where [J] is the m x n Jacobian matrix whose column is Si. It is observed that [J] can be obtained by eq. (2.11) or eq. (2.13). While the objective of this work is to develop a meaningful algorithm that effects the incremental displacement control of the end effector of a serial redundant manipulator, it is now assumed that at. each revolute joint there exists a torsional spring which deflects due to the rotation of the joint. In general, the torsional spring can be thought of as deflected away from its free position angle, and it is thus initially loaded with a finite torque, 7, at any general configuration. Consequently, the stiffness elements can be considered at each instant to be loaded and not to be at their free positions. Then this system of springs would be in static equilibrium, with an appropriate external resultant wrench providing a load necessary to balance the finite torsional spring deflections. In an infinitesimal movement, the i'h torsional spring is considered to have linear characteristics, and ri in the i'1 spring torque is related to a joint rotation ei eoi by r, = k,(e9 e~), (2.14) where ei e0, is the difference between the current and the free position joint angle 10 and k, is the non zero i"h joint stiffness that has dimensions (FL). The eq. (2.14) may be more conveniently written as 8 0 = [k]^., (2.15) where [k] is an n x n diagonal matrix of joint stiffnesses. The joint stiffness, kl, can also be a desiredjointdisplacement stiffness, which is defined as how stiff a joint is desired to be while contributing to the generation of a known end effector twist. The larger k, will cause less joint rotation while the smaller ki will cause more joint rotation. Thus a proper choice of k, based on mechanical limits of each joint helps to avoid joint limitation during the specified path. The joint torques, acting in each joint, combine to give a resultant wrench (w = [f: mlo]) given in ray coordinates at the end effector, and this can be expressed for the it joint as ri = S,' w, (2.16) which can be more conveniently written in the form for all joints, r = [J]T w, (2.17) where r is the nvector of the joint torques. Combining the stiffness relationships in eq. (2.15) with eq. (2.17) gives the vector of joint motions a o = [k][J]T w. (2.18) Multiplying eq. (2.18) by [J] and rearranging eq. (2.18) yields S= ([J][k][J]T) [J] ( ). (2.19) This can be solved provided that [J] is of full rank. Kinematic singularities occur 11 when [J] loses rank. Such kinematic singularities are intrinsic to the structure of the mechanism. Taking the derivative eq. (2.18) yields 6s = [k]'[J]T 6w + [k]6J]T w, (2.20) The second term of the righthand side of eq. (2.20) goes to the lefthand side, 6 [k][6J]T w = [k]'J] 6w. (2.21) The second term on the lefthand side of eq. (2.21) contains 68 in [6J]T. In order to extract 6e from [SJ] the following will be accomplished. The ith component (i = 1, 2, ..., n) of the vector [6J]T w can be expressed in the form: ([6J] v), = E ( {(o2r,6.1)/(Baeae)}f, = r.6ei, (2.22) j=1 Q where ri = E {rrJ(aeiaej)}fi. In vector notation, eq. (2.22) becomes [6J]T w = [r]69. (2.23) Since, by hypothesis, eq. (2.10) is continuous and differentiable in the task space, it can be said that Fr,/(ae,aej) = Fr1/(aeae). From the above equation, it follows that [r] is an n x n symmetric matrix (ru = r,). Substituting eq. (2.23) into eq. (2.21) yields (I [k][r])68 = [k]l[J] 6w. (2.24) Letting [T = I [k][r], then eq. (2.24) becomes [T]68 = [k].[J] 6w, (2.25) where [T] is an n x n matrix that consists of the joint angles, e,, the free angles of torsional springs, o0i, an external wrench, w, and spring stiffnesses, ki, i = 1, ...,n. Eq. (2.25) can be solved provided that [T] is of full rank (otherwise an algorithmic singularity occurs). Thus, Se = [T]'[k][J]T 6w. (2.26) Letting [A] = [k][T], which is an n x n symmetric matrix and premultiplying the left and right sides of eq. (2.26) by [J] yields [J]69 = [J][A][J]T w. (2.27) Substituting eq. (2.13) into eq. (2.27) yields D = [J][A][J]T 6w, (2.28) which can be expressed in the form D = [C]6w, (2.29) where [C] = [J][A]I[J]T is an m x m symmetric compliance matrix that relates the end effector external wrench to its corresponding twist. Provided that [T] and [J] are both full rank, then 6w = [C]'D, (2.30) relates the necessary deflection twist of the end effector relative to ground to the incremental change in resultant wrench. 13 In the problem of the inverse kinematics of the serial redundant manipulator, it is necessary to determine the joint increments explicitly. The elastic compliance relation of eq. (2.28) can be used to accomplish this objective. Substituting eq. (2.30) into eq. (2.26) yields the general pseudoinverse equation: s = [A]1[J]T [J][A][J]T}1 = [A][J](C)1D, (2.31) Application of the general pseudoinverse solution depends upon the knowledge of [A]. Because [A] is manipulator configuration dependent, it must be calculated every time eq. (2.31) is to be solved. Eq. (2.31) can be also derived by using the Lagrangian multiplier method. THEOREM 2.1. If J is an m x n matrix (m < n) of rank m, then the solution of the equation D = [J]69 that minimizes 6ET[A]6e is given by 5s = [A]'T[J] I[A]1[J]r *TD. (2.32) Proof: We consider the method of Lagrange multipliers. The quantity D that minimizes 69T[A] 6 subject to D = [J]68 can be found by minimizing M = 6eT[A]6 + 21T(D [J]6_), where I is an m x 1 column vector of Lagrange multipliers, and the factor of 2 is inserted for convenience. Setting aM/a6 e = 0, j = 1,..., n, M/lal, = 0, i = 1,..., m, we find [A]69 = [J]'T or 6S = [A][J]f. (2.33) D = [J]6,. (2.34) Substituting eq. (2.33) into eq. (2.34), we obtain D = [J][A].'[J]T or A = ([J][A]'[JT)f),^, (2.35) Substituting eq. (2.35) for 1 into (2.33) gives (2.32). This solution, eq. (2.31), ensures the repeatability in the joint space of a serial redundant manipulator whose end effector undergoes a cyclic type motion. Taking the derivative of eq. (2.15) yields S6 = [k]1r. (2.36) A further extension of this analysis can be achieved by combining eq. (2.13) and eq. (2.36) with eq. (2.30), to obtain 6w = [C]1 [J][k]1 6., (2.37) which is the forward static relationship for the serial redundant manipulator. Eq. (2.37) describes an onto mapping of joint torque increments into wrench increments. From the above theorem 2.1, the general pseudoinverse equation minimizes 6eT[k][T]Se. It is important to recognize that potential energy is not being minimized, i.e., pe = z 0.5ki(ei eoi)2. Furthermore, it will be now demonstrated that i1 the second derivative of potential energy, which is denoted by S'pe, is also not being minimized. Now the relationship between 6eT[k][T]Se and the second derivative potential energy function will be derived. The second derivative potential energy can be written in the form 62pe = 6(fT) ), = 6DTw + D 6w (2.38) Premultiplying the left and right sides of eq. (2.25) by 6ST[k] yields 6eT[k][T]Se = 69a[J]T6V = D' 6w (2.39) Substituting eq. (2.39) into eq. (2.38) and rearranging eq. (2.38) yields 6a'[k][T]_E = 6pe 6DT ^ (2.40) Because potential energy is not being minimized, the use of the control strategy does not require at any time the robot to be in a minimum (or maximum) potential energy state, i.e., where 6pe = 0. In fact the robot whose end effector undergoes some general cyclictype motion can begin in different initial configurations (different initial joint angles). A theoretical proof of repeatability is given (section 2.4) following a brief discussion of other control strategies. 2.3 Relationship of New Control Strategy to Previous Control Strategies In this section, it is assumed that the manipulator system is initially stationary and unloaded. Consequently, the stiffness elements can be considered at each instant to be unloaded or at their free positions, i.e., e, = 60 and from eq. (2.19) w = 0. Therefore from eq. (2.20) e = [k]J] jw. (2.41) Premultiplying the left and right sides of eq. (2.41) by [J] yields [J]68 = [J][k][Jf 6w. (2.42) Substituting eq. (2.13) into eq. (2.42) yields D = [J][k][J] r aw, (2.43) which can be expressed in the form D = [C]6w, (2.44) where [C] = [J][k][J] is an m x m symmetric compliance matrix that relates the end effector external wrench to its corresponding twist. Provided that [J] is of full rank, thus 6w = [C]f), (2.45) relates the necessary deflection twist of the end effector relative to ground to the incremental change in the resultant wrench. In the problem of the inverse kinematics of the serial redundant manipulator, it is necessary to determine the joint increments explicitly. The elastic compliance relation of eq. (2.41) can be used to accomplish this objective. Substituting eq. (2.45) into eq. (2.41) yields the weighted pseudoinverse equation: 6_ = [k]l[J]T([J][k][J]T)l), = [k][J]T(C)'D. (2.46) The result, which is the weighted pseudoinverse of [J], represents a minimization of the second derivative of elastic potential energy function 62pe: 62pe = 6Se [k] Se. (2.47) The work here agrees with the established work of Whitney [3]. It is recognized that eq. (2.46) does not ensure repeatability of cyclic motion of the end effector. The MoorePenrose pseudoinverse solution can also be obtained from the eq. (2.31) by disregarding the free angles and the joint stiffnesses. Thus, both [k] and [T] are identity matrices and from eq. (2.31), e = [J]T([J][J]T)D. (2.48) Eq. (2.48) is the same as that of Noble [8]. The result, which is the MoorePenrose pseudoinverse of [J], represents a minimization of eT6'6. It is very important to note that eq. (2.48) does not ensure repeatability of the cyclic motion of the end effector. Furthermore it is not invariant with change of unit of length if there are both revolute and prismatic joints present. 2.4 Discussion of Repeatability Based Upon the Lie Bracket Condition Shamir and Yomdin [5] introduce a repeatability condition of redundant manipulators. In order to explain the repeatability condition, they introduce 7 definitions and 4 theorems. In this section the definitions and the theorems are discussed briefly. DEFINITION 2.1: An n x m matrix function P = P(9) such that [J][P] = I, will be called a local control strategy. DEFINITION 2.2: Let x = x(t)(t E [0,1]) be a continuous curve in the task space. If a = a(t) is a continuous curve in the joint space such that f(a(t)) = x(t), then a is called a lifting of x. DEFINITION 2.3: Let r0 be a point in the task space W. The set f(r0) = {e f(4) = r0} will be called the fiber over r0. DEFINITION 2.4: A control strategy P is repeatable over U (open subset of W) if for any closed path x in U, every lifting of x determined by P is a closed path a in f'(U), and a is never tangent to any fiber. DEFINITION 2.5: A control P is repeatable at pe c if for any closed path x in U that passed through f(op), the lifting determined by P and passing through p, is a closed curve in c. DEFINITION 2.6 [9, p. 88;10, p. 123]: An mdimensional distribution D on a set A in R" is a law that assigns to each point e A, a unique linear mdimensional subspace D,(of R"), with origin at e. It is sometimes also called an mvector field [11, p. 145]. DEFINITION 2.7: An integral surface S for an mdimensional distribution D is a smooth mdimensional surface S c R", such that at each point e E S, the tangent space to S is exactly the linear mdimensional space D. assigned by the distribution. THEOREM 2.2: Let P be our control matrix, which we assume to be a smooth function in an open subset i of the joint space. Let U e f(t) be a simply_connected region of the task space. Then the control is repeatable for any closed paths lying in U, if and only if the following condition (LBC) holds. LBC: For any two columns of Pi and Pj of P, their Lie Bracket [P,, P,] is a linear combination of the columns of P. The Lie bracket of two vectors P, and Pj, that are both functions of e is the vector [Pi, Pj] = (aPj/ae)P, (aP/ae)P, where aP/~d and aP,/3e are the n x n matrix of partial derivatives. To determine whether this is a linear combination of the columns, consider the 19 extended n x (m+1) matrix (P, [Pi, Pj]) and check if its (m+1) x (m+1) minors are zero. THEOREM 2.3 [9,10]: Let D be an mdimensional distribution. Then there exists an integral surface for D, containing a point 9, if and only if D is involutive in an open neighborhood of e. THEOREM 2.4: Let D be a distribution associated with a local control strategy P. Let U be an open subset of W, and let op E o be a point such that f(ep) e U. Then P is repeatable at over U, if and only if there exists an integral surface S containing op, such that the restriction of f to S is a homeomorphism of S onto U. THEOREM 2.5: Let D be a distribution associated with the control strategy P, and let S be an integral surface for D, over a simply connected underlying set U c W. Then S intersects each fiber at no more than one point. It is well known from differential geometry (for example, [8]) that if S c R" is any smooth mdimensional surface, and u, v are any two tangent vectors to S at a point e, then their Lie bracket [u, v] is also a tangent vector to S at e. A distribution D is called involutive at if, for any two vectors u, v that belong to a subspace D,(u and v are both evaluated at e), their Lie bracket [u, v] also belongs to the subspace D,. From theorem 2.3 and theorem 2.4, it is clear why repeatability implies the Lie bracket condition (LBC) stated in theorem 2.2. If our control P is repeatable, then there exists an integral surface for the distribution. Such a surface exists if and only if LBC holds. 20 It is important to note that if the LBC holds at every point, then there is a foliation of integral surfaces and the control is repeatable for any initial configuration. If integral surfaces do exist for a given distribution, then they must either be disjoint or coincide, because at each point, the distribution assigns a unique tangent space. In other words, if the distribution is involutive at every point _e E , then we obtain a family of disjoint surfaces (called a foliation) which covers . From the above definition 2.1, an inverse kinematic equation can be expressed in the form 60 = P(e) . For example, the general pseudoinverse is given by P,(a) = ([k][T])'[J{ {[J]([k][T])'[J]T}', the weighted pseudoinverse is given by w(a) = [k][J]T([J][k]'[J]'), and the MoorePenrose pseudoinverse is given by P.p(e) = [J]T([J][J]'. If we consider one degree of redundancy (nm = 1), the Lie bracket condition (LBC) can be expressed in the form LBC = IP [P, P]I = 0. Suppose that m = 3 and n = 4, then LBC = IP[PI, P211 = IP[P,P 311 = IP[P3, P11 =0. The computations involving the Lie bracket condition are very difficult to do by hand, but they can be done with the use of Maple, Mathematica or another symbolic 6 21 software package. Even the simplest case (a planar 3R manipulator) requires a huge amount of memory in order to obtain a symbolic expression of the Lie bracket condition. In this dissertation a numerical verification for repeatability is done in terms of the Lie bracket condition. CHAPTER 3 REPEATABILITY OF PLANAR SERIAL REDUNDANT MANIPULATORS The proposed general pseudoinverse control in chapter 2 is applied to planar manipulators (3R, PRR, 4R). 3.1 Planar 3R Manipulator Consider a 3 degree of freedom manipulator which moves in the (x,y) plane. When only the position of the end effector is of concern, this manipulator is kinematically redundant. The kinematic model of a planar 3R manipulator is shown in Fig. 31. 3.1.1 Derivation of Inverse Kinematic Equation The coordinates for a point P (see Fig. 31) in the end effector are given as x = ac, + a23c1+2 + ac+2,3, (3.1) y = aus1 + a2,s+2 + a4rs12+3, where a2, a2, aM represent the length of each link, while the variables with subscript are defined as si = sin(e.), si+,j...+n = sin(,+ j+...+ ) c, = cos(e,), c,++...+ = cos(ei+ej+...+e.). Eq. (3.1) for an instantaneous motion can be expressed as P: Position(x, y) of end effector a : Orientation Fig. 31: Kinematic Model of a Planar 3R Manipulator 6x 61 e = [J] 62 (3.2) 6y 6 E3 The Jacobian matrix is obtained as a2s,a23s, 2a34s +2+3 a23s+22a34sl+2+3 a34S1+2+3 [J] = (3.3) L.a2c+ a23c1+2+ a34c1+2+3 a23c1+2 a34cl+2+3 a34 +2+3 The joint torques acting in each joint combine to give resultant forces at the end effector, and these can be expressed as r, = [J] T (3.4) Joint torques are related to joint rotations by r1 ]1 01o T2 = [k] e2 (3.5) T3 e 03 where [k] is a 3x3 diagonal matrix of joint stiffnesses as follows ki 0 0 [k] = k2 0 , O 0 k3 k, is the nonzero ith joint stiffness that has dimensions (FL) and ei o,i is the difference between the current and the free position joint angle, i = 1, 2, 3. Thus eq. (3.5) can be written in the form Substituting eq. [ e1 001 T1 2 o2 = [k]1 2 e3 03 3 (3.4) into eq. (3.6) yields ,0 80o 2 02 = [k][J]T 63 03 Premultiplying the left and right sides of eq. (3.7) by [J] yields e0 01 f [J] e2 0e2 = [J][k]'lJ].r 3 3 03 . Provided that [J] is of full rank, eq. (3.8) can be expressed in the form f_ e1 eoi ]= ([J][k]'[]T )1 [J] e2 02 3 60 Taking the derivative eq. (3.7) yields (3.6) (3.7) (3.8) (3.9) 681 [6f1 6e2 = [k][J]T + [k]'[6J (3.10) 6e3 LfJ [ where 6SJ S6Ju [SJf]= 6J2 6J22 (3.11) /L6J3 S6J32 se o 0 6e2 0 693 0 = [u] + [v] + [w] 0 692 .0 683 e. 6 9 . ull U12 [u = U21 u , U31 U32 V11 V12 [V] = V21 v22, [ V31 V32 J Wnl W12 [W] = W2 W2 , S= 1 32 ), sJnll = a,2c6819 a23c,+2(6e+ 692) a34cl+2+3(6e+ 602+ 6e3), 6J2 = a2s,6e, as+2(8+ 62) a34s,+2+3(698+ 682+ 63), J21 = a23c+2(6e1+ 682) ac,34+2(691+ 682+ 683), 6J22 = a23s+2(68,+ 682) a3s42+3(6 1+ 682+ 6e3), 6J31 = a34C,23(66'+ 682+ 683), 6J32 = a34s,2,3(61+ 6e2+ 6e3), ul= a22c ac1+2 a34c1+2+3, 12 = 23 a34Sl+2+3 U21 = a23C1+2 a34 1+2+3 22 = a231+2 a34S1+2+3, U31 = a34C+2+3, U32 = 34S1+2+3 VI = a23c+2 a34C1+2+3, V12 = 34s+2+3 V21= a231+2 a34c1+2+3, V22 = a34S1+2+3 V31 = a341+2+3, V32 = a34S+2+3, W1 a34c1+2+3,12 = a2s1 a23S1+2 a34Sl+2+3, W21= a341+2+3, W22 = a23S1+2 a34+2+3 31 = a34c+2+3, w32 = a34s1+2+3. The second term of the righthand side of eq. (3.9) goes to the lefthand side, 682 [k]l[J]T = [k][J]T (3.12) 683] f J f J Substituting eq. (3.9) into eq. (3.12) and rearranging the lefthand side of eq.(3.12) yields S 1, 6f, [T] S62 =[k]'[J ]T 68e3 l T1 T1 T13 [T] = T21 T2 T3 LT3 T32 T33 Tn = 1 ( unf + wfy )/kl, T, = ( wif, + vf, )/k,, T2 = 1 (V21f. + u22f )/k2, T31 = ( u31f + w,2f, )/k3, T33 = 1 ( W31f + V32L )/k3. [T] is of full rank, eq. (3.13) Tu = ( vjfx + uf, )/k,, T21 = ( uA21f + w2f )/k2, T = ( w21f + V22f )/k2, T32 = ( Vf. + U32f )/k3, can be expressed in the form S68e 6e2 .693 1 [A]6f[]T = [AJ] [ j] 6^ Letting [A] = [k][T], and premultiplying the left and right sides of eq. (3.14) by [J] yields where (3.13) Provided that (3.14) 6e 6f, [J] e:, = [J][A]`'[J]T Substituting eq. (3.2) into eq. (3.15) yields 6x sfx = [C] 6y sf where [C] = [J][A][J]T. Provided [T] and [k] are both of full rank, eq. (3.16) can be expressed in the form E 6f1 = [C]1 6x 6y (3.17) Substituting eq. (3.17) into eq. (3.14) yields a general pseudoinverse equation 801 682 683 Sez . = [A]'[J]T( [J][A][]T )' = [A]I[J][C]"1 6x 6y (3.18) 6x 6y (3.15) (3.16) 3.1.2 Numerical Verification and Simulation of Repeatability For the first example, the system consists of three revolute joints as shown in Fig. 31 and link lengths are chosen as a. = 1.0, a3 = 1.0, and a4 = 1.0 (m). The ranges of each joint limit are assumed as follows: 170" e9 < 180" 180" 5 e, < 170" 175" 5 03 < 175" Thus the stiffnesses for each joint are taken as being equal (ki = k2 = k3 = 1.0 Nm/radian) based on these joint limits. The free position angles of each joint are chosen as eo0 = 5", e2 = 5*, o03 = 0*. The xy coordinates of the four vertices of the command path (1000 consecutive equidistant segments each side) in the task space are successively given counterclockwise from the lower left vertex as follows: (0.5, 0.6) (0.6, 0.6) (0.5, 0.5) (0.6, 0.5) where the units are meters. The end effector is to move along a predefined square path with a constant speed of 10 mm/sec and a time step size At = 0.01 sec. Section 2.4 demonstrated theoretically the requirement of repeatability for the kinematic control strategy of the serial redundant manipulator. Succinctly, the joint angles repeat for a cyclic motion of end effector only when the Lie bracket condition holds for the kinematic control strategy. Section 2.4 also discussed how this repeatability is independent of the initial configuration (choice of initial joint angles). These concepts will now be validated numerically for the planar 3R manipulator. 31 Different initial configurations corresponding to the same end effector position (x = 0.5, y = 0.5) can be obtained by assigning the end effector orientation a (see Fig. 31). In the reverse analysis, two sets of solutions can be obtained for the given position and orientation of the end effector (see Duffy [12]). The numerical verification of the LBC is demonstrated by the orders of magnitude difference between the LBC columns in Table 31 and 32 (twenty degree increments for a are displayed. But the analysis was performed in one degree increment.). Table 31 shows that Lie bracket condition values are near zeroes (10'  10") for different initial configurations when the general pseudoinverse equation is used. On the other hand Table 32 shows that numerical Lie bracket condition values are not zeroes for different initial configurations when the MoorePenrose pseudoinverse equation is used. Three different initial configurations (see Fig. 32) for the same end effector position are chosen from Table 31. For example, first initial configuration at maximum potential energy (e1 = 155.7", e2 = 138.59", e3 = 65.7"), second initial configuration at intermediate potential energy (e, = 129.060, e2 = 146.01, e3 = 63.04') and third initial configuration at minimum potential energy (e1 = 37.33", e2 = 87.19", e3 = 110.13") are chosen. For these three different initial configurations, Lie bracket condition values during the 10 cycle motion are shown in Figs 33, 34, and 35. The initial and the final joint angles through 10 cycles of motion of the end effector are generated in Table 33. At the end of the 10 cycles the position difference (x, xtt, yfi yinitia.) and the joint angle difference ( Table 31 General PseudoInverse Equation a LBC 0 2.391285e07 0 8.037865e07 20 2.822548e09 20 1.134500e06 40 1.699821e07 40 1.845255e08 60 2.862487e07 60 3.383497e07 80 3.885419e07 80 2.043804e07 100 2.257927e07 100 2.339417e07 120 5.409503e08 120 1.207760e07 140 2.251315e07 140 6.572133e08 160 4.203282e07 160 4.425800e08 180 5.961847e07 180 4.398267e08 200 2.795830e06 200 3.656248e08 220 1.069061e05 220 1.577192e07 240 3.336771e07 240 1.612510e06 260 3.237957e08 260 5.514746e06 280 9.248140e09 280 5.093987e08 300 3.424381e08 300 5.399946e07 320 6.938001e08 320 4.092887e07 340 1.354084e07 340 1.913770e07 360 2.414215e07 360 8.035577e07 X 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 Y 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 TH1 155.7048 65.7048 123.2720 83.7465 70.4574 126.9086 10.5431 169.4516 16.9563 129.0618 29.7393 101.2229 37.7244 77.9331 43.9929 56.8635 49.8612 37.3383 56.1950 19.3265 63.8168 3.1748 73.5563 10.5839 85.9640 21.6226 100.9853 30.2064 118.1286 37.0779 136.9206 43.0783 157.1842 49.0250 179.2173 55.9277 155.7032 65.7036 TH2 138.5904 138.5904 152.9815 152.9815 162.6340 162.6340 158.9085 158.9085 146.0181 146.0181 130.9622 130.9622 115.6575 115.6575 100.8564 100.8564 87.1995 87.1995 75.5215 75.5215 66.9916 66.9916 62.9725 62.9725 64.3415 64.3415 70.7789 70.7789 81.0507 81.0507 93.8423 93.8423 108.1592 108.1592 123.2896 123.2896 138.5932 138.5932 TH3 65.7048 155.7048 63.7465 143.2720 86.9086 110.4574 130.5483 70.5432 150.9381 63.0437 158.7770 70.2608 162.0668 82.2757 163.1364 96.0072 162.6616 110.1389 160.6734 123.8051 156.8251 136.1833 150.5837 146.4438 141.6224 154.0361 130.2062 159.0148 117.0777 161.8716 103.0781 163.0796 89.0249 162.8160 75.9276 160.7829 65.7034 155.7034 PE 8.670344 6.307208 7.881616 6.648197 7.216641 7.038628 7.535189 8.601602 7.675252 5.969477 7.370122 4.511340 6.920488 3.602386 6.454121 3.080577 6.022204 2.876694 5.646737 2.935046 5.339623 3.187467 5.117468 3.547688 5.017191 3.933885 5.098199 4.306150 5.423747 4.670212 6.045742 5.047785 7.008238 5.453265 8.366526 5.883567 8.670371 6.307222 Table 32 MoorePenrose PseudoInverse Equation a LBC 0 1.664743e+00 0 3.449764e+00 20 2.359110e+00 20 1.748641e+00 40 8.067412e01 40 6.070027e01 60 9.022816e01 60 1.580794e+00 80 2.840095e+00 80 2.233481e+00 100 3.464188e+00 100 1.188109e+00 120 3.064482e + 00 120 7.010876e01 140 3.166571e+00 140 5.498406e01 160 4.294162e+00 160 5.814914e01 180 7.471548e+00 180 8.518371e01 200 1.197545e+01 200 1.796072e+00 220 8.342236e+00 220 5.095851e+00 240 2.963700e + 00 240 1.142484e+01 260 1.180420e + 00 260 9.990296e+00 280 6.738749e01 280 5.537862e+00 300 5.441282e01 300 3.557440e +00 320 5.985302e01 320 3.021057e + 00 340 8.830314e01 340 3.247780e + 00 360 1.664950e+00 360 3.449667e+00 X 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 Y 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 0.5000 TH1 155.7048 65.7048 123.2720 83.7465 70.4574 126.9086 10.5431 169.4516 16.9563 129.0618 29.7393 101.2229 37.7244 77.9331 43.9929 56.8635 49.8612 37.3383 56.1950 19.3265 63.8168 3.1748 73.5563 10.5839 85.9640 21.6226 100.9853 30.2064 118.1286 37.0779 136.9206 43.0783 157.1842 49.0250 179.2173 55.9277 155.7032 65.7036 TH2 138.5904 138.5904 152.9815 152.9815 162.6340 162.6340 158.9085 158.9085 146.0181 146.0181 130.9622 130.9622 115.6575 115.6575 100.8564 100.8564 87.1995 87.1995 75.5215 75.5215 66.9916 66.9916 62.9725 62.9725 64.3415 64.3415 70.7789 70.7789 81.0507 81.0507 93.8423 93.8423 108.1592 108.1592 123.2896 123.2896 138.5932 138.5932 TH3 65.7048 155.7048 63.7465 143.2720 86.9086 110.4574 130.5483 70.5432 150.9381 63.0437 158.7770 70.2608 162.0668 82.2757 163.1364 96.0072 162.6616 110.1389 160.6734 123.8051 156.8251 136.1833 150.5837 146.4438 141.6224 154.0361 130.2062 159.0148 117.0777 161.8716 103.0781 163.0796 89.0249 162.8160 75.9276 160.7829 65.7034 155.7034 Max, pe O 155.7, 138.5, 65.7 (deg) ( 129.0, 146.0, 63,0 ( 37.3, 87.1, 110,1 Min, pe Intermediate pe Fig. 32: Three Different Initial Configurations of Planar 3R Manipulator LBC VS CYCLIC MOTION INITIAL JOINT ANGLE: 155.7, 138.5, 65.6 3 2 m   J 1 3 0 2 4 6 8 10 12 CYCLE(SYMBOL: GENERAL PSEUDOINVERSE. 5LID MOOREPENROSE PSEUDO INVERSE) Fig. 33: Lie Bracket Condition Values Through 10 Cycles of the Motion LBC VS CYCLIC MOTION Initial Joint Angle: 129, 146, 63 3 U m 1 I I I I I I ! 0 2 4 6 8 10 12 CycleC Syrool: General Pseudo Inverse. Sol a MoorePenrose PseudOonverse) Fig. 34: Lie Bracket Condition Values Through 10 Cycles of the Motion LBC VS CYCLIC MOTION Initial Joint Angle: 37.3, 87.1, 110.1 0.8 0.6 m 0.4 J 0.2 0.2 I I I I I 02 4 6 8 10 12 CycleC 5ymlol: General Pseudoonverse, Solid MoorePenrose PseuOoinverse) Fig. 35: Lie Bracket Condition Values Through 10 Cycles of the Motion 38 (i)inarl (ei)nita, i = 1, 2, 3) are also shown in Table 33. The simulation results given in Figs 33, 34, 35, and Table 33 are a clear comparison of the general pseudoinverse control and the MoorePenrose pseudoinverse control. By the orders of magnitude comparison, it is clear that the general pseudoinverse control (eq. 3.18) is repeatable in practice whereas the MoorePenrose pseudoinverse control (eq. 2.48) is not. An additional explanation for repeatability can be done by considering potential n energy curves (pe = 0.5ki(ei e0,)2) for different initial configurations. If these curves are never intersecting each other during the cyclic motion, then pe is cyclic and the initial joint angles return to their initial values when the end effector returns to its initial position. Fig. 36 shows the overall potential energy curves from maximum pe curve to low pe curve. Fig. 37 shows curves of near intermediate pe curve of Fig. 36. From Figs 36 and 37, the potential energy curves are not intersecting each other during the cyclic motion when the general pseudoinverse equation is used. As stated in section 2.4, if the Lie bracket condition holds at every point e e , then we obtain a family of disjoint surfaces (called a foliation) which covers The potential energy curves in Fig. 36 and 37 are a family of potential energy curves which are never intersecting each other. In the second example, the link lengths are chosen as a, = 0.3, az = 0.425, a4 = 0.1 (m), the general pseudoinverse equation is also used. The ranges of each joint limit are also assumed as follows: Table 33 Simulation Results Through 10 Cycles of the Motion General PseudoInverse Equation LBC 2.513085e07 2.502815e07 1.983124e07 2.080915e07 4.482646e08 4.406034e08 X Y 0.5000 0.5000 0.5009 0.5008 0.0009 0.0008 0.5000 0.5000 0.5008 0.5009 0.0008 0.0009 0.5000 0.5000 0.5010 0.5008 0.0010 0.0008 MoorePenrose PseudoInverse Equation LBC 1.664750e+00 2.043967e + 00 2.233483e + 00 1.848593e + 00 5.815293e01 5.977754e01 X Y TH1 0.5000 0.5000 155.7048 0.5009 0.5008 146.5125 0.0009 0.0008 9.1923 0.5000 0.5000 0.5008 0.5009 0.0008 0.0009 0.5000 0.5000 0.5010 0.5008 0.0010 0.0008 129.0618 118.6704 TH2 TH3 138.5904 65.7048 143.4404 63.5879 4.8500 2.2169 146.0181 140.9852 63.0437 64.5052 10.3914 5.0329 1.4615 37.3383 87.1995 110.1389 34.7540 85.4007 112.0434 2.5843 1.7988 1.9045 CYCLE 0 10 DIFF. 0 10 DIFF. 0 10 DIFF. TH1 155.7048 155.7293 0.0245 129.0618 129.0571 0.0047 37.3383 37.3323 0.0060 TH2 138.5904 138.6359 0.0455 146.0181 146.0755 0.0574 87.1995 87.1560 0.0435 TH3 65.7048 65.5864 0.1184 63.0437 62.9211 0.1226 110.1389 110.1093 0.0296 CYCLE 0 10 DIFF. 0 10 DIFF. 0 10 DIFF.    0 0.2 0.4 0.5 0. I SQUARE PATH C 0 0.25 0.5 0.75 1 ) Fig. 36: Overall Potential Energy Curve Profiles P.E. VS CYCLIC MOTION 0 0.2 0.4 0.6 0.8 1 1.2 SOUARE PATH C 0 0.25 0.5 0.75 1 ) Fig. 37: Detailed Potential Energy Curve Profiles P. E. VS CYCLIC MOTION w I] a 80* < e1 5 90* 160" e2 < 180" 160" < e3 < 180" Thus the free position angles of each joint can be also chosen as e01 = 50, 802 = 003 = 10*). The initial jointangles are 9 = [40", 40", 70*]. The end effector is to track a straight line from point (0.21701, 0.66138) to point(0.34299, 0.66138) with a constant speed of 20 mm/sec. Fig. 38 shows the simulation result when the joint stiffnesses are given values such as k, = k2 = k3 = 1 (Nm/radian). The first joint angle (e1 = 90.99*) is not within the range of joint limit while the second (e2 = 33.1*)and the third joint angle (e3 = 50.52*) are within. Fig. 39 shows the simulation result when the joint stiffnesses are given values such that the first joint is stiffer than the other two joints (k, = 10, k2 = 1, k3 = 1). All the joint angles (e8 = 85.22", e2 = 45.240, e3 = 26.55*) are within the range of joint limits during the specified path. These two figures (Fig. 38 and Fig. 39) indicate that stiffer joint moves less than softer joint. Thus a proper choice of stiffness, ki, based on the mechanical limits of joint helps to avoid the joint limit during the specified path. 3.2 Planar PRR Manipulator 3.2.1 Derivation of Inverse Kinematic Equation The kinematic model of a planar PRR manipulator is shown in Fig. 310. The coordinates for a point P (see Fig. 310) in the end effector are given as x = ssi + azc2 + a342+3 (3.19) y = azs2 + a34s23, Fig. 38: Simulation Result with k, = k2 = k3 = 1.0 (Nm/radian) Fig. 39: Simulation Result with k, = 10, k2 = k3 = 1.0 (Nm/radian) kk2 Fig. 310: Kinematic Model of a Planar PRR Manipulator P Oa3 0 02 44 where az, a3 represent the length of each link and ss, is defined as a displacement of the prismatic joint. The Jacobian matrix is obtained as 1.0 aas2a3S2+3 a34S2+3 [J] = (3.20) 0.0 a3c2 + a34C3 a34c2+3 The general pseudoinverse equation is obtained as 6ssi 6x 6S2 = [T]l[k]''[J]( [J][T1[k]1[J]T ) (3.21) .69 Sy  Detailed derivation of the inverse kinematic equation for a planar PRR redundant serial manipulator is presented in Appendix A. 3.2.2 Numerical Verification and Simulation of Repeatability For the first example, the system consists of one prismatic and two revolute joints as shown Fig. 310. Link lengths are chosen as a = a3 = 1 (m). The ranges of each joint limit are assumed as follows: 1 (m) < ssi < 2 (m) 180" < e92 160" 180* < e3 < 160". The stiffnesses for each joint are taken as k, = 1 (N/m), and k2 = k3 = 1.0 (Nm/radian). The free position angles of each joint can be chosen sseo = 0.5 (m), and eo2 = oa3 = 10". The initial values are ss, = 1.0 (m), e2 = 30", and e3 = 150". 45 The xy coordinates of the four vertices of the command path (1000 consecutive equidistant segments each side) in the task space are successively given counterclockwise from the lower left vertex as follows: (1.3660, 0.2660) (1.4660, 0.2660) (1.3660, 0.3660) (1.4660, 0.3660) where the units are meters. The end effector is to move along the square path with a constant speed of 10 mm/sec and a time step size At = 0.01 sec. Lie bracket condition values through 10 cycles of the motion are shown in Fig. 311. The initial and the final joint angles through each cyclic motion of the end effector are generated in Table 34. At the end of the 10 cycles the position difference ( xfn, xii., ya. yiial) and the joint angle difference ( (ss),, (ssji) (eoi), (e)init i = 2, 3) are also shown in Table 34. By the orders of magnitude comparison, it is clear that the general pseudoinverse control (eq. 3.21) is repeatable in practice whereas the MoorePenrose pseudoinverse control (eq. 2.48) is not. In the second example, the same link lengths, initial joint angles and joint stiffnesses as the first example are used. This example is to illustrate that the results are independent of choice of unit of length. The desired end effector twist is D = ( 0.0001 m, 0.0 m )T. The resulting joint motions are obtained via an application of the general pseudoinverse equation: s = ( 6ssI m, 6e2 deg, 6e3 deg )T = (0.000119 m, 0.001092 deg, 0.000799 deg )T. LBC VS CYCLIC MOTION LBC: LIE BRACKET CONDITION S2 J 01  0 2 4 6 8 10 12 CYCLECSYMBOL:GENERAL PSEUDO INVERSE, SOL ID:MOOREPENROSE PSEUDO INVERSE) Fig. 311: Lie Bracket Condition Values Through 10 Cycles of the Motion Table 34 Simulation Results Through 10 Cycles of the Motion General PseudoInverse Equation LBC 0.0000038 0.000004 0.000004 0.000004 0.000004 0.000004 0.000004 0.000004 0.000004 0.000004 0.000004 x 1.366025 1.366105 1.366186 1.366267 1.366347 1.366428 1.366509 1.366589 1.366670 1.366751 1.366831 0.000806 Y 0.366025 0.365935 0.365844 0.365754 0.365664 0.365574 0.365484 0.365394 0.365303 0.365213 0.365123 0.000902 SSi 1.000000 1.000105 1.000234 1.000364 1.000493 1.000622 1.000751 1.000880 1.001010 1.001139 1.001268 0.001268 TH2 TH3 30.000013 150000066 29.997632 1500L062 30.003800 15(006499 30.009970 150012316 30.016142 150018132 30.022316 150.02948 30.028491 15Q029763 30.034669 1505577 30.040849 15Q01391 30.047031 151047"2 30.053214 15053016 0.053201 0.05235 MoorePenrose PseudoInverse Equation SSi 1.000000 0.987656 0.975260 0.962832 0.950435 0.938130 0.925970 0.913997 0.902246 0.890743 0.879506 0.120494 TH2 TH3 30.000013 15Q00066 30.692766 149.473779 31.361522 148940235 31.996296 148396360 32.595150 147845347 33.157023 147290315 33.681691 146734159 34.169666 146179456 34.622052 145628402 35.040392 145.82806 35.426522 144544104 5.426509 5.45596 CYCLE 0.000 1.000 2.000 3.000 4.000 5.000 6.000 7.000 8.000 9.000 10.000 DIFF. CYCLE 0.000 1.000 2.000 3.000 4.000 5.000 6.000 7.000 8.000 9.000 10.000 DIFF. LBC 2.610043 2.610684 2.600207 2.578829 2.547220 2.506420 2.457731 2.402590 2.342458 2.278733 2.212683 x 1.366025 1.366110 1.366194 1.366278 1.366361 1.366444 1.366527 1.366610 1.366692 1.366775 1.366858 0.000833 Y 0.366025 0.365932 0.365839 0.365747 0.365654 0.365561 0.365468 0.365374 0.365280 0.365185 0.365090 0.000935 48 These same results are obtained if the unit of length measure for all elements is changed to centimeters. In this case, the stiffnesses are ki =0.01 (N/cm), and k2 = k3 = 100 (Ncm/radian). The results can be recalculated and shown to be identical to the above, bearing in mind the changed dimension for the incremental length change of the prismatic joint, viz. 6 = ( 6ssi cm, 680 deg, 683 deg ) = (0.0119 cm, 0.001092 deg, 0.000799 deg )T. This clearly illustrates that the analysis is independent of the chosen unit of length. This is not the case when the MoorePenrose pseudoinverse equation is used, which is analogous to the minimumnorm solution. In such a case, there is no means available in the analysis whereby the units of the physical dimensions or of the mechanical properties can be accounted for. Here, this compensation takes place by virtue of the elastic relationships that are modeled from the real manipulator system or are specified by virtue of n desiredjointstiffnesses. 3.3 Planar 4R Manipulator Consider a 4 degree of freedom manipulator which moves in the (x,y) plane. The position and the orientation of the end effector is of concern, thus making it kinematically redundant. The kinematic model of a planar 4R manipulator is shown in Fig. 312. 3.3.1 Derivation of Inverse Kinematic Equation The coordinates for a point P (see Fig. 312) in the end effector are x = alC + a23c1 + a34c+2+3 + ac41+2+3+4, (3.19) 045 aCk / 0O23 0( Fig. 312: Kinematic Model of a Planar 4R Manipulator y = aus1 + + a3+2 S1+2+3 + a4s+2+3+4, a = e+ + 2 3 + 4. where a2, az3, a34, a4 represent the length of each link and a represents the orientation of the end effector. By analogy with the section 3.1.1, the Jacobian matrix is obtained as 11 J12 J13 J14 [J]= 21 22 23 24 (3.20) [1.0 1.0 1.0 1.0 where J1 = a2s1 a23s+2 a34s1+2+3 a45S+2+3+4, J12 = a23sl+2 a34s+2+3 a45s+2+3+4, J13 = a34S+2+3 a4551+2+3+4, J14 = 45s1+2+3+4, 21 = a,2c" + ac12 + a34c1+2+3 + a45c1+2+3+4, J22 = a23cl+2 + a341+2+3 + a45l1+2+3+4,J23 = a34c1+2+3 + a45c1+2+3+4, J24 = a45cl+2+3+4 The general pseudoinverse equation is obtained as 6 [ 6x 682 = [A]1[J]T( [J][A]I[J]T )1 6y (3.21) 693 6 a 684 Detailed derivation of the inverse kinematic equation for a planar 4R serial redundant manipulator is presented in Appendix B. 3.3.2 Numerical Verification and Simulation of Repeatability Link lengths are given as a, = 0.7, a% = 0.9, a3 = 0.145, as = 0.1 (m). The ranges of each joint limit are assumed as follows: 47* < o, < 129", 120" 5 e < 50*, 118" < eo < 60*, 180" 5 04 180". Based on above the range of each joint limitation the free position angles of each joint can be chosen as 001 = (129 + 47)/2 = 88, 02 = (120 50)/2 = 85", o03 = (118 + 60)/2 = 29, 0o4 = (180 + 180)/2 = 0", and the stiffness for each joint are also taken as ki = 360/(129 47) = 4.4, k2 = 360/(50 + 120) = 5.14, k3 = 360/(118 + 60) = 2.02, k4 = 360/(180 + 180) = 1.0 Nm/radian. The initial jointangle values are a = [100", 110", 30", 30O]T. The xy coordinates of the four vertices of the command path (2000 consecutive equidistant segments each side) in the task space are successively given counterclockwise from the lower left vertex as follows: (0.9653, 0.9592) (1.2653, 0.9592) (0.9653, 0.6592) (1.2653, 0.6592) where the units are meters. The end effector is to track the square path with a 52 constant speed of 15 mm/sec, a constant orientation a = 50', and a time step size At = 0.01 sec. Lie bracket condition values through 10 cycles of the motion are shown in Fig. 313. The initial and the final joint angles through each cyclic motion of the end effector are generated in Table 35. At the end of the 10 cyclic motions of the end effector, the position difference ( x. X,,tia, Ynal yiatw) and the joint angle difference ( (ei)ii (e)aw, i = 1, 2, 3, 4) are also shown in Table 35. By the orders of magnitude comparison, it can be said that the general pseudoinverse control (eq. 3.21) is repeatable in practice whereas the weighted pseudoinverse control (eq. 2.46) is not. 3.3.3 Implementation Actual implementation is accomplished on a planar GE 4R manipulator (see Fig. 314). The same link lengths, initial joint angles, free angles, spring stiffnesses, and the square path motion of the end effector as section 3.3.2 are used. The final manipulator configurations after 10 cycles of the motion are shown in Figs. 315 and 316, respectively. From the Figs 315 and 316, it is clear that the general pseudoinverse control is repeatable whereas the weighted pseudoinverse control is not. LBC VS CYCLIC MOTION LBC: LIE BRACKET CONDITION 0. 0.3 0.2 m .J 0.1 0.1 I II 0 2 4 6 8 10 12 CYCLECSYMBOL: GENERAL PSEUDOINVERSE, SOLID: WEIGHTED PSEUOO INVERSE) Fig. 313: Lie Bracket Condition Values Through 10 Cycles of the Motion Table 35 Simulation Results Through 10 Cycles of the Motion General PseudoInverse Equation CY. LBC 0 8.201034e07 1 8.181804e07 2 8.176153e07 3 8.158862e07 4 8.144222e07 5 8.130164e07 6 8.115498e07 7 8.106662e07 8 8.090450e07 9 8.076453e07 10 8.066382e07 DIFF. X 0.965307 0.965408 0.965510 0.965611 0.965713 0.965814 0.965916 0.966017 0.966118 0.966220 0.966321 0.001014 Y 0.659279 0.659363 0.659446 0.659530 0.659614 0.659697 0.659781: 0.659864 0.659948 0.660032 0.660115 0.000836 TH1 TH2 100.0000 110.0000 99.990615 109.9958 99.982685 109.9842 99.974756 109.9726 99.966828 109.9611 99.958901 109.9535 99.950974 109.9460 99.943049 109.9364 99.935124 109.9248 99.927201 109.9133 99.919278 109.9017 0.000836 0.08073 Weighted PseudoInverse Equation CY. LBC 0 3.614851e01 1 3.567477e01 2 3.517397e01 3 3.464733e01 4 3.409614e01 5 3.352178e01 6 3.292568e01 7 3.230936e01 8 3.167435e01 9 3.102225e01 10 2.945564e01 DIFF. X 0.965307 0.965408 0.965510 0.965612 0.965714 0.965816 0.965919 0.966021 0.966124 0.966227 0.966329 0.001022 TH3 30.0000 30.0026 30.0034 30.0043 30.0051 30.0059 30.0067 30.0076 30.0084 30.0092 30.0101 0.0101 TH4 30.0000 30.0025 29.9980 29.9936 29.9891 29.9847 29.9802 29.9757 29.9713 29.9668 29.9624 0.0376 Y 0.659279 0.659363 0.659446 0.659530 0.659613 0.659695 0.659778 0.659861 0.659943 0.660025 0.660107 0.000828 TH1 100.0000 100.1065 100.2098 100.3082 100.4016 100.4899 100.5733 100.6515 100.7246 100.7926 100.8555 0.8555 TH2 110.0000 109.9503 109.8983 109.8115 109.7200 109.6239 109.5233 109.4384 109.3493 109.2962 109.2594 0.7406 TH3 30.0000 29.3482 28.7963 28.0469 27.2007 26.2586 25.3914 24.6900 23.0649 22.2070 21.3369 8.6631 TH4 30.0000 30.7754 31.1421 31.9064 32.8677 33.6252 34.4785 35.2269 36.0697 36.7066 37.5437 7.5437 Fig. 314: Initial Configuration of a Planar GE 4R Manipulator (eI = 10(), 02 = 110", e, = 30, e4 = 30) Fig. 315: Implementation Result after 10 Cycles with the General Pseudo inverse Equation (Final Configuration: 9, = 100.03, e =  110.09, 83 = 30.0", e4 = 30.01) Fig. 316: Implementation Result after 10 Cycles with the Weighted Pseudo inverse Equation (Final Configuration: e, = 100.75", =  109.49", 93 = 21.12", 94 = 37.6) CHAPTER 4 REPEATABILITY OF A SPATIAL 7R MANIPULATOR The proposed general right pseudo inverse method in the chapter 2 is also applied to a spatial 7R manipulator. It is therefore necessary to develop its forward analysis. 4.1 Notation The notation used throughout this analysis is that developed by Duffy [12]. Briefly stated, a manipulator is composed of a series of rigid links. One such link is shown in Fig. 41. In this figure it is shown that the link connects the two kinematic pair (joint) axes S. and S. The perpendicular distance between the pair axes is ay and the vector along this mutual perpendicul:: i' aj. The twist angle between the pair axes is labelled aij and is measured in a right handed sense about the vector aj. The particular kinematic pair under consideration is the revolute joint which is shown in Fig. 42. The perpendicular distance between links, or more specifically the perpendicular distance between the vectors aij and ajk, is labelled as the offset distance Sj. The relative angle between two links is shown as eO and is measured in a right handed sense about the vector S. 4.2 Mechanism Dimensions of 7R Manipulator Shown in Fig. 43 is a kinematic model of the 7R Manipulator (SSRMS). In the kinematic model the joint axes are labeled sequentially with the unit vectors Fig. 41: Spatial Link Ijk sj 9,j Fig. 42: Revolute Pair ~~Bi_ 1 60 04/ a 34 S~ a 45 sS 5 5 75 67 0 7 a 23 Z a7 3 1 1 56 S Fig. 43: Kinematic Model of the 7R Manipulator (SSRMS) 61 S. (i = 1, 2,..., 7). The directions of the common normal between two successive joint axes Si and S are labeled with the unit vectors aj (ij = 12, 23,..., 78). As previously stated, the link lengths aij, the offsets Sj, and the twist angles aij are constants which are specific to the geometry of a particular manipulator. The values of these constants are tabulated in Table 41 for the SSRMS. In addition to the constant dimensions, S, and a7, are selected such that the point at the end of vector a2 is the point of interest of the tool connected to the manipulator. For example this point may be the tip of a welding rod that the manipulator is moving along a path. Once a particular tool is selected, constant values for S7 and a78 are known. Furthermore it is noted that the link lengths a2, a6, equal zero. However it is still necessary to specify the direction of the unit vectors a2, a in order to have an axis about which to measure the corresponding twist angles. The vector aQ must be perpendicular to the plane defined by the vectors S, and Sj and as such can have two possible directions. For the vectors al, ~, this direction is arbitrarily selected as the direction parallel to the vector SxS,. The values for the corresponding twist angles a,, a,7 listed in Table 41 are determined based upon this convention. 4.3 Specification of Position and Orientation The first step of the analysis is to establish a fixed coordinate system. For this analysis a fixed coordinate system is established as shown in Fig. 44 such that the origin is at the intersection of the vectors S, and S2. The Z axis is chosen to be parallel to the S, vector and the X axis bisects the allowable range of rotation of the Table 41 Mechanism Parameters for SSRMS a, = 0 m a, = 90* S2 = 0.635 S3 = 0.504 S4 = 0.504 Ss = 0.504 S6 = 0.635 S7 = 0.300* az3 = 0.38 a3 = 6.85 a45 = 6.85 ass = 0.38 a67 = 0 = 0.1' S7, a7s: Tool Dependent Parameters a78: an Arbitrary Twist Angle Si = Om az3 = 270* a34 = 0 a45 = 00 a56 = 90* a67 = 90* an = 90* 63 *s* NO R _s8 a78 Fig. 44: Hypothetical Closure Link N/ NC N7 Fi. 4 ypthtca Cour Nn 64 angle 0,. Throughout the rest of this analysis, this coordinate system will be referred to as the fixed coordinate system. Using this fixed coordinate system it is possible to specify the position and orientation of the end effector by specifying the vector to the tool tip, 4, (see Fig. 44) and the pair of orthogonal unit vectors & and as. Although Rp, a,, and S have a total of nine components, the latter two are related by the three conditions, a78' 78 = 1, a78.S = 0, (4.1) S8. = 1, so that the three vectors (Rp, a7s, &) represent the 93 = 6 independent parameters necessary to locate a rigid body in space (see [13]). 4.4 Derivation of Forward Kinematic Equation The vector loop equation of the equivalent closed loop spatial mechanism in the first coordinate system can be written by (see set 1 in Table 42, [12]) R) = SS( )+ + a2a( SS + + SS%' + aa3_ a + S4(1 + + S ) + aS, + SXI)1 + S77(l + + a78a), (4.2) %(1) = [0,s, cu]T, = [0, 1, 0f, (4.3) a1' = [c2, U21, U2fT, = [c2, 0, s2, (4.4) S, = [XI Z, T, = [s2, 0, c2I, (4.5) Table 42 Direction Cosines Spatial Octagon set 1: S ( 0, 0, 1) 2 ( 0, S12, C ) S( X,, Y, ,2 ) 4 (X32, Y3, Z3 ) I ( X4n, Y43, Z432 ) S6 ( X5432, Y5432, Z5432 ) a7 ( X5432, Y65432, Z65432 ) S8 ( X765432, Y765432, Z765432 ) set 8: 84 (0, 0, 1) Si ( 0, s81, c81 ) 2 ( X, 1 Z, ) S( Xl, Y21, ,21 ) S4 ( X32, Y32x, Z321) I ( X4321, Y432, Z4321 ) 6 ( X54321, Y54321, 5432 ) , (X4321, Y654321, Z654321) an ( 1, 0, 0 ) 23 ( C2, U21, U21) 34( W32, U321, U321 ) 4 ( W432, U4321*, U4321 ) a56 ( 5432 1, U521 54321 ) 67 ( W62, U654321, U65432 ) a78 ( 765432, U7654321, U7654321 ) a1 ( c, s,, 0 ) a ( 1, 0, 0 ) a12 (ci, Us*, Usi ) 23 ( W21, U21, U21 ) 34 ( W321, U321, U321 ) 45 (W4321, U4321 U4321 ) 56(W 54321, U54321*, U54321 ) 67 ( W65421 U654321, U654321 ) 78 ( W7654321, U7654321, U7654321) () = [W32 U321, U321T, = [C2C3, S3, S ,3]T, 41) = [X32, Y32, Z32T, = [s2, c2], 45) = [ 432, U432, U432,] = [c2c3+4, S3+4, 2C3+4]T, 5(1) = [X432 Y432, Z432], = [s2, 0, c2T, ) = [W5432, U54321, U54321 T, = [C2C3+4+5, S3+4+5, S2C3+4+5]T, 61) = [X5432, Y432, Z5432, = [C2S3+4+5, C3+4+5, S2S3+4+5]T S7(1) = [X65432, Y65432, Z65432]T, = [S6C2C3+4+5+S2C6, S6S3+4+5, S6S2C3+4+5 CC6]T, 781) = [W765432, U7654321*, U76543211T = [S2s6c7+ C2(s7s3+4+5 + c6C7C3+4+5), s7c3+4+5 + c6c7s3+4+5, + s2(s7s3+4+5 + c6C7C3+4+5)]T, Substituting eq. (4.7) through (4.17) into eq. (4.6) yields RP() = [Rpx(), R,(), R (' T, (4.14) where (4.6) (4.7) (4.8) (4.9) (4.10) (4.11) (4.12) C2S6C7 (4.13) 67 Rp1 = a232 S3s2 + a3c2c3 S4s2 + a4c2c3+4 Ss2 + a56c2c34+ + S6c2s3+4+5 + S7(s6C3+4+c,2 + c6s2) + a7(s2s6c7 + c2(s7s3,4+5 + C6C7C3+4+5) ), R,() = S2 + a34S + a45s3+4 + a56s3+4+5 S63+4, + S76S3+4+5 + a7,( S7C3+4+5 + C6C7S3+4+5), P'() = a23s2 + S3c2 + a34S2c3 + S4c2 + a4s2c3+4 + S5c2 + as2c3+4+5 + S6s2s3+4+5 + S7(s6c3+4+5s2 c6c2) + a78(C2s6c7 + S2(S7S3+4+5 + C6c73+4+5) )9 the orientation vector of end effector, 8() = [X765~, Y765432, z765432T = [c2(c6S7C3+4+5 C7s3+4+5) 82S6S7, C6S7S3+4+5 + C7c3+4+5, S2(C6S7C3+4+5 C7s3+4+5) + C2S67]T. (4.15) Position and orientation vectors known in terms of the first coordinate system can be found in terms of the fixed coordinate system as follows: R = [M][Ro)], (4.16) a78 = [M][a7()], (4.17) S = [M][Sg )], (4.18) where cosoi M = sino, 0 sin01 COS01 cos 0 0 S . 1 Detailed derivation of these kinematic equations for the position and orientation of end effector is presented in Appendix C. As previously stated, although eq. (4.16), 68 (4.21), and (4.22) have a total of nine components, eq. (4.17) and (4.18) are related by the three conditions of equation set (4.1) so that eqs. (4.16), (4.17), (4.18) represent the 93 = 6 independent parameters necessary to position and orient the end effector in three dimensional space. Throughout the remainder of this paper, si and ci will represent the sine and cosine of ei while sij and c,, will represent the sine and cosine of ai. The terms s,.j..., and ci+j...+, represent the sine and cosine of the angle (O + ei + .. + e). 4.5 The Derivation of Jacobian Matrix Fig. 44 shows a coordinate system attached to ground. All line coordinates will be found in terms of a coordinate system which is attached to the SSRMS and has its origin located at the intersection of the vectors S and a7,. The Z axis of this coordinate system will be along & and the X axis will be along a78. The Plicker coordinates for each of the seven axes of the SSRMS will now be found in terms of this new coordinate systemeighth coordinate system. It is important to recognize that the conditions for the linear dependency of the line coordinates are independent of the choice of coordinate system. The Plicker coordinates of each of the seven joint axes are comprised of the direction of the joint axes and the moment of that line about the origin of the coordinate system. The directions of each joint axis measured with respect to the current coordinate system are listed directly as set 8 in Table 42. It has also been shown in Duffy [12] that the moment of each line about the origin is equal to the 69 dual of the direction vector. The Plucker axis coordinates of the seven axis lines, $= [l;oiS are listed as follows: $18) = [(a7A78s8 S 7( S6S) a5.86 S S58) a,) SA 48 a..(8) S3%3 a 23 S(())xS(8) ; S() ], (4.19) 28) = [(a7 78(8) SS7) S66) a6 (8) S(8) a454() S4S) a.4348) S3( a23a8))x(8) ; %(8) ]T, $(8) = [(aa,(8) S7) SS a56 (8) SS () a4(8) S() aa(8) )X_%, ; S3(8) IT, 4(8) = [(a7 g788( S S7 S(8) 8) S a568 (8))x () ; &8() ]f, 1() = [(aa 7(8) SS7) SS6) aZ()xS() ; (8) ]r, (g) = [(a7,a8 ) S7))x(8) ; () ]T, $7) = [a78~78, x S) ; S() ]T, The Jacobian matrix can be expressed as [J] = [1(8) $8) $(8) ) 5(8) (8)]. (4.20) The Pliicker coordinates for each of the seven lines in eq. (4.19) can be simplified by substituting the actual SSRMS mechanism parameters from Table 41 into the equations. The simplified Plicker axis line coordinates are as follows: $1(8 = [ So Soy SSoz ; S Sy SJ]T, (4.21) $2() = [ S2x S02y S2z ; S2 S2y S2.]T, 3(8) = [ o3x S03y S03z ; S3, S3 S3T, $4( = [S SO,, S, So; S5 84, s, 4] $5) = [ SoSx SS, Soz ; S,, s, Sz]T, $s) = [ S06 So( SoZ ; S, SSy Sf], $7() = [ SoI S 07y So,; S7, S7 SZJ , So, = {az3sc2 +S2C1 + S3ss,2 a34(slcc3 + cs3) + S4sIs2 a4,(sc2c3 + cs34) + S5ss2  a56(,c2c345 + c1s345) + S6(ccC345 s2s345) S7(S1S6C4Sc2 + s1c6sz + cIs6s345) + a78(S1S2S6C7 S1C2(S7S345 + C6c7c34) + cS7C34s 1C6C7S345))}, Soy = {a23Cc2 + S2s S3C1S2 + a34(CC2C3 SS3) S4c1s2 + a45(C2C34 s1l34) Sscs2 + a56(cc2cC34 81s345) + S6(s1C345 + c12s345) + S7(CS6C345c2 + CC6s2 SS6S345) + a78( CIS2S6C7 + CiC2(S7S345 + QC6C345) + SS7C345 SC6C7S345))}, Soz = 0, Si = 0, Sly = 0, Si = 1, So2x = {a23 1S2 S3CaC2 ac23 S4c 45234 S a54ccc s 4 S5c 345s S6CLS28345  S,(CIS6C352 CiC6C2) a78(CC2S6C7 + C1S2(S7S345 + C6C7C345))}, S02y = {a23S1S2 S3SC2 a34sls2C3 S4s1c2 a45s1s2c34 S5s1c2 a56ss2c345 S6ss2ss345  S7(S1s6c345s2 scc2) a78(s1c2s6c7 + ss2(s7s345 + cc7c45))}, So2 = {a23c2 Ss2 + a45c2c34 5s2s + a56c2c345 + 62s345 + S7(6c345c + C6S2) + a78(s2S6C7 + 2(s7S345 + C6CC345))}, S2x = Sy = C1, S2. = 0, S03x = {a34(CC2S3 + SlC3) a45(cc2s34 + sc34) a56(cs2s345 + SC1345) + S6(Cc2C345 SS345)  S7(CIS6S345C2+ SIS6345) + a7,(cc2(s7c345 ccs345) s1(s7s345 + C7C345))}, S3y {a34(s 3 CC3 ) c 45(3) a45sc2s 34 c a561(css345 CLC345) + S6(1c2c345 + cls4)  S7(SS6S345C2 C1s6C345) + a78(sc2(s7c345 c6cs345) + c1(s7s345 + c6c7C345))}, S03z {a34s2s3 a45ss34 a56ss234 + S6s2c, S7 5s782(ss345 c6C7345))}, S3x = CS2, S3y = S1S2, S3. = C2, 71 SX = {a4,(c,2s, + sc3) a(c,1css + sIcs) + S6(c c2c3 sS,3) S7((CSSMC2 + SS6Ci34) + a78(cC2(s7C345 c6c7s5) s1(s7345 + C(C7C345))}, So4 = {a45(s2s34 + clc3) a6(sc2s + cc345) + S,(sic2c34 + c1s34) S,(s1s6s3c2 csc3s) + a((scS7C34s C6C7s3) + c,(s7S34 + c,6c7c3))}, So4 = {a4ss234 a5us345 + S6c2c, 5 S7s6s3s2 + a78s2(s7c34 c6c7s3)}, S4x = 12, S4, = S1S2, 54 = C2, S0 = {a5(cc24s + s1345) + S6(cc4c ss3) S7(cs6S345C2 + sis6c34) + a78(cc2(s7c5 cC7S345) sI(S7S34 + CC7C34S))I, S05y = a56(SlC2S3 CC345) + S6(SC2C345 + CS34) S7(SiS6S345C2c6C345) + a78(SC2(S7C345 6CS345) + C1(SS345 + C6C3))}, Soz = {a56s2s345 + S62c345 S7s6s3452 + a78S2(S7C345 C67s345)}, Ss CIS2, S = sIs2, S5z = 2, So6x = {S7(CC6C34c2 ClS6S2 SlC6S345) aT7(czs2c6c7 + CiC2S6C7C345 S16CS345)}, So6 = {S7(sCC345C2 SIS6S2 + ccs345) a78(S1s2c6c + 1C2S6C7C345 + cS6C7S345)}, Soz = {S7(c6c34,s2 + s62) + a7(c2cc7 s2s6cc345)}, S6x = c2S345 + sc345, S6y = sc12S34 CC345, S6z = s2345, S07O = a78(cis2s6s7 + c1c2(c7s345 c6s7c345) + s1(c7c345 + cs7s345)), S07Y = a78(sis2s6s7 + sc12(c07345 c6s7c345) cI(cC345 + cs7s345)), So7 = a7,(c2s6s7 + s2(c7345 scS345)), ST, = CC2SC345 + ClS2C6 SS6S345, ST, = Si2S6c345 + sS2C6 + CS6S345, S7z = s2sc5 C2C6. 72 A detailed derivation of Plicker axis coordinates of the seven joint axes of the 7R manipulator (SSRMS) is presented in Appendix D. Now the Plicker axis coordinates of the seven joint axes of the 7R manipulator (SSRMS) are known. 4.6 The Derivation of Inverse Kinematic Equation Joint torques are related to joint rotations by [r1 T2 r3 74 75 r6 7]T = [k][0101ox 2802 e3o03 4004 e505 E6806 e7e07]'. (4.22) where 010o, and ei eo, (i = 2,...,7) are differences between the current and the free position joint angle and [k] is a 7 x 7 diagonal matrix of joint stiffnesses as follows: kl 0 0 0 0 0 0 0 k2 0 0 0 0 0 (4.23) 0 0 k3 0 0 0 0 [k] = 0 0 0 k4 0 0 0 0 0 0 0 k5 0 0 0 0 0 0 0 k 0 0 0 0 0 0 0 k7 The eq. (4.22) may be more conveniently written as [01001 92802 e3803 848040 e5005 e6806 E 78]T = [k].l[r, T2 73 T4 5 T 77]T. (4.24) The joint torques, acting in each joint, combine to give forces at the end effector, and this can be expressed as [7" T2 73 74 75 6 T7 ]T = [J]T[fx fy fm my n11T. (4.25) Combining the stiffness relationships in eq. (4.24) with eq. (4.25) gives the joint motions [1000o1 202 3o03 0,4 0 5 05 606 7e07]T = [k].q[J]T[f fy my myM1. (4.26) 73 Premultiplying the left and right sides of eq. (4.26) by [J] and rearranging eq. (4.26) yields [f, f m mz]T = ([J][k]n[J]T)'[J][1o0l e2e2 ,, e806 ,ee7]'. (4.27) This can be solved provided that [J] is of full rank. Taking the derivative of eq. (4.26) yields [641 682 683 6e4 6e5 606, 6]T = [k]'[J]T[6ff 6f5 6f, 6n 6my 6mZ1 + [k] [J][f fy m y my mj (4.28) where 6J1n 6J21 J31 6J41 653 6J1 6J2 6J22 J32 42 6J52 J62 6J3 6J23 6J33 6J43 6J53 6J63 [6J]T = 6Ji4 6J24 6J34 6J4 654 6J64 6J,5 J25 635 6J45 6J55 6J65 6J6 6J26 66 6J46 6J56 J66 6J17 7 37 J7 7 6 45765 6J 67 Jpq = Upq~ + Vpqe2 + Wp6e3 + UVpq5e4 + vwpq6e + uwpq6e + uvwpqe7, p = 1, 2, ..., 6, q = 1, 2, ..., 7. Detailed derivation of upq, Vp, wpq, uvpq, vwp, uwq and uvwq is in appendix E. The second term of right hand side of eq. (4.28) goes to the left hand side, [65, 6e2 6e3 6e4 6S5 6E6 6e7]T [k]l[J]T[fx f ff rM my mrT = [k]Fl[J][fx S6f 6f, 5m, 6my 6rnmT. (4.29) Substituting eq. (4.27) into eq. (4.29) and rearranging the left hand side of eq. (4.29), this can be expressed in the form 74 [T][6p,0 68, 3 64e 695 6e, 6e7,] = [k]' []T[6f 6 f 6t m, 6n, 6m]T, (4.30) where [T] is a 7 x 7 matrix as follows T11 T T13 T14 T5 T16 T17 T21 T22 T T24 T25 T26 T27 T31 T32 T33 T T35 T36 T37 [T]= T41 T42 T43 T44 T45 T4 T47 T51 T52 T53 T,4 T55 T56 T57 T61 T62 T63 T64 T65 T66 T67 T71 T7n Tn T74 T75 T76 T7 3 6 T, = I (3 u, f, + 6u m,) /k, (if p = 1, I = 1, else I = 0, p = 1,2,...,7) Tp = I (z 3Vf,f + 6 vqmq) /k2 (if p = 2, I = 1, else I = 0, p = 1,2,...,7) 3 6 Tp3 = I ( wpfq + q=4wqpm) /k3 (if p = 3, I = 1, else I = 0, p =1,2,...,7) 6 T = I (uvqpfq + Z uvqpmq) /k4 (if p = 4, I = 1, else I = 0, p = 1,2,...,7) q=1 q=4 3 6 TP = 1 ( v1 f + 4v,,qpm,) /k5 (if p = 5, I = 1, else I = 0, p = 1,2,...,7) T, = I (z3uw ,fq + uw,m,q) /k (if p = 6, I = 1, else I = 0, p = 1,2,...,7) T,, = I (qZuvwq,fq + E uvwqpmq) /k7 (if P = 7, I = 1, else I = 0, p = 1,2,...,7) q=1 q=4 Eq. (4.30) can be also solved provided that [T] is of full rank. Thus, [601 682 6e3 684 65e 6(6 6,7]T = [A]i[J]T[6fS 6fx 5 6m,. 6mn 6mj]T. (4.31) Letting [A] = [k][T], and premultiplying the left and right sides of eq. (4.31) by [J] yields [J][651 682 683 654 5, 686 687]1 = [J][A][J]T[6f, 6f, St 6m. 6my 6mj,]. (4.32) The forward instantaneous motion equation for the manipulator is given by 75 [J][6i 682 683 684 6e5 Se 68 1T = [6x 6y 6z 6a S6p 6y]. (4.33) Substituting eq. (4.33) into eq. (4.32) yields [6x 6y 6z 6a 6S 6y]T = [J][A]'[J]T[6f 6fy 6f, 6m, 6mn 6SmT, (434) which can be expressed in the form [6x 6y 6z 6a 6S 6y]T = [C][6f, 6fy 6 5m, 6m 6nTSm (4.35) where [C] = [J][A]I[J] is an 6 x 6 symmetric compliance matrix that relates the end effector external wrench to its corresponding twist. Provided that [T] and [J] are both full rank, then [6fS 6 Sfy m 6fm, 6rrmy T = [C]11[6x y 6z 6S 66 6y]T, (4.36) relates the necessary deflection twist of the end effector relative to ground to the incremental change in resultant wrench. In the problem of the inverse kinematics of the redundant serial manipulator, it is required to determine the joint increments explicitly. The elastic compliance relation of eq. (4.35) can be used to accomplish this objective. Substituting eq. (4.36) into eq. (4.31) yields the general pseudoinverse equation: [618 6e2 683 684 685 6 6 7]T = [A][J]T([J][A]l[J]T'[6x Sy 6z 6a 6f 6y]T = [A]'[J]T(C)[6x 6y 6z 6a S6 6y]T. (4.37) 4.7 Numerical Verification and Simulation of Repeatability Mechanism dimensions are given as table 41. The joint limits are selected as follows: 180 5< < 180", 180 : o, 6 60, 180" < e3 < 180", 180* < e4 < 180, 180 e5s 5 1000, 180 < e6, 5 180, 180 < e7 < 120". The free position angles and the stiffnesses of each joint can be chosen as 0o, = (180 + 180)/2 = 0", 902 = (180 + 60)/2 = 60", Oo3 = (180 + 180)/2 = 0", 04e = (180 + 180)/2 = 0", e5 = (180 + 100)/2 = 40, 9o6 = (180 + 180)/2 = 0*, eo7 = (180 + 120)/2 = 30", k, = 360/360 = 1.0, k2 = 360/240 = 1.5, k3 =360/360 = 1.0, k4 = (360/360 = 1.0, k5 = 360/280 = 1.28, k6 =360/360 = 1.0, k7 = 360/300 = 1.2 Nm/radian. The initial jointangle values are O =. [190%, 1900, 190*, 190%, 190*, 190*, 190"]T. The xy coordinates of the four vertices of the command path (3000 consecutive equidistant segments horizontal side and 1000 segments vertical side) in the workspace are successively given counterclockwise from the lower left vertex as follows: (0.7535, 0.8966) (0.4535, 0.8966) (0.7535, 0.9866) (0.4535, 0.9866) where the units are meters. The end effector is to track the rectangular path with a constant speed of 10 mm/sec, a constant orientation (a, = 0.86994, a, = 0.49171, a78 = 0.03704, S, = 0.49310, Sy = 0.86615, S, = 0.86615), and a time step size At = 0.01 sec. 77 Lie bracket condition values through 5 cycles of the motion are shown in Fig. 45. The initial and final joint angles through each cyclic motion of the end effector are generated in Table 43. At the end of 5 cyclic motion of the end effector, the position difference ((Rp) (Rp,)i~.) and the joint angle difference ((Pi)r (p1)ina (ei)i (e)inia, i = 2, 3,,,, 7) are also shown in Table 43. By the orders of magnitude comparison, it is clear that the general pseudoinverse control (eq. 4.37) is repeatable in practice whereas the weighted pseudoinverse control (eq. 2.46) is not. LBC VS CYCLIC MOTION LBC: LIE BRACKET CONDITION 0.1 0(BOeeOOOOO OOOOOOOOOeOOOOOOOOOOOOOOOO OGO OO SI I I N 1 2 3 4 5 CYCLECSYMBOL :GENERAL PSEUDOINVERSE, SOLID:WEIGHTED PSEUDO INVERSE) Fig. 45: Lie Bracket Condition Values Through 5 Cycles of the Motion Table 43 Simulation Results Through 5 Cycles of the Motion General PseudoInverse Equation 19 18 18 18 18 18 PHI1 TH2 0.0000 190.0000 9.9600 190.0127 9.9205 190.0252 9.8817 190.0376 9.8435 190.0499 9.8047 190.0629 TH3 TH4 190.0000 190.0000 189.9615 189.9234 189.8858 189.8487 189.8124 0.1953 0.0629 0.006 189.9991 189.9981 189.9972 189.9963 1 8999401 TH5 190.0000 189.9996 189.9992 189.9989 189.9986 18R99972 0.006 0.0028 1 1 1 1 1 1 TH6 TH7 .90.0000 190.0000 90.0145 190.0005 90.0288 190.0011 .90.0430 190.0018 .90.0570 190.0025 90.0716 190.0045 0.0716 0.0045 LBC 5.247532e6 5.251108e6 5.256534e6 5.261504e6 5.265164e6 5.269522e6 CY. LBC 0 7.080107e01 1 7.064213e01 2 7.011823e01 3 6.923246e01 4 6.871246e01 5 6.826156e01 DIFF. Weighted PseudoInverse Equation PHI1 190.0000 190.8676 191.7274 192.5792 193.4232 194.2600 TH2 190.0000 189.7794 189.5745 189.3841 189.2067 1 9.0409 TH3 190.0000 190.7038 191.3862 192.0502 192.6974 913 3296 4.2600 0.9591 3.3296 TH4 TH5 190.0000 190.0000 189.9964 190.0549 189.9928 190.1164 189.9880 190.1820 189.9819 190.2508 189.9746 190.3220 0.0254 0.3220 LBC 7.080107e01 7.064213e01 7.011823e01 6.923246e01 6.871246e01 6.826156e01 CY. LBC 0 5.247532e6 1 5.251108e6 2 5.256534e6 3 5.261504e6 4 5.265164e6 5 5269522e6 DIFF. CY. 0 1 2 3 4 5 DIFF. Rpx 0.75357 0.75345 0.75333 0.75321 0.75309 0.75297 0.00078 R, 0.98664 0.98659 0.98654 0.98650 0.98645 0.98645 0.00019 Rpz 1.69162 1.69163 1.69163 1.69163 1.69163 1.69163 0.00001 TH6 190.0000 189.7342 189.4839 189.2479 189.0248 188.8132 1.1868 TH7 190.0000 190.1011 190.2099 190.3269 190.4515 190.5831 0.5831 CY. 0 1 2 3 4 5 DIFF. Rpx 0.75357 0.75345 0.75334 0.75323 0.75311 0.75299 0.00058 RN 0.98664 0.98643 0.98637 0.98632 0.98626 0.98621 0.00043 R, 1.69162 1.69163 1.69165 1.69166 1.69167 1.69168 0.00006 1 R.4 1. 062 Q7 .... V V01 V40 193 1296 CHAPTER 5 CONCLUSION The analysis presented here has provided a new closedform solution for the inverse instantaneous kinematic problem of any serial redundant manipulator. An essential requirement for this solution is to provide the free angles (e ) and spring stiffnesses (k,). Here a hypothetical stiffness matrix was constructed using joint limit data of the robot manipulator. Free angles of torsional springs are also conveniently chosen based on joint limits. This procedure yields a control strategy that facilitates repeatability in the joint space of a manipulator whose the end effector undergoes a cyclic type motion. The numerical verification for repeatability is done in terms of the Lie bracket condition. From the numerical verification and simulation results, it can be said that the general pseudoinverse control is repeatable in practice for different initial configurations whreas the weighted pseudoinverse and the Moore Penrose control are not. The implementation for repeatability was accomplished successfully on the planar GE 4R manipulator. A symbolic proof for repeatability will be a future research work. APPENDIX A DERIVATION OF INVERSE KINEMATIC EQUATION OF PRR MANIPULATOR The kinematic model of a planar PRR manipulator is shown in Fig. 310. The coordinates for a point P in the end effector are x = ssi + a3c2 + a34c2+3 (A.1) y = a23s + a34s2+3 The Jacobian matrix is obtained as [ 1.0 [J] = .0.0 a23S2a34S2+3 The joint spring force acting in the firs third joints combine to give forces at as 72 = []r L 73 J t joint and joint torques acting in the second, the end effector, and these can be expressed (A.3) Joint torques are related to joint rotations by a34s2+3 a^e^+3 (A.2) f ,2 = [k] J 3 ss, ss ' SS1 SSolI 02 E02 3 ) 03 where [k] is a 3x3 diagonal matrix of joint stiffnesses as follows [k] = [k] = 0 L0 0 0 k3 . ssi sso0 is the difference between current and the free position displacement while 8, 8o, are the difference between current and free position joint angles, i = 2, 3. The eq. (A.4) can be written in the form SS1 SSo1 If ssl ssol jf 82 802 = [k]"1 T2 e3 803 .r3 J Substituting eq. (A.3) into eq. (A.5) yields SSt SSol ss, sso1 82 02 = [k]' [J]T 83 E03 Multiplying eq. (A.6) by [J] yields El (A.5) (A.6) (A.4) SSi SSo, ] f 1 [J] e 2 2 = [J][k][J]T (A.7) e3 eo f Rewriting eq. (A.7) yields SSi SSol = ( [J][k]. [J]T ) [] e2 02 (A.8) L ea03 Taking derivative of eq. (A.6) yields 6ss 1f 1 [f Se2 = [k][JT + [k]A[6J] (A.9) 683 J where A11 A12 [6J]' = A21 A, (A.10) LA3 A32 Anl = 0.0, A, = 0.0 A21 = (a232 a342+3) 682 a342+3 683 A22 = (azs2 a3s2+3) 6e2 a4S2+3 683 A31 = a3,c2+3 682 a34c2+3 683 A32 = a,4s2+3 6e2 a34s2+3 683 Rewriting eq. (A.9) yields 6ss, 6f, 6e, [k] [k][]T k] 7[J]T (A.11) 603, f, , Rearranging left hand side of eq. (A.11) yields 6ss, 4 Sf [T] e = [k]'[J]T (A.12) [6e3 j Sf where Tn T,1 T1 MT] = T21 T22 T23 T3, T32 T33 TI, = 1.0, TU = 0.0, T3 = 0.0, T21 = 0.0 T, = 1 { (a23C a3c2+3) f + (a2s, aMs2) f }/k2 T23 = ( a3c2+3 f a4S2 3 f, )/k2 T31 = 0.0, T3 = ( a34C2+3 f a34S23 fy )/k3 T33 = 1 ( a34C2,3 f a34s2+3 f )/k3 Rewriting eq. (A.12) yields 6ssi f  682 = [T1 [k][J (A.13) .893 6f Multiplying eq. (A.13) by [J] yields 6ss, f  [J] 2, =[J][[k][J (A.14) The forward instantaneous motion equation for the manipulator is given by 6ss [Sx [J] 682 = (A.15) Substituting eq. (A.15) into eq. (A.14) yields s6x Sf~ = [C] (A.16) sy sf, . where [C] = [Jl[T]n[k][J]T Rewriting eq. (A.16) yields = [C]' (A.17) 6Sf y 86 Substituting eq. (A.17) into eq. (A.13) yields the general pseudoinverse equation [sse 6D2 6 3 = [T]j[k]'[J]( [J][T]'[k]'[]T ) L Lx 6y (A.18) E6x  [T]i'[k] '[J]T[C]"I APPENDIX B DERIVATION OF INVERSE KINEMATIC EQUATION OF 4R MANIPULATOR The cinematic model of a planar 4R manipulator is shown in Fig. 312. The coordinates for a point P in the end effector are x = ac + + c+ a341+2+3 + a4c1+2++4, (B.1) y = ausx + a23s12 + a34s1+2+3 + as4+2+3+4, a = e1 + e2 + 03 + e4, where a2, a2, a34, a, represent the length of each link. Eq. (B.1) for an instantaneous motion can be expressed as [Sx 6y 6a]T = [J][60x 602 6 63 S]T. (B.2) The Jacobian matrix is obtained as J11 J12 J13 J14 [J] = J21 J22 23 24 (B.3) [1.0 1.0 1.0 1.01 where J1 = a2s, a23s1+2 a34s+2+3 a45s+2+3+4, J12 = a23S+2 a34s1+2+3 a45s1+2+3+4, J13 = a34si+2+3 a45s1+2+3+4 J14 = a45Sl+2+3+4, J21 = a12c + a23c1+2 + a34c1+2+3 + a45c1+2+3+4, J22 = a231+2 + a34l1+2+3 + a45c1+2+3+4, J23 = a34c1+2+3 + a4,c1+2+3+4* J24 = a45c1+2+3+4, Joint torques acting in each joint combine to give forces at the end effector, and these can be expressed as T2 = [J] fT (B.4) 7T3 ST4 Joint torques are related to joint rotations by T1 Ie Oe0 r2 e2 02 (B.5) = [k] 73 3 e03 T4 0o4 where [k] is a 4x4 diagonal matrix of joint stiffnesses as follows ki 0 0 0 0 k2 0 0 [k] = 0 0 k3 0 0 0 0 k4 ei eno are differences between current and free position joint angles, i = 1, 2, 3, 4. The eq. (B.5) can be written in the form , 001 e2 o02 63 e,3 64 ,04 = [k] Substituting eq. (B.4) into eq. (B.6) yields e o60 02 802 e3 o03 64 e04 = [k]'[J]T Premultiplying the left and right sides of eq. (B.7) by [J] yields 1 e01 62 02 63 e03 4 ,04 = [J][k]'[J]T Rewriting eq. (B.8) yields f] = ([J][k]'[J]T) [J] rMo 62 02 e3 e03 e4 04  Taking the derivative of eq. (B.7) yields (B.6) (B.7) (B.8) (B.9) f, rnom, 605 1 j'fx ] 2 = [k][Jf]T 6 + [k[6J]T f (B.10) 653 m mo 604 where 6Jn 6Ji2 0.0 65,, 21 ,22 0.0 [6J]T = 2 (B.11) 6J1 6J32 0.0 5J4x 8J42 0.0 6J, = Un1e6 + vn1682 + w11603 + uv1n604, 6J2 = u,260, + v2682 + w,2603 + uv2604, 6J21 = u21,6, + v2,602 + w21683 + uv21,64, 6J,2 = u226O, + v2682 + w2603 + uv22694, 6J31 = u31681 + V31682 + w31603 + uv31(e4, 6J32 = u360 + v3260 + W3263 + UV32 64, 6J41 = u41601 + v4162 + w416%3 + UV41604, 642 = u,6e, + v42602 + 42603 + UV42604, Un = ac a23+2 a34c,+2+3 45c1+2+3+4, U12 = aC31+2 a34+2+3 a45C1+2+3+4, 13 = a34C12+3 a45c1+2+3+4~ U14 = a45sl+2+3+4, U21 = a2s a 1+2 a34s+2+3 a45S1+2+3+4 U22 = a23S1+2 341+2+3 a45S1+2+3+4, U23 = a34+2+3 a45s+2+3+4, U24 = a4,5s+2+3+4, V = 23C1+2 a34c1+2+3 a451+2+3+4 V12= a231+2 a341+2+3 a451+2+3+4, V1 = 34C1+2+3 a451+2+3+4, V14 = 45C1+2+3+4, V21 = 231+2 a341+2+3 45S1+2+3+4, V22 = a23S1+2 a34s1+2+3 a41+2+3+4, V23 = a34S1+2+3 a451+2+3+4, V24 = a451+2+3+4, 11 = a34c1+2+3 a45C1+2+3+4, W2 = a34C1+2+3 45c1+2+3+4, 13 = 341+2+3 a45C1+2+3+4, W14 = 451+2+3+4, w21= 34S1+2+3 a45s1+2+3+4 W22 = 341+2+3 a45S1+2+3+4 23 = a34s1+2+3 a4551+2+3+4,. W24 a451+2+3+4, UV1 = a45c1+2+3+4, UV2 = 45C1+2+3+4, UV13 45C1+2+3+4 UV14 = 845C1+2+3+4, UV21 45s1+2+3+4, UV22 = a45S1+2+3+4, UV23 = a45s1+2+3+4, UV24 = 451+2+3+4 Rewriting eq. (B.10) yields 61 [fx 1 [ Se6 [k][6J]T = [k]l[J]T f (B.12) 683 6m 6e4 Rearranging left hand side of eq. (B.12) yields Where I = [ 6f = [k][J 6f , .6mo . Tl = 1 ( uf + u21f )/k,, T1 = ( wAf + w,2f )/k1, T2, = ( u,f, + u22, )/k,, T23= ( wt + wf22 )/k2, T31 = ( u13f + uzf )/k3, T33 = 1 ( w,3 + w, )/k3, T41 = ( u14fA + U4fy )/k4, T43 = ( w4f + w4 )/k4, Rewriting eq. (B.13) yields 668 683 6 e4 = ( Vif +v2f, )/kj, = ( uvfx + uvf, )/k,, = 1 ( vf + vf2 )/k2, = ( uvA2 + uv22f )/k2, = ( v1fV + vf2 )/k3, = ( uv13fA + uv23, )/k3, = ( vlf~ + V24f )/k4, = 1 ( uv4 + uv24 )/k4,, 6fx 6 6681 682 683 s.e4 (B.13) (B.14) Premultiplying the left and right sides of eq. (B.14) by [J] yields 681 602 683 . 6e4 . = [J][A]'[J]T 6ff] [sm 6fj (B.15) (B.16) Substituting eq. (B.2) into eq. (B.15) yields 6x 6' ' Sy = [C] 6f4 , 6a 6rno. where [C] = [J][A]'[J]T. Rewriting eq. (B.16) yields 6f, 6x S6f = [C]1 6y . n. o 6a (B.17) Substituting eq. (B.17) into eq. (B.14) yields the general pseudoinverse equation 6,2 = [A][J]T( [J][A][J]T ) Sy (B.1 603 6a 604 8) APPENDIX C DERIVATION OF FORWARD KINEMATIC EQUATION OF 7R MANIPULATOR (SSRMS) The vector loop equation of the equivalent closed loop spatial mechanism in the first coordinate system can be written by (see set 1 in Table 42, [12]) Rp() = S2(I) + a23,) + S3(1) + a4a,(1) + S44) + a455) + S5w(1) + a.56(1) + S666s'(1 + SS,7(1) + a7,a78' (C.1) S2x) = 0, S2() = s1, S(= c2 (C.2) a = 2, a23) U21, 23 U21 (C.3) U21" = S2C2, U21 = S2S2 S3() = X2 S31) Y2 S3z )= Z2 (C.4) X2= 232 2= (sc,23 + CUS2C2 ) Z2= C223 ss23C2 a34x(1)= W32, a U321 a3) U321 (C.5) U31 = U3212 V32SU, U321' = U32s2 + V32c U32 = S3S3, V32 = ( 2C3 + c2s33 ) W, = c2c3 Sz3c2 S4x() = X2, S4y) = Y32, S4 ) = Z32 (C.6) X32 = X3 C Y3 S2, Y32 = C(X3 S2 + Y3 c2) Z3 Za2 = s((X3 S2 + Y3 c2) + CUZ3 