RSD-TR-12-87 Coordination of Dual Robot Arms Using Kinematic Redundancy I.H. Suh K.G. Shin Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109 September 1987 Center for Research on Integrated Manufacturing Robot Systems Division College of Engineering The University of Michigan Ann Arbor, Michigan 48109-2110

RSD-TR-12-87 TABLE OF CONTENTS 1. Introduction.................................................................................. 2 2. Problem Formulation...................................................... 3 3. Solution Approaches.............................................. 8 4. A Numerical Example.............................................. 15 5. Concluding Remarks.............................................. 17 References.............................................................................................. 18 Figures................................................................................................... 19 i

COORDINATION OF DUAL ROBOT ARMS USING KINEMATIC REDUNDANCY * II Hong Suh and Kang G. Shin Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, MI 48109-2122 August 27, 1987 ABSTRACT A new method is developed to coordinate the motion of dual robot arms carrying a solid object, where the first robot (leader) grasps one end of the object rigidly and the second robot (follower) is allowed to change its grasping position at the other end of the object along the object surface while supporting the object. It is shown that this flexible grasping is equivalent to the addition of one more degree of freedom (dof), giving the follower more maneuvering capability. Especially, for industrial robots with six dofs, motion commands for the follower are generated by using kinematic redundancy. To show the utility and power of the method, an example system with two PUMA 560 robots carrying a beam is analyzed in detail. *This work was supported in part by the NASA Johnson Space Center under Grant NCC-9-16, NSF under Grant ECS-8409938 and the Post Doctoral Program of the Korean Science and Engineering Foundation. All correspondence regarding this paper should be addressed to Kang G. Shin at the above address. 1

1 INTRODUCTION Recently, several methods have been proposed to coordinate the control of two robot arms carring a solid object [1] [2] [3] [4], where the common solid object was assumed to be rigidly grasped by both arms. The relative position and orientation of the two robot arms were fixed during the entire execution of the robots' task. Under this setting, however, some tasks such as carrying an object along a prespecified path with two robots may not be accomplished due to the insufficient number of degrees of freedom (dofs) available to them. Call one of the two robots the leader and the other the follower. Consider the motion planning for these two robots, where motion commands for the follower are to be generated based on the leader's motion. When the leader's motion leads the follower to a singular region, or when the motion command generated from the leader's motion requires the follower to violate its joint limits and/or to collide with obstacles, the task cannot be accomplished with the invariant grasping position unless the desired path is modified or the number of the follower's dofs is increased. In this paper, a new method to overcome the above difficulty is proposed by relaxing the assumption of invariant grasping position of the follower. With this relaxation, the follower will first be shown to be considered as a redundant manipulator even without physically adding joints to the follower. Then, the follower's motion commands are generated by employing the kinematic control techniques used for redundant manipulators [5] [6] [7] [8]. The kinematic control of a redundant manipulator is known to find 2

joint angles and/or velocities such that its end effector attains a desired position and orientation, while minimizing some cost function. It is remarked that the object surface is required to be smooth enough for the follower to change its grasping position while keeping its (supporting) orientation and position in order not to drop the object. This requirement may be satisfied by placing the ball bearings on the follower's palm as depicted in Fig. 1 such that the sliding contact friction between the follower and the object can be made sufficiently small for the follower's hand to move along the object surface smoothly. It is also remarked that our method may be considered as a generalization of the Zheng and Luh's method [1] in the sense that if the smoothness assumption of the object does not hold, then our method reduces to theirs. The organization of the paper is as follows. In Section 2, a problem formulation is presented along with the definition of "supporting orientation". In Section 3, a solution approach is proposed by employing the control techniques for redundant manipulators. In Section 4, a numerical example is presented to show the utility and power of our proposed method, where two PUMA 560 manipulators are considered. The paper concludes with Section 5. 2 PROBLEM FORMULATION Consider two robots each with n joints carrying a rigid object as shown in Fig. 2, which is too large and too long for a single robot to handle. The 3

