A Fast Algorithm and System for the Inverse

To Appear in Proc. IEEE Conf. on Robotics and Automation'94

A Fast Algorithm and System for the Inverse Kinematics of General Serial Manipulators Yunshan Zhu y Department of Computer Science University of North Carolina Chapel Hill, NC 27599-3175

Dinesh Manocha  Department of Computer Science University of North Carolina Chapel Hill, NC 27599-3175 Abstract: We present fast and robust algorithms for the inverse kinematics of serial manipulators consisting of six or fewer joints. When stated mathematically, the problem of inverse kinematics reduces to simultaneously solving a system of algebraic equations. In this paper, we use a series of algebraic and numeric transformations to reduce the problem to computing the eigenstructure of a matrix pencil. To eciently compute the eigenstructure, we make use of the symbolic formulation of the matrix and use a number of techniques from linear algebra and matrix computations. The resulting algorithm computes all the solution of a serial manipulator with six or fewer joints in the order of tens of milliseconds on the current workstations. It has been implemented as part of a generic package, KINEM, for the inverse kinematics of serial manipulators.

Among all manipulator con gurations, inverse kinematics of 6R manipulators seems to have received the most attention [RRS73, AA79, DC80, Pri86, LL88a, RR89]. It was nally shown that a general 6R manipulator can have at most 16 solutions [LL88a, RR89]. Similar bounds on manipulators consisting of a combination of revolute and prismatic joints have been derived in [RR90]. When it comes to practice, three dierent approaches have been highlighted in the literature. The algebraic formulations given in [DC80, Pri86, LL88a, RR89, RR90] are constructive and can be used along with algorithms for nding roots of a univariate polynomial. However, this approach can be slow in practice (due to symbolic expansion) and suer from numerical problems, in the context of oating point arithmetic. This is mainly due to the fact that the problem of computing roots of high degree polynomials can be numerically ill-conditioned [Wil59]. Moreover, the formulations in [LL88a, RR89, RR90] are for generic manipulators and do not easily generalize to manipulators with special geometries. For example, if we apply the algorithm presented in [LL88a, RR89] to construct the univariate polynomial for a PUMA 560, the resulting polynomial corresponds exactly to zero for all poses of the endeector. Given a speci c geometry it is possible to make use of its structure in the symbolic derivation, as shown for 6R manipulators with the last last three intersecting joints [RR90]. However, no equivalent algorithmic approach is known for all possible special geometries. The second approach is based upon homotopy methods to solve a system of polynomial equations and has been demonstrated in [TM85, WM91]. As opposed to eliminating variables from a system of equations, they compute the solutions of the algebraic system by following paths in the complex space. Though robust in practice, the current algorithms and implementations are slow for practical applications. For example, the best known algorithm for the inverse kinematics of a general 6R manipulator takes about 10 seconds on an average of CPU time on an IBM 370 ? 3090 using double precision arithmetic [WM91]. The nal approach is based on use of linear algebra formulation of the problem [MC92]. In particular, the algebraic formulation given in [RR89] is used along with matrix

1 Introduction

The problem of inverse kinematics is fundamental in the design of robot manipulators. Its other applications include molecular modeling and computer animation. Given the manipulator geometry and the pose of the end eector, we are interested in computing all the joint positions corresponding to that pose. In this paper, we restrict ourselves to serial manipulators with up to six joints. For special geometries such as those characterized by intersecting or parallel joint axes, the solution can be expressed as a closed form function. However, no such formulation is known for the general case and no good practical solutions are available to solve them using iterative methods. As a result, most industrial manipulators are designed such that a closed form solution exists [Cra89]. The problem of inverse kinematics has been extensively studied in the literature. In the general case, the problem reduces to computing all the solutions of a multivariate algebraic system. The main interest has been in establishing tight bounds on the number of solutions and computing them eciently and robustly. The robustness of the algorithm refers to the correct computation of all the solutions.  Supportedin part by DARPA Cont. # DAEA-18-90-C-0044 and a Junior Faculty Award y Supported in part by NIH National Center for Research Resources grant RR-02170

