IEEE Transactions on Robotics and Automation 6:4 (August 1990) PROGRAMMED COMPLIANCE FOR ERROR-CORRECTIVE ASSEMBLY Michael A. Peshkin ABSTRACT: Suppose that before an assembly task commences we can specify at will the manipulator's response to assembly forces, by providing a single compliance (or damping) matrix to be used for the duration of the operation. Can we choose the matrix elements so that the force which characterizes every possible error condition maps into a motion which reduces it? If so we are assured that the operation will evolve toward decreased errors and eventual success. In this paper we describe a framework and a method of synthesis of an error-corrective matrix. 1.1 BACKGROUND AND MOTIVATION It is widely recognized that for robots to perform tasks involving the contact of rigid objects some form of compliance2, either of the robot or of the objects, is required. Under position-control and in the absence of such compliance, the presence of even the slightest error in the positions of the objects can produce unbounded contact forces and therefore failure of the operation. Depending upon the task being performed and the geometry of the objects in contact, different forms of compliance are needed -- compliance should be specific to the task. Mason [Mason, 1981 #37] formalizes this idea, traces its origins in the literature, and describes a method of creating a compliance matrix consistent with a given task. The term consistent is introduced here in order to contrast it with error-corrective, below. By consistent we mean that the compliance of the manipulator is such that unbounded forces do not develop. Roughly speaking, this requires that the manipulator's stiff (position controlled) axes not be perpendicular to rigid surfaces in the environment. Recent work on the specification of compliance for consistency with a task may be found in [De Schutter, 1988 #40] [Lee, 1988 #7]. The compliance of a manipulator has a profound effect on the evolution of a robotic assembly operation. Depending upon its compliance a manipulator's response to errors may drive the system toward or away from proper mating of the parts. Even if a compliance matrix consistent with a task is in use (i.e. unbounded forces do not develop), the operation will not necessarily evolve toward decreased errors. We will call a form of compliance error- corrective if it causes an operation to evolve toward decreased errors. Figure 1 shows an example in which a compliance consistent with a task is distinctly different from one which is both consistent and error-corrective. Insert figure 1 here. Caption: A simple example of a task in which compliance consistent with the task will indeed prevent unbounded forces from developing, but will not cause error-reduction and eventual success of the mating operation. In (a), a block is to be placed into a shallow depression or detent in a rigid surface. Since the task's rigid axis is the y axis, Mason's work would suggest that we make the manipulator soft (force controlled) along the y direction and stiff (position controlled) along the x axis and the q (rotational) axis - thus consistent with the mechanical constraints imposed by the task. Forces due to contact as shown in (b) are in the +y direction, and there is a clockwise torque about point q. The compliance of the manipulator allows some "give" in the y direction, and so the block comes to rest on the lip of the detent. If we let the q axis be soft also, the outcome is (c), in which the block rests on the lip and bottom of the detent. (d) shows the evolution if we have arranged the compliance of the manipulator for error-reduction as well: in particular we include an off- diagonal matrix element which maps torque into horizontal displacement. Clockwise torque maps into +x motion, and counterclockwise into Üx motion. Thus when the corner of the block comes into contact with the left edge, the torque generated results in a displacement which moves the block toward proper mating. A compliance which maps torques into translations in the way described above may seem counterintuitive (see also section 1.5). Intuition tells us that sliding toward proper mating will not occur unless the environment surface is steeply sloped toward the detent (i.e. has chamfered edges.) Our intuition, however, corresponds to a very limited class of compliances. If we allow ourselves freedom in choosing a compliance matrix we can obtain the surprising behavior above. This behavior in no way violates the "friction cone" construction from classical mechanics. Classical mechanics says that imposed forces within the friction cone result in sticking (no motion.) Here, in contrast, we do not impose a force and observe the motion which occurs. Rather we observe a force, then compute and impose a motion. Since Mason's work, several distinct approaches to the automated planning of contact and assembly tasks under force control3 have developed: In Lozano-Perez, Mason & Taylor (LMT) fine motion planning [Lozano-Perez, 1984 #45; Erdmann, 1984 #49; Buckley, 1987 #50; LaTombe, 1989 #61] the manipulator is given a priori a particular form of compliance (usually a generalized damper with a diagonal damping matrix; see section 1.5). The goal of LMT planning is to design a nominal motion plan. The nominal motion plan is a sequence of robot motions (usually given as velocities) each one of which is executed until motion ceases; from there the next motion commences. It is sometimes possible to design a sequence of motions which proceeds inevitably despite the initial errors to a final stopping point at which the operation (perhaps the mating of two parts) is completed. Error detection and recovery (EDR) [Donald, 1986 #42; Donald, 1988 #43] shares with LMT fine motion planning the same paradigm of a given (usually diagonal) compliance and a synthesized (generally multi-step) nominal motion plan. The objective of the nominal motion plan is now broadened so that a plan which inevitably leads either to task completion, or to termination at a recognizable failure configuration, is acceptable. If motion terminates at other than task completion, a secondary nominal motion plan which commences from that point is invoked. EDR plans, like LMT plans, are fully formed off-line. In replanning [Xiao, 1988 #44][Desai, 1988], both the compliance and the nominal motion plan are given a priori. The nominal motion plan, in particular, is just the one that would be created without any consideration of errors, and so is unlikely to involve multiple sequential sliding motions. If motion terminates at other than task completion, and the resulting configuration is identifiable by force sensors or other sensors, an on-line replanner is invoked. The replanner computes the motion needed to bring the workpiece back into a configuration such that the nominal motion plan can proceed. The above works are approaches to the planning of generic assembly tasks, as is this paper. One particular assembly task, peg- into-chamfered-hole, has due to its importance been heavily studied: Draper Lab's analysis of peg-into-hole led to development of the RCC wrist (remote-center compliance). RCC [Whitney, 1982 #38] can be implemented as a physical wrist, instead of a control loop involving force sensor and computer. In RCC a simple nominal motion is assumed, in which errors of peg orientation are ignored. It is the compliance of the grasped peg, and the forces which arise due to errors, which cause the peg to self-align with the hole. Another line of work, which results in what are sometimes described as skeleton strategies for particular tasks, again uses multi-step motion plans, sometimes combined with non-diagonal compliances. Strategies for round pegs were considered in [Inoue, 1979 #64; Simunovic, 1975 #53]. More recent work considers strategies for non-round pegs [Strip, 1988 #63; Caine, 1985 #62]. In contrast to off-line planning of a motion plan as in the above examples, or the off-line design of the manipulator's compliance as in this work, one might also consider an on-line learning of the forces which characterize errors and the proper motion response to them [Vaaler, 1987 #65][Hirzinger, 1985 #66]. 1.2 RELATION TO THIS WORK Programmed compliance, the subject of this paper, differs from LMT, EDR, and replanning in that we assume we are given a nominal motion plan, presumably a simple (one-step) one, which takes no account of possible errors. We then design the compliance (here a damping matrix) so that the plan will succeed despite errors. In LMT, EDR, and replanning it is the compliance which is given and the motion plan which is to be designed. Design of the compliance matrix for error-correction is the idea behind the development of the RCC wrist, and several others have addressed this idea also, (e.g. [Drake, 1977 #54; Simunovic, 1975 #53]) The present work is, to our knowledge, the first attempt to describe a method of synthesis of an error-corrective compliance, given a description of a task. We do not claim to solve the problem, but to pose it and to suggest one approach to it. We should also note at this point that this paper is limited in scope to quasistatic assembly, i.e. to assembly speeds low enough that inertial effects of the robot and workpiece are small compared to frictional effects. Thus we only create a single, dynamics- independent real-valued accommodation (inverse-damping) matrix, in contrast to the work of Hogan on impedance control [Hogan, 1985 #20]. In the opposite (high-speed) regime the inertia matrix becomes the dominant factor in the evolution of an operation. There has been recent work [Asada, 1988 #41] on achieving inertia matrices appropriate for some individual tasks, particularly high-speed peg- in-hole insertion. 1.3 OBJECTIVE AND SUMMARY The main points of the paper are these: 1. We observe that in many tasks it is possible for any one of several qualitatively distinct contact configurations between grasped workpiece and environment to arise, depending on small initial errors. It is necessary to plan the compliance of a manipulator so that it is consistent with every one of the contact configurations, as we do not know in advance which ones may arise. We seek a compliance matrix which is to be employed for the entire duration of an assembly task; we do not here consider changing the compliance of the manipulator during a task. Our requirements on the compliance matrix are therefore quite severe: it must be simultaneously consistent with each of several possible contact configurations. 2. We suggest that it is not enough for the compliance of a manipulator to be consistent with a contact task, in the sense of the work of Mason and others, (i.e. preventing the occurrence of unbounded forces.) For many tasks, especially assembly tasks, we need the compliance to be error-corrective as well. We ask that the compliance of the manipulator have the effect of converting the forces which arise from misalignments of the workpiece into corrective motions. In section 2 we formalize these two desired properties of the manipulator's compliance. 3. We develop a set of sufficient conditions for our desired properties, all of a single mathematical form well suited to synthesis. We demonstrate an optimization method which efficiently finds a matrix satisfying the sufficient conditions if one exists, and if not finds one which comes close. In the latter case it is possible to verify whether the original desired properties are in fact satisfied. 4. We demonstrate the synthesis of error-corrective compliance matrices for three example assembly tasks. We find it encouraging that the result for the peg-into-chamfered-hole task is consistent with the remote-center-compliance (RCC) solution. 1.4 NOTATION t scalars are in light type A matrices are bold capital script F, V vectors are bold capitals Roman f unit vectors are bold lower case Our examples are planar. In these examples vectors have three components (x, y, q) where x is horizontal, y is vertical, and q is rotation about the z axis (i.e. counter-clockwise rotation in the plane of the paper.) By "forces" we mean forces and torques - a six vector in a three- dimensional world, or a three-vector in a planar world. Similarly, by "displacements" we mean displacements and rotations. 1.5 THE DAMPER MODEL AND THE ACCOMMODATION MATRIX The function which maps the forces applied to a manipulator into the motion which ensues due to those forces may take many forms. For instance, the function may state that displacement of the manipulator from some nominal position Xo (which could be changing with time) is proportional to the applied force: X - Xo = C F (1) This is Hooke's law, representing the behavior of an ideal spring, where C is the compliance matrix. An object held by a manipulator may have six degrees of freedom: three translational and three rotational. For such an object, equation (1) may be understood in vector form with X and Xo representing locations in six-dimensional configuration space, F a six-dimensional vector of forces and torques, and C a six-by-six matrix. Equation (1) is often a good approximation for the natural compliance of mechanical systems composed of solids. We need not, however, restrict ourselves to natural compliance. If a manipulator is equipped with a force/torque sensor, we can perform arbitrary computation on those measurements and arrive at a motion, which the manipulator then executes. Thus any compliance function may be achieved. This is called active compliance. If we implement the control law given in (1), we have simulated an ideal spring by using active compliance. As a practical matter the control law we implement must be evaluated rapidly, so there is a premium on simple control laws. The one we choose in this paper is simple, has been studied extensively in the past, and is usually called the generalized damper: V = Vo + A F (2) Control law (2) states that in the absence of any perceived forces F, the manipulator is to follow a nominal trajectory given by velocity Vo. In the presence of forces, the matrix A times the six-vector of forces F specifies a term AF which is simply added to the nominal velocity Vo to yield the velocity V which the manipulator is to perform4. A is an accommodation matrix, the inverse of a damping matrix, and characterizes a generalized damper. We have a further reason to restrict ourselves to the design of an accommodation matrix Ü i.e. to a linear relationship of assembly force to consequent velocity. It may be possible to design a passive mechanical wrist which can assume any (or a large range of) accommodation matrices. The range of accommodation matrices which could be assumed by such a wrist is considered in [Goswami, 1990 #97]. 1.6 CONTACT CONFIGURATIONS Our approach to the creation of an error-corrective matrix for a given assembly operation proceeds from the identification of a set of contact configurations as shown in Figure 2. In this example the assembly operation being performed is again the placement of a block into a shallow detent in a fixed chassis. (Note that the RCC wrist would not be able to perform this operation as there are no chamfers.) The nominal assembly trajectory of the manipulator, Vo, is straight down. In this paper we do not address the selection of Vo. Insert figure 2 here. Caption: Qualitatively distinct contact configurations which may result during the block-into-detent assembly task. The nominal assembly trajectory Vo is -y (down, not shown). Each contact configuration corresponds to a deviation of the movable part (the block) from its properly mated configuration. Sections a and b show contact configurations in which the edge of the block has come into contact with the left or right lip of the detent. In sections c and d the block is in contact with the left or right inside wall of the detent. We should note that specification of a complete set of contact configurations depends on more geometric information than is available in a CAD model of the parts alone. In particular we must know how great the initial configurational errors are. As an extreme example, if large horizontal initial errors are possible it may happen that the block will miss the detent entirely. In the selection of contact configurations for Figure 2, we have assumed that orientational errors are negligible and that translational errors are limited to about 2mm. Specification of the nominal trajectory Vo and the contact configurations is our starting point in this paper. We do not here address the problem of extracting the contact configurations from geometric models, nor is it clear how difficult this task would be. Donald's error detection and recovery regions (EDR regions) [Donald, 1986 #42; Donald, 1988 #43] and Desai & Volz' contact formations [Desai, 1988] contain much the same information as our contact configurations. Other related work includes [Brooks, 1982 #56; Buckley, 1987 #50; Erdmann, 1984 #49]. 2.1 FORMAL PROPERTIES OF THE ACCOMMODATION MATRIX FOR ERROR-CORRECTIVE MANIPULATION We pose the problem of creating an appropriate error-corrective accommodation matrix for a manipulator as that of realizing two desired properties of the manipulator's motion during an operation. These we will express in terms of the contact configurations. Property 1: "Bounded forces": For every contact configuration, A must be such that the contact forces which arise between mating parts are bounded. Property 1 is necessary to prevent damage to the manipulator and/or workpiece. This is akin to the condition justified and studied by Mason [Mason, 1981 #37]. Mason translated the bounded force condition into requirements on an accommodation matrix, and showed how an accommodation matrix could be found which guaranteed bounded forces for a given contact configuration. Our work differs in that we now consider several distinct contact configurations to all of which the bounded forces condition must apply simultaneously. For example, in figure 2a, AF (the product of the accommodation matrix A with the forces created by contact) must cause the lower left corner of the block to have a velocity component in the surface-normal direction +y (upward) to avoid unbounded forces. Property 2: "Error-reduction": For every contact configuration, A must be such that the magnitude of the displacement from proper mating E is reduced. For example, in figure 2a, AF must have a component in the +x (rightward) direction to make progress toward proper mating. For the moment we leave unspecified the metric M by which the "magnitude of the displacement from proper mating E" is to be measured. This is a non-trivial detail because the error vector E has both translational and orientational components (e.g. "centimeters" and "degrees"). The L2 metric (in which we square and add the components) has mixed units and makes no sense. See [Lipkin, 1990 #99] for further discussion of suitable unit- and frame-invariant metrics. Fortunately, if there is any metric M for which we can show that an accommodation matrix we synthesize results in decreasing |E| for all contact configurations, we are assured that the operation must terminate at proper assembly. Our choice of metric is important: if we make a poor choice we will not be able to synthesize an accommodation matrix A for which the distance to proper mating |E| is guaranteed to decrease. Yet our choice of metric need not pass any formal test of non-arbitrariness. 3.1 A METHOD OF SYNTHESIS AND OPTIMIZATION In this section we describe a synthesis and optimization method. It is by no means the only one which might be used to create a matrix A satisfying the "bounded forces" and "error reduction" properties above, and some of its shortcomings will be discussed in section 5. We will divide the "error reduction" property into two new conditions on A so that the new conditions and the "bounded forces" property will have a common mathematical form Ü one that is convenient for synthesis. We will show how A may be efficiently created so that it closely satisfies the whole set of conditions. For each contact configuration we determine the forces which we may expect the manipulator to perceive. It turns out that we only need the direction, and not the magnitude, of these characteristic forces. The direction is easily accessible to us from the familiar friction-cone construction. Given the direction of a characteristic force fi , we can express all of the conditions in the form ui × Afi = ti , where ui is a unit vector available to us from the geometry of the contact configurations (for instance ui may be a surface normal), and ti is a scalar. Generally it turns out that the number m of conditions on A: ui × Afi = ti i = 1...m (3) exceeds the number of elements5 in the matrix A. It is therefore not generally possible to find a accommodation matrix A which simultaneously satisfies all of the conditions exactly. All is not lost, because the values ti are not critical. The scalar ti corresponds to an inverse stiffness. In the literature on hybrid control, for instance, there are "soft" axes and "stiff" axes, represented by stiffnesses ksoft and kstiff, but the exact values of ksoft and kstiff are not critical. So too here we expect that while typical values of the ti can be given, if it is not possible to form a accommodation matrix with those precise values, one with slightly different values will also be acceptable. We can use the freedom to adjust the inverse stiffnesses ti to allow us to form a accommodation matrix A which almost satisfies the whole set of requirements, even when there is none that exactly satisfies them. We can then return to the "bounded forces" and "error reduction" properties to check that the matrix A synthesized does indeed satisfy them. 3.2 PREDICTING THE FORCES WHICH CHARACTERIZE A CONTACT CONFIGURATION. To design a accommodation matrix A which causes a desired motion in a particular situation we must be able to predict the contact force F exerted on the manipulator in that situation. It turns out that if we know the direction of the force F, we can calculate its magnitude, and this will be done immediately below. In the case of one-point contacts, we do know something about the direction of the force F, namely that it must lie within the friction cone as shown in Figure 3. Further, if we know that the direction of motion of the block in Figure 3 will be to the right (+x), we may predict that F will lie on the left extreme of the friction cone (f+) as shown. Contacts at more than one point are considered in section 3.2.3. Insert figure 3 here. Caption: Here we have added to each of the qualitatively distinct contact configurations of figure 2 the surface normal (n), home (h), and orthogonal (o) vectors (there is only one orthogonal vector in planar examples.) Also shown are the friction cones, the extremes of which are the vectors f±. We could build our accommodation matrix A using the assumption that F will lie on the left extreme of the friction cone in Figure 3a. However, given the present state of the art in implementing force control, the assumption of a monotonic +x motion which will keep F on the left extreme seems questionable at best. Instead we will assume only that F lies within the friction cone, and ask that no matter where in the friction cone F lies, the "bounded forces" and "error reduction" properties are satisfied. Our conservative assumption about the direction of F contributes to a robustness of the resulting accommodation matrix A and, hopefully, relative insensitivity to the accuracy of a physical implementation of the control law Væ=æVoæ+æAF. 3.2.1 EXPRESSING THE "BOUNDED FORCES" PROPERTY Property 1 requires that the motion of the manipulator must be such that the contact forces which arise between mating parts are bounded. To find the implications of this requirement for A, we evaluate the magnitude of the force F which arises in a particular contact configuration. First, we express F as a product of its magnitude and direction: F\D\FO4()=\D\FO4()| F | f. Let n be the surface normal as shown in the Figure 3. Since the velocity of the block V can have no component in the direction n, we have n × V = 0. (Some care must be taken in performing dot products among vectors of mixed distances and angles [Ball, 1900 #102; Hunt, 1978 #103]) Combining the above equations with our control law Væ=æVoæ+æAF we can solve for the magnitude of the force |F| = - \F(n × Vo,n × Af) (4) The scalar (n× Af)Ü1 represents the stiffness of the manipulator to interactions with a particular surface. To guarantee that unbounded contact forces do not arise due to velocities which bring the parts together, no matter what the source of these velocities, it is only necessary to require a finite (non-infinite) stiffness. Property 1 becomes Property 1: "Bounded forces" Þ n × Af È t1 (5) A reasonable value for the scalar stiffness 1/t1 depends on the task being performed, the materials involved, the nominal velocity Vo chosen, and other factors. We will presume that while a user can select a typical value for t1, he or she will not feel very strongly about the exact value. For the rest of this paper it is important only to notice that t1 is positive, and its value user-selectable. Our method for creating A (section 3.3) will take advantage of the freedom to adjust t1. Property 2 will result in a similar parameter (t2) being introduced. 3.2.2 EXPRESSING THE "ERROR REDUCTION" PROPERTY Property 2 requires that the motion of the manipulator reduce the distance from the manipulator's present configuration to a properly mated configuration, as measured by some metric M. We will use as our metric M a modified L2 metric in which the angular components (in radians) are multiplied by a characteristic dimension of the assembly task (e.g. the width of the grasped object) and then the components are squared and summed6. This metric is physically justifiable though still arbitrary. Again, we are content with a matrix A if it satisfies property 2 for any metric so we may indeed choose M arbitrarily. To express property 2 in a form similar to (5) above, let us now identify a "home" unit vector h for each contact configuration. We choose h to be the direction consistent with sliding constraints in which the distance from proper mating is most steeply decreasing, as measured by our metric M. With h and n identified, there are several7 other vectors oi orthogonal to both h and n. Figure 3 shows examples of h, n, and o. We can now write sufficient conditions for property 2 (error- reduction) as: h × V > 0, and (7a) oi × V = 0 "i (7b) These conditions ask that there be motion in the h "home" direction (7a), and that there not be motion in orthogonal directions (7b). The required velocity at which errors are reduced in (7a) is not critical, but for definiteness let us request that hæ‡æVæÈæ|Vo|, i.e. we wish the manipulator to move toward proper mating at approximately the full nominal velocity. Using the control law V = Vo + AF, our requirement h × V È |Vo|, and n × V = 0, we find h × Af È (h × Vo Ü |Vo| ) \F(n × Af,n × Vo) (8) Note that n × Vo is negative8. Vo is our nominal assembly velocity and we have already required that n × Af È t1 for some user- selectable parameter t1. The right side of (8) can be evaluated and we will denote it t2, another positive constant. We may now write property 2 as Property 2: "Error Reduction" Þ h × Af = t2, and (9a) oi × Af = 0 "i (9b) For example, consider contact configuration a in Figure 3. The nominal direction Vo\D\FO4()=\D\FO4()-y, the surface normal n = y, and the home direction h = +x. The "bounded forces" property is thus y\D\FO4()×\D\FO4()Af\D\FO4()=\D\FO4()t1. In place of the "error- reduction" property, condition (9a) gives us xæ×æAfæ=æt2. Condition (9b) applies only to the one remaining degree of freedom q, and is qæ×æAfæ= 0, i.e. Af should have no rotational component. 3.2.3 EXTENSION TO CONTACT CONFIGURATIONS WITH MORE THAN ONE POINT OF CONTACT If there are two or more points of contact between workpiece and environment, the total force felt by the manipulator is the sum of the forces at the individual contacts. For simplicity we will work here with only two contacts; the analysis extends trivially to any number. Denote these individual forces F1, F2. The motion of the manipulator is still given by the control law Væ= Vo + AF, which can now be written Væ=æVo + AF1 + AF2. We can break each force into a product of its magnitude with its direction Fi = |Fi| fi. We also have one condition of the form Væ×æni = 0 for each contact. We can therefore construct a set of equations from which the magnitude of the forces can be extracted: n1 × Vo + |F1| n1 × Af1 + |F2| n1 × Af2 = 0 (10a) n2 × Vo + |F1| n2 × Af1 + |F2| n2 × Af2 = 0 which can be written as \B(\A\CO2\AC \HS20\VS5(n1 × Af1,n1 × Af2,n2 × Af1,n2 × Af2)) \B(\A\CO1\AC \HS20\VS10(|F1|,|F2|)) = Ü \B(\A\CO1\AC \HS20\VS5(n1 × Vo,n2 × Vo)) (10b) As in section 3.2.1 we may identify the determinant of the two-by- two matrix as the inverse stiffness of the manipulator. Property 1 (bounded forces) therefore becomes det \B(\A\CO2\AC \HS20\VS5(n1 × Af1,n1 × Af2,n2 × Af1,n2 × Af2)) È t12 (11a) For the general case of m contacts where {ænæ} represents the m surface normals and {æfæ} represents the m force directions we may write this condition compactly (if opaquely) as an outer product det ( { n } € A { f } ) È t1m (11b) It is useful to notice that if a contact configuration with more than one point of contact can occur, it is also possible for the points of contact to occur individually as contact configurations. The "bounded forces" property applied to these individual contact configurations involve the terms ni ‡ Afi that appear on the diagonal in (11a). Since we have already required that ni ‡ Afi È t1, one way to satisfy (11a) is simply to require that ni ‡ Afj È 0 when i Ð j, i.e. that all the off-diagonal terms be zero. This observation will be useful in the following section, because it allows us to reduce all of the properties on all of the contact configurations to a single mathematical form. 3.3 FORMING AN ACCOMMODATION MATRIX All of our conditions on the accommodation matrix A have now been reduced to numerous conditions of the form ui × Afi = ti (14) where fi is a force to be expected, ai is a direction vector determined from the geometry of the contact configuration, and ti is a target value for the dot product. For some conditions the target value t is zero. For others t is a user-selectable positive value whose choice depends on the task at hand, materials, etc, and indeed is somewhat arbitrary. Selection of these ti is guided by equations (4) and (8). Recall that f in equations of the form (14) represents any force direction which could arise in a particular contact configuration. f must necessarily lie within the friction cone, but we are reluctant to presume it any better localized than that. Let the two extremes of the friction cone be denoted f±. Then any force f that arises can be written as a positive linear combination of f+ and f-. It will not be possible to design a accommodation matrix A for which uæ× Af+ = u × Af- = t. However our target value t was not critical, and perhaps we can design a accommodation matrix A for which uæ×æAf+æ= t+, u × Af- = t-, with both t+ and t- acceptably close to the target t. If so we are assured that the value of u × Af will be acceptably close to t for any f which may arise. 3.3.1 LEAST-SQUARES OPTIMIZATION For each of several contact configurations we consider two forces f±, and for each force we have several conditions of the form ui × Afi = ti. Let m be the total number of such equations -- clearly m may be large. The ui, fi, and ti are all given; it is A we wish to create. A is a matrix consisting of n real numbers. In our two- dimensional examples n=9; in three dimensions we would have n=36. The usual case is that there is no A which satisfies all m equations. We describe next a way of finding the best A in the "least-squares" sense: the A which minimizes \I\SU\IN(i=1,m,) (ui × Afi Ü ti)2. Solving for this A is computationally quick even for m=200, n=36. How are we to interpret the resulting A? A satisfactory A is one for which the "bounded forces" and "error reduction" properties hold, so that motion can only terminate at proper mating. Our m conditions of the form ui × Afi = ti are stronger (sufficient) conditions, so if an A satisfies them all exactly, that A is satisfactory. Further, of the { ti }, some were zero (equation 9b), and others were arbitrary positive values (equations (5) or (9a)) . If the A we create satisfies the (9b)-type conditions and gets any positive values for the (5) and (9a)-type conditions, that A is also satisfactory, but for a different choice of the arbitrary stiffnesses. Even if the A we create does not satisfy the (9b)-type conditions, it may still be satisfactory, but then we must check the "bounded forces" and "error reduction" properties explicitly. We now abandon the matrix structure and meaning of A, and simply consider it to be a set of n real-valued variables (here n=9) for which we desire a solution satisfying m equations. To maintain a compact notation we will form these n variables into a n-vector Aso, here a 9-vector. A is "strung-out" by rows and displayed as a column vector. Explicitly, A = \B(\A\CO3\AC \HS10\VS5(Axx,Axy,Axq,Ayx,Ayy,Ayq,Aqx,Aqy,Aqq)) Þ Aso = \B(\A\CO1\AC \HS15\VS3(Axx,Axy,Axq,Ayx,Ayy,Ayq,Aqx,Aqy,Aqq)) (17) Having formed this unintuitive construction, an equation of the form u × Af = t which is explicitly \B(\A\CO3\AC \HS10\VS5(ux,uy,uq)) × \B(\A\CO3\AC \HS10\VS5(Axx,Axy,Axq,Ayx,Ayy,Ayq,Aqx,Aqy,Aqq)) \B(\A\CO1\AC \HS10\VS5(fx,fy,fq)) = t (19) can be written as: \B(\A\CO9\AC \HS5\VS10(uxfx, uxfy, uxfq, uyfx, uyfy, uyfq, uqfx, uqfy, uqfq))× Aso= t (20) Each of the m equations of form u × Af = t can be written as above, for i = 1 ... m, so that we have an enormous matrix equation: \B(\A\CO9\AC \HS5\VS0(u\S(1,x)f\S(1,x), u\S(1,x)f\S(1,y), u\S(1,x)f\S(1,q), u\S(1,y)f\S(1,x), u\S(1,y)f\S(1,y), u\S(1,y)f\S(1,q), u\S(1,q)f\S(1,x), u\S(1,q)f\S(1,y), u\S(1,q)f\S(1,q),×, ×, ×, ×, ×, ×, ×, ×, ×,×, ×, ×, ×, ×, ×, ×, ×, ×,u\S(m,x)f\S(m,x), u\S(m,x)f\S(m,y), u\S(m,x)f\S(m,q), u\S(m,y)f\S(m,x), u\S(m,y)f\S(m,y), u\S(m,y)f\S(m,q), u\S(m,q)f\S(m,x), u\S(m,q)f\S(m,y), u\S(m,q)f\S(m,q))) × Aso = \B(\A\CO1\AC \HS15\VS0(t\S(1, ),×,×,t\S(m, ))) (21) We will name the great m-row by n-column matrix on the left G, and the m-high column vector of target values t. In these terms the problem of finding a accommodation matrix A which satisfies our m conditions is reduced to finding a n-vector Aso which satisfies G× Aso = t (22) Generally there is no Aso which satisfies (22) exactly. However it is easy to check that œ\D\BA2()œ G× Aso - t œ\D\BA2()œ 2 = \I\SU (i=1, m , ( ui × Afi - ti )\D\FO2()2) (23) The right side is exactly the sum we wish to minimize Ü the one that defines the "best" A. There is a good and fast way of finding the Aso which minimizes the left side. Using the singular value decomposition (described particularly clearly in [Press, 1985 #39]) we can form an n-by-m pseudo-inverse9 G* of the m-by-n matrix G for which the value of Aso given by Aso = G* t (25) minimizes œ\D\BA2()œ G Aso - t œ\D\BA2()œ 2 over all possible Aso. If (fortuitously) there is a Aso which satisfies (22) exactly then (25) finds it. In this case the sum-of-squares is zero. G contains all the geometry of the contact configurations and forces which arise in assembly. t contains our preferences for the user- selectable parameters ti. If we pick a target vector of parameters t, we can easily compute the accommodation matrix A which comes closest to achieving those parameters using Asoæ=æG* t. Usually we can't have that set of parameters; the closest available set is t~æ=æG Asoæ= G G* t. To see how well A achieves our preferred parameters, we compare t to t~. Once the m-by-m matrix G G* has been formed, we can try out different target vectors t with little overhead. As mentioned above if t~ has zeros in the same components that t has zeros, and is positive in all other components, then A possesses the "bounded forces" and "error reduction" properties. The only consequence of not satisfying all of the conditions exactly is that the stiffnesses are not exactly as requested. If t~ does not match t in sign, component by component, then to verify that A possesses the "bounded forces" and "error reduction" properties we must compute Væ=æVoæ+æAæF for every contact configuration and explicitly check that the properties are satisfied. 4.1 EXAMPLES In this section we set up (by hand) the contact configurations and the normal, home, and orthogonal vectors for some sample planar assembly tasks. To create the accommodation matrix A which achieves the desired "bounded forces" and "error-reduction" properties we proposed in section 2, we use the stronger conditions and the optimization procedure described in section 3. When even the stronger conditions can be satisfied (or satisfied for a slightly different set of stiffnesses) we are done. This occurs in the first two examples. In the third example we must verify that the desired properties are in fact satisfied for all contact configurations. 4.2 PLACE BLOCK INTO CHAMFERLESS DETENT Here a block is to be placed into a shallow detent in a surface (Figure 1). We wish to create a accommodation matrix so that the torque which arises if the edge of the block hits the lip of the detent is mapped by the accommodation matrix into a horizontal motion of the block towards the center of the detent. This motion is counterintuitive since the detent has no chamfers, but it is not impossible. It is necessary to choose a coordinate system: an origin about which torques are measured and rotations are performed. The choice of origin is of absolutely no account; in fact if we create a accommodation matrix for origin q, it is trivial to convert it to a accommodation matrix for origin p simply by Ap = \B(\A\CO3\AC \HS20\VS5(1 ,0,qy - py,0 ,1,px - qx,0 ,0,1)) Aq \B(\A\CO3\AC \HS20\VS5(1,0, 0,0,1, 0,qy - py,px - qx, 1)) (26) For a given force in the real world, the accommodation matrix Ap at origin p produces exactly the same motion of the manipulator as would accommodation matrix Aq at origin q. Inconsequential as it is, we must choose an origin, so we choose it at q as shown in Figure 1. The four possible contact configurations are shown in Figure 3. It should be noted that implicit in the specification of only these contact configurations is information about the maximum possible magnitudes of the robot's initial error. If, for instance, the possible horizontal translational error were much greater, the block could miss the detent altogether. We presume, not unrealistically, that errors of orientation are minimal. For each of the four contact configurations there are two extremes of the friction cone. Translating the "bounded forces" and "error- reduction" properties into three stronger conditions on each of the eight force-and-configuration pairs, we find a total of 24 conditions on the accommodation matrix A to be synthesized, each of the form ui × Afi = ti. For the user selectable target inverse-stiffnesses ti we have used the value 1 or 0 as appropriate from equations (5), (9a) or (9b). We find that it is not possible to satisfy all the conditions exactly. The accommodation matrix which most closely fulfills the conditions is Aq = \B(\A\CO3\AC \HS10\VS5(2.91,0.00,- 1.94,0.00,1.03,0.00,0.00,0.00,0.00)) (27) Evaluating t~ = G G* t, the available set of inverse-stiffnesses ti nearest to our target set, we find all values within 30% of target. This is surely good enough - it would be very strange if stiffnesses more precise than this would ever be needed. The (9b)-type conditions (ui × Afi = 0) are satisfied exactly. For this task the accommodation matrix synthesized cannot be described as a center of accommodation, no matter what choice of origin we use. The result is relatively insensitive to the coefficient of friction assumed, which we took as .25. 4.3 PUT A BLOCK DOWN GENTLY Here a block is simply to be placed on a surface. There are only two contact configurations, shown in Figure 4: either corner may touch first, and we would like the behavior of the manipulator to be such that upon contact the block rotates into alignment with the surface, without scraping along the surface. Insert figure 4 here. Caption: Contact configurations for the put-block- down-gently task. Property 1 requires bounded forces. Property 2 requires that the block rotate clockwise or counterclockwise as appropriate. The (9b)-type conditions (uiæ×æAfiæ=æ0) require that there be no horizontal movement of the corner which makes contact first: no "skidding". Two contact configurations, by two extremes of the friction cone, by three conditions each, yield twelve conditions of the form uiæ×æAfi = ti. Using the origin q as shown in Figure 4 (at the top center of the block), and again using 1 or 0 as target values for all the user- selectable inverse-stiffnesses, we find that there is an accommodation matrix which satisfies all the conditions exactly: Aq = \B(\A\CO3\AC \HS10\VS5(2.06,0.00,- 2.06,0.00,0.52,0.00,-2.06,0.00,2.06)) (28) It can easily be verified from equation (26) that this accommodation matrix is equivalent to a diagonal matrix at the bottom center of the block, meaning that our synthesis has suggested that in this case a "center of compliance" description is appropriate Ü with center of accommodation at the bottom center of the block (point p). The equivalent accommodation matrix there is Ap = \B(\A\CO3\AC \HS10\VS5(0.00,0.00,0.00,0.00,0.52,0.00,0.00,0.00,2.06)) (29) It is relatively easy to imagine the operation of an accommodation matrix which is diagonal, and the reader may check that a center of accommodation at p will give the proper response to contact at either corner. 4.4 PEG INTO CHAMFERED HOLE REVISITED Here we find 10 contact configurations, some of which are two- point contacts. Five of these are shown in figure 5, and the other five are reflections about a vertical axis. Insert figure 5 here. Caption: Contact configurations for the peg-into- chamfered-hole task. We consider these contact configurations shown, and five (not shown) which are these reflected about a vertical axis. The nominal assembly trajectory Vo is straight down ("south"). One of the configurations (E) involves two points of contact; the rest are single point contact configurations. To avoid confusion the normal (n), home (h), and orthogonal (oi) directions are not shown in the figure. In configuration A, n is northeast h is southeast, and o is rotation. In configurations B, C, and D, n is east and o is rotation. The h direction is not needed because south is colinear with Vo. In configuration E, h is counterclockwise rotation. It turns out that there is no accommodation matrix A which simultaneously gives the desired stiffnesses in every contact configuration. In fact there is no way to satisfy all the (9b)-type conditions uiæ×æAfiæ=æ0 even if we allow any positive value for ti in the (5) and (9a)-type conditions ui × Afi = ti. The accommodation matrix which comes closest in the least- squares sense is Aq = \B(\A\CO3\AC \HS10\VS5(0.66,0.00,0.19,0.00,1.57,0.00,0.20,0.00,0.35)) (30) based on an origin at q as shown in figure 5. Interestingly, we find that Aq is in agreement with [Whitney, 1982 #38] results which are incorporated in the RCC device. Specifically, if (following [Whitney, 1982 #38]) we let l2 be the insertion depth at which two point (jamming) contact may first occur, the accommodation matrix A which our optimization produces can be described as a center of accommodation at a point p a distance l2/2 above the tip of the peg, on-axis. Rewriting A for origin at point p we have Ap = \B(\A\CO3\AC \HS10\VS5(0.55,0.00,0.01,0.00,1.57,0.00,0.02,0.00,0.35)) (31) which is (almost) diagonal. DISCUSSION In our view the main contribution of this paper is to pose the problem of synthesizing an error-corrective compliance for an entire task. To the best of our knowledge this is the first attempt at a method of synthesis. The underlying idea (common to other works in this field) is that the forces which arise during an assembly task often contain enough information to figure out how the operation has gone wrong. Forces characterize the errors in assembly tasks in a more direct way than other sensors, (e.g. vision). The question then becomes how best to interpret and act on this force information. Others researchers have (implicitly or explicitly) interpreted this information too, and a summary of those ideas in given in section 1. Our approach may be distinguished from Mason's in that Mason synthesizes a accommodation matrix consistent with a task, where we synthesize one which is also error-corrective (terms defined in section 1.1). It may be conceptually contrasted with most other fine motion planning work in that we assume the nominal motion plan to be given, and synthesize the accommodation matrix of the manipulator so that assembly proceeds to completion. Most other workers have assumed the compliance of the manipulator to be given, and have synthesized a motion plan. We draw inspiration from error-corrective compliances which have been created by others for a few particular tasks, most notably the RCC. Our work differs from these in that we attempt a method of synthesis, not a solution for an individual task. This work has several major limitations: 1. We have assumed that a finite and complete set of qualitatively distinct contact configurations has been (and can be) generated and presented to us. These configurations, together with a nominal assembly trajectory which would succeed in the absence of error, are the starting point of our synthesis. While finding these configurations does not seem to be difficult to do by hand for simple examples, we do not know of a formal definition of "qualitatively distinct" or of a method of generating such configurations. 2. Our "error-reduction" property (section 2.1) requires strictly decreasing errors for all contact configurations as measured by any metric. We have chosen a particular metric, and therefore might not recognize as valid an accommodation matrix which decreases errors according to some other metric10. Further, one might imagine compliances which do guarantee eventual success, but do not achieve it monotonically by any metric. 3. The optimization method described in section 3 does not directly use even the one particular metric we have chosen (and by which we verify the resulting A matrix). Rather it optimizes adherence to a set of stronger conditions which, if they are all satisfied, would also guarantee strictly decreasing errors. These stronger conditions are not always satisfied, so we do not necessarily find an accommodation matrix which guarantees strictly decreasing errors by our metric even when one exists Ü we are optimizing not quite the right thing. Though our examples were two dimensional, most of the development applies immediately to three dimensions. An exception is our bounding of the friction cone by two vectors (section 3.3). It is possible to generalize the friction cone to three dimensions [Goyal, 1990 #101; Erdmann, 1984 #49]. It might also be that providing a set of qualitatively distinct contact configurations (the input to the method of synthesis described here) may be much more difficult in three dimensions than in two. ACKNOWLEDGMENTS The author wishes to acknowledge helpful discussion with Bruce Donald (Cornell), Joe Schimmels (Northwestern), Ambarish Goswami (Northwestern), Harvey Lipkin (Georgia Tech), Matt Mason (Carnegie-Mellon), and useful comments from the reviewers. [Editor - please leave the acknowledgment of the reviewers as written. Thank you.] REFERENCES 1 This work was supported by NSF grant DMC-8857854 and Northwestern University. Author's electronic mail address is peshkin@eecs.nwu.edu 2 We use the generic meaning of the term "compliance" Ü the relationship of a manipulator's motion to the forces imposed on it. (Compliance has a specific meaning as well, namely the matrix characterizing spring-like behavior Ü see section 1.5) In this paper we are designing an accommodation matrix, the inverse of a damping matrix. 3 By "force control" we include all forms of control in which the response of the manipulator to forces it encounters is intentionally controlled. Among these are free-joints, hybrid control, compliance control, stiffness control, impedance control, and many generalizations and extensions. 4 For example, to compare the motion of a manipulator under control laws (1) vs. (2), suppose that the manipulator is moving steadily in the -z direction, and we give it a push in the +x direction. If C (or A) is a diagonal matrix with positive elements, in either (1) or (2) the manipulator will respond by deflecting in the +x direction. After we cease pushing, a manipulator following law (1) will spring back to its original path and proceed as if it had not been disturbed. A manipulator following law (2) will resume its original direction of motion (-z) but will not move back to its original path: its displacement in the +x direction is permanent. With a diagonal matrix C (or A) the motion of the manipulator under either (1) or (2) is intuitive: it moves in the direction pushed. However in selecting a matrix we have considerable freedom, and highly non-intuitive behaviors may result. For instance we can, by making the proper element of C (or A) non-zero, cause forces in the +x direction to map into motions in the +y direction Ü or even into a rotation about the z axis if so desired. 5 Nine in two-space or thirty-six in three-space. 6 M(x, y, z, qx, qy, qz) = (x2 + y2 + z2) + w2 (qx2 + qx2 + qz2), where w is a characteristic length of the task. 7 One in 2-d, or four in 3-d. 8 If n × Vo È 0 it will be best to impose no requirement on h × Af. In this case if hæ×æVoæ>æ0 the nominal trajectory Vo itself causes reduction of error, and no adjustment of A will increase or decrease the rate much. If h × Voæ<æ0 the situation is hopeless. 9 There are other pseudo-inverses. Computation of the SVD pseudo-inverse is numerically robust, and the SVD pseudo-inverse possesses the needed least-squares property. 10 Much work in C-space seems to be dogged by the current lack of sensible metrics.