RSD-TR-1-85 A GEOMETRIC APPROACH TO DERIVING POSITION/FORCE TRAJECTORY IN FINE MOTION* C. S. G. Lee D. Huang Department of Electrical Engineering and Computer Science The University -of Michigan Ann Arbor, Michigan 48109-1109 February 1985 CENTER FOR RESEARCH ON INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 'This wrork was supported in part by the AFOSR Grant F49620-82-C-0089. Any opinions, findings, and conclusions or recommendations expressed in this article are those of the authors and do not necessarily reflect the vriers of the funding agency.

TABLE OF CONTENTS 1. Introduction...................................................................................... 3 2. T-C and C-PF Mapping Concepts............................................. 6 3. Geometrical Knowledge Base (GKB)................................................... 13 4. Algorithms of the T-C and C-PF Mappings........................................ 16 4.1. T-C Mapping.................................O*....* 17 4.1.1. Deriving nominal position trajectory P(t)...................... 17 4.1.2. Algorithm for Freedom Decomposition Function S(t) O.~~~~~... O.. O... O... O..........e****@**@s..e@*....e 23 4.2. C-PF Mapping................. 25 5.e Conclusions........................................ 28 6. References..............................e..e. 29

RSD-TR-1-85 ABSTRACT The synthesis of position/force trajectories for compliant motion is an important task for the control of industrial robots in assembly tasks. The position and orientation of the manipulator can be specified by a set of six linear independent constraints. Some are caused by contact with other objects (natural position constraints) and the others are applied artificially (artificial position constraints). Since the uncertainties and the modeling errors make the. exact natural position constraints incomputable, a set of contact forces (artificial reaction-force constraints) is servoed to implicitly reflect their effects. Two mappings, T-C and C-PF, are defined which derive the nominal position trajectory P(t) and freedom decomposition function S(t) from the attributes of a given task, and then the desired artificial position and reaction-force constraints. In the T-C mapping, for an insertion task, the convex operand shrinks to a line and the three 2 -D cut-diagrams of the concave operand are derived and expanded correspondingly. The nominal position trajectory is determined from these expanded cutdiagrams. Three 2-D trapezoids are defined to model the uncertainties. The intersections between the trapezoids and the cut-diagrams are checked to determine the values of freedom decomposition function S(t). Consequently, in the C-PF mapping the time sequences of the artificial position constraints and artificial reaction-force constraints.are derived from the nominal position trajectory and the freedom decomposition function. Position/Force Trajectory 2