task given to the two robots is to move the object from one location to another along a prescribed path, while not exceeding joint limits and/or avoiding obstacles. Under the smoothness assumption mentioned earlier, the problem is to determine the joint trajectories of two robots as well as the trajectory of the follower's gripping position to accomplish the task. Let (~, y0, z~), (z, y, z, ), and (xf, yf, zf) be the coordinate frames of object, end effectors of the leader and the follower, respectively (see Fig. 2). Let rl and r2 be the position vectors of the origins of (z~, y~, z~) and (xf, y, zf) with reference to (x^, y, z) and (x~, y~, z), respectively. Let H1 and H2 be the 4 x 4 homogeneous matrices representing the coordinate frame of the object with reference to (xn, yX, zn) and that of the follower's end effector with reference to (x~, y~, z~), respectively. Also, let q be an n-dimensional vector representing joint positions, and n(q), s(q), a(q) and p(q) be the normal, sliding, approaching and position vectors of the leader with reference to the base coordinate (XB, YB, ZB). Define Tn(q) and Rn(q) as Tn(q) _ n(q) s(q) a(q) p(q) (1) O O 0 1 and RS(q) A [n(q) s(q) a(q) ] (2) Let D(t) be the 4 x 4 homogeneous matrix representing the desired motion trajectory of the object with reference to the base coordinate. To obtain the holonomic constraints for the two robots, define a "supporting orientation" as a relative orientation between the two robots or 4

between the object and the follower in order not to drop the object. Such a supporting orientation may vary with the type of object. Two different cases are considered as follows. Case 1 When the follower's hand has to grasp the object in order to support it (Fig. 3). Case 2 When the follower's hand needs to support the object without grasping it (Fig. 4). A supporting orientation for Case 1 may be obtained by the following steps (see Figs. 2 and 3). Step 1 Suppose the equation of object surface represented in the object coordinate (x~, y~, z~) is given by 4(r2) = 0, rm < r < r"m, (3) where b: R3 -- R1 is twice continuously differentiable and r"n and rm" are the 3 x 1 vectors representing the positional limits within which the object must lie. Then, obtain the two-dimensional surface equations of the object in the object coordinate by projecting it onto the (x~ - z~, y~ - z~) plane. Step 2 Obtain the center-line equation of the two-dimensional surfaces, and let r' A [Ir, r r,]T be the current position vector of the follower's hand with reference to the object coordinate. Then, find the derivatives d - a and _ p at a position z0 = r,. 5

