Rigid-Body Kinematics 2 -11 Explicitly, this is J R = sin β sin γ cos γ 0 sin β cos γ −sin γ 0 cos β 01 These Jacobian matrices are important because they relate rates of change of Euler angles to angular velocities.Whenthe determinantsof thesematricesbecomesingular, theEuler-angledescription ofrotation becomes degenerate. 2.4.3 Infinitesimal Rigid-Body Motions As with pure rotations, the matrix exponential can be used to describe general rigid-body motions [2,14]. For “small” motions the matrix exponential description is approximated well when truncated at the first two terms: exp v 0 T 0 t ≈ 1I + v 0 T 0 t (2.24) Here =− T and vect() = ω describe the rotational part of the displacement. Since the second term in Equation (2.24) consists mostly of zeros, it is common to extract the information necessary to describe the motion as v 0 T 0 ∨ = ω v This six-dimensional vector is called an infinitessimal screw motion or infinitessimal twist. Given a homogeneous transform H(q) = R(q) b(q) 0 T 0 parametrized with (q 1 , , q 6 ), which we write as a vector q ∈ IR 6 , one can express the homogeneous transform corresponding to a slightly changed set of parameters as the truncated Taylor series H(q + q) = H(q) + 6 i=1 q i ∂ H ∂q i (q) This result can be shifted to the identity transformation by multiplying on the right or left by H −1 to define an equivalent relative infinitessimal motion. In this case we write [6] ω L v L = J L (q) ˙ q where J L (q) = ∂ H ∂q 1 H −1 ∨ , , ∂ H ∂q 6 H −1 ∨ (2.25) where ω L is defined as before in the case of pure rotation and v L =−ω L × b + ˙ b (2.26) Similarly, ω R v R = J R (q) ˙ q where J R (q) = H −1 ∂ H ∂q 1 ∨ , , H −1 ∂ H ∂q 6 ∨ (2.27) Copyright © 2005 by CRC Press LLC 2 -12 Robotics and Automation Handbook Here v R = R T ˙ b The left and right Jacobian matrices are related as J L (H) = [Ad(H)]J R (H) (2.28) where the matrix [Ad(H)] is called the adjoint and is written as [2, 6, 8]: [Ad(H)] = R 0 BR R (2.29) The matrix B is skew-symmetric, and vect(B) = b. Notethat whentherotationsare parametrizedas R = R(q 1 , q 2 , q 3 )and thetranslations areparametrized using Cartesian coordinates b(q 4 , q 5 , q 6 ) = [q 4 , q 5 , q 6 ] T ,onefinds that J R = J R 0 0 R T and J L = J L 0 BJ L 1I (2.30) where J L and J R are the left and right Jacobians for the case of rotation. Given H 0 = R 0 b 0 0 T 1 J L (HH 0 ) = ∂ H ∂q 1 H 0 (HH 0 ) −1 ∨ ··· ∂ H ∂q 6 H 0 (HH 0 ) −1 ∨ Since (HH 0 ) −1 = H −1 0 H −1 , and H 0 H −1 0 = 1, we have that J L (HH 0 ) = J L (H). Similarly, J L (H 0 H) = H 0 ∂ H ∂q 1 H −1 H −1 0 ∨ ··· H 0 ∂ H ∂q 6 H −1 H −1 0 ∨ where H 0 ∂ H ∂q i H −1 H −1 0 ∨ = [Ad(H 0 )] ∂ H ∂q i H −1 ∨ Therefore, J L (H 0 H) = [Ad(H 0 )]J L (H). Analogous expressions can be written for J R , which is left invariant. Further Reading This chapter has provided a brief overview of rigid-body kinematics. A number of excellent textbooks on kinematics and robotics including [1–4,8,16–18] treat this material in greater depth. Other classic worksfromanumberoffields in which rotations are described include [5,7,9–11]. The interested reader is encouraged to review these materials. References [1] Craig, J.J., Introduction to Robotics, Mechanics and Control, Addison-Wesley, Reading, MA, 1986. [2] Murray, R.M., Li, Z., and Sastry, S.S., A Mathematical Introduction to Robotic Manipulation, CRC Press, Boca Raton, 1994. Copyright © 2005 by CRC Press LLC Rigid-Body Kinematics 2 -13 [3] Angeles, J., Rational Kinematics, Springer-Verlag, New York, 1988. [4] Bottema, O. and Roth, B., Theoretical Kinematics, Dover Publications, New York, reprinted 1990. [5] Cayley, A., On the motion of rotation of a solid Body, Cam. Math. J., 3, 224–232, 1843. [6] Chirikjian, G.S. and Kyatkin, A.B., Engineering Applications of Noncommutative Harmonic Analysis, CRC Press, Boca Raton, 2001. [7] Goldstein, H., Classical Mechanics, 2nd ed., Addison-Wesley, Reading, MA, 1980. [8] McCarthy, J.M., Introduction to Theoretical Kinematics, MIT Press, Cambridge, MA, 1990. [9] Rooney, J., A survey of representations of spatial rotation about a fixed point, Environ. Plann., B4, 185–210, 1977. [10] Shuster, M.D., A survey of attitude representations, J. Astron. Sci., 41, 4, 439–517, 1993. [11] Stuelpnagel, J.H., On the parameterization of the three-dimensional rotation group, SIAM Rev., 6, 422–430, 1964. [12] Chasles, M., Note sur les propri ´ et ´ es g ´ en ´ erales du syst ´ eme de deux corps semblables entr’eux et plac ´ es d’une mani ` ere quelconque dans l’espace; et sur le d ´ esplacement fini ou infiniment petit d’un corps solids libre. F´erussac, Bulletin des Sciences Math´ematiques, 14, 321–326, 1830. [13] Ball, R.S., A Treatise on the Theory of Screws, Cambridge University Press, Cambridge, England, 1900. [14] Brockett, R.W., Robotic manipulators and the product of exponentials formula, in Mathematical Theory of Networks and Systems (A. Fuhrman, ed.), 120–129, Springer-Verlag, New York, 1984. [15] Denavit, J. andHartenberg, R.S.,A kinematicnotationfor lower-pair mechanismsbased onmatrices, J. Appl. Mech., 22, 215–221, June 1955. [16] Tsai, L W., Robot Analysis: The Mechanics of Serial and Parallel Manipulators, John Wiley & Sons, New York, 1999. [17] Karger, A. and Nov ´ ak, J., Space Kinematics and Lie Groups, Gordon and Breach Science Publishers, New York, 1985. [18] Selig, J.M., Geometrical Methods in Robotics, Springer-Verlag, New York, 1996. Copyright © 2005 by CRC Press LLC 3 Inverse Kinematics Bill Goodwine University of Notre Dame 3.1 Introduction 3.2 Preliminaries Existence and Uniqueness of Solutions • Notation and Nomenclature 3.3 Analytical Approaches Reduction of Inverse Kinematics to Subproblems • Pieper’s Solution • Example • Other Approaches 3.4 Numerical Techniques Newton’s Method • Inverse Kinematics Solution Using Newton’s Method 3.5 Conclusions 3.1 Introduction Thischapterpresentsresultsrelatedto theinversekinematicsproblem forroboticmanipulators. Aspresented elsewhere, the forward kinematics problem of a manipulator is to determine the configuration (position and orientation) of the end effector of the manipulator as a function of the manipulator’s joint angles. The inverse problem of that, i.e., determining the joint angles given a desired end effector configuration, is the inverse kinematics problem and the subject of this chapter. This chapter will outline and provide examples for two main categories of approaches to this problem; namely, closed-form analytical methods and numerical approaches. The main difficulty of the inverse kinematics problem in general is that for some desired end effector configuration, there may be no solutions, there may be a unique solution, or there may be multiple solutions. The advantage of a numerical approach is that it is relatively easy to implement. As illustrated subsequently, however, one drawback is that the method only leads to one solution for one set of starting values for what is fundamentally an iterative method. Also, if no solutions exist, a numerical approach will simply fail to converge, so care must be taken to distinguish between an attempted solution that will never converge and one that is simply slow to converge. The advantage of analytical approaches is that all solutions can be found and if no solutions exist, it will be evident from the computations. The disadvantage is that they are generally algebraically cumbersome and involve many steps and computations. Also, closed form solutions only exist for certain categories of manipulators, but fortunately, the kinematics associated with the most common manipulators generally seem to belong to the class of solvable systems. 3.2 Preliminaries This sectionwill elaborateupon thenature ofthe inherentdifficulties associatedwiththe inversekinematics problem and also provide a summary of the nomenclature and notation used in this chapter. The first part of this section provides simple examples illustrating the fact that a various number of solutions may exist Copyright © 2005 by CRC Press LLC Inverse Kinematics 3 -5 q 6 q 5 q 4 q 3 q 2 q 1 l 1 l 2 FIGURE 3.6 Elbow manipulator. 3.3.1.1 Inverse Kinematics for Two Examples via Subproblems Consider the schematic illustration of the “Elbow Manipulator” in Figure 3.6. The link frame attachments areillustrated inFigure3.7. With respecttothe elbowmanipulator, wecan make thefollowing observations: r If 0 6 T des is specified for the manipulator in Figure 3.6, generally two values for θ 3 may be determined since the location of the common origin of frames 4, 5, and 6 is given by 0 6 T des and the distance from Z 4 Z 3 Z 2 Z 5 X 0,1,2 X 5,6 X 3 Z 0,1 X 4 , Z 6 FIGURE 3.7 Link frame attachments for the elbow manipulator. Copyright © 2005 by CRC Press LLC 3 -6 Robotics and Automation Handbook the common origin of each of the 0, 1, and 2 frames to the origins of any of the 4, 5, and 6 frames is only affected by θ 3 . r Once θ 3 is determined, the height of the origins of frames 4, 5, and 6, i.e., 0 P iORG , i = 4, 5, 6, is only affected by θ 2 . Again, generally two values of θ 2 can be determined. r For each pair of (θ 2 , θ 3 ) values, the x and y components of 0 P iORG , i =4, 5, 6, determines one unique θ 1 value. r Once θ 1 , θ 2 , and θ 3 are known, 0 3 T can be computed. Using this, 3 6 T des can be computed from 3 6 T des = 0 3 T −10 6 T des r Since axes 4, 5, and 6 intersect, they will share a common origin. Therefore, a 4 = a 5 = d 5 = d 6 = 0 Hence, 3 6 T des = c 4 c 5 c 6 − s 4 s 6 c 6 s 4 − c 4 c 5 s 6 c 4 s 5 l 2 c 6 s 5 −s 5 s 6 −c 5 0 c 5 c 6 s 4 + c 4 s 6 c 4 c 6 − c 5 s 4 s 6 s 4 s 5 0 0001 (3.2) where c i and s i are shorthand for cos θ i and sin θ i respectively. Hence, two values for θ 5 can be computed from the third element of the second row. r Once θ 5 is computed, one value of θ 4 can be computed from the first and third elements of the third column. r Finally, two remaining elements of 3 6 T des , such as the first two elements of the second row, can be used to compute θ 6 . Generically, this procedure utilized the two following subproblems: 1. Determining a rotation that produced a specified distance. In the example, θ 3 determined the distance from the origins of the 1, 2, and 3 frames to the origins of the 4, 5, and 6 frames and subsequently, θ 2 was determined by the height of the origins of the 4, 5, and 6 frames. 2. Determining a rotation about a single axis that specified a particular point to be located in a desired position. In the example, θ 1 was determined in this manner. Othersubproblems are possibleaswell, suchas determining tworotations,which, concatenatedtogether, specify a particular point to be located in a particular position. These concepts are presented with full mathematical rigor in [6] and are further elaborated in [8]. As an additional example, consider the variation on the Stanford manipulator illustrated in Figure 3.8. The assumed frame configurations are illustrated in Figure 3.9. By analogous reasoning, we can compute the inverse kinematic solutions using the following procedure: r Determine d 3 (the prismatic joint variable) from the distance between the location of 0 P 6ORG and 0 P 0ORG . r Determine θ 2 from the height of 0 P 6ORG . r Determine θ 1 from the location of 0 P 6ORG . r Determine θ 4 , θ 5 , and θ 6 in the same manner as for the elbow manipulator. The presentation of these subproblems is intended to be a motivational conceptual introduction rather than a complete exposition. As is clear from the examples, for some manipulators, the inverse kinematics problem may be solved using only one or two of the subproblems. In contrast, some inverse kinematics problems cannot be solved in this manner. The following section presents Pieper’s solution, which is a more mathematically complete solution technique, but one based fundamentally on such subproblems. Copyright © 2005 by CRC Press LLC 3 -8 Robotics and Automation Handbook however, this happens to be the case when the manipulator is equipped with a three-axis spherical wrist for joints four through six. Assuming a six degree of freedom manipulator and assuming that axes four, five, and six intersect, then the point of intersection will be the origins of frames 4, 5, and 6. Thus, the problem of solving for θ 1 , θ 2 , and θ 3 simplifies to a three-link position problem, since θ 4 , θ 5 , and θ 6 do not affect the position of their common origins, 0 P 4ORG = 0 P 5ORG = 0 P 6ORG . Recall that the first three elements in the fourth column of Equation (3.1) give the position of the origin of frame i expressed in frame i − 1. Thus, from 3 4 T, 3 P 4ORG = a 3 −sin α 3 d 4 cos α 3 d 4 Expressing this in the 0 frame, 0 P 4ORG = 0 1 T 1 2 T 2 3 T a 3 −sin α 3 d 4 cos α 3 d 4 1 Since 2 3 T = cos θ 3 −sin θ 3 0 a 2 sin θ 3 cos α 2 cos θ 3 cos α 2 −sin α 2 −sin α 2 d 3 sin θ 3 sin α 2 cos θ 3 sin α 2 cos α 2 cos α 2 d 3 0001 we can define three functions that are a function only of the joint angle θ 3 , f 1 (θ 3 ) f 2 (θ 3 ) f 3 (θ 3 ) 1 = 2 3 T a 3 −sin α 3 d 4 cos α 3 d 4 1 where f 1 (θ 3 ) =a 2 cos θ 3 + d 4 sin α 3 sin θ 3 + a 2 f 2 (θ 3 ) =a 3 sin θ 3 cos α 2 − d 4 sin α 3 cos α 2 cos θ 3 − d 4 sin α 2 cos α 3 − d 3 sin α 2 f 3 (θ 3 ) =a 3 sin α 2 sin θ 3 − d 4 sin α 3 sin α 2 cos θ 3 + d4cosα 2 cos α 3 + d 3 cos α 2 and thus 0 P 4ORG = 0 1 T 1 2 T f 1 (θ 3 ) f 2 (θ 3 ) f 2 (θ 3 ) 1 (3.3) If frame0 is specifiedin accordance with step fourof the Denavit-Hartenberg frame assignment outlined in Section 3.2.2,then ˆ Z 0 will beparallel to ˆ Z 1 , and henceα 0 = 0.Also,since the origins offrames 0and 1will be coincident, a 0 = 0 and d 1 = 0. Thus, substituting these values into 0 1 T and expanding Equation (3.3), Copyright © 2005 by CRC Press LLC Inverse Kinematics 3 -9 we have 0 P 4ORG = cos θ 1 −sin θ 1 00 sin θ 1 cos θ i 00 0010 0001 × cos θ 2 −sin θ 2 0 a 1 sin θ 2 cos α 1 cos θ i cos α 1 −sin α 1 −sin α 1 d i sin θ 2 sin α 1 cos θ 2 sin α 1 cos α 1 cos α 1 d 2 0001 f 1 (θ 3 ) f 2 (θ 3 ) f 3 (θ 3 ) 1 Since the second matrix is only a function of θ 2 , we can write 0 P 4ORG = cos θ 1 −sin θ 1 00 sin θ 1 cos θ i 00 0010 0001 g 1 (θ 1 , θ 2 ) g 2 (θ 1 , θ 2 ) g 3 (θ 1 , θ 2 ) 1 (3.4) where g 1 (θ 2 , θ 3 ) =cos θ 2 f 1 (θ 3 ) −sin θ 2 f 2 (θ 3 ) +a 1 (3.5) g 2 (θ 2 , θ 3 ) =sin θ 2 cos α 1 f 1 (θ 3 ) +cos θ 2 cos α 1 f 2 (θ 3 ) −sin α 1 f 3 − d 2 sin α 1 (3.6) g 3 (θ 2 , θ 3 ) =sin θ 2 sin α 1 f 1 (θ 3 ) +cos θ 2 sin α 1 f 2 (θ 3 ) +cos α 1 f 3 (θ 3 ) +d 2 cos α 1 . (3.7) Hence, multiplying Equation (3.4), 0 P 4ORG = cos θ 1 g 1 (θ 2 , θ 3 ) −sin θ 1 g 2 (θ 2 , θ 3 ) sin θ 1 g 1 (θ 2 , θ 3 ) +cos θ 1 g 2 (θ 2 , θ 3 ) g 3 (θ 2 , θ 3 ) 1 (3.8) It is critical to note that the “height” (more specifically, the z coordinate of the center of the spherical wrist expressed in frame 0) is the third element of the vector in Equation (3.8) and is independent of θ 1 . Specifically, z = sin θ 2 sin α 1 f 1 (θ 3 ) +cos θ 2 sin α 1 f 2 (θ 3 ) +cos α 1 f 3 (θ 3 ) +d 2 cos α 1 (3.9) Furthermore, note that the distance from the origin of the 0 and 1 frames to the center of the spherical wrist will also be independent of θ 1 . The square of this distance, denoted by r 2 is simply the sum of the squares of the first three elements of the vector in Equation (3.8); namely, r 2 = g 2 1 (θ 2 , θ 3 ) + g 2 2 (θ 2 , θ 3 ) + g 2 3 (θ 2 , θ 3 ) = f 2 1 (θ 3 ) + f 2 2 (θ 3 ) + f 2 3 (θ 3 ) +a 2 1 + d 2 2 + 2d 2 f 3 (θ 3 ) (3.10) +2a 1 (cos θ 2 f 1 (θ 3 ) −sin θ 2 f 2 (θ 3 )) We now consider three cases, the first two of which simplify the problem considerably, and the third of which is the most general approach. 3.3.2.1 Simplifying Case Number 1: a 1 = 0 Note that if a 1 =0 (this will be the case when axes 1 and 2 intersect), then from Equation (3.10), the distance from the origins of the 0 and 1 frames to the center of the spherical wrist (which is the origin of Copyright © 2005 by CRC Press LLC 3 -10 Robotics and Automation Handbook frames 4, 5, and 6) is a function of θ 3 only; namely, r 2 = f 2 1 (θ 3 ) + f 2 2 (θ 3 ) + f 2 3 (θ 3 ) +a 2 1 + d 2 2 + 2d 2 f 3 (θ 3 ) (3.11) Since it is much simpler in a numerical example, the details of the expansion of this expression will be explored in the specific examples subsequently; however, at this point note that since the f i ’s contain trigonometric function of θ 3 , there will typically be two values of θ 3 that satisfy Equation (3.11). Thus, given a desired configuration, 0 6 T = t 11 t 12 t 13 t 14 t 21 t 22 t 23 t 24 t 31 t 32 t 33 t 34 0001 the distance from the origins of frames 0 and 1 to the origins of frames 4, 5, and 6 is simply given by r 2 = t 2 14 + t 2 24 + t 2 34 = f 2 1 (θ 3 ) + f 2 2 (θ 3 ) + f 2 3 (θ 3 ) +a 2 1 + d 2 2 + 2d 2 f 3 (θ 3 ) (3.12) Once one or more values of θ 3 that satisfy Equation (3.12) are determined, then the height of the center of the spherical wrist in the 0 frame is given by t 34 = g 3 (θ 2 , θ 3 ) (3.13) Since one or more values of θ 3 are known, Equation (3.13) will yield one value for θ 2 for each value of θ 3 . Finally, returning to Equation (3.8), one value of θ 1 can be computed for each pair of (θ 2 , θ 3 )whichhave already been determined. Finding a solution for joints 4, 5, and 6 is much more straightforward. First note that 3 6 R is determined by 0 6 R = 0 3 R 3 6 R =⇒ 3 6 R = 0 3 R T 0 6 R where 0 6 R is specified by the desired configuration, and 0 3 R can be computed since (θ 1 , θ 2 , θ 3 ) have already been computed. This was outlined previously in Equation (3.2) and the corresponding text. 3.3.2.2 Simplifying Case Number 2: α 1 = 0 Note that if α 1 = 0, then, by Equation (3.5), the height of the spherical wrist center in the 0 frame will be g 3 (θ 2 , θ 3 ) =sin θ 2 sin α 1 f 1 (θ 3 ) +cos θ 2 sin α 1 f 2 (θ 3 ) +cos α 1 f 3 (θ 3 ) +d 2 cos α 1 g 3 (θ 3 ) = f 3 (θ 3 ) +d 2 , so typically, two values can be determined for θ 3 . Then Equation (3.10), which represents the distance from the origin of the 0 and 1 frames to the spherical wrist center, is used to determine one value for θ 2 . Finally, returning to Equation (3.8) and considering the first two equations expressed in the system, one value of θ 1 can be computed for each pair of (θ 2 , θ 3 ) which have already been determined. 3.3.2.3 General Case when a 1 = 0 and α 1 = 0 This case is slightly more difficult and less intuitive, but it is possible to combine Equation (3.7) and Equation (3.11) to eliminate the θ 2 dependence and obtain a fourth degree equation in θ 3 .Forafewmore details regarding this more complicated case, the reader is referred to [1]. Copyright © 2005 by CRC Press LLC 3 -12 Robotics and Automation Handbook At this point, we have four combination of (θ 2 , θ 3 ) solutions; namely (θ 2 , θ 3 ) =(−28.68 ◦ ,45.87 ◦ ) (θ 2 , θ 3 ) =(77.14 ◦ ,45.87 ◦ ) (θ 2 , θ 3 ) =(102.85 ◦ , 143.66 ◦ ) (θ 2 , θ 3 ) =(−151.31 ◦ , 143.66 ◦ ) Now, considering the x and y elements of 0 P 6ORG in Equation (3.8) and substituting a pair of values for (θ 2 , θ 3 ) into g 1 (θ 2 , θ 3 ) and g 1 (θ 2 , θ 3 )inthefirst two lines and rearranging we have g 1 (θ 2 , θ 3 ) −g 2 (θ 2 , θ 3 ) g 2 (θ 2 , θ 3 ) g 1 (θ 2 , θ 3 ) cos θ 1 sin θ 1 = 1 1 or cos θ 1 sin θ 1 = g 1 (θ 2 , θ 3 ) −g 2 (θ 2 , θ 3 ) g 2 (θ 2 , θ 3 ) g 1 (θ 2 , θ 3 ) −1 1 1 Substituting the (θ 2 , θ 3 ) = (−151.31 ◦ , 143.66 ◦ ) solution for the g i ’sgives cos θ 1 sin θ 1 = −1.3231 −0.5 0.5 −1.3231 −1 1 1 = −0.4114 −0.9113 =⇒ θ 1 = 245.7 ◦ Substituting the four pairs of (θ 2 , θ 3 ) into the g i ’s gives the following four sets of solutions including θ 1 : 1. (θ 1 , θ 2 , θ 3 ) = (24.3 ◦ , −28.7 ◦ ,45.9 ◦ ) 2. (θ 1 , θ 2 , θ 3 ) = (24.3 ◦ , 102.9 ◦ , 143.7 ◦ ) 3. (θ 1 , θ 2 , θ 3 ) = (−114.3 ◦ ,77.14 ◦ ,45.9 ◦ ) 4. (θ 1 , θ 2 , θ 3 ) = (−114.3 ◦ , −151.32 ◦ , 143.7 ◦ ) Now that θ 1 , θ 2 , and θ 3 are known, 0 3 T can be computed. Using this, 3 6 T des can be computed from 3 6 T des = 0 3 T −10 6 T des Since axes 4, 5, and 6 intersect, they will share a common origin. Therefore, a 4 = a 5 = d 5 = d 6 = 0 Hence, 3 6 T des = c 4 c 5 c 6 − s 4 s 6 c 6 s 4 − c 4 c 5 s 6 c 4 s 5 l 2 c 6 s 5 −s 5 s 6 −c 5 0 c 5 c 6 s 4 + c 4 s 6 c 4 c 6 − c 5 s 4 s 6 s 4 s 5 0 0001 (3.19) where c i and s i are shorthand for cos θ i and sin θ i respectively. Hence, two values for θ 5 can be computed from the third element of the second row. Considering only the case where (θ 1 , θ 2 , θ 3 ) = (−114.29 ◦ , −151.31 ◦ , 143.65 ◦ ), 3 6 T des = 0.38256 0.90331 −0.194112 2. −0.662034 0.121451 −0.739 568 24. −0.644484 0.411438 0.644484 −2.922 0.0 0. 0. 0. 1. Copyright © 2005 by CRC Press LLC [...]... picked randomly: Iteration 0 1 2 3 4 5 6 7 8 9 1 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 theta1 -1 36 .23 -2 .20 44. 32 69 .22 12 0. 32 63.89 -7 0.59 -5 0.98 -8 8.86 -1 73.40 -1 30.68 -7 6 . 12 16 4.40 -1 8 .18 -4 7.83 -9 9 .11 -7 5.66 -1 62. 83 -1 21 . 64 -1 10 .76 -1 14 . 32 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 -1 14 .30 Copyright © 20 05 by CRC Press LLC theta2 -1 39 .17 -2 7. 61 69.98 -3 2. 48... -8 7. 01 -1 43.86 -1 56.97 -1 74. 21 -1 04 .16 10 6.37 13 . 01 74. 92 45.87 -1 77.35 -1 32. 83 10 0. 41 77 .15 54.07 47.39 17 .15 -1 2. 91 35.38 88.80 57.85 57.38 56. 02 56. 02 56. 02 theta5 13 7. 92 14 .86 97 .29 10 9 .17 -3 . 81 33.74 13 5.05 14 1 .17 13 5.54 4.49 14 .69 -2 7 .14 43.00 92. 84 11 2. 59 -3 0.07 -9 .84 11 3 .24 15 1. 31 160.48 15 8 .26 15 4 .29 13 4.74 14 0.57 10 1 .19 63.60 51. 07 51. 00 51. 01 51. 01 theta6 -2 3.59 21 . 61 20 . 31 -6 8.76 -8 0 .29 4 .29 ... 56.05 -2 0.66 32. 21 108 .14 53.58 12 7.80 72 .18 15 .76 -1 55.70 -1 53 .18 -1 60.55 16 .87 14 . 52 83 .18 63. 91 77 .23 77.03 77 .14 77 .14 77 .14 77 .14 77 .14 77 .14 77 .14 77 .14 77 .14 theta3 88.74 17 8.80 51. 25 11 4 .17 29 .90 91. 33 16 9 .15 54. 72 81. 08 38.88 81. 86 58. 71 58.08 14 8.99 99.49 21 . 47 84.43 34 .29 57. 62 47.40 45.94 45.87 45.87 45.87 45.87 45.87 45.87 45.87 45.87 45.87 theta4 10 2. 89 34 .10 -3 8.03 -5 1. 25 -1 1. 28 -1 15 .55 -8 7. 01. .. + (T2 [1] [2] -T [1] [2] )/(-PERTURBATION)) /2. 0; J[4][i -1 ] = ((T1 [2] [2] -T [2] [2] )/(PERTURBATION) + (T2 [2] [2] -T [2] [2] )/(-PERTURBATION)) /2. 0; J[5][i -1 ] = ((T1 [2] [1] -T [2] [1] )/(PERTURBATION) + (T2 [2] [1] -T [2] [1] )/(-PERTURBATION)) /2. 0; J[6][i -1 ] = ((T1[0][0]-T[0][0])/(PERTURBATION) + (T2[0][0]-T[0][0])/(-PERTURBATION)) /2. 0; J[7][i -1 ] = ((T1[0] [1] -T[0] [1] )/(PERTURBATION) + (T2[0] [1] -T[0] [1] )/(-PERTURBATION)) /2. 0;... 4 .29 16 1.33 10 7 . 12 54.00 -2 7.35 55.74 -4 0.86 -1 76.37 -3 2. 89 25 .23 -1 15 .73 -1 22 .40 -9 3.66 -9 7.00 -1 17 .69 -1 29 .10 -1 65.67 -2 8 .24 26 .89 89.46 69.39 78.59 79.53 79.53 79.53 3 -1 8 Robotics and Automation Handbook let θ 1 − θ0 ≤ b and let n k =1 ∂ 2 f i (θ) c ≤ ∂θ j ∂θk n for all θ ∈ θ − θ ≤ 2b, where i, j ∈ {1, , n} If abc ≤ 1 2 then 1 θi defined by Equation (3 .22 ) are uniquely defined as θn − θ0 ≤ 2b and 2. .. (T2[0] [1] -T[0] [1] )/(-PERTURBATION)) /2. 0; J[8][i -1 ] = ((T1[0] [2] -T[0] [2] )/(PERTURBATION) + (T2[0] [2] -T[0] [2] )/(-PERTURBATION)) /2. 0; J[9][i -1 ] = ((T1 [1] [0]-T [1] [0])/(PERTURBATION) + (T2 [1] [0]-T [1] [0])/(-PERTURBATION)) /2. 0; J [10 ][i -1 ] = ((T1 [1] [1] -T [1] [1] )/(PERTURBATION) + (T2 [1] [1] -T [1] [1] )/(-PERTURBATION)) /2. 0; J [11 ][i -1 ] = ((T1 [2] [0]-T [2] [0])/(PERTURBATION) + (T2 [2] [0]-T [2] [0])/(-PERTURBATION)) /2. 0; } free(T1); free(T2); return J;... pseudoinverse(J,Jinv); f[0] f [1] f [2] f[3] f[4] f[5] f[6] = = = = = = = T[0][3]-Tdes[0][3]; T [1] [3]-Tdes [1] [3]; T [2] [3]-Tdes [2] [3]; T [1] [2] -Tdes [1] [2] ; T [2] [2] -Tdes [2] [2] ; T [2] [1] -Tdes [2] [1] ; T[0][0]-Tdes[0][0]; Copyright © 20 05 by CRC Press LLC 3 -2 2 Robotics and Automation Handbook f[7] = T[0] [1] -Tdes[0] [1] ; f[8] = T[0] [2] -Tdes[0] [2] ; f[9] = T [1] [0]-Tdes [1] [0]; f [10 ] = T [1] [1] -Tdes [1] [1] ; f [11 ] = T [2] [0]-Tdes [2] [0]; for(k=0;k . -1 32. 83 -9 .84 -1 22 .40 17 -1 62. 83 83 .18 34 .29 10 0. 41 113 .24 -9 3.66 18 -1 21 . 64 63. 91 57. 62 77 .15 15 1. 31 -9 7.00 19 -1 10.76 77 .23 47.40 54.07 16 0.48 -1 17.69 20 -1 14. 32 77.03 45.94 47.39 15 8 .26 -1 29 .10 21 . 69 .22 -3 2. 48 11 4 .17 -5 1. 25 10 9 .17 -6 8.76 4 12 0. 32 56.05 29 .90 -1 1 .28 -3 . 81 -8 0 .29 5 63.89 -2 0.66 91. 33 -1 15.55 33.74 4 .29 6 -7 0.59 32. 21 169 .15 -8 7. 01 135.05 16 1.33 7 -5 0.98 10 8 .14 54. 72 -1 43.86. -1 29 .10 21 -1 14.30 77 .14 45.87 17 .15 15 4 .29 -1 65.67 22 -1 14.30 77 .14 45.87 -1 2. 91 134.74 -2 8 .24 23 -1 14.30 77 .14 45.87 35.38 14 0.57 26 .89 24 -1 14.30 77 .14 45.87 88.80 10 1 .19 89.46 25 -1 14.30 77 .14