1

computations. As opposed to deriving a univariate polynomial, the problem of inverse kinematics is reduced to computing the eigendecomposition of a numeric matrix. It has been applied to general 6R manipulators in [MC92] and involves symbolic preprocessing, numeric substitution and matrix computations. In this paper, we extend on the linear formulation of the inverse kinematics problem and derive properties of manipulators with arbitrary geometry. Based on the algebraic derivations in [RR90], we show that the inverse kinematics problem corresponds to eigendecomposition of a matrix polynomial of the form A + B + C2 , where A, B and C are numeric square matrices. We make use of the structure of the matrix polynomial along with techniques from linear algebra to compute solutions of non-linear polynomial equations. As a result, for all classes of serial manipulators with special or general geometries, we are able to reduce the problem of inverse kinematics to eigendecomposition of a matrix. This symbolic reduction has been used to design a generic system, KINEM, for ecient inverse kinematics of serial manipulators. For most instances the total running time of KINEM for any manipulator geometry with six or fewer joints and a given pose of the end eector is of the order of tens of milliseconds on an SGI/ONYX. KINEM computes all the nite solutions for the inverse kinematics problem or reports that there are in nite solutions. The rest of the paper is organized in the following manner. In Section 2, we review the algebraic formulation of the inverse kinematics problem. In Section 3, we reduce the problem to eigendecomposition of a matrix. Methods to handle special geometry are presented in Section 4. Finally, in Section 5 we discuss how these methods are implemented in KINEM and discuss their performance and implementation on the PUMA con guration.

2 Background

The serial manipulators are modeled using the DenavitHartenberg formalism [SV89]. In particular, there is a 4  4 transformation matrix, Ai , relating the position of joint i+1 to joint i. The matrix is formulated as:

0 ci ?sii sii Ai = B @ s0i cii i ?ciii 0

0

0

ai ci ai si di 1

1 C A;

(2:1)

where si = sini , ci = cosi , i = sini , i = cosi , and i is the ith joint angle and i is the ith twist angle between the axes ith and i + 1. Moreover, ai is the length of link i + 1 and di is the oset distance at joint i. For a given manipulator, each Ai contains one unknown. We will use Vi to represent this unknown. Vi corresponds to di for prismatic joints and i for revolute joints. The position of the end eector is represented as a 4  4 transformation matrix, Ahand , with respect to the world origin. As a result, the problem of inverse kinematics corresponds to computing the unknowns which satisfy the matrix equation: A1 A2 A3 A4 A5 A6 = Ahand : (2:2)

Although the matrix equation corresponds to 12 scalar equations, only 6 of them are independent. Thus, the problem of inverse kinematics is reduced to solving 6 equations in 6 unknowns.

2.1 Raghavan and Roth Formulation

Raghavan and Roth have described methods to reduce this system of six equations to a univariate polynomial [RR89, RR90]. The overall approach is similar for all combinations of revolute and prismatic joints, though the degrees of the resulting polynomial are dierent. For most con gurations of serial manipulators, the matrix equation (2.2) is rearranged as:

A3 A4 A5 = A?2 1 A?1 1 Ahand A?6 1

(2:3)

We use the 3rd and 4th columns of both sides of (2.3) and it results in 6 equations in 5 unknowns. From these 6 equations, 8 additional equations are obtained by taking their cross products and other combinations [RR90]. Taken as a group, there are 14 equations in 5 unknowns and they are represented as a linear system by taking power products of dierent variables together. In formulating the linear system, one of the variables is suppressed and eventually a polynomial in this suppressed variable is obtained. Typically this unknown is V3 . Furthermore the left-hand-sides of the equations are functions of V4 and V5 , and the righthand-side of the equations are functions V1 and V2 . For manipulators consisting of at most one prismatic joint, the linear system can be expressed as: (R) ( f1 (V4 ; V5 ) f2 (V4 ; V5 ) . . . f9 (V4 ; V5 ))T = (Q) ( g1 (V1 ; V2 ) . . . g8 (V1 ; V2 )))T ;

(2.4)