Step 3 Calculate the sliding vector s*(r*) of the object at r* [a / 1]T s*(r*) = +a + 1 IT (4) Step 4 Compute the unit vectors a*(r*) and n*(r*) with a*(r*) A V(r2) (5 - ifv O(r2) ll2 = and n*(r*) A a*(r') x s*(r*). (6) Then, align zf and xf of the follower's hand coordinate with a*(r*) and s*(r*), respectively. For Case 2, a supporting orientation may be given simply by aligning zf and xf with a*(r*) and s*(r*) given by B*(r) = III, (7) where T7 E [ { | V(r)TTl = 0 } It is remarked that Steps 1 and 2 in Case 1 can be executed off-line regardless of the movement of the object, since all the equations related to the object are defined in the object coordinate. Now, we can obtain the following holonomic constraints; for all t E [to, tl], D(t) = Tg(q')H1(r1) (8) and Tr(q )Hl(rl)H2(r2) = T(q/), (9) 6

where to and tf are the initial and final time of the desired trajectory, D(t), and H (ri) is a constant 4 x 4 matrix resulting from the prespecified distance vector ri and H2(r2) is given by H2(r2) s(r2) n*(r2) a(r2) r210) H,2(r2) == (10) 0 0 0 1 In Eqs. (8) and (9), q1 and qf are bounded as q in - q' < qmax (11) and /in < qf q < qx. (12) Now, the problem becomes: For given D(t), to < t < tf and Hi(ri), find q'(t), qf(t), and r2(t) such that Eqs. (8-12) and (3) are satisfied for allt E [to,tfl. It is here assumed that the leader has a sufficient number of dofs such that Eqs. (8) and (11) can be completely solved for all t E [to, t]. Thus, we will concentrate on solving Eqs. (3), (9), (10) and (12). It is remarked that in Eq. (9), the maximum number of independent constraint equations is six as discussed in [9]. Thus, if r2 is prespecified as in [1], and each of the two robots has six dofs, then the the follower's position qf can be uniquely determined by the leader's position qL from the six constraint equations. However, in our case, r2 is a vector to be determined and, thus, the number of unknown variables could be greater than that of constraint equations. This implies that the follower be considered as a redundant manipulator with more than six dofs due to the variable r2. 7

3 SOLUTION APPROACHES A solution to the problem formulated in Section 2 will be developed using the following steps: * Derive the linear relationship among joint velocities and the time derivative of the supporting position vector r2 of the follower. This relation is necessary to solve the holonomic constraint equations (3) and (9). The supporting orientation is not updated here to get a simple rotational velocity relationship between the two robots. * Find the supporting position vector r2 and joint vector q! of the follower by integrating 2 A -d and qi!-f A subject to the follower's joint limits and the bounds of r2. The local optimization technique developed for the control of redundant manipulators [5] [6] [7] [8] is employed for this. - Recalculate the follower's joint vector q! to get the desired supporting orientation with r2 fixed. Note that the above solution approach does not simultaneously calculate the desired supporting orientation and r2 to avoid the computational complexity. Instead, we first find r2 and then calculate the supporting orientation. This is realistic if the object surface is smooth and not severely curved; otherwise, the grasping position cannot be changed while two robots are carrying an object. 8

3.1 Derivation of Linear Relationships Among Joint Velocities Consider the holonomic constraint equation (9) which can be rewritten as Rn(q')R1(r1)R2(r2) R (qf) (13) and p(q') + Rn(q')rl = -R(q')R1(rl)r2 + p(qf). (14) Eqs. (13) and (14) represent the holonomic constraints for orientation and position, respectively. To obtain a simple rotational velocity relationship between the two robots, let R2(r2) = [ s(r') n*(r) a*(r) ] (15) where r* is the current supporting position vector. Then, from Eq. (13), we know that there is no relative motion between the two robots' end effectors. Consequently, we can establish a linear relationship between the angular velocities of the leader and the follower given by Ja(q')' = J,(q/)ql, (16) where Ja(q) is the 3 x n Jacobian matrix relating the angular velocity of the leader's (follower's) end effector with reference to base coordinate to q1 (q!). To calculate the follower's velocity, differentiate Eq. (14) with respect to time. Then we can obtain 9

[Jp(q') + L(q') ] q' = Jp(qf)f4 - Rn(q') RI(rl)'2 (17) - L2(ql,r*) 4l, where Jp(q) is another 3 x n Jacobian matrix relating the positional velocities of the leader's (follower's) end effector with reference to base coordinate to iq (qi), and Ll(q') and L2(ql,r*) are respectively given by L'(q') - a[R (q')r1] Li(q') = l aq, and L(qL, r*) = a[Rn(q')R1(r1)r2] To impose the constraints on r2 as in Eq. (3) along with Eqs. (16) and (17), differentiate Eq. (3) with respect to time. Then, i2 should satisfy v; = ( ) r2 = 0, for all t E [to,tf. (18) Let the moving velocity of the follower's end effector r2 = ms* (r*), where m is the magnitude of i2. Recall that s*(r*) has been chosen to be tangential to the object surface that the follower's hand touches. Then, Eq. (18) becomes an inactive constraint, implying that the magnitude of moving velocity m be a design variable. By plugging Eq. (18) into Eq. (17) and combining Eq. (16) with Eq. (17), we obtain the desired linear relationship between joint velocities: J q= Jlq', (19) 10

where the 6 x (n + 1) matrix Jf, 6 x n matrix J1, and (n + 1) dimensional vector q are respectively given by J E Jp(qf) -Rn(q) R(rl)s*(r*) J!- - (20) J.(q) 0 J = Jp(q1) + Ll(q1) + L2(q ) (21) J, -, (21) l Ja(q') and = [(q)mF. (22) Note from Eq. (19) that the number of dofs is increased by one. Thus, if the follower, like most industrial robots, has six joints, Eq. (19) plays a role of the Jacobian equation for a redundant manipulator with seven dofs. It is also noted that if r2 is prespecified, then m = 0 and, thus, Eq. (19) reduces to the Jacobian equation developed by Zheng and Luh [1]. It is further remarked that in Eq. (19), Jq14 implies the desired differential changes in the follower's hand position. J14 can thus be replaced by the differential discrepancy between two homogeneous matrices TB(qL)Hl(ri)H2(r*) and T(qf). 3.2 Use of Kinematic Redundancy In this section, the number of the follower's dofs is assumed to be six. By employing the local optimization techniques used for the control of redundant manipulators, we will derive a solution for the linear equation (19) subject to the inequality constraints on r2 and qf in Eqs. (3) and 11

(12), respectively. Define Jt as the generalized inverse of Jf, i.e., Jt J (Jf J)-1. Then a general solution of Eq. (19) can be given by [5] q = JtJq + (I - JtJ/), (23) where z is an arbitrary vector lying in the null space of Jf. Note that Eq. (23) implies the relationship among joint velocities, but Eqs. (3) and (12) are expressed in terms of position vectors. Thus, it is necessary to change Eqs. (3) and (12) to the inequalities imposed on velocities. To do so, let AT be the sampling interval and the subscript i denote the ith component of the corresponding vector. Also let r. ( - r - Pmin = max Pi:Pi = min (r)Ai T ) s AT* ) AT (24) s,(r') f 0, i = 1,2,3) and rm - r'i r -" ri_ Pm- = min pi pi - max (r')AT' s(rI^ ) 8 s*(r*)AT (25) s;(r*) 0, i 1,2,3}. Here, if s (r*) = 0, then no more motion along the ith axis is generated. Noting that r2 can be approximated as r2 - r* + mATs'(r*), (26) Eq. (3) is modified to Pmin < m < Pmax. (27) 12

Similarly, using the first order approximation of qf, the inequality constraints on qf can be modified to qnmin < 4q < 6qmax, (28) where 6qmin and 6qm, are given by f f A qmin - qg 6qmin - qI, (29) and a mx - qf qmx = - AT (30) To find i such that the inequality constraints (27) and (28) are satisfied, let the cost function I(q) be chosen as 1(q) = II q) - q, IIs. (31) S is a 7 x 7 positive definite weighting matrix and 6q, is a 7 x 1 vector defined by oqc [ 6qlc sqx ** eq Pc ], (32) where 6qi,, i = 1, 2,...,6 and p, are given by 6qi, = qi,ma + imi i = 1, 2,- **,6 (33) 2 and Pmax + Pmn (34) Pc (34) 2 Such a i can be found by minimizing I(q) in Eq. (31) subject to Eq. (24), since minimization of I(q) implies that all the joint variables and r2 be 13

located at the centers of their motion ranges. The solution vector z to the minimization of I(q) subject to Eq. (24) is given by z= [ JJf) S(I - Jti) ] (I - JJ) S(6qc - JtJq41). (35) 3.3 Resolved Motion for Desired Supporting Orientation It may be necessary to resolve the follower's joint vector qf into the desired supporting orientation when the supporting position vector r2 is updated. This can be done easily by solving the follower's Jacobian equation of the follower as follows: first, obtain the required differential change 6w,, 6wy and 6w, of the hand orientation by employing two rotation matrices iR(qQ)R1(rl)R2(r; ) and RS(q')R1(r1)R2(r2) as in [9]. Then, find the corresponding differential change of the joint vector 6qf by -1 6qf = J(q [0 0 6w, 6w, w,]T. Ja (qf) Finally, obtain the desired qf by adding 6qf to the old qf. It is remarked that the updated qf may not satisfy the constraints (12). In such a case, the weighting matrix S in Eq. (31) may have to be adjusted such that heavier weights are placed on those joints violating the limits in this updating procedure. 14

4 A Numerical Example Two PUMA 560 manipulators, each of which has six rotational joints as shown in Fig. 5, are employed to demonstrate the utility and power of the proposed coordination method. The task given to the two arms is to transfer an 850 mm long rigid bar shown in Fig. 6 along a straight line path. Numerical values used in our simulation are: 1. Link parameters and joint ranges of the PUMA 560 manipulator are listed in Table 1. 2. Initial joint values and initial grasping position of the follower are tabulated in Table 2. 3. rmi and rm, are chosen to be [0,0,0]T and [0 0 400]T, respectively. 4. The sampling interval AT is chosen to be 31 msec. 3. The desired trajectory D(t) with reference to the Oth coordinate frame of the leader is given by 0 1 0 d,(t) 0 0 1 690 D(t) = 1 0 0 260.4 000 1 where d,(t) = - 0.3. 15

6. The 7 x 7 weighting matrix S is chosen to be a diagonal matrix given by S = diag(l,1, 1, 1,1,1, s7), where s7 is a constant representing the weighting for the grasping position. Note that these numerical values are chosen arbitrarily and the capability of our method is independent of the choice of these values. It is remarked that the sliding vector s*(r*) in Eq. (4) is easily obtained as [0 0 1]T and fixed during the entire motion for the task. Thus, the supporting orientation in this example does not change with time during the entire task. Fig. 7 shows position trajectories of joint 3 of the follower for the following three cases. Case (a): When the grasping position vector r2 of the follower is fixed as in [i1 and chosen as [0 0 450]T, Case (b): When the proposed method is applied with 87 equal to 100, Case (c): When the proposed method is applied with s7 equal to 1000. From Fig. 7, it is observed that in case (a), a portion of joint trajectory violated the joint limit, while in cases (b) and (c), joint trajectories lie within the specified joint ranges as a result of moving the follower's hand toward the 3/4 position of the bar. Changes in the grasping position of cases (b) and (c) are shown in Fig. 8. It is noted from Figs. 7 and 8 that 16

the initial joint motion in case (c) is smaller than that in case (b). Thus, we may expect that if the initial movement of the follower's hand is large enough to cause the violation of the limits of joint velocity qfin and qlx, then such violations can be avoided by adjusting the weighting matrix S together with rmin and rm, and/or by modifying Eq. (28) as max(6qmin, 4qfin) < q < min (6qmax, qmax) This example has shown that our coordination method is more powerful and flexible than those methods in [1, 2, 3, 4]. 5 Concluding Remarks A coordination method for dual robot arms was proposed, where allowing the follower to change its grasping position was equivalent to adding one dof to the follower without actual installation of an additional joint motor. The method was then mathematically and numerically shown to be effective for the task of carrying a solid object along a prespecified path while avoiding the joint limits. It is worth mentioning that our method can be considered as a generalization of Zheng and Luh's work [1] and can be further generalized by allowing not only the follower but also the leader to change their grasping position and orientation. As a future work, dynamics of dual arms holding and/or supporting a common object need to be investigated to deal with the constraints of joint torques. 17

References [1] Y. H. Zheng and J. Y. S. Luh, Joint torques for control of two coordinated moving robots, In Proc. IEEE Conf. on Robotics and Automation, pages 1375-1380, San Francisco, CA, April 1986. [2] J. Lim and D. H. Chyung, On a control scheme for two cooperating robot arms, In Proc. IEEE Conf. on Decision and Control, Fort Lauderdale, FL, December 1985. [3] A. K. Bejczy T. J. Tarn and X. Yun, Coordinated control of two robot arms, In Proc. IEEE Conf. on Robotics and Automation, San Francisco, CA, April 1986. [4] N. Iwasawa M. Uchiyama and K. Hakomori, Hybrid position/force control for coordination of a two-arm robot, In Proc. IEEE Conf. on Robotics and Automation, pages 1242-1247, Raleigh, NC, April 1987. [5] A. Liegeois, "Automatic supervisory control of the configuration and behavior of multi-body mechanisms", IEEE Trans. Syst. Man and Cybern., vol. SMC-7, no. 12, pp. 868-871, December 1977. [6] C. A. Klein and C. H. Huang, "Review of pseudoinverse control for use with kinematically redundant manipulators", IEEE Trans. Syst. Man and Cybern., vol. SMC-13, no. 2, pp. 245-250, March/April 1983. [7] W. D. Fisher, The kinematic control of redundant manipulators, PhD thesis, Purdue University, School of Electrical Engineering, 1984. [8] P. H. Chang, A closed-form solution for the control of manipulators with kinematic redundancy, In Proc. IEEE Conf. on Robotics and Automation, pages 9-14, San Francisco, CA, April 1986. [9] R. C. Gonzalez K. S. Fu and C. S. G. Lee, Robotics: control, sensing, vision and intelligence, McGraw-Hill, New York, 1987. 18

gripper 0a4 IO 0 ball bearings0 ball bearings Figure 1. A robot hand with ball bearings in the palm. 19

xIn f x x -H2( r 2) n nil en H1(r1) t ----.....-. T q ie e' --- - " f" Y;, S B( q, ) _ XB B~~~~~~~~~n 20

* * a (r ) Fg r 3. S p otn oretto whennml u a* ro o grasps a o r 4P ~follo wer Figue 3.Supporting orientation when a robot grasps an object,,

one of these vectors object is given as s (r ) leader follower's hand Figure 4. Supporting orientation when the robot does not have to grasp the object. 22

a(a n 2 Y d ge 5c6 Figure 5. Link coordinates for a PUMA robot.6 Fiure.Likcoordinates for a P UArob.

1 f f x o y z n x n n......................... 4 0 0................................................... 4 5 0............................. leader _ _ \ vs^ — z1 _ f <-\v - nZ X I f0 yn n y 0010 0 10 0 H (r O O 400'' O o0 0 0 14 100 2 2 0 0 0 1 Figure 6. A 850 mm long rigid bar and hand orientations of the leader and the follower. 24

9: *JeMOllO; e9q 0o i lu!or o se!JoperfeJ uo!!Sod'L ejnO! (sBu!idwes lo Jeqwnu) eWoj! 009 OSS OO O~ 009 09 WO OS9 00OO OZ 0L 0 L 0.........., I 0 999OSIu!o.|/ S\' _o. S"-?( SCo0Pl - OL(o) eso --- (q) eseo --- (e) ese

- case (b) case (c).45.4.35 0.3 C a.25.2.15 X __*_*___ _* *__ _. 0 50 100 150 200 250 300 350 400 450 500 550 600 Time (number of samplings) Figure 8. Change in the gripping position. 26

UNIVERSITY OF MICHIGAN 3 9015 03526 7585