- Email: [email protected]
Solving multiloop linkages with limited-range joints
@
Mech. Mach. Thcoey Vol. 29. No. 3, pp. 373--391. 1994 Copyright 'C 1994 Elsev~r Science Lid Printed in Great Britmn. ,All rights R~.rv~l 0094-114X/94 $6.00 + 0.00
Pergamon
SOLVING MULTILOOP LINKAGES WITH LIMITED-RANGE JOINTS ENRIC CELAYA and C A R M E TORRAS lnstitut de Cibernetica(UPC-CSIC). Diagonal 647. 08028-Barcelona, Spain (Received I 1 January 1993. receicedfor publication 17 March 1993)
Abstract--An algorithmto obtain the solutionof underconstraincdrotation equationsin the form of sets of valuesthat the variablescan take is given. Furthermore.an interval propagation algorithmis presented which parmits obtainingthe subsets of the solution that are compatiblewith a given interval for one of the variables. The propagation techniqueprovidesa way to take into account non-intersectionconstraints in the solution. In addition, propagation can b¢ used to solve systemsof rotation equations. Finally.it is shown how the algorithmsdescribed can be used in the solution of certain spatial problems, including (>-bar mechanisms,
I. I N T R O D U C T I O N The primary concern of this paper is to give an answer to the following questions: Can a particular set of links be assembled into a closed kinematic chain? Which are the configurations that produce such assembly? What is the range of permissible motion? Such issues find applications in mobility analysis of mechanisms, assembly planning, inverse kinematics of redundant and non-redundant manipulators, robot programming, etc. We characterize this kind of problems as assembly problems, defined in the following terms: given a set of bodies and a set of kinematic constraints that have to hold between them, find all the possible relative positions of the bodies that simultaneously satisfy all the kinematic and non-intersection constraints. We consider the general case of muhiloop kinematic chains, which is relevant for the study of parallel manipulators and of problems involving two or more cooperating manipulators. We also address the problem of limitations or ends of travel in the articulations, which usually exist in real mechanisms. As already noted in [I], such problems can in principle be investigated through the kinematic analysis of the relative displacements in the assembled mechanism for a series of positions of the input link. Positions for which assembly is not possible correspond to imaginary solutions of the loop equation. An important drawback of this approach is that it is unnecessarily costly in computational terms. A more fundamental limitation comes from the fact that the usual techniques to perform displacement analysis can deal only with single loop mechanisms with a single overall degree of freedom [2-5]. Thus, for example, traditional approaches to the solution of the inverse kinematics of redundant manipulators require to impose additional constraints, usually minimizing some specific cost function, in order to force the uniqueness of the solution [6]. In our approach the objective is to find the complete set of possible configurations, from which an appropriate one can be selected according to different criteria in each particular case. We propose a hierarchical form of description of the solution set, which allows the construction of explicit solution configurations. Other approaches have been taken from the perspective of assembly planning and robot programming [7-1 I], but none of them permits finding the solution when it contains ranges for the variables. On the other hand, methods aimed at obtaining the limit positions for articulations rely either on the use of analytic expressions, which are not always available, or on monitorization of extensive data computations [I 2-17]. 373
374
ENRICCELAYAand CARMETORRAS
We begin by introducing, in Sections 2 and 3, some basic concepts and notations, upon which a standard formulation of spherical problems is developed. Section 4 states the main theoretical results that provide the basis for the construction, in Section 5. of the solution of any single-loop spherical mechanism assuming completely rotatable joints. In Section 6. an interval propagation algorithm is given, which permits including in the solution the limitations existing in the joints. The same propagation algorithm is then shown to be useful to obtain solutions for complex graphs of rotations containing chains and even loops of rotation equations. The way to solve chains of rotation equations is shown in Section 7, while Section 8 is devoted to the case of loops. Finally. in Section 9, we show how the propagation method can be applied to certain types of spatial problems and. in Section 10, we solve as an example a spatial six-bar problem. 2. S P H E R I C A L M E C H A N I S M S AS A R T I C U L A T E D S P H E R I C A L P O L Y G O N S Consider a sphere of unit radius and center at O. Planes passing through O are called central planes. The intersection of a central plane with the surface of the sphere is a great circle. Arcs of great circles are geodesics in the surface of the sphere, and they play the role of straight lines in plane geometry. The diameter perpendicular to a central plane is its pole axis and the two intersection points of the sphere and the pole axis are called the poles of the plane and. by extension, of any arc of the great circle defined by the plane. When a direction is assigned to an arc of a great circle, we define as the positive pole the one lying at the left side of the arc, and we will refer to it simply as the pole of the arc. The angle between two arcs of great circles is the same as that formed by the corresponding central planes. The length of an arc is its measure in radians. A spherical polygon is a closed figure composed by arcs of great circles or sides, limited by common points or vertices. We allow the sides of a polygon to intersect forming a folded spherical polygon. If we assign a circulating direction to the sides of a spherical polygon, the exterior angle between two adjacent sides is defined as the angle measured from the prolongation beyond the common vertex of the first side to the second side. A spherical mechanism may be represented by an articulated spherical polygon in which sides correspond to links, and vertices correspond to revolute pairs. A link of length a is kinematically equivalent to a link of length 2n - a [18], thus the sides of the polygon may always be taken of length ~n/2 with the same mobility as the original polygon. Given a spherical polygon ~ , we define the reduced polygon over the side ~ as the supplementary polygon with sides a/~< n/2, for a l l j ¢: i. There is always a unique reduced polygon over each side ,,,. In the case that there is a supplementary polygon with all sides ~
(!)
where the operators Rx( ) and Rz( ) denote rotations about the x and z axes. respectively. The following relations are satisfied: R z ( ~ ) R x ( 0 ) = Rx( - 0)Rz(~ )
Rz(a) = Rx(n)Rz(rc - a)Rx(n)Rz(n).
(2)
(3)
Consider a spherical polygon ~ with vertices p, . . . . . p.. sides a~2. . . . . a., and exterior angles 0, . . . . . 0. (Fig. 1). Take a system of coordinate axes centered at O with its x-axis passing through Pt and its :-axis passing through the pole of side at:. A rotation of at2 radians about the z-axis
Solving multiloop linkages with limited-range joints
375
Y G23
P3
/
i
/
Gx2
.17".......................................................
.......... t .....
C1~34
Jr
Px
%.
~ %%****
*
~
•-.a...
:
g
.
i
.."
\ii °.......
..
Pn-
1
Fig. I. Spherical polygon.
will move the x-axis along the side ~t,2 till vertex P2. Next, a rotation of angle 02 about the new x-axis will make the z-axis pass through the pole of ~t23. Going on in this way all around the polygon, the x-axis will return to p, and the coordinate system will end up being in its original position. This may be expressed by means of a rotation equation stating that the composition of the successive rotations equals the identity transformation: Rx(0, )Rz(~'I2)Rx(02)Rz(~23) " • • Rx(0,)Rz(~,t ) = 1.
(4)
We say that (4) is the rotation equation associated to the spherical polygon ~ . If the polygon ~ represents a spherical mechanism, angles Gt0 correspond to the fixed angle lengths of the links, while 01 are the rotation variables of the revolute pairs. Equations associated to supplementary polygons are equivalent, and the use of relations (2) and (3) permits transforming one of them into any other• When writing the equation associated to a reduced n-gon over a side ctj, we will follow the convention of renaming the reduced side corresponding to ~tj as ~,, and the remaining reduced sides as ¢~,: Rx(li )Rz(c~t )Rx(12)Rz(~2) • • • R x ( / , ) R z ( ~ . )
= 1.
(5)
This notation makes a clear distinction between sides 0,, restricted to be ~ n / 2 , and ~. ~ n. Variables i~ are in general the original 0,s with a shift in their subindices, and possibly affected by a sign and an added constant. When one of the O~s is 0, the corresponding Rz rotation can be eliminated and the two neighboring Rx rotations are joined using the property R x ( 0 , )Rx(02) = R x ( 0 , + 02). MMT ~qJ~--C
376
ENmC ~ , P ,
and CAll.METORI~AS
In this case the number of variables l, will be lower than the number of variables 0,. A rotation equation with the conditions stated for equation (5), in which variable Rx rotations alternate with non-null constant Rz rotations, will be called the reduced rotation equation over variable O, 4. C L O S U R E C O N D I T I O N S FOR S P H E R I C A L P O L Y G O N S For a spherical triangle, equation (4) can be written in the form: RX(01 )Rz(~ 12)Rx(02 ) "~- Rz( - rt)l ) R x ( - 03 )Rz( - ~[23).
(6)
Using the usual orthogonal matrix representation for rotations, and performing the matrix products, we obtain from the matrix elements (!, I), (1, 2), and (I, 3): cos ~,2 = cos ~3t cos ~'23- sin ~'3t sin ~2; cos 03 -
sin ~t2 cos 02 = cos ~%, sin ~23 + sin ~3, cos ~23 cos 03 sin ~12 sin 02 = sin 03 sin %1
(7) (8) (9)
that are respectively the cosine, sine-cosine and sine laws, the three fundamental formulae of spherical trigonometry. Given a spherical triangle with reduced sides 01, 02, and ~j, the cosine law gives cos ~j = cos 0, cos 02 - sin 0! sin 02 cos 12.
(10)
In the last term, the factor cos 12 takes values between - I and I, and therefore cos(0, + 02)
cos
cosl0, - 021.
(I I)
Since both 01 and 02 are ~<~/2, the arguments of the cos function are all in the interval [0, ~t], in which this function is monotonically decreasing, therefore we obtain the closure condition for reduced spherical triangles: 10, - 021 0, + 02. (12) From (10) it is clear that, given 01 and 02, each value between the two extremes in (12) can be reached by ~3 for some value of/2 and thus (12) is a necessary and sufficient condition for the existence of a spherical triangle with reduced sides 0,, 02, ~3. Note that these inequalities hold also for any spherical triangle with (non reduced) sides 0~, 02, ~3 ~< it, but (12) ceases to be a sufficient condition when 01 + 02 > ~. The closure condition can be generalized to reduced spherical n-gons. For that we need the following lemmas: Lemma 1. Let 0, . . . . . 0, -,, ~, be the reduced sides o f a spherical n-gon where the 0,s are angles in the interval [0, lr/2], and ~ e[O, 1¢]. Then n--|
Y'. 0.
(13)
Proof. We will proceed by induction. It is true for n = 3 because it reduces to the second inequality of (12). Consider the spherical triangle with sides ~, 0 , - i , ~,, where ~ is an arc ~< ~ closing a reduced (n - l)-gon with the remaining first n - 2 sides (Fig. 2). By induction hypothesis
We have n-2
n-I
t-|
J-I
[] Lemma 2. [n the conditions o f Lemma !, for any 0 , , k --- i " . . n - 1 n-I
0,. i-l.t~k
(14)
Solving multiioop linkages with limited-range joints Qa
377
Pl
I
/'"
P,,4
¢.4
P.-I
Fig. 2. Proof of Lemma !.
Proof. Let ~A, q~a and ~c be arcs of length ~< n, as shown in Fig. 3. We have, using (12) and (13): k-I
a-I
a-I
~¢,-Y~Im l ¢,- l i nI;k + | ¢,--¢,-
2; ¢,.
[]
~ - I.i~tk
Theorem I (Closure condition for spherical polygons) Let dp, . . . . .
dp, _ , be angles ~ [O. n / 2l a n d q~,e [O. n]. Let cku = max(dp , . . . . .
~P, - i ). Then. the
necessary and sufficient condition f o r sides dpl . . . . . ¢~. - i, ~ to f o r m a spherical polygon is
CM-
Y'. ~,-<~.-
(ts)
i-I
iC t ~R t t
~m Fig. 3. Proof of/..emma 2.
378
ENRICC~t~Y~ and CAR~ TO~AS
The necessity part is a consequence of Lemmas I and 2. The proof for sufficiency can be found in [19] and [20] and will not be reproduced here. An alternative but equivalent form to write the closure condition is ~,~<~
~
~b,
(16)
where ~M is now the greatest of ~ . . . . . ~b,_ ~, ~,. In particular, when ~, > n/2, we will have ~M - ~,, and (16) becomes ~, ~ IK~.:,' ~b,. 5. F I N D I N G
SOLUTION
INTERVALS
FOR
VARIABLES
W e say that a vector a = (at ..... a,) is a solution of the rotation equation (4), if the substitution of the values a, for the variables O, satisfies the equation. The set of all the solution vectors a of (4) is an (n -3)-dimensional subset of the n-torus T " = S t x .-. × S ~. The projection of the solution subset on the axis O, gives the set S, of values of O, for which there is a solution. Since a general rotation equation can be written in reduced form over any side, it is sufficient to find the solution for variable l, in a reduced equation. Then, the solution set for variable O, is easily obtained from the solution set of the variable I, of the corresponding reduced equation. Next, given a reduced rotation equation Rx(It )Rz(~b, )Rx(12) • • • Rz(b. _, )Rx(l.)Rz(~.)
=
I,
(I 7)
we provide a general procedure for finding the set S, of values for I, for which a solution for the equation can be found. Equation (17) will have a solution if and only if sides ~ ..... ~, _ ~, ~, can form a closed spherical polygon, which can be tested using (15) or (16). Essentially, the methodology followed to find the solution set for I, consists in substituting the two sides adjacent to I, by a single variable-length side, and then imposing the closure condition to the thus obtained (n - l)-gon. Consider the arc #~ ~< n joining vertices p~ and p, _ ~ (Fig. 2). The arc ~ forms a spherical triangle with sides ~,_ t and ~,. and its length ~(I,) dcpends on the angle I, in the form cos ~(I,) = cos ~, _ tcos ~, - sin ~),_ i sin ~. cos I,
(I 8)
according to (7). O n the other hand, the closure condition (15) applied to the reduced spherical polygon ~, ..... ~,-2,~ provides a range of possible values J=[J~,J2] for ~, where j, = m a x ( 0 , ~M - Y~':~,,, Mq6,), and J2 = rain(n, Y-7£~~,). We say that a value of I, is excessive if ~(I,) > ~ , and defective if ~(1,)
Solving multiloop linkages with limited-range joints
379
The above procedure permits obtaining the projections of the solution set on the axes 0~, by writing the reduced equation over the different variables and finding the corresponding solution set S,. The set of all projections S, define an enclosing box for the solution. Solution vectors a can be obtained in the following constructive way: Fix a variable 0, to a constant value a~ from its solution set S,. A new equation with one less variable is obtained which, in turn, may be used to obtain a solution set S; for another variable 0s. T h e solution set S; thus obtained will be a subset of the solution set S s for this variable in the original equation. Now, variable 0s may be fixed to a constant value as within S;, and the process iterated until all variables are fixed. The method guarantees arrival at a solution vector for whatever values we choose from the corresponding solution sets. 6. INTERVAL P R O P A G A T I O N WITHIN A ROTATION EQUATION In the preceding section we obtained the set of values that each rotation variable 0~ can take in the equation extracted from a loop of rotations. But sometimes a revolute pair is not able to perform complete rotations due to design constraints, collision between the two links, or even limitations coming from other loops sharing the same revolute pair. Thus, in general, the values for the variable 0, are restricted to an interval [il, iz]- The problem is then finding all the values for the remaining variables that are compatible with those actually possible for 0~, i.e. the propagation of intervals. Consider the mapping M,j that for each value of 0~ gives the set of compatible values for 0s. Figure 4 shows the graphical representation of M , for a particular rotation equation. This graph can be obtained by fixing 01 at different values and then finding the solution set for 0s in the resulting
05 2~"
/I
............................. i.i...iiiii.ii
....
!
Sj
iiiiiiiilIiiiiiiiiiiiiii b ..................................... i
.i
\
/ :
M~l(b )
01
'
:
"~--. s , J Fig. 4. Mapping M , for the equation: Rx(01 )Rz(4.5)Rx(02)Rz(3.5)Rx(0~ )Rz(6.1)Rx(0,)Rz(I. l)Rx(0 s)Rz(1.8) = !.
2~
380
E,~alc CEL.*YAand CAa~ Toaa~ (a)
Ib~
0s
e1
2x
J
2:t mh
My
b ......... V
......
0 g
2g
Fig. 5. Unfeasible mappings M,j.
equations. Conversely, Fig. 4 can be seen as giving the values for 05 compatible with each value of 0s, i.e. the representation of the reciprocal mapping Ms~. The projection of the shaded region on the horizontal axis is the set S~ of the kinematically allowed values for 0~, and the projection on the vertical axis is the set S~ of the kinematically allowed values for 0s. An important property of mappings M,~ is that each of its local extrema must correspond to one of the limits of the intervals forming Sj. This is so because the existence of a local extremum different from one of such limits would give rise to a situation of one of the two types shown in Fig. 5. Situations such as that in Fig. 5(a) are forbidden since the set of values for 0, corresponding to the value 0s-- b would be formed by an interval plus a single point, while, as noted in the preceding section, if the set of solutions has two disconnected parts, both must have the same length. Situations such as that in Fig. 5(b) are also excluded for the following reason. For 0j = b the solution set for 01 is formed by two separate points, which implies that there are both excessive and defective values for this variable, according to the terminology introduced in the preceding section. However, for values of 0j arbitrarily close to b only one interval remains, which implies that either the defective or the excessive set suddenly vanishes. This is not possible since the defective and excessive sets vary continuously with the lengths of the sides, and hence with the values of 0j. Our objective is finding the set M,A[i~, i,]) of values of 0j compatible with values of 0~ in the interval [i,, i:]. Clearly, M,j([i,, ~]) is a subset of Sj that includes the images of i~ and !'.,, M,j(i~ ) and M,j(i2), respectively. Thus, we are led to examine the difference set B = Sj - (M,s(i ~)uM,j(i,)), which is formed by at most 6 different intervals. We have seen that no interior point of Ss can correspond to an extreme value, and the same applies to the interior points of B. This implies that, given one of the intervals constituting B, either all of its points are in M~j([i~, i2]), or none are. Then it is sufficient to check a single interior point in each interval to either include the whole interval in the solution or exclude the whole interval from it. More precisely: given a rotation equation, the set M,j([i~,/2]) of values of 0j kinematically compatible with some value of 0, in a given interval [i~,i,] can be found with the following algorithm.
Propagation algorithm - - Find the sets Ss, Mij(i~) and Mij(i2), and form the difference set B = Sj - (M,j(il)uMq(i,)). - - For each interval (ak,bk) of B take the interior point p k = ( a ~ + b k ) / 2 and find the corresponding solution set Mj,(pk) for 0,. If the intersection Ms~(p~)c~[it,i2] ---0, then include index k in the set V of indices of non image intervals.
Solving multiloop finkages with limited-range joints
381
0~ 2x
.'..'?.22.'.2.'~2.'.?i.:7.'.'..'.?,.'..'[22: '.:.'.-.'..'Z~2Z.. .~.7.~.~.~.y.~.~.~.~.~.L~`.~.7~.~.~.~.~.~.~.~.~.~°22~2~22Z~22~2Z~2227~2Z2~
-IIIIII:III:0
!
Mv([i t, iz ]) B
Mv(i I) U Mv(i~)
$~
0 '
I
0
q
O~ z
2~
Fig. 6. Propagation of the interval [i,.is].
- - ReturnM,j([i~, i21)= S j - U~. vial, bk] as the solution. Figure 6 shows an example of how the different sets used in the algorithm are obtained.
7. INTERVAL P R O P A G A T I O N IN A CHAIN OF ROTATION EQUATIONS Suppose we have two rotation loops sharing a revolute pair. This yields a system of two rotation equations E~ and E2 with a common rotational variable 01.2. Each equation provides a set of solution values S~.2 and S~.2 for 0~.2, and values compatible with both equations are obtained by intersection of these sets. Once the set of compatible values has been obtained for the common variable 01~, the propagation algorithm permits finding the solution sets compatible with both equations for all variables. Suppose that, instead of two, we have N rotation equations E~, i -- ! . . . . . N. We say that they form an open chain of rotation equations if each equation shares one variable 0~_ ~ with the previous one, and another variable 0~ +~ with the next one, except for the extreme equations of the chain, E~ and E~, as is diagrammatically represented in Fig. 7(a). The globally compatible solution sets for shared variables in an open chain of rotation equations are obtained after a double pass along the chain, as explained next. The first equation E, provides a solution set S~.2 for 0~.2. This set is propagated within E~ to the following shared variable 0~. Successive propagations are performed for the shared variables in the corresponding equations until a set is obtained for variable 0n_ ~.N. This set is then intersected with the solution set provided by EM for 0M_ ~.~, and the result is propagated again in the reverse direction and intersected with the sets obtained in the first pass. When E, is reached, as the set obtained for 0~.~ will be a subset of the original S~.~, a new intersection will not change it, and no further propagations are needed. The sets obtained for all shared variables after these propagations will be fully compatible with global solutions of the chain of equations. From the solution sets of the shared variables, solution sets for the remaining ones can be obtained by propagation within the corresponding equations.
382
E,~RzcCEI.AYAand CA~t~TORRAS
a) Chain of rotation equations
b) Loop of rotation equations Fig. 7. Basicgraphs of rotation equations.
8. INTERVAL PROPAGATION IN A LOOP OF ROTATION EQUATIONS Now we consider the case of a closed chain of rotation equations, i.e. a chain in which the first and last equations, E, and Es, share also one variable 0s.t [Fig. 7(b)]. This will be called a loop of rotation equations. In a loop of rotation equations the propagation method for chains is not sufficient to obtain global compatible solution sets. For that we need some new concepts. Given a loop of rotation equations, we denote by M~+ the mapping that for each value of the shared variable 0~_ tj gives the set of compatible values for the next shared variable 0,~ +t according to equation E~. Similarly M f will denote the reciprocal mapping from 0,~+, to 0~_ ,j. Definition I. The descendants at level i of a value p for Os.l is the set of values for 0~.,+t obtained by the composition of mappings M T " ' " M ~ (p). The set M~'(p) of descendants at level I o f p is obtained by fixing Os.j to p and applying the procedure described in Section 5. Descendants at higher levels are obtained from the descendants set at the preceding level by propagating the intervals as described in Section 6. Thus, the descendants at different levels are obtained by the successive propagations of intervals for the shared variables throughout the equations of the loop. As in each rotation equation the image of one interval may be split into two disconnected intervals, the descendant set at level i can have a maximum of 2' intervals.
Solving multiloop linkages with limited-range joints
383
If the propagation is performed in the opposite direction, we have similarly: Definition 2. The ancestors at level i of a value p for ON.~is the set of values for ON_ ,.N-,+' obtained by the composition of mappings M ~_,+ ~ . " M.~ (p). If, instead of the complete sets given by mappings M~" or M F , only a single value is chosen and propagated at each level, the result will be a sequence of values for the shared variables compatible within each equation. Definition 3. A trajectory from a value p for ON.I to a value q for 0,~+, is a sequence of values rj,j = 0 . . . i, such that ro = p, r, = q and each rj, j = ! . . . i, is a value for Ojj +i contained in the set M ~ (rj_ i) of descendants at let,el ! of r, _ i. Trajectories are reversible in the sense that if a trajectory is taken in the reverse direction, each value is an ancestor of the preceding one. The sets of descendants (respectively, ancestors) of 0N.~ at level N are the values for the same variable ON.I obtained after a complete forward (respectively, backward) propagation all around the loop. Definition 4. The set of successors of a value p for ON: denoted by Succ(p), is the set of descendants at level N of p. Definition 5. The set of predecessors of a value p for ON.Idenoted by Pred(p), is the set of its ancestors at level N. The mappings Succ and Pred are reciprocal (in the same sense as the mappings M,j and Mjt introduced in Section 6) and the graphical representation of Succ can be seen as the representation of Pred when interpreted in the reverse direction. Definition 6. A value p for ON: is a fixed point of a loop if it is contained in the set of its successors Succ(p ). If a value p of ON: is a fixed point, then there is a trajectory with origin and final point in p. As trajectories are reversible, the definition of fixed point does not change if the predecessors set Pred(p) is used instead of Succ(p). Fixed points, together with the periodic trajectories containing them, provide the globally compatible solutions of a loop of rotation equations. Therefore the problem of finding global solutions of a loop of equations reduces to finding the fixed points of the mapping of the circumference onto itself defined by SUcC(ON: ). All fixed points can be obtained from the graphical representation of Succ(ON:) as the intersection with the diagonal (Figs 8 and 9). 8. ! Convergence to fixed points
The graphical method of finding fixed points is not optimal. The sets Succ(ON.i) must be computed at regular intervals scanning all the allowed set of ON.I . In addition the precision obtained in the location of the fixed points will be of the same order as the sampling step of ON:. We will discuss how a more efficient procedure can be used to isolate the set of fixed points. The basic idea is that, given an interval I0 for 0~:, its fixed points must be contained into the sets Succ(lo) and Pred(lo), and hence I0 may be replaced by the set I, = lo~Succ(lo)nPred(lo). Iterating the process more reduced sets will be obtained, that eventually will converge to a stable set for which l,~Succ(l,)t~ered(l,) = !,. To simplify, let us assume for the moment that the map SUCC(ON.I) is a continuous single-valued function (Fig. 10). The evolution of the sequence of sets obtained by the iterated substitution l,-.l,r~Succ(l,)~Pred(l,) = !,+ i will be one of the following: (A) The interval vanishes. This situation occurs when I0 does not contain fixed points. (B) The interval converges to a fixed point. (C) The interval converges to a stable interval. (D) The interval splits into two or more separate intervals. This implies the existence of either a change in the sign of the first derivative [Fig. ! I(a)] or a zero crossing [Fig. I l(b)] of the Succ function within the interval.
ENRIC CELAYA and CARME TOR~AS
384
Succ( 0 ,~ ) 2x'a
'I .......................... t/Jl?l.../I
0
~" 0
~P'
¢
I
n P'P'
0 3.1
2x
I:ig. s. Fixed points or 0,., for the loop of equations: Rx(O)., + n )Rz(5.2)Rx(v I )Rz(3.3)Rx(v: )Rz(4.7)Rx(0,. 2 )Rz(2.6) = I Rx(01. :)Rz( 1.8)Rx(v))Rz(2.6)Rx(v,)Rz(2.6)Rx(02j )Rz(1.5) = I Rx(O:.) )Rz(2.3)Rx(vs )Rz(2.6)Rx(v 6 )Rz(3.8)Rx(0). I + n )Rz(2. I ) = I.
In case (D), each of the intervals may be considered separately and, as the number of extrema and zero crossings must be finite, the iteration of the process for each of them will end in one of the situations (A), (B) or (C). Case (C) implies the existence of a fixed point in the stable interval I, (if I, # [0, 2n]). In this case, we split I, into two halves (IA and la in Fig. 10) and treat each of them separately. Proceeding in this way, all intervals will eventually vanish or converge to fixed points. In practice, it is more efficient to reach convergence for Succ and Pred functions in turn, because usually one of them converges before the other (typically, after the first or second iteration). Thus, we can perform the substitution l , ~ l ~ S u c c ( l , ) = I,÷, until convergence to an interval I,+ is reached, and next, departing from 1~, perform the substitution lf-,i/aPred(lj) = lj. j until a new convergence. The above analysis was done for mappings Succ(OM.,) constituting continuous single-valued functions. For mappings Succ(ON, ) decomposable in a finite number of branches or determinations, each of them being a continuous single-valued function, things are not essentially different, since each branch will evolve in one of the forms described. Thus, the above treatment permits dealing with all cases in which none of the equations E~ in the loop has more than 4 variables. In the general case, in which the map Succ(OM., ) may assign an interval (or a number of intervals) to each point, things are a little worse: a stable interval I, could, in principle, contain non-fixed points and remain stable even after splitting it. Some of these hypothetic cases are shown in Fig. 12. Checking the" extremes of a stable interval for fixed points permits finding, by successive approximations, the limits of a fixed interval [Fig. 12(a)], but the fact that the extremes of a stable interval are fixed points does not guarantee that all points in the interval are fixed [Fig. 12(b)]. More
Solving multilooplinkageswith limited-rangejoints
385
SuccfO ,.i )
0
I~ 0
Pa
, 7t
[
po
0 J,I
2~
Fig. 9. Fixed points of 0~.~for the loop of equations: Rx(0~.l + n )Rz(5.2)Rx(v I )Rz(3.3)Rx(r 2 )Rz(4.8IRx(01. ;)Rz(2.6)
--
I
Rx(01.: )Rz( 1.8)Rx(v~ )Rz(2.6)Rx(v~ )Rz(2.6)Rx(0;.~ )Rz(1.5) = I RX(02. ~)Rz(2.3)Rx(v~)Rz(2)Rx(v b)Rz(2.6)Rx(r7 )Rz( 3.7)Rx(0~. I + n )Rz(2. I ) = / .
research is needed in order to disclose whether such ill-behaved cases can be detected, or even if they can arise at all. Tests performed over randomly generated examples are encouraging, in the sense that convergence to the fixed set is often obtained after a short number of iterations. Since chains and loops are the elementary constituents or arbitrary graphs, Sections 7 and 8 provide the basis for solving coupled systems of rotation equations corresponding to complex graphs of rotations. In addition, as the same propagation machinery can be used for intervals imposed by kinematic constraints or by other types of constraints, non-intersection conditions imposed by collisions between adjacent links can also be dealt with. Yet, the general non-intersection problem cannot be tackled using this approach, since for non-adjacent links, the colliding interval depends on the whole linkage configuration. 9. APPLICATION TO SPATIAL PROBLEMS Until now, we have been dealing only with rotation equations, i.e. spherical mechanisms. In this section we show how the above methods can be applied to solving some types of spatial mechanisms. A spatial mechanism has associated a loop equation of transformations in the same form as a spherical mechanism, with the difference that it contains translation operators of the form Tram(u, d), where u is the director vector along which the translation is performed and d is a length parameter. All translation transformations can be written in terms of a uniparametric translation
Esatc CELAYA and CAaJ~t T o a a ~
386
Succ(O M.J) 2~
/ "
'i
Is
*
~ t o e J ~ I I l
*
E:
0 0 0
io !
.... ....
:
iP
i
l~
i
e~.l
: t i
In
:
2
Is [0
Fig. lO. Evolution of an interval for a hypothetic continuous single valued function Succ(O~): interval Io converges to the stable interval Is. which we split into two intervals !~ and Ij. Then 1~ vanishes while IR converges to the fixed point p.
(a)
(b)
Succ(0 ..~) 2g
Succ(e..,)
/
-
,
21g
i I
i
l,
//' /
Io
I ,
, I
, ,
I !
, ,
I ,
:
,
,,
°".....°
! !
/
/
\ e N.I
0
/
r
.e"
i i
i
!
! !
!
,
~i
e 1¢.1
2 /0
Io Fig. I 1 Splitting
intervals.
Solving multiloop linkages with limited-range joints (a)
387 (b)
S ucc( 0 Ma )
Succ(OM, I 2n
2x
-
I,
/
0 NJ U
"'
I:
"z
2rt
V
0
O N.I
I,
2rt
Fig. 12. Stable intervals.
operator, that we denote by Tranx(d), which represents a translation in the x direction. Thus, an arbitrary translation can be expressed as: Trans(u, d) = R, Tranx(d)R.'
where R, is a rotation that leaves the x-axis in the direction of the vector u. Associated with each spatial mechanism there is a spherical mechanism called its spherical indicatrix, whose revolute axes are parallel to those in the spatial mechanism. The equation associated to the spherical indicatrix is obtained from the spatial equation simply by removing the translation operators. 9. I Mechanisms with three translational pairs
A solution of a spatial equation must satisfy simultaneously a rotational condition, given by the rotation equation of its spherical indicatrix, plus a translational condition stating that the sum of all translations equals 0. As translations do not affect orientation, each solution of the spatial mechanism corresponds to a solution of its spherical indicatrix. Conversely, given a particular solution of the spherical indicatrix, the directions of the translations are fixed, and only if their lengths are such that they cancel each other out, or if the translational freedoms allow it, the rotational solution can be extended to a spatial solution. It is clear that spatial mechanisms containing three or more translational freedoms can cope always with the translational condition, provided the translational freedom directions are not all coplanar. In this case the translational condition imposes no restriction to the rotational solution (apart from the non-coplanarity of the freedom directions) and all solutions of the spherical indicatrix can be extended to a solution of the spatial mechanism. Since all rotational solutions can be found using the procedure in Section 5, spatial problems containing at least three translational pairs can be considered solved. 9.2. Triangulable spatial mechanisms
Next we deal with a class of spatial mechanisms, that we call triangulable, for which the translation condition can be formulated as a compatibility problem in a loop of rotation equations and, consequently, can be solved using the propagation algorithm given in the preceding section. All spatial mechanisms containing no translational freedoms and no more than three constant
388
EsaJc C~X~AVAand CAgMETOaaA.S
translation operators are triangulable. A number of spatial mechanisms with more than three constant translations can actually be rewritten with only three Traax operators using the relations: Tranx(d)Rx(0) = Rx(0)Tranx(d) Tranx(d, )Rz(~bt )Tranx(d: ) = Rz(~b:)Tranx(d~)Rz(q~3 )
(19) (20)
where 0z = arctan(d., sin ¢bI/(dl + d, cos ¢b, ))
a3 = (a~ + a~ + 2a, a'2 cos 4,, ),,,2 ¢3 = arctan(dl sin 4h/(d2 + dl cos ~b,)). The general equation of a triangulable mechanism will take the form Tranx(dt )Ri Tranx(d2)Rz Tranx(d3)R3 - I
(2 I)
where R, are chains of rotations with an arbitrary number of variables, and d, are constant lengths. We can formally decompose the complex rotations R, in the form given in equation (1):
R, = Rx(0,1 )Rz(~i)Rx(0,:)
(22)
and (21) takes the form Tranx(dl )Rx(0, )Rz(q~l )Rx(012 )Tranx(d2 )Rx(02, )Rz(~b2)Rx(0z2) Tranx(d3)Rx(0~,)Rz(~3)Rx(0j2) = L
(23)
Using (19) we obtain Tranx(d, )Rz(O, )Rx(0,z + 02, )Tranx(d2 )Rz(q~ 2)Rx(022 + 03! ) Tranx(d3)Rz(O3)Rx(032 + 01,) = I.
(24)
The only rotations in (24) that change the directions of the translation vectors are the Rz rotations, and since the translations must form a triangle, i.e. a planar figure, the three rotation axes must be parallel. This implies that the Rx rotations must be null, yielding O,z=-O:,;
0.,2=-03,;
03.=-0,,,
(25)
and (23) reduces to Tranx(dl )Rz(¢l )Tranx(d:)Rz(~2)Tranx(d3)Rz(~3) : L
(26)
Now angles ~, can be found using elementary planar trigonometry. Thus we obtain a loop of three rotation equations given by substituting (25) in (22) for i = I, 2, 3: RI = Rx(Oll )Rz(01 )Rx( - 0, I ).
(27)
R 2 -- Rx(021 )Rz(02)Rx ( - 031).
(28)
R 3 -- Rx(03! )Rz(~ 3)Rx( - 0it ).
(29)
The system of rotation equations (27), (28) and (29) is equivalent to the spatial equation (21) and involves all the variables in (21) plus three auxiliary variables 00~, which are the shared variables defining a loop of rotation equations. Thus we have transformed the equation of a spatial mechanism into a loop of three rotation equations that can be solved by the propagation method. 10. E X A M P L E Figure 13 represents a spatial 6-bar mechanism with 6 fixed-length links connected by revolute pairs. A seventh extra revolute pair in link 6 provides mobility to the mechanism. Our objective is to find the solutions for the angles of the pairs. This can be stated as an assembly problem as defined in the introduction: if we consider the open chain (Fig. 14), the goal is to find the pair configurations for which the mechanism closes.
Solving multiloop linkages with limited-range joints
~,mea
Fig. 13. A 6-bar mechanism.
The spatial equation is: Tranx(0.5)Rx(0, )Rz(2n/3)Tranx(I.3)Rx(02)Rz(n/3) Tranx(0.7)Rx(03)Rz(5n/9)Tranx(0.9)Rx(04)Rz(4n/9) Tranx(I.5)Rx(0~ )Rz(7n/9)Tranx(I)Rx(0,)Rz(n/4)Rx(07)Rz(n/3) = [.
, ~ l l d l,~im
Fig. 14. Open chain corresponding to the 6-bar mechanism.
389
ENRIC CELAYAand CARMETOaaA.S
390
We see that this spatial mechanism is triangulable, since using (19) and (20) we obtain Tranx(i. 136)Rz(0.391)Rx(0,)Rz(n/3)Rx(03)Rz(1.02 !) Tranx(I.040)Rz(0.725)Rx(04)Rz(4n/9)Rx(05)Rz(0.719) Tranx(0.976)Rz( ! .724)Rx(06)Rz(n/4)Rx(07 )Rz(n/3)Rx(01 )Rz(1.703) = L which is in the form of (21). Equation (26) is: Tranx( I. 136)Rz(01 )Tranx(1.040)Rz(4,:)Tranx(0.976)Rz(O3) = L and solving for the exterior angles of the triangle we obtain: ~ = 2.215;
~2= 1.946;
~3= 2.122,
thus the loop of equations can be written as: Rz(0.391 )Rx(0z)Rz(n/3)Rx(0~ )Rz( !.021 )Rx( - 0b)Rz(2.215)Rx(0o) = I Rz(0.725)Rx(04 )Rz(4n/9)Rx(05 )Rz(0.719)Rx( - 0c)Rz(1.946)Rx(0b) = I
Rz(I.724)Rx(Or)Rz(n/4)Rx(OT)Rz(n/3)Rx(Ot
)Rz( 1.703)Rx(-0a)Rz(2.122)Rx(0¢) = L
The function Succ(Oa) for this loop is represented in Fig. 15, from which the set of fixed points of 0~ is found to be the interval [p.~,ps]. When the method for finding the set of fixed points explained in Section 8.1 is applied, convergence is reached after the first iteration, and the values pa -- 1.832 and Pa = 4.451 are found. Now we can obtain explicit solutions by assigning values to 0~ within the interval of its fixed points. For example, if we choose the value 0~ = 2, we find a periodic trajectory with 0~ -- 3.882 and O, -- 4.182 for the auxiliary variables, from which the values 0, = !.823, 02 -- 0.593, 0~ -- 0.363, 04 = 4.859, 05 = 1.649, 06 = 0.902, and 07 = 2.288 can be determined for the variables of the revolute pairs, which correspond to the configuration shown in Fig. 13.
Succ(O . ) 21
0
0o 0
Fig. 15. Mapping
rA
It
ra
2~
Succ(Oo) corresponding to the loop o f equations associated to the (>-bar problem.
Solving multiloop linkages with limited-rangejoints
391
I1. C O N C L U S I O N S The procedure to find solution intervals for variables, combined with the propagation algorithm presented in this paper, constitute the basis for the solution of arbitrary systems of rotation equations or, equivalently, o f multiloop spherical linkages. An enclosing box containing all the solution vectors is obtained as the direct product o f the sets of allowed values for each variable. Each time a variable is fixed to a particular value within its solution set, the solution sets for the remaining variables are reduced to the compatible values. A solution vector far from limit positions is easy to obtain just by fixing each variable at the centre of its interval. This way to obtain the solution is convenient for what we have called assembly problems, in which there is a need to find specific solutions, as well as the extreme values that variables can reach. This form o f solution can be seen as complementary to that of implicit equations relating variables (the usual form in Kinematics). The latter is amenable to velocity and acceleration analysis, but extreme values for variables are difficult to obtain and little guidance is provided for finding actual solution vectors (values for variables leading to real solutions for all the remaining variables). The constraints derived from having limited-range joints have been taken into account for the first time here, as far as the authors are aware of. The successful application of the propagation method to the solution o f spatial loops, gives the approach a remarkably increased interest. Acknowledgements--This work has been partially supported by the Comisibn Interministerial de Ciencia y Tecnologia (CICY'r), under the project "Automatic spatial reasoning based on constraints" (TIC 88-0197). and by the ESPRIT I!1 Basic Research Actions Program of the EC under contract No 6546 (project PROMOTION).
REFERENCES I. 2. 3. 4. 5. 6. 7. 8. 9.
C. V. K. Gupta and C. W. Radclife, J. Engng Ind. Trans. ASME 125-130 (1971). D. L. Pieper. Ph.D. c~issertation. Artificial Intelligence laboratory. Stanford University (1968). J. Duffy, Analysis of Mechanisms and Robot Manipulators. Edward Arnold. London (1980). J. Angeles, Spatial Kinematic Chains. Springer, Berlin (1982). R. P. Paul, Robot Manipulators: Mathematics, Programming. and Control. MIT Press. Cambridge, Mass. (1985). D. R. Baker and C. W. Wampler II. Robot. Res. 7, No. 2, 3-21 (1988). A. P. Ambler and R. J. Popplestone, Artif. Intell. 6, 157-174 (1975). R. J. Popplestone, A. P. Ambler and I. M. Bellos. Artif. Intell., 14, 79-107 (1980). E. Mazer, Proc. Int. Meeting on "Advanced Software in Robotics" (Edited by A. Danthine and M. G~rardin). North-Holland, Amsterdam (1983). I0. F. Thomas and C. Torras, IEEE J. Robot. Automat. 4, 622-634 (1988). 11. G. A. Kramer. Proc. of the Workshop on Clichis and Intermediate Metbodsfor Problem Solving. CEAB, Blanes (1991). 12. J. E. Baker, Mech. Mac& Theory 13, 333-343 (1978). 13. J. E. Baker, Mech. Mac& Theory IS, 255-265 (1980). 14. K. H. Hunt, Kinematic Geometry of Mechanisms. Clarendon Press, Oxford (1978). 15. F. L. Litvin, Mech. Mac& Theory IS, 115-125 (1980). 16. K. Sugimoto, J. Duffy and K. H. Hunt, Mech. Mac& Theory 17, 119-132 (1982). 17. C. Gosselin and J. Angeles, IEEE Trans. Robot. Automat. 6, 281-290 (1990). 18. C. H. Chiang, Kinematics of Spherical Mechanisms. Cambridge University Press, Cambridge (1988). 19. E. Celaya and C. Torras, Pro(:. of the ECAi'90, Stockholm, pp. 147-152 (1990). 20. E. Celaya, Ph.D. dissertation. Department of LSI Polytechnic University of Catalonia (1992).
S O L U T I O N DES C H A I N E S C I N E M A T I Q U E S A P L U S I E U R S B O U C L E S F E R M E E S ET AVEC L I M I T A T I O N S DE M O U V E M E N T AUX C O U P L E S R/~mt~'---Nous pr~sentons un¢ m(~thode pour obtenir la solution d'une &lUation souscontrainte de rotations, clans la forme de l'ensemble des valeurs que chaque variable peut pmndre. Un algorithme de propagation d'intervallesnous permet d'obtenir les sous-ensembles de la solution compatibles avec une intervalledonn~e pour une des variables, La tL'chniquede propagation nous permet de considc~rerdes contraintes de non-intersection.La propagation peut itre aussi utilisC-epour la rC-solutionde syst~mes de plusieures~luations de rotations.Finalement, on montre comment les algorithmes d/:critsicipeuvent ¢~tre utilis~-spour trouver la solution de certains probl~'mes espatiales, y comprenant mechanismes ,; six maillons. M M T ~!]~--D
Mech. Mach. Thcoey Vol. 29. No. 3, pp. 373--391. 1994 Copyright 'C 1994 Elsev~r Science Lid Printed in Great Britmn. ,All rights R~.rv~l 0094-114X/94 $6.00 + 0.00
Pergamon
SOLVING MULTILOOP LINKAGES WITH LIMITED-RANGE JOINTS ENRIC CELAYA and C A R M E TORRAS lnstitut de Cibernetica(UPC-CSIC). Diagonal 647. 08028-Barcelona, Spain (Received I 1 January 1993. receicedfor publication 17 March 1993)
Abstract--An algorithmto obtain the solutionof underconstraincdrotation equationsin the form of sets of valuesthat the variablescan take is given. Furthermore.an interval propagation algorithmis presented which parmits obtainingthe subsets of the solution that are compatiblewith a given interval for one of the variables. The propagation techniqueprovidesa way to take into account non-intersectionconstraints in the solution. In addition, propagation can b¢ used to solve systemsof rotation equations. Finally.it is shown how the algorithmsdescribed can be used in the solution of certain spatial problems, including (>-bar mechanisms,
I. I N T R O D U C T I O N The primary concern of this paper is to give an answer to the following questions: Can a particular set of links be assembled into a closed kinematic chain? Which are the configurations that produce such assembly? What is the range of permissible motion? Such issues find applications in mobility analysis of mechanisms, assembly planning, inverse kinematics of redundant and non-redundant manipulators, robot programming, etc. We characterize this kind of problems as assembly problems, defined in the following terms: given a set of bodies and a set of kinematic constraints that have to hold between them, find all the possible relative positions of the bodies that simultaneously satisfy all the kinematic and non-intersection constraints. We consider the general case of muhiloop kinematic chains, which is relevant for the study of parallel manipulators and of problems involving two or more cooperating manipulators. We also address the problem of limitations or ends of travel in the articulations, which usually exist in real mechanisms. As already noted in [I], such problems can in principle be investigated through the kinematic analysis of the relative displacements in the assembled mechanism for a series of positions of the input link. Positions for which assembly is not possible correspond to imaginary solutions of the loop equation. An important drawback of this approach is that it is unnecessarily costly in computational terms. A more fundamental limitation comes from the fact that the usual techniques to perform displacement analysis can deal only with single loop mechanisms with a single overall degree of freedom [2-5]. Thus, for example, traditional approaches to the solution of the inverse kinematics of redundant manipulators require to impose additional constraints, usually minimizing some specific cost function, in order to force the uniqueness of the solution [6]. In our approach the objective is to find the complete set of possible configurations, from which an appropriate one can be selected according to different criteria in each particular case. We propose a hierarchical form of description of the solution set, which allows the construction of explicit solution configurations. Other approaches have been taken from the perspective of assembly planning and robot programming [7-1 I], but none of them permits finding the solution when it contains ranges for the variables. On the other hand, methods aimed at obtaining the limit positions for articulations rely either on the use of analytic expressions, which are not always available, or on monitorization of extensive data computations [I 2-17]. 373
374
ENRICCELAYAand CARMETORRAS
We begin by introducing, in Sections 2 and 3, some basic concepts and notations, upon which a standard formulation of spherical problems is developed. Section 4 states the main theoretical results that provide the basis for the construction, in Section 5. of the solution of any single-loop spherical mechanism assuming completely rotatable joints. In Section 6. an interval propagation algorithm is given, which permits including in the solution the limitations existing in the joints. The same propagation algorithm is then shown to be useful to obtain solutions for complex graphs of rotations containing chains and even loops of rotation equations. The way to solve chains of rotation equations is shown in Section 7, while Section 8 is devoted to the case of loops. Finally. in Section 9, we show how the propagation method can be applied to certain types of spatial problems and. in Section 10, we solve as an example a spatial six-bar problem. 2. S P H E R I C A L M E C H A N I S M S AS A R T I C U L A T E D S P H E R I C A L P O L Y G O N S Consider a sphere of unit radius and center at O. Planes passing through O are called central planes. The intersection of a central plane with the surface of the sphere is a great circle. Arcs of great circles are geodesics in the surface of the sphere, and they play the role of straight lines in plane geometry. The diameter perpendicular to a central plane is its pole axis and the two intersection points of the sphere and the pole axis are called the poles of the plane and. by extension, of any arc of the great circle defined by the plane. When a direction is assigned to an arc of a great circle, we define as the positive pole the one lying at the left side of the arc, and we will refer to it simply as the pole of the arc. The angle between two arcs of great circles is the same as that formed by the corresponding central planes. The length of an arc is its measure in radians. A spherical polygon is a closed figure composed by arcs of great circles or sides, limited by common points or vertices. We allow the sides of a polygon to intersect forming a folded spherical polygon. If we assign a circulating direction to the sides of a spherical polygon, the exterior angle between two adjacent sides is defined as the angle measured from the prolongation beyond the common vertex of the first side to the second side. A spherical mechanism may be represented by an articulated spherical polygon in which sides correspond to links, and vertices correspond to revolute pairs. A link of length a is kinematically equivalent to a link of length 2n - a [18], thus the sides of the polygon may always be taken of length ~
(!)
where the operators Rx( ) and Rz( ) denote rotations about the x and z axes. respectively. The following relations are satisfied: R z ( ~ ) R x ( 0 ) = Rx( - 0)Rz(~ )
Rz(a) = Rx(n)Rz(rc - a)Rx(n)Rz(n).
(2)
(3)
Consider a spherical polygon ~ with vertices p, . . . . . p.. sides a~2. . . . . a., and exterior angles 0, . . . . . 0. (Fig. 1). Take a system of coordinate axes centered at O with its x-axis passing through Pt and its :-axis passing through the pole of side at:. A rotation of at2 radians about the z-axis
Solving multiloop linkages with limited-range joints
375
Y G23
P3
/
i
/
Gx2
.17".......................................................
.......... t .....
C1~34
Jr
Px
%.
~ %%****
*
~
•-.a...
:
g
.
i
.."
\ii °.......
..
Pn-
1
Fig. I. Spherical polygon.
will move the x-axis along the side ~t,2 till vertex P2. Next, a rotation of angle 02 about the new x-axis will make the z-axis pass through the pole of ~t23. Going on in this way all around the polygon, the x-axis will return to p, and the coordinate system will end up being in its original position. This may be expressed by means of a rotation equation stating that the composition of the successive rotations equals the identity transformation: Rx(0, )Rz(~'I2)Rx(02)Rz(~23) " • • Rx(0,)Rz(~,t ) = 1.
(4)
We say that (4) is the rotation equation associated to the spherical polygon ~ . If the polygon ~ represents a spherical mechanism, angles Gt0 correspond to the fixed angle lengths of the links, while 01 are the rotation variables of the revolute pairs. Equations associated to supplementary polygons are equivalent, and the use of relations (2) and (3) permits transforming one of them into any other• When writing the equation associated to a reduced n-gon over a side ctj, we will follow the convention of renaming the reduced side corresponding to ~tj as ~,, and the remaining reduced sides as ¢~,: Rx(li )Rz(c~t )Rx(12)Rz(~2) • • • R x ( / , ) R z ( ~ . )
= 1.
(5)
This notation makes a clear distinction between sides 0,, restricted to be ~ n / 2 , and ~. ~ n. Variables i~ are in general the original 0,s with a shift in their subindices, and possibly affected by a sign and an added constant. When one of the O~s is 0, the corresponding Rz rotation can be eliminated and the two neighboring Rx rotations are joined using the property R x ( 0 , )Rx(02) = R x ( 0 , + 02). MMT ~qJ~--C
376
ENmC ~ , P ,
and CAll.METORI~AS
In this case the number of variables l, will be lower than the number of variables 0,. A rotation equation with the conditions stated for equation (5), in which variable Rx rotations alternate with non-null constant Rz rotations, will be called the reduced rotation equation over variable O, 4. C L O S U R E C O N D I T I O N S FOR S P H E R I C A L P O L Y G O N S For a spherical triangle, equation (4) can be written in the form: RX(01 )Rz(~ 12)Rx(02 ) "~- Rz( - rt)l ) R x ( - 03 )Rz( - ~[23).
(6)
Using the usual orthogonal matrix representation for rotations, and performing the matrix products, we obtain from the matrix elements (!, I), (1, 2), and (I, 3): cos ~,2 = cos ~3t cos ~'23- sin ~'3t sin ~2; cos 03 -
sin ~t2 cos 02 = cos ~%, sin ~23 + sin ~3, cos ~23 cos 03 sin ~12 sin 02 = sin 03 sin %1
(7) (8) (9)
that are respectively the cosine, sine-cosine and sine laws, the three fundamental formulae of spherical trigonometry. Given a spherical triangle with reduced sides 01, 02, and ~j, the cosine law gives cos ~j = cos 0, cos 02 - sin 0! sin 02 cos 12.
(10)
In the last term, the factor cos 12 takes values between - I and I, and therefore cos(0, + 02)
cos
cosl0, - 021.
(I I)
Since both 01 and 02 are ~<~/2, the arguments of the cos function are all in the interval [0, ~t], in which this function is monotonically decreasing, therefore we obtain the closure condition for reduced spherical triangles: 10, - 021 0, + 02. (12) From (10) it is clear that, given 01 and 02, each value between the two extremes in (12) can be reached by ~3 for some value of/2 and thus (12) is a necessary and sufficient condition for the existence of a spherical triangle with reduced sides 0,, 02, ~3. Note that these inequalities hold also for any spherical triangle with (non reduced) sides 0~, 02, ~3 ~< it, but (12) ceases to be a sufficient condition when 01 + 02 > ~. The closure condition can be generalized to reduced spherical n-gons. For that we need the following lemmas: Lemma 1. Let 0, . . . . . 0, -,, ~, be the reduced sides o f a spherical n-gon where the 0,s are angles in the interval [0, lr/2], and ~ e[O, 1¢]. Then n--|
Y'. 0.
(13)
Proof. We will proceed by induction. It is true for n = 3 because it reduces to the second inequality of (12). Consider the spherical triangle with sides ~, 0 , - i , ~,, where ~ is an arc ~< ~ closing a reduced (n - l)-gon with the remaining first n - 2 sides (Fig. 2). By induction hypothesis
We have n-2
n-I
t-|
J-I
[] Lemma 2. [n the conditions o f Lemma !, for any 0 , , k --- i " . . n - 1 n-I
0,. i-l.t~k
(14)
Solving multiioop linkages with limited-range joints Qa
377
Pl
I
/'"
P,,4
¢.4
P.-I
Fig. 2. Proof of Lemma !.
Proof. Let ~A, q~a and ~c be arcs of length ~< n, as shown in Fig. 3. We have, using (12) and (13): k-I
a-I
a-I
~¢,-Y~Im l ¢,- l i nI;k + | ¢,--¢,-
2; ¢,.
[]
~ - I.i~tk
Theorem I (Closure condition for spherical polygons) Let dp, . . . . .
dp, _ , be angles ~ [O. n / 2l a n d q~,e [O. n]. Let cku = max(dp , . . . . .
~P, - i ). Then. the
necessary and sufficient condition f o r sides dpl . . . . . ¢~. - i, ~ to f o r m a spherical polygon is
CM-
Y'. ~,-<~.-
(ts)
i-I
iC t ~R t t
~m Fig. 3. Proof of/..emma 2.
378
ENRICC~t~Y~ and CAR~ TO~AS
The necessity part is a consequence of Lemmas I and 2. The proof for sufficiency can be found in [19] and [20] and will not be reproduced here. An alternative but equivalent form to write the closure condition is ~,~<~
~
~b,
(16)
where ~M is now the greatest of ~ . . . . . ~b,_ ~, ~,. In particular, when ~, > n/2, we will have ~M - ~,, and (16) becomes ~, ~ IK~.:,' ~b,. 5. F I N D I N G
SOLUTION
INTERVALS
FOR
VARIABLES
W e say that a vector a = (at ..... a,) is a solution of the rotation equation (4), if the substitution of the values a, for the variables O, satisfies the equation. The set of all the solution vectors a of (4) is an (n -3)-dimensional subset of the n-torus T " = S t x .-. × S ~. The projection of the solution subset on the axis O, gives the set S, of values of O, for which there is a solution. Since a general rotation equation can be written in reduced form over any side, it is sufficient to find the solution for variable l, in a reduced equation. Then, the solution set for variable O, is easily obtained from the solution set of the variable I, of the corresponding reduced equation. Next, given a reduced rotation equation Rx(It )Rz(~b, )Rx(12) • • • Rz(b. _, )Rx(l.)Rz(~.)
=
I,
(I 7)
we provide a general procedure for finding the set S, of values for I, for which a solution for the equation can be found. Equation (17) will have a solution if and only if sides ~ ..... ~, _ ~, ~, can form a closed spherical polygon, which can be tested using (15) or (16). Essentially, the methodology followed to find the solution set for I, consists in substituting the two sides adjacent to I, by a single variable-length side, and then imposing the closure condition to the thus obtained (n - l)-gon. Consider the arc #~ ~< n joining vertices p~ and p, _ ~ (Fig. 2). The arc ~ forms a spherical triangle with sides ~,_ t and ~,. and its length ~(I,) dcpends on the angle I, in the form cos ~(I,) = cos ~, _ tcos ~, - sin ~),_ i sin ~. cos I,
(I 8)
according to (7). O n the other hand, the closure condition (15) applied to the reduced spherical polygon ~, ..... ~,-2,~ provides a range of possible values J=[J~,J2] for ~, where j, = m a x ( 0 , ~M - Y~':~,,, Mq6,), and J2 = rain(n, Y-7£~~,). We say that a value of I, is excessive if ~(I,) > ~ , and defective if ~(1,)
Solving multiloop linkages with limited-range joints
379
The above procedure permits obtaining the projections of the solution set on the axes 0~, by writing the reduced equation over the different variables and finding the corresponding solution set S,. The set of all projections S, define an enclosing box for the solution. Solution vectors a can be obtained in the following constructive way: Fix a variable 0, to a constant value a~ from its solution set S,. A new equation with one less variable is obtained which, in turn, may be used to obtain a solution set S; for another variable 0s. T h e solution set S; thus obtained will be a subset of the solution set S s for this variable in the original equation. Now, variable 0s may be fixed to a constant value as within S;, and the process iterated until all variables are fixed. The method guarantees arrival at a solution vector for whatever values we choose from the corresponding solution sets. 6. INTERVAL P R O P A G A T I O N WITHIN A ROTATION EQUATION In the preceding section we obtained the set of values that each rotation variable 0~ can take in the equation extracted from a loop of rotations. But sometimes a revolute pair is not able to perform complete rotations due to design constraints, collision between the two links, or even limitations coming from other loops sharing the same revolute pair. Thus, in general, the values for the variable 0, are restricted to an interval [il, iz]- The problem is then finding all the values for the remaining variables that are compatible with those actually possible for 0~, i.e. the propagation of intervals. Consider the mapping M,j that for each value of 0~ gives the set of compatible values for 0s. Figure 4 shows the graphical representation of M , for a particular rotation equation. This graph can be obtained by fixing 01 at different values and then finding the solution set for 0s in the resulting
05 2~"
/I
............................. i.i...iiiii.ii
....
!
Sj
iiiiiiiilIiiiiiiiiiiiiii b ..................................... i
.i
\
/ :
M~l(b )
01
'
:
"~--. s , J Fig. 4. Mapping M , for the equation: Rx(01 )Rz(4.5)Rx(02)Rz(3.5)Rx(0~ )Rz(6.1)Rx(0,)Rz(I. l)Rx(0 s)Rz(1.8) = !.
2~
380
E,~alc CEL.*YAand CAa~ Toaa~ (a)
Ib~
0s
e1
2x
J
2:t mh
My
b ......... V
......
0 g
2g
Fig. 5. Unfeasible mappings M,j.
equations. Conversely, Fig. 4 can be seen as giving the values for 05 compatible with each value of 0s, i.e. the representation of the reciprocal mapping Ms~. The projection of the shaded region on the horizontal axis is the set S~ of the kinematically allowed values for 0~, and the projection on the vertical axis is the set S~ of the kinematically allowed values for 0s. An important property of mappings M,~ is that each of its local extrema must correspond to one of the limits of the intervals forming Sj. This is so because the existence of a local extremum different from one of such limits would give rise to a situation of one of the two types shown in Fig. 5. Situations such as that in Fig. 5(a) are forbidden since the set of values for 0, corresponding to the value 0s-- b would be formed by an interval plus a single point, while, as noted in the preceding section, if the set of solutions has two disconnected parts, both must have the same length. Situations such as that in Fig. 5(b) are also excluded for the following reason. For 0j = b the solution set for 01 is formed by two separate points, which implies that there are both excessive and defective values for this variable, according to the terminology introduced in the preceding section. However, for values of 0j arbitrarily close to b only one interval remains, which implies that either the defective or the excessive set suddenly vanishes. This is not possible since the defective and excessive sets vary continuously with the lengths of the sides, and hence with the values of 0j. Our objective is finding the set M,A[i~, i,]) of values of 0j compatible with values of 0~ in the interval [i,, i:]. Clearly, M,j([i,, ~]) is a subset of Sj that includes the images of i~ and !'.,, M,j(i~ ) and M,j(i2), respectively. Thus, we are led to examine the difference set B = Sj - (M,s(i ~)uM,j(i,)), which is formed by at most 6 different intervals. We have seen that no interior point of Ss can correspond to an extreme value, and the same applies to the interior points of B. This implies that, given one of the intervals constituting B, either all of its points are in M~j([i~, i2]), or none are. Then it is sufficient to check a single interior point in each interval to either include the whole interval in the solution or exclude the whole interval from it. More precisely: given a rotation equation, the set M,j([i~,/2]) of values of 0j kinematically compatible with some value of 0, in a given interval [i~,i,] can be found with the following algorithm.
Propagation algorithm - - Find the sets Ss, Mij(i~) and Mij(i2), and form the difference set B = Sj - (M,j(il)uMq(i,)). - - For each interval (ak,bk) of B take the interior point p k = ( a ~ + b k ) / 2 and find the corresponding solution set Mj,(pk) for 0,. If the intersection Ms~(p~)c~[it,i2] ---0, then include index k in the set V of indices of non image intervals.
Solving multiloop finkages with limited-range joints
381
0~ 2x
.'..'?.22.'.2.'~2.'.?i.:7.'.'..'.?,.'..'[22: '.:.'.-.'..'Z~2Z.. .~.7.~.~.~.y.~.~.~.~.~.L~`.~.7~.~.~.~.~.~.~.~.~.~°22~2~22Z~22~2Z~2227~2Z2~
-IIIIII:III:0
!
Mv([i t, iz ]) B
Mv(i I) U Mv(i~)
$~
0 '
I
0
q
O~ z
2~
Fig. 6. Propagation of the interval [i,.is].
- - ReturnM,j([i~, i21)= S j - U~. vial, bk] as the solution. Figure 6 shows an example of how the different sets used in the algorithm are obtained.
7. INTERVAL P R O P A G A T I O N IN A CHAIN OF ROTATION EQUATIONS Suppose we have two rotation loops sharing a revolute pair. This yields a system of two rotation equations E~ and E2 with a common rotational variable 01.2. Each equation provides a set of solution values S~.2 and S~.2 for 0~.2, and values compatible with both equations are obtained by intersection of these sets. Once the set of compatible values has been obtained for the common variable 01~, the propagation algorithm permits finding the solution sets compatible with both equations for all variables. Suppose that, instead of two, we have N rotation equations E~, i -- ! . . . . . N. We say that they form an open chain of rotation equations if each equation shares one variable 0~_ ~ with the previous one, and another variable 0~ +~ with the next one, except for the extreme equations of the chain, E~ and E~, as is diagrammatically represented in Fig. 7(a). The globally compatible solution sets for shared variables in an open chain of rotation equations are obtained after a double pass along the chain, as explained next. The first equation E, provides a solution set S~.2 for 0~.2. This set is propagated within E~ to the following shared variable 0~. Successive propagations are performed for the shared variables in the corresponding equations until a set is obtained for variable 0n_ ~.N. This set is then intersected with the solution set provided by EM for 0M_ ~.~, and the result is propagated again in the reverse direction and intersected with the sets obtained in the first pass. When E, is reached, as the set obtained for 0~.~ will be a subset of the original S~.~, a new intersection will not change it, and no further propagations are needed. The sets obtained for all shared variables after these propagations will be fully compatible with global solutions of the chain of equations. From the solution sets of the shared variables, solution sets for the remaining ones can be obtained by propagation within the corresponding equations.
382
E,~RzcCEI.AYAand CA~t~TORRAS
a) Chain of rotation equations
b) Loop of rotation equations Fig. 7. Basicgraphs of rotation equations.
8. INTERVAL PROPAGATION IN A LOOP OF ROTATION EQUATIONS Now we consider the case of a closed chain of rotation equations, i.e. a chain in which the first and last equations, E, and Es, share also one variable 0s.t [Fig. 7(b)]. This will be called a loop of rotation equations. In a loop of rotation equations the propagation method for chains is not sufficient to obtain global compatible solution sets. For that we need some new concepts. Given a loop of rotation equations, we denote by M~+ the mapping that for each value of the shared variable 0~_ tj gives the set of compatible values for the next shared variable 0,~ +t according to equation E~. Similarly M f will denote the reciprocal mapping from 0,~+, to 0~_ ,j. Definition I. The descendants at level i of a value p for Os.l is the set of values for 0~.,+t obtained by the composition of mappings M T " ' " M ~ (p). The set M~'(p) of descendants at level I o f p is obtained by fixing Os.j to p and applying the procedure described in Section 5. Descendants at higher levels are obtained from the descendants set at the preceding level by propagating the intervals as described in Section 6. Thus, the descendants at different levels are obtained by the successive propagations of intervals for the shared variables throughout the equations of the loop. As in each rotation equation the image of one interval may be split into two disconnected intervals, the descendant set at level i can have a maximum of 2' intervals.
Solving multiloop linkages with limited-range joints
383
If the propagation is performed in the opposite direction, we have similarly: Definition 2. The ancestors at level i of a value p for ON.~is the set of values for ON_ ,.N-,+' obtained by the composition of mappings M ~_,+ ~ . " M.~ (p). If, instead of the complete sets given by mappings M~" or M F , only a single value is chosen and propagated at each level, the result will be a sequence of values for the shared variables compatible within each equation. Definition 3. A trajectory from a value p for ON.I to a value q for 0,~+, is a sequence of values rj,j = 0 . . . i, such that ro = p, r, = q and each rj, j = ! . . . i, is a value for Ojj +i contained in the set M ~ (rj_ i) of descendants at let,el ! of r, _ i. Trajectories are reversible in the sense that if a trajectory is taken in the reverse direction, each value is an ancestor of the preceding one. The sets of descendants (respectively, ancestors) of 0N.~ at level N are the values for the same variable ON.I obtained after a complete forward (respectively, backward) propagation all around the loop. Definition 4. The set of successors of a value p for ON: denoted by Succ(p), is the set of descendants at level N of p. Definition 5. The set of predecessors of a value p for ON.Idenoted by Pred(p), is the set of its ancestors at level N. The mappings Succ and Pred are reciprocal (in the same sense as the mappings M,j and Mjt introduced in Section 6) and the graphical representation of Succ can be seen as the representation of Pred when interpreted in the reverse direction. Definition 6. A value p for ON: is a fixed point of a loop if it is contained in the set of its successors Succ(p ). If a value p of ON: is a fixed point, then there is a trajectory with origin and final point in p. As trajectories are reversible, the definition of fixed point does not change if the predecessors set Pred(p) is used instead of Succ(p). Fixed points, together with the periodic trajectories containing them, provide the globally compatible solutions of a loop of rotation equations. Therefore the problem of finding global solutions of a loop of equations reduces to finding the fixed points of the mapping of the circumference onto itself defined by SUcC(ON: ). All fixed points can be obtained from the graphical representation of Succ(ON:) as the intersection with the diagonal (Figs 8 and 9). 8. ! Convergence to fixed points
The graphical method of finding fixed points is not optimal. The sets Succ(ON.i) must be computed at regular intervals scanning all the allowed set of ON.I . In addition the precision obtained in the location of the fixed points will be of the same order as the sampling step of ON:. We will discuss how a more efficient procedure can be used to isolate the set of fixed points. The basic idea is that, given an interval I0 for 0~:, its fixed points must be contained into the sets Succ(lo) and Pred(lo), and hence I0 may be replaced by the set I, = lo~Succ(lo)nPred(lo). Iterating the process more reduced sets will be obtained, that eventually will converge to a stable set for which l,~Succ(l,)t~ered(l,) = !,. To simplify, let us assume for the moment that the map SUCC(ON.I) is a continuous single-valued function (Fig. 10). The evolution of the sequence of sets obtained by the iterated substitution l,-.l,r~Succ(l,)~Pred(l,) = !,+ i will be one of the following: (A) The interval vanishes. This situation occurs when I0 does not contain fixed points. (B) The interval converges to a fixed point. (C) The interval converges to a stable interval. (D) The interval splits into two or more separate intervals. This implies the existence of either a change in the sign of the first derivative [Fig. ! I(a)] or a zero crossing [Fig. I l(b)] of the Succ function within the interval.
ENRIC CELAYA and CARME TOR~AS
384
Succ( 0 ,~ ) 2x'a
'I .......................... t/Jl?l.../I
0
~" 0
~P'
¢
I
n P'P'
0 3.1
2x
I:ig. s. Fixed points or 0,., for the loop of equations: Rx(O)., + n )Rz(5.2)Rx(v I )Rz(3.3)Rx(v: )Rz(4.7)Rx(0,. 2 )Rz(2.6) = I Rx(01. :)Rz( 1.8)Rx(v))Rz(2.6)Rx(v,)Rz(2.6)Rx(02j )Rz(1.5) = I Rx(O:.) )Rz(2.3)Rx(vs )Rz(2.6)Rx(v 6 )Rz(3.8)Rx(0). I + n )Rz(2. I ) = I.
In case (D), each of the intervals may be considered separately and, as the number of extrema and zero crossings must be finite, the iteration of the process for each of them will end in one of the situations (A), (B) or (C). Case (C) implies the existence of a fixed point in the stable interval I, (if I, # [0, 2n]). In this case, we split I, into two halves (IA and la in Fig. 10) and treat each of them separately. Proceeding in this way, all intervals will eventually vanish or converge to fixed points. In practice, it is more efficient to reach convergence for Succ and Pred functions in turn, because usually one of them converges before the other (typically, after the first or second iteration). Thus, we can perform the substitution l , ~ l ~ S u c c ( l , ) = I,÷, until convergence to an interval I,+ is reached, and next, departing from 1~, perform the substitution lf-,i/aPred(lj) = lj. j until a new convergence. The above analysis was done for mappings Succ(OM.,) constituting continuous single-valued functions. For mappings Succ(ON, ) decomposable in a finite number of branches or determinations, each of them being a continuous single-valued function, things are not essentially different, since each branch will evolve in one of the forms described. Thus, the above treatment permits dealing with all cases in which none of the equations E~ in the loop has more than 4 variables. In the general case, in which the map Succ(OM., ) may assign an interval (or a number of intervals) to each point, things are a little worse: a stable interval I, could, in principle, contain non-fixed points and remain stable even after splitting it. Some of these hypothetic cases are shown in Fig. 12. Checking the" extremes of a stable interval for fixed points permits finding, by successive approximations, the limits of a fixed interval [Fig. 12(a)], but the fact that the extremes of a stable interval are fixed points does not guarantee that all points in the interval are fixed [Fig. 12(b)]. More
Solving multilooplinkageswith limited-rangejoints
385
SuccfO ,.i )
0
I~ 0
Pa
, 7t
[
po
0 J,I
2~
Fig. 9. Fixed points of 0~.~for the loop of equations: Rx(0~.l + n )Rz(5.2)Rx(v I )Rz(3.3)Rx(r 2 )Rz(4.8IRx(01. ;)Rz(2.6)
--
I
Rx(01.: )Rz( 1.8)Rx(v~ )Rz(2.6)Rx(v~ )Rz(2.6)Rx(0;.~ )Rz(1.5) = I RX(02. ~)Rz(2.3)Rx(v~)Rz(2)Rx(v b)Rz(2.6)Rx(r7 )Rz( 3.7)Rx(0~. I + n )Rz(2. I ) = / .
research is needed in order to disclose whether such ill-behaved cases can be detected, or even if they can arise at all. Tests performed over randomly generated examples are encouraging, in the sense that convergence to the fixed set is often obtained after a short number of iterations. Since chains and loops are the elementary constituents or arbitrary graphs, Sections 7 and 8 provide the basis for solving coupled systems of rotation equations corresponding to complex graphs of rotations. In addition, as the same propagation machinery can be used for intervals imposed by kinematic constraints or by other types of constraints, non-intersection conditions imposed by collisions between adjacent links can also be dealt with. Yet, the general non-intersection problem cannot be tackled using this approach, since for non-adjacent links, the colliding interval depends on the whole linkage configuration. 9. APPLICATION TO SPATIAL PROBLEMS Until now, we have been dealing only with rotation equations, i.e. spherical mechanisms. In this section we show how the above methods can be applied to solving some types of spatial mechanisms. A spatial mechanism has associated a loop equation of transformations in the same form as a spherical mechanism, with the difference that it contains translation operators of the form Tram(u, d), where u is the director vector along which the translation is performed and d is a length parameter. All translation transformations can be written in terms of a uniparametric translation
Esatc CELAYA and CAaJ~t T o a a ~
386
Succ(O M.J) 2~
/ "
'i
Is
*
~ t o e J ~ I I l
*
E:
0 0 0
io !
.... ....
:
iP
i
l~
i
e~.l
: t i
In
:
2
Is [0
Fig. lO. Evolution of an interval for a hypothetic continuous single valued function Succ(O~): interval Io converges to the stable interval Is. which we split into two intervals !~ and Ij. Then 1~ vanishes while IR converges to the fixed point p.
(a)
(b)
Succ(0 ..~) 2g
Succ(e..,)
/
-
,
21g
i I
i
l,
//' /
Io
I ,
, I
, ,
I !
, ,
I ,
:
,
,,
°".....°
! !
/
/
\ e N.I
0
/
r
.e"
i i
i
!
! !
!
,
~i
e 1¢.1
2 /0
Io Fig. I 1 Splitting
intervals.
Solving multiloop linkages with limited-range joints (a)
387 (b)
S ucc( 0 Ma )
Succ(OM, I 2n
2x
-
I,
/
0 NJ U
"'
I:
"z
2rt
V
0
O N.I
I,
2rt
Fig. 12. Stable intervals.
operator, that we denote by Tranx(d), which represents a translation in the x direction. Thus, an arbitrary translation can be expressed as: Trans(u, d) = R, Tranx(d)R.'
where R, is a rotation that leaves the x-axis in the direction of the vector u. Associated with each spatial mechanism there is a spherical mechanism called its spherical indicatrix, whose revolute axes are parallel to those in the spatial mechanism. The equation associated to the spherical indicatrix is obtained from the spatial equation simply by removing the translation operators. 9. I Mechanisms with three translational pairs
A solution of a spatial equation must satisfy simultaneously a rotational condition, given by the rotation equation of its spherical indicatrix, plus a translational condition stating that the sum of all translations equals 0. As translations do not affect orientation, each solution of the spatial mechanism corresponds to a solution of its spherical indicatrix. Conversely, given a particular solution of the spherical indicatrix, the directions of the translations are fixed, and only if their lengths are such that they cancel each other out, or if the translational freedoms allow it, the rotational solution can be extended to a spatial solution. It is clear that spatial mechanisms containing three or more translational freedoms can cope always with the translational condition, provided the translational freedom directions are not all coplanar. In this case the translational condition imposes no restriction to the rotational solution (apart from the non-coplanarity of the freedom directions) and all solutions of the spherical indicatrix can be extended to a solution of the spatial mechanism. Since all rotational solutions can be found using the procedure in Section 5, spatial problems containing at least three translational pairs can be considered solved. 9.2. Triangulable spatial mechanisms
Next we deal with a class of spatial mechanisms, that we call triangulable, for which the translation condition can be formulated as a compatibility problem in a loop of rotation equations and, consequently, can be solved using the propagation algorithm given in the preceding section. All spatial mechanisms containing no translational freedoms and no more than three constant
388
EsaJc C~X~AVAand CAgMETOaaA.S
translation operators are triangulable. A number of spatial mechanisms with more than three constant translations can actually be rewritten with only three Traax operators using the relations: Tranx(d)Rx(0) = Rx(0)Tranx(d) Tranx(d, )Rz(~bt )Tranx(d: ) = Rz(~b:)Tranx(d~)Rz(q~3 )
(19) (20)
where 0z = arctan(d., sin ¢bI/(dl + d, cos ¢b, ))
a3 = (a~ + a~ + 2a, a'2 cos 4,, ),,,2 ¢3 = arctan(dl sin 4h/(d2 + dl cos ~b,)). The general equation of a triangulable mechanism will take the form Tranx(dt )Ri Tranx(d2)Rz Tranx(d3)R3 - I
(2 I)
where R, are chains of rotations with an arbitrary number of variables, and d, are constant lengths. We can formally decompose the complex rotations R, in the form given in equation (1):
R, = Rx(0,1 )Rz(~i)Rx(0,:)
(22)
and (21) takes the form Tranx(dl )Rx(0, )Rz(q~l )Rx(012 )Tranx(d2 )Rx(02, )Rz(~b2)Rx(0z2) Tranx(d3)Rx(0~,)Rz(~3)Rx(0j2) = L
(23)
Using (19) we obtain Tranx(d, )Rz(O, )Rx(0,z + 02, )Tranx(d2 )Rz(q~ 2)Rx(022 + 03! ) Tranx(d3)Rz(O3)Rx(032 + 01,) = I.
(24)
The only rotations in (24) that change the directions of the translation vectors are the Rz rotations, and since the translations must form a triangle, i.e. a planar figure, the three rotation axes must be parallel. This implies that the Rx rotations must be null, yielding O,z=-O:,;
0.,2=-03,;
03.=-0,,,
(25)
and (23) reduces to Tranx(dl )Rz(¢l )Tranx(d:)Rz(~2)Tranx(d3)Rz(~3) : L
(26)
Now angles ~, can be found using elementary planar trigonometry. Thus we obtain a loop of three rotation equations given by substituting (25) in (22) for i = I, 2, 3: RI = Rx(Oll )Rz(01 )Rx( - 0, I ).
(27)
R 2 -- Rx(021 )Rz(02)Rx ( - 031).
(28)
R 3 -- Rx(03! )Rz(~ 3)Rx( - 0it ).
(29)
The system of rotation equations (27), (28) and (29) is equivalent to the spatial equation (21) and involves all the variables in (21) plus three auxiliary variables 00~, which are the shared variables defining a loop of rotation equations. Thus we have transformed the equation of a spatial mechanism into a loop of three rotation equations that can be solved by the propagation method. 10. E X A M P L E Figure 13 represents a spatial 6-bar mechanism with 6 fixed-length links connected by revolute pairs. A seventh extra revolute pair in link 6 provides mobility to the mechanism. Our objective is to find the solutions for the angles of the pairs. This can be stated as an assembly problem as defined in the introduction: if we consider the open chain (Fig. 14), the goal is to find the pair configurations for which the mechanism closes.
Solving multiloop linkages with limited-range joints
~,mea
Fig. 13. A 6-bar mechanism.
The spatial equation is: Tranx(0.5)Rx(0, )Rz(2n/3)Tranx(I.3)Rx(02)Rz(n/3) Tranx(0.7)Rx(03)Rz(5n/9)Tranx(0.9)Rx(04)Rz(4n/9) Tranx(I.5)Rx(0~ )Rz(7n/9)Tranx(I)Rx(0,)Rz(n/4)Rx(07)Rz(n/3) = [.
, ~ l l d l,~im
Fig. 14. Open chain corresponding to the 6-bar mechanism.
389
ENRIC CELAYAand CARMETOaaA.S
390
We see that this spatial mechanism is triangulable, since using (19) and (20) we obtain Tranx(i. 136)Rz(0.391)Rx(0,)Rz(n/3)Rx(03)Rz(1.02 !) Tranx(I.040)Rz(0.725)Rx(04)Rz(4n/9)Rx(05)Rz(0.719) Tranx(0.976)Rz( ! .724)Rx(06)Rz(n/4)Rx(07 )Rz(n/3)Rx(01 )Rz(1.703) = L which is in the form of (21). Equation (26) is: Tranx( I. 136)Rz(01 )Tranx(1.040)Rz(4,:)Tranx(0.976)Rz(O3) = L and solving for the exterior angles of the triangle we obtain: ~ = 2.215;
~2= 1.946;
~3= 2.122,
thus the loop of equations can be written as: Rz(0.391 )Rx(0z)Rz(n/3)Rx(0~ )Rz( !.021 )Rx( - 0b)Rz(2.215)Rx(0o) = I Rz(0.725)Rx(04 )Rz(4n/9)Rx(05 )Rz(0.719)Rx( - 0c)Rz(1.946)Rx(0b) = I
Rz(I.724)Rx(Or)Rz(n/4)Rx(OT)Rz(n/3)Rx(Ot
)Rz( 1.703)Rx(-0a)Rz(2.122)Rx(0¢) = L
The function Succ(Oa) for this loop is represented in Fig. 15, from which the set of fixed points of 0~ is found to be the interval [p.~,ps]. When the method for finding the set of fixed points explained in Section 8.1 is applied, convergence is reached after the first iteration, and the values pa -- 1.832 and Pa = 4.451 are found. Now we can obtain explicit solutions by assigning values to 0~ within the interval of its fixed points. For example, if we choose the value 0~ = 2, we find a periodic trajectory with 0~ -- 3.882 and O, -- 4.182 for the auxiliary variables, from which the values 0, = !.823, 02 -- 0.593, 0~ -- 0.363, 04 = 4.859, 05 = 1.649, 06 = 0.902, and 07 = 2.288 can be determined for the variables of the revolute pairs, which correspond to the configuration shown in Fig. 13.
Succ(O . ) 21
0
0o 0
Fig. 15. Mapping
rA
It
ra
2~
Succ(Oo) corresponding to the loop o f equations associated to the (>-bar problem.
Solving multiloop linkages with limited-rangejoints
391
I1. C O N C L U S I O N S The procedure to find solution intervals for variables, combined with the propagation algorithm presented in this paper, constitute the basis for the solution of arbitrary systems of rotation equations or, equivalently, o f multiloop spherical linkages. An enclosing box containing all the solution vectors is obtained as the direct product o f the sets of allowed values for each variable. Each time a variable is fixed to a particular value within its solution set, the solution sets for the remaining variables are reduced to the compatible values. A solution vector far from limit positions is easy to obtain just by fixing each variable at the centre of its interval. This way to obtain the solution is convenient for what we have called assembly problems, in which there is a need to find specific solutions, as well as the extreme values that variables can reach. This form o f solution can be seen as complementary to that of implicit equations relating variables (the usual form in Kinematics). The latter is amenable to velocity and acceleration analysis, but extreme values for variables are difficult to obtain and little guidance is provided for finding actual solution vectors (values for variables leading to real solutions for all the remaining variables). The constraints derived from having limited-range joints have been taken into account for the first time here, as far as the authors are aware of. The successful application of the propagation method to the solution o f spatial loops, gives the approach a remarkably increased interest. Acknowledgements--This work has been partially supported by the Comisibn Interministerial de Ciencia y Tecnologia (CICY'r), under the project "Automatic spatial reasoning based on constraints" (TIC 88-0197). and by the ESPRIT I!1 Basic Research Actions Program of the EC under contract No 6546 (project PROMOTION).
REFERENCES I. 2. 3. 4. 5. 6. 7. 8. 9.
C. V. K. Gupta and C. W. Radclife, J. Engng Ind. Trans. ASME 125-130 (1971). D. L. Pieper. Ph.D. c~issertation. Artificial Intelligence laboratory. Stanford University (1968). J. Duffy, Analysis of Mechanisms and Robot Manipulators. Edward Arnold. London (1980). J. Angeles, Spatial Kinematic Chains. Springer, Berlin (1982). R. P. Paul, Robot Manipulators: Mathematics, Programming. and Control. MIT Press. Cambridge, Mass. (1985). D. R. Baker and C. W. Wampler II. Robot. Res. 7, No. 2, 3-21 (1988). A. P. Ambler and R. J. Popplestone, Artif. Intell. 6, 157-174 (1975). R. J. Popplestone, A. P. Ambler and I. M. Bellos. Artif. Intell., 14, 79-107 (1980). E. Mazer, Proc. Int. Meeting on "Advanced Software in Robotics" (Edited by A. Danthine and M. G~rardin). North-Holland, Amsterdam (1983). I0. F. Thomas and C. Torras, IEEE J. Robot. Automat. 4, 622-634 (1988). 11. G. A. Kramer. Proc. of the Workshop on Clichis and Intermediate Metbodsfor Problem Solving. CEAB, Blanes (1991). 12. J. E. Baker, Mech. Mac& Theory 13, 333-343 (1978). 13. J. E. Baker, Mech. Mac& Theory IS, 255-265 (1980). 14. K. H. Hunt, Kinematic Geometry of Mechanisms. Clarendon Press, Oxford (1978). 15. F. L. Litvin, Mech. Mac& Theory IS, 115-125 (1980). 16. K. Sugimoto, J. Duffy and K. H. Hunt, Mech. Mac& Theory 17, 119-132 (1982). 17. C. Gosselin and J. Angeles, IEEE Trans. Robot. Automat. 6, 281-290 (1990). 18. C. H. Chiang, Kinematics of Spherical Mechanisms. Cambridge University Press, Cambridge (1988). 19. E. Celaya and C. Torras, Pro(:. of the ECAi'90, Stockholm, pp. 147-152 (1990). 20. E. Celaya, Ph.D. dissertation. Department of LSI Polytechnic University of Catalonia (1992).
S O L U T I O N DES C H A I N E S C I N E M A T I Q U E S A P L U S I E U R S B O U C L E S F E R M E E S ET AVEC L I M I T A T I O N S DE M O U V E M E N T AUX C O U P L E S R/~mt~'---Nous pr~sentons un¢ m(~thode pour obtenir la solution d'une &lUation souscontrainte de rotations, clans la forme de l'ensemble des valeurs que chaque variable peut pmndre. Un algorithme de propagation d'intervallesnous permet d'obtenir les sous-ensembles de la solution compatibles avec une intervalledonn~e pour une des variables, La tL'chniquede propagation nous permet de considc~rerdes contraintes de non-intersection.La propagation peut itre aussi utilisC-epour la rC-solutionde syst~mes de plusieures~luations de rotations.Finalement, on montre comment les algorithmes d/:critsicipeuvent ¢~tre utilis~-spour trouver la solution de certains probl~'mes espatiales, y comprenant mechanismes ,; six maillons. M M T ~!]~--D