where fi (V4 ,V5 ) is a monomial in V4 and V5 , where Vi corresponds to di or si and ci. It is a monomial of degree at most two. Similarly gi (V1 ,V2 ) corresponds to a monomial in rst two joint variables. R is a 14  9 matrix, whose entries are functions of V3 and Q is a 14  8 matrix, whose elements are independent of the joint variables. For manipulators with two prismatic joints, R may be 14  9 or 14  6 matrix. This depends on the position of the prismatic joint in the serial chain. We use the representation (2.4), except that f7 (V4 ; V5 ); . . . ; f9 (V4 ; V5 ) are all zero. In manipulators with two prismatic joints separated by two revolute joints between them or manipulators consisting of three prismatic joints, the matrix equation (2.3) is arranged dierently and ultimately the system is again reduced as a linear system of the form (2.4), but the number of linear equations is less than 14. We will proceed with the symbolic formulation corresponding to (2.4) in the rest of the paper. The overall algorithm for these special cases is similar, except that the orders of the matrices are dierent and therefore, the number of equations chosen to eliminate the variables and the order of the resulting matrices derived from these equations are dierent. The rest of the algorithm in [RR90] proceeds by using 8 of the 14 equations to solve for the power products on the right-hand-side, and then use these solutions to eliminate these power products from the remaining 6 equations. For

example, for a general 6R manipulator, we obtain a system of the form: () (s4 s5 s4 c5 c4 s5 c4 c5 s4 c4 s5 c5 1)T = 0; (2:5) where  is 6  9 matrix, whose entries are linear combinations of s3 ; c3 and 1.

2.2 Symbolic Representation

In our algorithm, we represent the entries of R and Q as symbolic functions of the Denavit-Hartenberg parameters of the input manipulator. Given a particular geometry, we substitute the corresponding parameters and depending on the fact whether the corresponding joints are prismatic or revolute, we treat the di 's or si and ci 's as the unknowns. It is possible that for some special geometries Q is rank de cient. As a result, we perform the rank computation after substitution using the singular-value decompositions (SVD) [GL89]. If the rank r is less than 8, we only use r equations to eliminate the power products gi (V1 ; V2 ) in (2.4) and thereby obtain (14 ? r) equations. This system of equation is represented in the matrix form as: (S ) ( f1 (V4 ; V5 ) . . . f9 (V4 ; V5 ))T = 0; (2.6) where S is (14 ? r)  9 matrix and its entries are functions of V3 . 0 represents a (14 ? r)  1 null vector. Typically, Q has rank 8 and therefore, S consists of 6 rows. In case S consists of more than 6 rows, we are given an overconstrained system. To solve such a system, we take 6 linear combinations of the rows of S and let the matrix S now correspond to this new system of 6 combinations. Once we obtain all the solutions to these new 6 equations, we back substitute them into the original overconstrained system consisting of 14 ? r equations to eliminate the extraneous solutions. In the rest of the paper, we work with the assumption that S consists of 6 rows. The resulting system is transformed into an algebraic system by replacing the variables si and ci corresponding to revolute joint by: x2i ; where xi = tan(i =2): si = 1 2+xxi 2 ; ci = 11 ? + x2i i The resulting system of equations are multiplied by (1 + x2i ) to transform them into polynomial equations. We will use the variable i to represent xi or di . As a result, we get a system of polynomial equations in three unknowns, 3 ; 4 ; 5 . Furthermore it can be represented as a linear system of the form M (3 ) H (4 ; 5 ) = 0; (2.7) where M (3 ) is a 6  9 matrix, whose entries are quadratic polynomial in 3 and H (4 ; 5 ) is a 9  1 vector consisting of entries, h1 (4 ; 5 ); . . . ; h9 (4 ; 5 ). Each hi (4 ; 5 ) is a monomial function of degree at most four in 4 and 5 .

3 Inverse Kinematics and Matrix Formulation

In the previous section, we have represented the inverse kinematics in terms of the matrix polynomial, M (3 ). In