RSD-TR-1-85 1. Introduction Most manipulator tasks will likely require some form of compliant motion control which is defined as the ability to modify the manipulator motion based on sensed contact forces during the motion. Compliant motion is a common phenomenon in robot tasks and occurs when the position of a manipulator is constrained by the task geometry. Previous work on active compliant motion control focused on controlling the manipulator to follow the desired position/force trajectories as closely as possible and can be divided into two categories: hybrid position/force control [7-9] and explicit feedback control [5,11]. The hybrid position/force control consists of separate position and force control loops. For a given task, the input to the controller is the desired position and force trajectory set points in the C-framet. The robot motions in the C-frame are partitioned into position and force constraint subsets and the controller servos each degree of motion freedom, position or force, by a feedback loop. Unlike the hybrid position/force control, the explicit, feedback control approach uses a force control law which relates the deviation of the hand position or velocity to the required force applied to the hand. Both methods require correct synthesis of the position and/or force trajectory which will lead the t C-frame: Compliance frame [8-91 is an orthogonal coordinate system in the Cartesian space. The frame is so chosen that the task freedoms are defined to be translation along and rotation about each of the three principal axes. Position/Force Trajectory 3

RSD-TR-1-85 robot to complete its assembly task. This paper addresses the problem of generating position/force trajectory for the hybrid position/force control scheme. Current manipulator trajectory planning methods focused on position trajectories [1,5,6]. For compliant motion control, a force trajectory is required as well. Mason [7] developed a formal approach to derive velocity and force constraints from a C-surfacet, not the partitioned position and force trajectory set points for the hybrid position/force controller. Since the C-surface is task dependent, his method is not completely general. For any robot task no matter whether it is a gross motion or compliant motion, there exists an ideal nominal position trajectory which must be followed by the manipulator to complete the task. This trajectory can be viewed as a set of six linear independent constraints to eliminate the six degrees of freedom in a six-dimensional configuration space which consists of all possible positions and orientations of the manipulator. These position constraints can be partitioned into two subsets of constraints. They are natural position constraints which occur due to contacting with other objects and artificial position constraints which are applied artificially. Due to uncertainties and errors present in the modeling of the objects and the manipulator, obtaining the natural position constraints exactly may be impossible. However, natural position constraints can be indirectly represented by artificial reaction-force constraints because contact is always associated with reaction force. Therefore, the task of designing a t C-surface is defined on a C-frame. It is a task configuration which allows only partial position freedoms. Along its tangent is the positional freedom and along its normal is the force freedom [71. 4 Position/Force Trajectory

RSD-TR-1-85 position trajectory which will lead the robot to complete an assembly task can be decomposed into defining two subsets of constraints, artificial position constraints and artificial reaction-force constraints. Their combined effect to the controller should be equivalent to that of the ideal nominal position trajectory. This paper extends Lozano-Perez's shrink-expand approach [6] and combines it with a 2-D geometric cut-diagram approach to derive a 3-D position/force trajectory for the hybrid position/force control scheme. The problem of automatic synthesis of position/force trajectory can be expressed as a mapping T-PF (task to position/force mapping) which in turn consists of two submappings. They are T-C submapping (task to configuration space mapping) which derives a nominal position trajectory P(t) and a freedom decomposition function S(t) from the attributes of a given task and C-PF submapping (configuration space to position/force trajectory mapping) which derives the artificial position constraints and the artificial reaction-force constraints from the P(t) and S(t). In the T-C mapping, for an insertion task, an algorithm similar to Lozano-Perez's shrink-expand approach is used to shrink the convex object grasped by the manipulator into a line with one of its end points at the origin of its body attached frame. This origin is called the nominal center. Then, the 3-D nominal trajectory planning is done on the three 2-D cut-diagrams of the other relevant object. The uncertainties cause a larger region of possible contact. These uncertainties are represented by three 2-D trapezoids. By checking the intersections between the trapezoids and the three 2-D cut-digrams, a piecewise Position/Force Trajectory 5

RSD-TR-1-85 constant vector function S(t) is determined which represents the partition of the six position constraints into artificial position constraints and natural position constraints. S(t) as well as the nominal position trajectory P(t) serve as inputs to the C-PF submapping which is responsible for determining the artificial position and reaction-force constraints. In the C-PF submapping, a time sequence of every element of the artificial position constraints and artificial reaction-force constraints is determined according to the corresponding element of the freedom decomposition function S(t). The outcome of this mapping is a time sequence of artificial position constraints Pa (t) and artificial reaction-force constraints Fa(t) which are ready to serve as reference inputs to the position/force servo loops. In the-next section, the concepts of T-C and C-PF mappings are defined and discussed. A geometrical knowledge base which provides the geometric information of the objects in the workspace to the T-C mapping is briefly described in Section 3. Then the algorithms of T-C and C-PF mappings are described in Section 4, followed by conclusions in Section 5. 2. T-C and C-PF Mapping Concepts In this section we formulate the problem of generating position/force trajectory for the hybrid controller by two mappings ( T-C and C-PF mappings) and introduce some related concepts. A configuration space is a generalized coordinate system used to specify certain entity. Therefore the position and orientation of an arbitrary object in a 6 Position/Force Trajectory

RSD-TR-1-85 3-D space can be specified by a six-tuple P = (p,,, q, q ) in a 6-D configuration space C, where p,,py, and p, are displacement along the X, Y, and Z axes, respectively, of the. base frame and q3,y, and q, are angular displacement about the the principal axes of a frame whose axes are parallel to the base frame and whose origin is at (p,, pY, pz ). In other words, the six orthogonal constraints specify the position and orientation of an object in a 3-D space. Similarly forces and torques in the configuration space C in the base frame can be represented by a six-tuple F = (f,, y,, fzr, I.r,, rz) where f, y, and f. are the forces along the X, Y, and Z axes, respectively, of the base frame and r, ri, and r, are the torques about the principal axes of a frame whose axes are parallel to the base frame and whose origin is at (ps, PY, p, ). All these possible six-tuples form another configuration space, called force configuration space Cl. However, it is not independent and is associated with the configuration space C. The following definitions are important in discussing the T-C and C-PF mappings. Position freedom in configuration space: A degree of configuration space is said to possess position freedom if the motion along this degree (i.e. the motion along or about the corresponding principal axis of the base frame) is free. Reaction-force freedom in configuration space: A degree of configuration space is said to possess reaction-force freedom if the reaction-force along this degree Position/Force Trajectory 7

RSD-TR-1-85 (i.e. the reaction-force along or the reaction-torque about the corresponding principal axis of the base frame) can take any value within some assumed domain. Natural position constraint: A position freedom of degree in configuration space of an object under consideration will be eliminated by contact with other objects, or in other words, constrained by the task geometry. This constraint is called natural position constraint on the object, while the object is said to be natural position constrained on the degree of configuration space. Artificial position constraint: A constraint artificially applied to a degree of configuration space of an object under consideration to eliminate the position freedom of that degree. The object is said to be artificial position constrained on the degree of configuration space. Natural reaction-force constraint: A reaction-force freedom of configuration space of an object under consideration will be eliminated by absence of contact on that degree with other objects. The corresponding reaction force can not take any arbitrary value besides zero or certain value of friction. This constraint is called the natural reaction-force constraint on the object, while the object is said to be natural reaction-force constrained on that degree of configuration space. Artificial reaction-force constraint: A constraint artificially applied to a degree of configuration space of an object under consideration to eliminate the reaction-force freedom of that degree. The object is said to be artificial ~~~~~~~~8 ~~Position/Force Trajectory

RSD-'T!R-1-85 reaction-force constrained on the degree of configuration space. For every robot task, there exists a time sequence of six orthogonal constraints in the configuration space, which specifies the positions and orientations where the manipulator must be if the task is successfully executed. This time sequence of constraints is called ideal nominal position trajectory. If we could derive such trajectory by some algorithms and we had an ideal controller which could drive the manipulator without any error, then the robot could perform any assembly task as precisely as possible. Unfortunately, with uncertainties and modeling errors, it is impossible to derive such an ideal nominal position trajectory from a set of uncertain data. What we can do is to derive a time sequence of six constraints in the configuration space based on the available uncertain data to approximate the ideal nominal position trajectory. The trajectory resulted from this approximation is called nominal position trajectory. At any time t, during the execution of a robot task, some of these required six position constraints are applied by the task geometry. They are the natural position constraints. Generally, the number of linear independent natural position constraints is less than six. Several artificial position constraints should be selected to constrain the remaining degrees of position freedom. The total number of natural position constraints, denoted as NC, and the total number of artificial position constraints, denoted as N, satisfy the following relations: O < NC < 6 and N. 6- NC When NC = 0, it implies gross motion; while when N, = 6, it implies that the Position/Force Trajectory 9

RSD-TR-1-85 grasped object is not movable. In an ideal case, these artificial and natural position constraints should form the ideal nominal position trajectory and serve as reference input to the robot controller. Since artificial position constraints are designed by the operator and are known exactly in advance, they can serve as the reference input to the controller easily. However, natural position constraints exist naturally and can be detected only with certain degree of uncertainty, which disqualifies them to be the reference input. This is the reason why the ideal nominal position trajectory can not be derived exactly. A solution to this difficulty is to indirectly servo the natural position constraints. Since the natural position constraints are caused by contact with other objects and the contact is always associated with reaction force, the reaction force along the degree of configuration space, which is natural position constrained, can indirectly provide natural position constraints to the controller. The desired reaction force is the artificial reaction-force constraint. The artificial position constraints can be extracted from the nominal position trajectory which is an approximation to the ideal nominal position trajectory. And the rest of the nominal position trajectory can be considered as an approximation of the natural position constraints and can be mapped to the desired artificial reaction-force constraints by a proper mapping. Since the same contact can generate different contact forces, the relation from natural position constraints to artificial reaction-force constraints is of one-to-many. There is a class of sets of artificial reaction-force constraints which can serve as an indirect input to replace the natural position constraints. This multiplicity makes it 10 Position/Force Trajectory

RSD-TR-1-85 possible to substitute the artificial reaction-force constraints derived from the nominal position trajectory for those from the ideal nominal position trajectory. In most cases, force control is used as an indirect position control to make the actual position trajectory of the manipulator to approach the incomputable ideal nominal position trajectory. Let us now formulate the above concepts by two mappings. Let C-= a six-dimensional configuration space, based on a Cartesian frame, Bframe for short. P(t)= a point in C at time t. It is a six-tuple (Ps(t), p,(t ), pz(t ), q,(t ), qy(t ), qZ(t )) S = the set of all possible S(t) which is a freedom decomposition function, S(t)E S. T = the set of all possible T which is a task, T E T. C,p = a subspace of C, which consists of all those degrees in C which are natural position constrained with the rest of the degrees having zero elements, e.g. C, = {(p,, py,,0,q,qy,O) I p,,py,qz,qy ER o P. (t)-= a point in C,,, which represents the natural position constraints. Cap = (Cnp,,), the orthogonal complement of CP,. It consists of all those degrees in C which are artificial position constrained with the rest of the degrees having zero elements, e.g. Cap - {(0,0, pz, O, q) I pz,qz ER }. Pa (t) = a point in Cap,, which represents the artificial position constraints. Position/Force Trajectory 11

RSD-TR-1-85 C= - a six-dimensional force configuration space associated with C. F(t)= a point in Cf at time t. It is a six-tuple (f (t), f,(t) f z(t), r(t), r,(t), r(t)). C, -= a subspace of C/ consisting of all those degrees which are artificial reaction-force constrained with the rest of the degrees having zero elements, e.g. C, -={(O,9, f,,0,0,r2) I fz, r ER }. The problem of automatic synthesis of position/force trajectory can be expressed as a mapping T-PF: T-PF: T — Cap X C It consists of two submappings. The first is called T-C submapping which derives a nominal position trajectory P(t) (not the ideal nominal trajectory, which can not be derived exactly) and a freedom decomposition function S(t) from the attributes of a given task. The second is the C-PF submapping which derives the artificial position constraints P,(t) and the artificial reaction-force constraints F,(t) from P(t) and S(t). That is: T-PF = C-PF o T-C T-C: T C-, X S C-PF: C XS - Gp X Ca/ where o denotes the left composition of functions. Notice that P(t) can be uniquely represented as a sum of two points P, (t )EC,,p and P (t)E Cp because (C, ) = Cap in the space C. Obviously, P, (t ) is the natural position 12 Position/Force Trajectory

RSD-TR-1-85 (1) The combinational operators used in this GKB are +' and -' which are the regularized set union and difference, respectively, of the sets of points in Cartesian space, that is, A +' B = closure(interior(A U B)) A -' B = closure (interior (A - B)) where A, B are sets of connected points in a Cartesian space, closure(A) is the closure of the set A and interior(A ) is the interior of the set A. (2) Every solid feature of an object is prefixed by a +', while every hollow feature is prefixed by a-~ (3) All the primitive solids are convex. Therefore an instance of a primitive solid or a set of them connected by +"'s represents a feature itself if it is a solid one, or the solid one which has the same boundary as the feature if it is a hollow one (see Figure 1). When a task command, which is represented by the five attributes, enters the geometrical knowledge base, it will provide the following data for the T-PF mapping: (1) operand = a subset of features in the object with one of the features being the feature-operand and with the rest being those directly connected with the feature-operand. (2) B-frame = the frame which is the base frame of the configuration space C in the given task. Generally it is the body attached frame of the feature-. Position/Force Trajectory 15

RSD-TR-1-85 operand which is fixed during the execution of the task and is represented by a 4 X4 HTM H~, relating the B-frame to the global reference frame. (3) P(O) = the initial state of the body attached frame of the operand to be moved with respect to the B-frame. (4) P(T) = the goal state of the body attached frame of -the operand to be moved with respect to the B-frame. (5) Ho,,(0) and H,,,2(0) = the HTMs which represent the body attached frames of the operands. All the objects present in the assembly workstation are constructed as a graph. Each object or its feature is linked to another through the lower pair relation or some other relations, if they exist. The GKB will be modified immediately after the completion of a subtask. This involves the modification of attachment relations and the various homogeneous transformation matrices of the objects or features involved. 4. Algorithms of the T-C and C-PF Mappings This section describes algorithms for the T-C and C-PF mappings based on the concepts developed in the previous sections. However only cylindrical and prismatic insertions are dealt with here because they form a majority of assembly tasks. Figure 2 gives an example of a peg-hole insertion task. 16 Position/Force Trajectory

RSD-TR-1-85 constraints given by the task geometry and P,(t) is the corresponding artificial position constraints. Furthermore, every point P,(t) in C,p has an associated subset F,(t ) Ca Each element F,(t) E F, (t) specifies a set of reaction forces (torques) along the degrees which are natural position constrained in the configuration space. 3. Geometrical Knowledge Base (GKB) In this paper, a given task is represented by the following attributes: 1. Feature-Operand = key names of the features relevant in the task. 2. Grasp position = a homogeneous transformation matrix Hh. relating the hand frame to the body attached frame of the grasped operand. 3. Goal state = P(T), a six-dimensional vector in the configuration space. Some of its elements can be "don't care." In this way, the goal state actually expands to a subset in the configuration space C and no longer a single point. 4. Attachment type = relation between two feature-operands, it may be one of the lower pairs: cylindrical, prismatic, planar, screw, spherical, and revolute. 5. Specification = a miscellaneous part, which may specify the speed of the hand or magnitude of contact force or torque, etc. Other relevant information about the task and its environment are supplied by the GKB. Position/Force Trajectory 13

RSD-TR-1-85 The "geometrical knowledge" are represented by the constructive solid geometry (CSG) scheme associated with primitive instancing. In this representation scheme, every object is represented by a tree. The root is the key name of the object (parts) and a pointer linking to a homogeneous transformation matrix (HTM) relating its local coordinate frame to the global reference coordinate frame. The second level non-terminal nodes are the key names of the feature classification (e.g. holes, cubes, etc). The third level non-terminal nodes are the key names of the features of the object (e.g. hole A, slot B, etc) and a pointer linking to a HTM relating the body attached frame of the feature to the object's local coordinate frame. The forth level non-terminal nodes, if exist, are the combinational operatorst. The terminal nodes are the instances of the primitive solids. They represent the features of the object by themselves or through combinational operators. A particular instance of the primitive solid is indicated by the key name of the solid and a tuple of specified parameters. The location of the body attached frame will be defined according to these rules: (1) The frame is always defined on the central line of the feature with one of the principal axes parallel to the central line. (2) For all concave features, there is a "mouth." The Z-axis of the frame should pass through the mouth. The combination of the instances of primitive solids follows the following rules: t Combinational operators are the regularized Boolean operators operating on primitive solids [1ol. 14 Position/Force Trajectory

RSD-TR-1-85 4.1. T-C Mapping The objective of the T-C mapping is to derive nominal position trajectory P(t) and freedom-decomposition function S(t) from the attributes of a given task. In an insertion task, there are two feature-operands, one is solid and the other is hollow. In the following discussion, we name the operand which contains the solid feature-operand as operandi and the other which contains the hollow one as operand2. Furthermore, we assume that operandl is grasped by the manipulator and is to be moved in completing the task, while operand2 is fixed in the workspace. The body attached frame of the hollow feature operand is selected as the B-frame. If these assumptions can not hold true for some insertion tasks, then appropriate transformations can be found to transform these tasks into their equivalent counterparts which satisfy the assumptions. 4.1.1. Deriving nominal position trajectory P(t) The 3-D nominal position trajectory P(t) is obtained by three coherent but separate steps. First, the three 2-D cut-diagrams of operand2, G., Gy, and Gy,, on the X-Z, Y-Z, and X-Y planes, respectively, of its body attached frame H,, 2 are computed. Second, operand1 shrinks to a line L ( with one of the, principal axes of its body attached frame parallel to the Zaxis of Hol,2 at the goal state), while G,, GY., and G,, expand correspondingly to GE, GE and GE, respectively. Third, a 3-D nominal position trajectory is planned on each U-V plane (uv = zz, y, zy) with the criterion Position/Force Trajectory 17

RSD-TR-1-85 that L,, representing operandl will not intersect with GE., representing operand2. Step 1: Find the cut-diagrams of the concave operand. Due to the representation of objects in the GKB by the CSG scheme, the task of computing the cut-diagrams of operand2 in its X-Z, Y-Z, and X-Y planes can be decomposed into computing the cut-diagrams of its component primitive solids by divide-and-conquer approach. The final result.is obtained by recombining these sub-cut-diagrams using the same CSG regularized operators. The divide-and-conquer approach greatly reduces the computational complexities of computing the the three cut-diagrams of operand2 and can be described by the procedure CUT_DIA. procedure CUT_DIA (operand2) If (operand2 = single instance of a primitive solid) then G2,9 Gy, Gzy cut diagrams of operand2 on XZ, YZ, XY planes, respectively. else divide operand2 into sl op s2, where 81 and s2 are subsolids with almost equal number of instances of primitive solids. G, G,G,, Gs,- CUT_DIA ( 81sl ) op CUT_DIA ( 82) end_if return (G,,, Gy, G, ) endprocedure where op E {+*,-* } The correctness of the divide-and-conquer procedure can be proved by induc18 Position/Force Trajectory

RSD-TR-1-85 tion (see Appendix A). The resultant cut-diagrams are denoted as: GUV -- the cut-diagrams of the operand2 on the U-V plane - cu B1 op 1 Cu B 2 OP2 2 o Pn-1 Cu Bc, where Bm = instance of primitive solids, m = 1,2,. n, opi E { +*,-* } and c, B, = the cut-diagrams of the instance Bm on the U- V plane, represented by a set of parametric function pairs: (u1 f 110(t1), v1 = f 12(t1)) (U2 =f 21(t2), V2 = f 22(t2)) (uK = Kl1(tK), VK = K2(tK)) where uv = zz,y, zy. The f, i = 1,2, o ~,K;j = 1,2 are so selected that when one point travels along the curve with increasing t, the interior of Bm lies on the left of the point. Step 2: Shrink-expand on operandl and the cut-diagrams. In this step, the operandl is shrunked into a line which is coincident with one of the principal axes of its body attached frame H0l,(T) and parallel to the Z-axis of Ho, 2. The line L is then projected onto the X-Z, Y-Z and X-Y planes of Ho,,2 as L,,,L,, and L,, respectively. Then, the cut-diagrams of operand2, G,z G, and G,, expand correspondingly to GEz, GEy, and GEy, respectively. Like operand2, operandl may consist of several instances of primitive solids combined by regularized set operators. Hence it can be represented as operandl — Al op1 A 2 ~P2 * OPm_ 1 Ao Position/Force Trajectory 19

RSD-TR-1-85 and the shrinking of operandl can be decomposed into the shrinking of Ai's too. Consequently, the expansion of operand2 can be decomposed into m sub-expansion of G:, Gy, and G,, corresponding to the shrinking of Ai's, i =11,2,..,m so that the computational complexities can be greatly reduced. The final expanded cut-diagrams corresponding to operandi, denoted as GE,9 GEyz, and GE,, are defined in the following assertion: Assertion 1. Assume operand 1 Al opl A2 oP2 ' oPm-I Am Then GE,, = U GEi.; uv zz,yz,zy where GEi.. is the expanded G., corresponding to A,,u = zz,yz,zy. The proof of Assertion 1 can be found in Appendix B. Based on above discussion, we define a divide and conquer procedure SHR EXP to implement the shrink-expand task with details omitted. procedure SHR_EXP (G,,, operandl ) If (operandl - instance of a primitive solid) then GE,, - expansion of G,, according to the shrinking of operandi; else divide operandl into C1 op C2, where C1 and C2 are subsolids with almost equal number of instances primitive solids. GEu, -SHR_EXP ( GU, C ) U SHREXP ( Gu,, C 2) endif 20 Position/Force Trajectory

RSD-TR-1-85 return GE,. endprocedure Step 3: Find nominal position trajectory. In this step the nominal position trajectory P(t) = (p(t),py,(t), p(t), q,(t),qy(t), q(t)) defined with respect to the frame Hom2 is to be obtained from the cut_diagrams GE,,, GEY, and GE,,. These cut-diagrams provide the information of natural position constraints. Deriving P(t) from these cut-diagrams consists of four phases: (1) Define a set of position nodes, { N1, N2, * ~, NT }, on a 3-D Euclidean space as well as their projections on the X-Z, Y-Z, and X-Y planes, {Nlu,, N2,, *-, NTu, }, uv = ZY,Yz,zz such that for all Ni,' Ni,, 4 GE,,, and Ni is visiblet to both Ni_. and Ni+,, i = 2,..., T-1, and N, = origin(H,,(O)), NT = origin(H,, ( T )); where origin(H) = the origin of frame H in the B-frame. (2) Define a set of straight lines, {(NI, N2), (N2, N3),..., (NT-, NT)}, which forms the nominal position trajectory of the nominal center of operandi. This trajectory is the position part (p, (t), py(t), p, (t )) of the P(t). (3) Define a set of orientation nodes corresponding to the position nodes, {M,, M2,..., MT)}, where Mi = (q,(t), qy(t), qz(t)). These orientation nodes are defined in such a way that with the nominal center at (pS(t),py(t), p2(t)), the line Lx,, L, and L., will not intersect with the t N, is said to be Risible to Nj.1 if there exists a straight line from N,.l to N, which will not overlap with GEt, E Position/Force Trajectory 21

RSD-TR-1-85 GE,,, GE,, and GE,,, respectively, and with M1 = (q, (0), qy(0), qz(0)), the initial orientation of operand1 with respect to the B-frame (Ho 2) and MT = (q (T),qy(T),q,(T)), the orientation of the goal state with respect to the frame Hof2. (4) Define a set of straight lines. in angular motion, {(M1, M2), (M2, M3), ',(MT-1, MT)}, which forms the orientation part of the nominal position trajectory P(t), (qz (t), qy(t), q3(t)). In this paper we do not consider the collision between operand1 and other obstacles besides operand2. Under this assumption, we select only three nodes for each insertion task: N1 = (p,(0), py(O), p,(0)), initial position of the nominal center with respect to the B-frame; N2 = (p,(tl), py(t1) p, p(tl)), a point on the Z-axis of the frame H0o92 which is visible to N1; and NT = (P,(T),py (T),p,(T)), the goal state of the nominal center with respect to the frame H.,,2. It is desired to have (q,(t),qy(t),q3(t)) reach its goal state (g(T),qy(T),q,(T)) before the nominal center reaches N2. The resultant nominal position trajectory can be expressed as a piecewise linear vector function, P(t) = (p(t), ppy(t), p,(t), q(t), qy(t), q5(t)), where pi,(0) + t (p,(t1) - p,(O)); N1 N2 Pi (t - (t +s-to (pi (T) - pi (tT )) 22 Psr +T-t — 22 Position]Force Trajectory~

RSD-TR-1-85 qi (0) + t (qj (T) - qi (o)) M M qi(t) |qi(T); M2 - Mr where i = z, y, z. 4.1.2. Algorithm for Freedom Decomposition Function S(t) Strictly, if the line L,, which represents operandl on the U-V plane of the frame Ho,2 is on the boundary of GE., (i.e. touch with one point or whole line, but not intersect with ), then operandl is under natural position constraints at some degree of the configuration space. Even in some region where L,, does not contact with GE,,, the natural position constraints may still exist. This is due to the uncertainties which will bring operandl and operand2. in contact if the free space between operandi and operand2 is not big enough to tolerate the uncertainties. These uncertainties will cause operandi to deviate from P(t). On the U-V plane, the result is that the line UV will deviate from its nominal position and orientation. In order to handle these uncertainties, we use a ball B.,, to represent the uncertainty of the position of the nominal center and a cone C,,. to represent the uncertainty of the orientation of LU on the U-V plane. They are combined into an uncertainty trapezoid, T-7,. Let 4 = (, v0) be the nominal position of the nominal center and 4, = (u, v ) be the position of the nominal center. Then the uncertainty ball of the nominal centeris defined to be Position/Force Trajectory 23

RSD-TR-1-85 B., [10o] ={ e U- V plane: d oo(, 4,0) r} where r is the radius of the ball and doo(4, 40) is a metric function defined on the U-V plane, doo(, o)- ma{ I-ao I, I-v-vo I } Let 'o be the nominal angular displacement of L.,, on the U-V plane and 4 be the angular displacement of L.,. Then the uncertainty cone of the orientation of L,, is defined to be c,, [o] = -{ 4 E [0, 2r): dl(', 'o) < } where s is a small positive angle and dl(*p, ho)= I 4'-4o I The uncertainty trapezoid TIO v[re, PoJ1 is the combination of Bu [4,0 and C,o [ol] and is given as T, [4o, olJ = { (L, (), 4'): 4 E Bv,, [%4o1 and 4 E Cu,, [ol}) where L4 (4Q, 4) represents the line Lu. with its nominal center at 4) and angular displacement of 4. Figure 3 gives illustrations of B,,, [4o], Cu,, [lo], and Tuvr. [4o0 0o] Obviously when L, (40, Ado) moves along the nominal position trajectory, some L,,(4), 4) E T,,o [4o, 0o] might contact, or even intersect, with GE,. Therefore operandi is natural position constrained at some degree of configuration space. The freedom decomposition function S(t) reflects such occurrence of natural position constraints and is defined as 24 Position/Force Trajectory

RSD-TR-1-85 S(t) = (Al(t), 2(t), s3(t ), a4(t ), a 5(t), 86(t)), where -1, case 1 O, case 2 1, case 3 2, case 4 where: case 1 = natural position constraint occurs on the direction against the i'k degree of the configuration space, case 2 = -no natural position constraint occurs on the ilt degree of the configuration space, case 3 = natural position constraint occurs on the direction along the ith degree of the configuration space, case 4 = natural position constraint occurs at both directions along and against the ith degree of the configuration space. The freedom decomposition function S(t) is determined by checking the intersections between the both edges of T,,, and GE, with uv = zz, lz, and xy planes. Then ai(t)' a are assigned with proper values according to the intersection checking. The resultant S(t) is a piecewise constant function which reflects the different subtask periods of the given task, e.g. gross motion subtask, compliant motion subtask, etc. 4.2. C-PF Mapping The C-PF mapping is to derive the artificial position constraints and the artificial reaction-force constraints from the nominal position trajectory P(t) Position/Force Trajectory 25

RSD-TR-1-85 and freedom decomposition function S(t). These constraints serve as the input to the hybrid controller. In general, the configuration space is represented as C = ((p,, py, pz, q,, q, q,) I p,, Py, pz, qs, qg, qz E R ) and Cp, Cap, and C,u vary with the task. For instance, in the insertion task shown in Figure 2, the Cpe,, Ca,, and C./ subspaces are represented as: =1(C (00,, ); t l e [, t ((O Y- I0o,0,,0,0,)) t, [02;,_ ={(1,,,, or r,o) I l,,,, Tr, ER };t E [to, T] At any time t, a point P'(t) on the ideal nominal position trajectory is the sum of the two points P,'(t) E Cp and P(t) E Cap. Hence, the ideal nominal position trajectory P'(t) can be uniquely represented as the sum of the time sequences of artificial position constraints P'(t) and natural position constraints P,(t), P (t) = Pa(t) + P,(t). The first step of C-PF mapping is to derive the time sequence of constraints, P (t)E Cap, from the nominal position trajectory P(t). Since the artificial position constraints are artificially selected, it will not be affected by uncertainties. Therefore Pa(t) extracted from P(t) is equivalent to P(t), 26 PsPosition/Force Trajectory

RSD-TR-1-85 Pe(t) = (r( l(t), p.(t)),r(.2(t), p,(t)), r(3(t), pz(t)) r(84(t ),.qs(t)),r(s 5(t), qy(t)), r(6(t), qz(t))) where: p (t) t E [tl,t2) and (t) — r(a (t), p(t)) -= 0 otherwise and (t) is an element of S(t) and p (t) is the corresponding element of P(t). The next step of C-PF mapping is to derive the time sequence of artificial reaction-force constraints F*(t). Since the relation from the natural position constraints to artificial reaction-force constraints is of one-to-many, Fa (t) actually is only the function of freedom decomposition function S(t). Therefore Fa(t) can implicitly represent the natural position constraints part of the ideal nominal position trajectory and defined to be Fa(t) = ( (1(t)), n( 2(t )), Q(3(t )), f(4 (t )), 1n(,(t) ), fn(,(t)) ) where: ifi; t E ItD, t2) and asi(t) 2 -fi; t E Itl, t2) and si(t)= 1 I(lk (t))= +fi; t E [t1, t2) and si(t)=-1 0;t E [tl, t2) and si(t) 0 and fi is the desired contact force either given by the task attributes or an arbitrary small positive number. Position/Force Trajectory 27

RSD-TR-1-85 The final outcome of the T-PF mapping serves as the reference input to the hybrid position/force controller with the artificial position constraints servoed by the position feedback loops and artificial reaction-force constraints servoed by the force feedback loops. 5. Conclusions In this paper, the concepts of compliant motion is developed and discussed. Based on the concepts, Lozano-Perez's shrink-expand approach is combined with a 2-D cut-diagram approach to derive the nominal position trajectory from which the artificial position and artificial reaction-force constraints are derived to serve as the reference inputs to the hybrid position/force controller. The synthesis of position/force trajectories for compliant motion was realized by two mappings, T-C and C-PF, which are defined to derive the nominal position trajectory P(t) and freedom decomposition function S(t) from the attributes of a given task and then the time sequence of the artificial position constraints and artificial reaction-force constraints from the P(t) and S (t). 28 Position/Force Trajectory

RSD-TR-1-85 6. References 1. Lee, C. S. G., Lee, B. H., "Planning of Straight Line Manipulator Trajectory in Cartesian Space with Torque Constraint," Proc. 23rd IEEE Conf. on Decision and Control, Las Vegas, Nevada, Dec. 1984, pp. 1603-1609. 2. Lee, C. S. G., Smith, R., "Pattern Recognition Methods for Force Recognition in Insertion Processes," Proc. 1984 American Control Conf., San Diego, June, 1984, pp 39-44. 3. Lee, C. S. G., "Robot Arm Kinematics, Dynamics, and Control," IEEE Computer, vol. 15, no. 12, December 1982, pp 62-80. 4. C. S. Lin, P. R. Chang, and J. Y. S. Luh, "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots," IEEE Trans. Auto. Contr., Vol. AC-28, No. 12, December 1983, pp. 1066-1073. 5. Lozano-Perez, T., Mason, M., and Taylor, R., "Automatic Synthesis of Fine-Motion Strategies for Robots," The International J. of Robotics Res., vol. 3, no. 1, Feb. 1984. 6. Lozano-Perez, T., "Spatial Planning: A Configuration Space Approach," IEEE Trans. Computers, vol. 32, no.2, Feb. 1983, pp.108-120. 7. Mason, M. T., "Compliance and Force Control for Computer-Controlled Manipulators," IEEE Trans. Syst., Man, Cybern., vol. SMC-11, no. 6, 1981, pp. 418-432. Position/Force Trajectory 29

RSD-TR-1-85 8. Paul, R., Shimano, B., "Compliance and Control," Proc. Joint Auto. Contr. Conf., Purdue University, July 1976, pp 694-699. 9. Raibert, M. H., Craig, J. J., "Hybrid Position/Force Control of Manipulators," Trans. ASME, J. Dynamic Syst., Meas., Contr., vol. 102, June 1981, pp 126-133. 10. Requicha, A., "Representations for Rigid Solids: Theory, Methods, and Systems," Computer Surveys, vol. 12, no. 4, Deco 1980, pp 437-464. 11. Salisbury, J., "Active Stiffness Control of Manipulator in Cartesian Coordinates," Proc. 19th IEEE Conf. on Decision and Control, 1980, pp. 95-100. 12. Wesley, M. A., "Construction and Use of Geometric Models," in Computer Aided Design, Modeling, System Engineering, CAD-Systems Chapter 2, edited by J. Encarnacao, Spring Verlag, 1980, pp. 79-136. 30 Position/Force Trajectory

RSD-TR-1-85 Appendix A The geometrical knowledge about an operand, as described in Section 3, is represented as follows: -= any operand, F or Fi = any feature, A or Ai = any instance of a primitive solid, opi E { +,-}, where + ' and -* are regularized set union and difference, respectively. Then 0 = F or 0 = F1P1 F2op2 '' oPi- l, A-i,) 2A-1) F -A or F Al opl A2 oP2 o ' ' ~Pk-1 Ak 1 Since A is always convex, a hollow feature can be represented as -'(A +* A +*.o +' A) while a solid feature can be represented as +*(A +~ A +*.'' +* A). Noting the following identities, Al +' (A2+' A3)-A1 +* A2+~ A3 A1 - (A2+ A3) A I-A1- A2- A3 +A _ A Eq. A-i can be rewritten as Position/Force Trajectory 31

RSD-TR-1-85 O = A or O = Al op A2op2 ~Pn,- A,n (A-2) Assertion 2 For any arbitrary 3-D operand 0 represented as in Eq. A-2, the 2-D cutdigrams, GU, uv = zz, yz, and zy planes of H. 2, can be decomposed as G, = CU, A 1 op1 Ic. A 2 op2 ' ' ' oPn-I cu, An (A-3) where c,, Ai is the cut-diagram of an instance of the primitive solid A, at the U- V plane. proof: Assertion 2 can be proved by induction. 1. basic step: Assume 0 =A1+ A2 0, A, A2 _ U where U is a 3-D Cartesian space. v< E U: ~ E 0 + * ' EA1+ A2 c 4 6E Ci(Al U A2) where Ci(o) represents closure( interior(-)) <+; (, E A U A2) and ( 4 f((A, U A2) - Ci(A1 U A2))) ( EA, or * EA2) and ( ((A U A2)- Ci(A1U A2))) (A-4) 32 Position/Force Tra3ectory

RSD-TR-1-85 Now, if we restrict 4 E P to a 2-D plane in U, then, 4 E P and 'E O -> ' E cpO q EP and E AA1 -- > E cpA1 E P and, E A2 — > E cpA2 where cpO, cpA 1, and cpA2 are the cut-diagrams of 0, A, and A2 on the plane P, respectively. Then A-4 becomes V4 E P: 4 E cpO 4 ( 4 E cpA 1 or c! E cpA 2 ) and (z f ((cpA 1 U cpA2)- Cj (cpA i U cpA2))). 4-E cpA1 + cpA2 > cpO ---- cpA + cpA2 Similarly, we can prove 0 = Al-* A2 - --- cpO -cpA -cp cpA2 Therefore O = A1 opl A2 — > cpO - CpA1 op1 CpA2 where op E {+', - } 2). Assumption step: Assume Position/Force Trajectory 33

RSD-TR-1-85 O - A 1 op1 A 2 OP2 ''' oP-2 An-;:> cpO =cpA 1 op1 cpA 2 op 2 op,,-2 A-i1 holds true. 3). Induction step: 0 - A opl A 2 oP2.. OP,-2 A,-1 ~P.-I An = (A 1 op, A 2 ~P2 ' O ~Pn-2 A.-1) opn-i An -B oP,-1 A. ->- cpO = cpB op,-1 cpA, > cpO = cp(A 1 op A2 op2 O ~PN-2 A,-1) op._- cpA. > cpO = cpA 1 op cpA2 OP2 ' P,,o2 cpA.-1 op,, - cpA, (A-5) Since P is any arbitrary 2-D plane in the 3-D Cartesian space, the induction A-5 holds true on the X-Z, Y-Z, and X-Y planes of the body attached frame Ho.2. Q.E.D. Consequently, the correctness of the divide-and-conquer procedure CUTIDIA described in Section 4 is the direct result of the Assertion 2. 34 Position/Force Trajectory

RSD-TR-1-85 Appendix B Assertion 1 in Section 4 is restated here for convenience. Assertion 1 Assume operand - = A oPl A2 op2 -i oPm- A, (B-I) Then GE,, - U GEi,,; uv = zy,yz,zz i-=1 where GEi,, is the expanded G,, corresponding to Ai, uv = zz,yz and zy planes of Ho, 2, and opi - + t Proof: 1. First we prove that 4 E GE,, — > E EU GEj.,,, where 4 - (u,v) is a point in the U-V plane. '4 E GE,, means that if operandl is put at a position such that L,, passes through <4, then operandI will collide with operand2 on the U-V plane. The collision point at least belongs to one of the Ai' a. Therefore 4 E GEijU j E {1, 2,..,m)} so that m E GE. -> E U GE... (B-2) i —1 in 2. Next we need to prove that 4 E U GEiuv — > 4 E GE,, i =1 t The object which operand1 belongs to might have some hollow features. However, in an insertion task, the feature-operand of operandj and its direct neighbors are solid ones. Therefore Position/Force Trajectory 35

RSD-TR-1-85 m 4, E U GE..v means that (4 belongs to at least one GE,, We denote it as i=l GEj,,, corresponding to Aj. Therefore, if L,U passes through -- = (u, v ), then Ai will collide with operand2 on the U-V plane and so will operandi, since A, is a solid part of operandl. Therefore 4 = E U GEi > 1 E GE,, (B-3) i - I From B-2 and B-3, we conclude that GEuo - U GEit, s u z zz yz 2XY =i U~~suv = vzz,yz,zyo Q.E.D. the combinational operators in Eq. B-i are all +* '. 36 Position/Force Trajectory

RSD-TR-1-85 -3. Xi ' Yl Feature 1 Feature 2 ObJect e Fe"ture 2 -' Feature I Figure 1. An object which is the Combination of Solid and Hollow Features Position/Force Trajectory 37

RSD-TR-1-85 IrTpper H1, operadl \~T; body ttacbIae gIb~l fresee~ r e Hl( I) \ || 1pe |oprrad2 o te boy tt Figure 2. An Example of Peg-hole Insertion 38 Posbaretion/Forerene Trajectory

RSD-TR-1-85 Lee Fiure l(-). Te ball.ec,IJ lb.. Free 3(b)- The "oneC,1 Figure (c). he trped 7"" As *j Figure 3. (a) The ball B,v, leo]j (b) The cone CF,, [rce. (c) The uncertainty trapezoid T [0, [toPosition/Force Trajectory 39

UNIVERSITY OF MICHIGAN 31I 9011 IIlI11 1111 0311111111111111111151111 3 9015 03023 0315