our case, the set of equations (2.7) correspond to a system of non-linear equations (in 3 ; 4 and 5 ) and we are interested in computing all their common solutions. The main advantage of the matrix representation arises from the fact that we can use a number of results from linear algebra to compute the corresponding zeros. To utilize the structure, we transform the system of equations, (2.7) into a square system using dialytic elimination, as shown in [RR89].

3.1 Square System

The order of the matrix formulation of the inverse kinematics problem is a function of the geometry of the given manipulator. Lets rst consider manipulators with two prismatic joints and there are two revolute joints separating the two prismatic joints. In this case, the matrix R in (2.4) is a 14  6 matrix and therefore, S and M (3 ) in (2.6) and (2.7), respectively are 6  6 matrices. As a result, we obtain a 6  6 square system and let us represent it as:

M (3 ) H (4 ; 5 ) = 0; (3:8) where M (3 ) is exactly equivalent to M (3 ) and H (4 ; 5 ) corresponds to H (4 ; 5 ). The analysis presented also include some manipulator geometries with at least three prismatic joints [RR90]. Lets consider manipulators consisting of at most one prismatic joints or manipulators with two prismatic separated by one or three revolute joints. In these cases, M (3 ) is a 6  9 matrix in (2.7). To obtain a square system we multiply each of the 6 equations by 4 and obtain 6 new equations represented as:

M (3 ) (4 H (4 ; 5 )) = 0: We combine these equations into 12 equations represented in the matrix form as: M (3 ) H (4 ; 5 ) = 0; (3:9) where M (3 ) is the 12  12 matrix:

M (3 ) =

 M ( ) O

3

O M (3 )



;

and O is a 6  3 null matrix. Moreover, H (4 ; 5 ) is a 12  1 vector, whose entries are monomial functions in 4 ; 5 of degree at most 5. H (4 ; 5 ) is constructed by combining the terms of H (4 ; 5 ) and 4 H (4 ; 5 ). As a result, M (3 ) represents a square system of order 6 or 12 depending on the geometry of the manipulator.

3.2 Matrix Formulation

Given the matrix representation, M (3 ) H (4 ; 5 ), the problem of inverse kinematics corresponds to nding all the solutions to the linear system (3.8) or (3.9). M (3 ) is a N  N matrix polynomial of degree 2, where N is either 6 or 12. It can be represented as: M (3 ) = M 0 + M 1 3 + M 2 23 ; (3:10) where M i are numeric matrices.

The matrix polynomial M (3 ) is regular if its symbolic determinant is non-zero. Otherwise it is a singular matrix polynomial. The following theorem shows how this problem of zero computation can be linearized, and the problem of inverse kinematics is reduced to computing appropriate zeros of a matrix pencil of the form, A ? B [MC92, VDD83].

Theorem 3.1 Given the matrix polynomial, M (3 ) the zeros of the matrix polynomial correspond to the zeros of the matrix generalized system A ? B , where B=

I

0

0 M2



A=



I ?M 0 ?M 1 0



: (3:11)

In case the matrix polynomial is regular and M 2 is well conditioned, this can be further reduced to computing the eigenvalues of [MC92]



0

?M ?2 1 M 0

I

?M ?2 1 M 1



:

In this section, we consider matrix polynomials, M (3 ) with a xed kernel. The algorithm for inverse kinematics follows from the following theorem.

Theorem 4.1 Given the matrix polynomial, M (3 ), such that (i) The system of equations, (3.9), has a nite number of solutions. right kernel. (ii) M (3 ) has a xed 0 0 0 0 For any solution, (3 ; 4 ; 5 ), to the equations (3.9), 3 is a solution of Det(Mm(3 )) = 0, where Mm(3 ) is a leading minor of M (3 ). Proof Let us assume there are r linearly independent vec-

O and I are null and identity matrices of order N .

C=

4.2 Matrix Polynomials with Constant Kernel

(3:12)

4 Robust Algorithm for Inverse Kinematics A generic serial manipulator results in regular matrix polynomials and the solutions are computed using the eigendecomposition of the matrices highlighted in the previous section. However, most of the special geometries result in singular matrix polynomials and matrix pencils. The current algorithms for eigendecomposition of such pencils can be ill-conditioned and relatively slow. In this section, we make use of the structure of the matrix polynomials corresponding to the kinematics formulation and derive and ecient and accurate algorithms. A simple test to check whether M (3 )'s is singular is based upon substituting random values of 3 into the matrix polynomial and checking the rank of the resulting numeric matrix. Furthermore, the kernel of singular M (3 ) may or may not be independent of 3 . The rest of the algorithm involves checking the kernel and performing appropriate matrix computations.

4.1 Kernel of the Matrix Polynomial We check whether the matrix has a xed kernel based on the following lemma.

Lemma 4.1 Given the N  N matrix polynomial M (3 ), its right kernel consists of r constant? linearly independent
vectors, i the N  3N matrix P = M T0 M T1 M T2 has r linearly independent vectors in its left kernel. In the above lemma, M Ti refers to the transpose of M i . The proof of this lemma is simple. Based on this lemma, it is trivial to check whether M (3 ) has a xed kernel or it is a function of 3 .

tors V1 ; V2 ; . . . ; Vr in the kernel of M (3 ). As a result, M (3 )U = 0, for all 3 , where U is a vector in the space spanned by V1 ; . . . ; Vr . Let Mm(3 ) be one of the largest non-vanishing minor of M (3 ). It follows that Mm(3 ) is a matrix of0 order 0 0 N ? r. Let (  ;  3 4 ; 5 ) be a solution of (3.9). Therefore, M (03 ) H (04 ; 05 ) = 0. However, the vector H (04 ; 05 ) does not lie in the kernel of M (3 ). If it is in the kernel, than all pairs of the form, (3 ; 04 ; 05 ) for any value of 3 would be a solution of (3.9). This would imply that the original con guration has in nite solutions. Therefore, the kernel of M (03 ) has dimension at least r + 1. As a result, all minors of M (03 ) of order N ? r are singular. Q.E.D. The last theorem is constructive and is used to compute all the solutions of inverse kinematics. We take a nonsingular minor of M (3 ). That corresponds to a regular matrix pencil of order N ? r and has degree 2. Using Theorem 3.1, all the roots of its determinant correspond to the eigenvalues of the equivalent matrix, C as shown in (3.12). 0 The eigenvalues correspond to  of a given solution. Given 3 03 , we compute (04 ; 05 ) in the following manner. Take any two linear combinations of the 6 equations in 4 and 5 : 0 M (3 ) H (4 ; 5 ) = 0: (4:13) Let the two equations be l1 (4 ; 5 ) = 0 and l2 (4 ; 5 ) = 0. These are two equations of degree 4 each, however the highest degree of 4 or 5 in each of these equation is 2. We compute all the solutions to these equations using Sylvester resultant formulation and reducing the problem to an eigenvalue formulation [Man92]. The resulting matrix is 8  8 and we get at most 8 real solutions of these equations. Given the real solution, we back substitute them into the system (4.13) and obtain the actual solution.

4.3 Matrix Polynomials with Dependent Kernel

In this section we consider matrix polynomials whose kernel is a function of 3 . We make use of the 6 equations represented in (2.7) and use them to compute all the (3 ; 4 ; 5 ) corresponding to the solutions of the inverse kinematics problem. Given the 6 equations being represented as M (3 ) H (4 ; 5 ) = 0. M (3 )

is rank de cient. Depending upon the rank of M (3 ), we use the following approaches.

Rank(M (3 )) < 3 In this case, there are in nite solutions

for the given pose of the end eector. This follows from the fact that at most 2 of the 6 equations are independent. A system of one or two algebraic equations, in three unknowns corresponds to a two dimensional or one dimensional algebraic set. As a result, there are in nite solutions of the system, M (3 ) H (4 ; 5 ) = 0, which implies that there are in nite solutions of the matrix equation (2.2).

Rank(M (3 )) > 3 We choose four independent equations

and represent them as 0

M (3 ) H (4 ; 5 ) = 0;

where M 0 (3 ) is a 4  9 minor corresponding to the four equations. In order to solve this system, we use dialytic elimination and linearize it. This is done by multiplying the four equations by 4 ; 5 and 4 5 . As a result we obtain additional 12 equations of the form:

M 0 (3 ) (4 H (4 ; 5 )) = 0 M 0 (3 ) (5 H (4 ; 5 )) = 0 M 0 (3 ) (4 4 H (4 ; 5 )) = 0:

These 16 equations are combined and put together in a linear form: MM (3 ) HH (4 ; 5 ) = 0; (4:14) where MM (3 ) is a 16  16 matrix polynomial0 in 3 of degree 2, and its entries consist of entries of M (3 ) and null matrices. HH (4 ; 5 ) is obtained by combining the power products in H (4 ; 5 ); 4 H (4 ; 5 ); 5 H (4 ; 5 ); 4 5 H (4 ; 5 ). This system is solved by reducing it to a 32  32 eigenvalue problem. From the real solutions of this system, we pick the solutions of the 6 equations, M (3 ) H (4 ; 5 ) = 0 by back substitution.

Rank(M (3 )) = 3 In this case, only 3 of the 6 equations

of M (3 ) H (4 ; 5 ) = 0 are independent. The resulting problem corresponds to solving three equations in three unknowns. Each equation has degree 6, however the highest degree term corresponds to 23 24 25 . Such equations are solved using Dixon's formulation of resultant [Dix08] and reducing the problem to eigendecomposition of a 48  48 matrix, followed by back substitution.

5 Implementation and Performance

The overall algorithm has been implemented as a package, KINEM, for inverse kinematics. It involves symbolic preprocessing, numerical substitution and matrix computations. We symbolically compute the entries of the matrices

Number Link Length Oset Distance Twist Angle

i

1 2 3 4 5 6

ai

0.0 0.65 0.0 0.0 0.0 0.0

di

0.0 0.0 -0.19 0.6 0.0 0.0

i

90.0 0.0 90.0 90.0 90.0 0.0

Table 1: Denavit-Hartenberg Parameters of a PUMA762 R and Q in (2.4) as a function of the Denavit-Hartenberg

parameters of a matrix. Given a particular manipulator geometry and the pose of the end eector, they are substituted into these matrices and the joint variables corresponding to the functions, fi(V4 ; V5 ) are computed. Based on the rank of Q, matrix S is formed and if there are more than 6 rows, we take their linear combinations. The rest of the algorithm proceeds by formulating M (3 ) and M (3 ) and checking whether M (3 ) is a regular or singular matrix polynomial. If M (3 ) is a regular matrix polynomial, we reduce the problem to eigendecomposition of a matrix, otherwise the algorithm involves checking the kernel elements of M (3 ) and eventually reducing the problem to an eigendecomposition problem as well. The symbolic precomputation uses Maple computer algebra system and the matrix computations are performed using LAPACK [ABB+ 92]. The performance of the algorithm is mainly determined by the singular value decomposition and eigendecomposition routines. In particular, the order of the matrix in the eigendecomposition routine may correspond to 24, 32 or 48 or even less than 24 (when we use the minor of M (3 )). The eigendecomposition step accounts for the overall running time of the system and takes about 75 milliseconds on an SGI ONYX in the worst case (when we are given a matrix of order 48). For most special geometries the running time is of the order of 20 ? 40 milliseconds.

5.1 Manipulators with fewer Degrees of Freedom

Any serial manipulator with less than 6 joints is treated as a 6-jointed system. We add additional revolute joints in the con guration. The additional joints are placed in such a manner that the joint angle corresponding to their coordinate systems are 0. Moreover, this joint variable is chosen as 3 . We treat the manipulator as a 6-jointed manipulator and compute all the solutions for the given pose. The original chain has any solutions, i the 6-jointed manipulator has a solution with 3 = 0.

5.2 Applications

In this section we apply our algorithm to a special manipulator geometry corresponding to PUMA672 model. PUMA The Denavit-Hartenberg parameters of a Puma Manipulator are listed in Table 1. The pose of the end eector is: 0 0:250 ?0:957 ?0:146 0:614 1 457 0:25 ?0:854 0:884 C : Ahand = B @ 00::854 0:146 0:50 0:46 A 0 0 0 1

On substituting these parameters, we obtain the 6  9 matrix polynomial, M (3 ) = M2 23 + M1 3 + M0 as:

References

H. Albala and J. Angeles. Numerical solution to the input output displacement equation of the general 7r spatial mechanism. In Proceedings of the Fifth World 0:01 0:00 ?0:01 0:00 0:00 0:00 ?0:01 0:00 ?0:01 0:12 0:13 0:00 0:00 ?0:08 0:00 0:12 ?0:13 0:00 Congress on Theory of Machines and Mechanisms, 0 :08 ? 0 :82 ? 0 :11 0 :00 ? 2 :09 0 :00 0 :08 0 :82 ? 0 :11 pages 1008{1011, 1979. M2 = 1:02 6:95 1:02 0:00 20:55 0:00 ?1:02 ?6:95 1:02 ?0:02 ?0:03 0:00 0:00 0:01 0:00 ?0:02 0:03 0:00 [ABB+ 92] E. Anderson, Z. Bai, C. Bischof, J. Demmel, J. Don0:02 0:02 ?0:01 0:00 ?0:01 0:00 0:02 ?0:02 ?0:01 garra, J. Du Croz, A. Greenbaum, S. Hammarling, and D. Sorensen. LAPACK User's Guide, Release 1.0. SIAM, Philadelphia, 1992. 0:02 0:00 0:02 0:00 0:00 0:00 0:02 0:00 0:02 0:13 0:00 ?0:13 0:00 0:00 0:00 0:13 0:00 ?0:13 [Cra89] J.J. Craig. Introduction to Robotics: Mechanics and ? 0 :35 ? 1 :02 0 :35 0 :00 ? 4 :36 0 :00 ? 0 :35 1 :02 0 :35 M1 = Control. Addison{Wesley Publishing Company, 1989. 2:74 9:13 ?2:74 0:00 34:58 0:00 2:74 ?9:13 ?2:74 ?0:06 0:00 0:03 0:00 0:02 0:00 ?0:06 0:00 0:03 [DC80] J. Duy and C. Crane. A displacement analysis of the 0:00 ?0:02 0:00 0:00 0:00 0:00 0:00 0:02 0:00 general spatial 7r mechanism. Mechanisms and Machine Theory, 15:153{169, 1980. ?0:01 0:00 ?0:01 0:00 0:00 0:00 ?0:01 0:00 ?0:01 0:12 ?0:13 0:00 0:00 ?0:08 0:00 0:12 0:13 0:00 [Dix08] A.L. Dixon. The eliminant of three quantics in two inde?0:43 ?0:12 0:39 0:00 ?2:09 0:00 ?0:43 0:12 0:39 M0 = pendent variables. Proceedings of London Mathematical 3:55 1:47 ?3:55 0:00 20:55 0:00 3:55 ?1:47 ?3:55 ?0:02 0:03 0:00 0:00 0:01 0:00 ?0:02 ?0:03 0:00 Society, 6:49{69, 209{236, 1908. ?0:02 0:02 0:01 0:00 0:01 0:00 ?0:02 ?0:02 0:0 [Gan59] F. R. Gantmacher. The Theory of Matrices. Chelsea Publishing Company, New York, 1959. Volume 1 and 2. In this case, the M (3 ) is singular and has rank 8. This [GL89] G.H. Golub and C.F. Van Loan. Matrix Computations. is concluded by taking some random values of 3 and comJohn Hopkins Press, Baltimore, 1989. puting the rank of the resulting matrix. Based on Lemma [LL88a] H.Y. Lee and C.G. Liang. Displacement analysis of the 4.1, we nd that the matrix polynomial has a constant kergeneral spatial 7-link 7r mechanism. Mechanisms and nel and the problem reduces to computing the eigenvalues Machine Theory, 23(3):219{226, 1988. of a 8  8 matrix polynomial, Mm(3 ). We construct an [Man92] D. Manocha. Algebraic and Numeric Techniques for Modeling and Robotics. PhD thesis, Computer Sciequivalent 16  16 matrix C , as shown in (3.12). The reence Division, Department of Electrical Engineering and sulting matrix has 12 real eigenvalues. They are 0:414213 Computer Science, University of California, Berkeley, and 2:414218 of multiplicity 4, ?0:969767 and ?0:471263 May 1992. of multiplicity 2. For each of them, we compute 4 and 5 [MC92] D. Manocha and J.F. Canny. Real time inverse kinematics of general 6r manipulators. In Proceedings of IEEE by solving for 2 equations in 2 unknowns and back substiConference on Robotics and Automation, pages 383{ tuting the solutions. The whole computation takes about 389, 1992. 20 milliseconds on an SGI ONYX machine. The nal set [Pri86] E.J.F. Primrose. On the input{output equation of the of solutions are listed in Table 2. general 7r{mechanism. Mechanisms and Machine Theory, 21:509{510, 1986. [RR89] M. Raghavan and B. Roth. Kinematic analysis of the 6r manipulator of general geometry. In International Symposium on Robotics Research, pages 314{320, Tokyo, 1989. In this paper we have presented a robust and ecient algorithm for the inverse kinematics of general serial ma[RR90] M. Raghavan and B. Roth. A general solution for the inverse kinematics of all series chains. Proc. of the 8th nipulators. Currently the algorithm is restricted to maCISM-IFTOMM Symposium on Robots and Manipulanipulators consisting of up to six joints, and it makes no tors, 1990. assumption about the geometry of the manipulator. Based [RRS73] B. Roth, J. Rastegar, and V. Scheinman. On the design on the algebraic formulation in [RR90], we reduce the probof computer controlled manipulators. In On the Theory lem to computing zeros of matrix polynomials and matrix and Practice of Robots and Manipulators, pages 93{113. pencils. In order to handle special geometries, we develop First CISM IFToMM Symposium, 1973. robust algorithms based on the matrix formulation. The [SV89] M.W. Spong and M. Vidyasagar. Robot Dynamics and overall algorithm has been implemented as part of a packControl. John Wiley and Sons, 1989. age, KINEM. KINEM computes all the solutions for a given [TM85] L.W. Tsai and A.P. Morgan. Solving the kinematics pose of the end eector taking a few tens of milliseconds on of the most general six and ve-degree-of-freedom maan SGI ONYX. nipulators by continuation methods. Transactions of the ASME, Journal of Mechanisms, Transmissions and Automation in Design, 107:189{200, 1985. [VDD83] P. Van Dooren and P. Dewilde. The eigenstructure of an arbitrary polynomial matrix: Computational aspects. i 1 2 3 4 5 6 Lin. Alg. Appl., 50:545{579, 1983. 1 45.00 45.00 45.00 45.00 45.00 45.00 [Wil59] J.H. Wilkinson. The evaluation of the zeros of ill{ conditioned polynomials. parts i and ii. Numer. Math., 2 -114.66 178.10 45.00 13.17 -78.07 -179.38 1:150{166 and 167{180, 1959. 3 45.00 45.00 45.00 -135.00 -45.00 -135.00 Whitney and C.A. Lozinski. Industrial robot cal4 -114.66 178.10 45.00 -166.83 78.07 0.62 [WL84] D.E. ibration methods and results. In Proceedings of the 5 45.00 1.90 135.00 30.23 83.22 113.68 International Computers in Engineering Conference, pages 92{100, 1984. 6 -114.66 135.00 135.00 24.03 -33.19 -161.68 7 45.009 1.90 135.00 -149.776 -83.22 -166.33 [WM91] C. Wampler and A.P. Morgan. Solving the 6r inverse position problem using a generic-case solution method8 -114.66 135.00 135.00 -155.97 33.20 18.32 ology. Mechanisms and Machine Theory, 26(1):91{106, 1991.

0? @?

1 A

0 @

1 A

0 @

1 A

6 Conclusion

Table 2: The joint angles corresponding to the PUMA con guration

6

[AA79]