UFDC Home  Search all Groups  UF Institutional Repository  UF Institutional Repository  UF Theses & Dissertations  Vendor Digitized Files   Help 
Material Information
Subjects
Notes
Record Information

Full Text 
KINESTATIC CONTROL: A NOVEL THEORY FOR SIMULTANEOUSLY REGULATING FORCE AND DISPLACEMENT BY MICHAEL WILLIAM GRIFFIS A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 1991 ACKNOWLEDGEMENT The author wishes to acknowledge the support of the National Science Foundation, Grant No. MSM 8810017, for research on the subject of kinestatic control. TABLE OF CONTENTS page ACKNOW LEDGEM ENT.................................................................................. ii A B ST R A C T ...................................................................................................... iv CHAPTERS 1.0 INTRODUCTION TO THE THEORY OF KINESTATIC CONTROL........................................................... 1 1.1 The General Mapping of Stiffness ................................................ .. 6 1.2 A PositiveDefinite Inner Product for Twists ................................. 9 1.3 The EigenScrews of Stiffness.................................. ............ 13 1.4 The General Model of Spatial Stiffness.......................................... 17 1.5 Parallel and Serial Arrangements................................................... 20 1.6 Robotic Applications........................................ .................... 22 2.0 IMPLEMENTING KINESTATIC CONTROL: USING DISPLACEMENTS TO NULL FORCES.............................. 31 2.1 Introduction................................................. .......................... 31 2.2 Twist Space Decomposition ..........................................................47 2.3 Implementing Kinestatic Control................................................... 62 3.0 GLOBAL STIFFNESS MODELING OF A CLASS OF SIMPLE COMPLIANT COUPLINGS.................................... 80 3.1 TwoDimensional Spring............................................................. 82 3.2 Planar ThreeDimensional Spring...................................... ............ 88 3.3 Spatial SixDimensional Spring .................................................... 102 4.0 THE NEW ROLE OF COMPLIANCE IN THE CONTROL OF FORCE.................................................... 116 4.1 The Need for Better Stiffness Models........................................ 121 4.2 The Need for Constraint Recognition........................................ 124 4.3 The Need to Filter Working Wrenches from Sensed Wrenches..... 125 R EFER EN CES .................................................... ....................................... 126 BIOGRAPHICAL SKETCH .............................................................................. 129 Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy KINESTATIC CONTROL: A NOVEL THEORY FOR SIMULTANEOUSLY REGULATING FORCE AND DISPLACEMENT By Michael William Griffis May 1991 Chairman: Dr. Joseph Duffy Major Department: Mechanical Engineering A new theory for the simultaneous control of force and displacement for a partially constrained endeffector is established based upon the general spatial stiffness of the robotic manipulator. In general, the spatial stiffness of a compliant coupling that connects a pair of rigid bodies is used to map a small twist between the bodies into the corresponding interactive wrench. This mapping is based upon a firm geometrical foundation and establishes a positivedefinite inner product (elliptic metric) that decomposes a general twist into a twist of freedom and a twist of compliance. While controlling the general twist of the endeffector of an experimental robot apparatus (which included compliance in the endeffector and a force/torque sensor), the constraint wrenches and the twist freedoms of the partially constrained gripper were simultaneously controlled, and this was accomplished by using the inner product established for twists. This implementation as well as new analytical results indicate that the stiffness mapping and the induced inner product will be asymmetric. CHAPTER 1 INTRODUCTION TO THE THEORY OF KINESTATIC CONTROL A knowledge of the mapping of spatial stiffness is an essential ingredient in establishing the control of both the force and the displacement of a partially constrained rigid body. Spatial stiffness transforms a relative twist (a small rotation on a screw) between two rigid bodies into the change in a wrench that interacts between the bodies through a compliant coupling. In the proposed robotic application, a soft spatial spring is introduced to connect a gripper/workpiece with the end link of its manipulator. This inherently allows the end link to possess six degreesoffreedom, no matter how the gripper/workpiece is constrained. "Kinestatic Control" exploits this knowledge of the spatial stiffness between the end link and the gripper/workpiece in a taskindependent effort of simultaneously controlling the force and the displacement of the gripper/workpiece while solely controlling the displacement of the end link. The spatial stiffness between two rigid bodies connected together by a compliant coupling, which is essentially a sixdimensional spring, is modeled by the spatial stiffness of a passive parallel mechanism. The simplest model for a "spatial spring" is a parallel mechanism that consists of six linear springs which act in parallel connecting two platforms together. One platform is considered to be embedded in each of the rigid bodies. (See Figure 1.1.) The model thereby obtained is the most general in the sense that it can be used to quantify the spatial stiffness between any two rigid bodies, regardless of how complex is the coupling 10 (0/ C , .0 a. C,) I U) ai C, Mo between the two bodies. It supersedes a previous and widely accepted model that is dependent on the existence of a centerofcompliance. (See Salisbury [1980], Mason [1981], Raibert and Craig [1981], Whitney [1982], and Mason [1982].) This is illustrated by Figure 1.2 where three inparallel TRPT serial chains cointersect at the centerofcompliance. The proposed parallel mechanism model consists of six inparallel SPS serial chains. (See Figure 1.1.) A more general parallel mechanism of this form would consist of six distinct connecting points on both platforms. This level of complexity is, however, not required to quantify a symmetric spatial stiffness between two rigid bodies, and the Stewart Platform of simple octahedral geometry can be used. The two platforms can thus be connected together by six legs that meet in a pairwise fashion, sharing connections at three points on each platform. In practice, the six leg springs of the Stewart Platform can be calibrated so that, over a wide range of motion, the spatial stiffness of the resultant mechanism quantifies the spatial stiffness of the actual coupling (the spatial spring) between the two given rigid bodies. The new theory of Kinestatic Control adopts this proposed stiffness modeling technique and applies it to the taskindependent, simultaneous regulation of the force and the displacement of the gripper/workpiece of a robotic manipulator. The author considers that this new control theory supersedes such theories as "Stiffness Control" and "Hybrid Control," which essentially rely on erroneous taskdependent estimations of robot stiffness that require the existence of a centerofcompliance. These theories inherently depend on the specialized model of stiffness illustrated in Figure 1.2. J a .: a. I .x cr a, 4 0 a) 0 h..  The author believes that an investigation of sparsity in the orthogonal complements associated with Hybrid Control reveals an uncertainty as to the modeled values of the three rotational and three translational spring constants of the mechanism shown in Figure 1.2. Hybrid Control theory thus restricts the centerofcompliance to be a task dependent point, and it does not in any way incorporate the stiffness of the manipulator into the hybrid controller. Special situations where a robot apparently has a centerofcompliance that is concident with a task dependent specification have been reported. (For example, see Raibert and Craig [1981] or An and Hollerbach [1989], where a point on the prismatic joint of a polar (RP) manipulator is aligned with a task, or see Whitney [1982], which discusses the positioning of the centerofcompliance of a RemoteCenterof Compliance (RCC) device along the axis of a peg.) Employing the theory of Stiffness Control allows a programmer to choose the location of a centerofcompliance and to adjust the three rotational and three translational spring constants according to a particular task. This mapping of stiffness is then reflected onto the individual joints of the robot. The technique thus essentially represents an adjustable set of gains, which must be synthesized for each given task as well as for each given robot. Here, a novel theory of Kinestatic Control is proposed, which essentially accepts the stiffness of a robot for what it is and uses it to simultaneously control force and displacement. As a foundation for the new theory, this chapter investigates two representations of the mapping of stiffness, describes the general model of stiffness, and finally, discusses three robotic applications. Throughout this chapter, only positivedefinite symmetric mappings of stiffness are considered. However, the remaining chapters employ more general stiffness mappings. 1.1 The General Mapping of Stiffness Consider a rigid body 1 to be connected to a rigid body 0 by a general but initially unloaded compliant coupling that via its spatial stiffness restricts any relative spatial motion between the bodies. Then, the resulting mapping of stiffness is a onetoone correspondence that associates a twist describing the relative displacement between the bodies with the corresponding resultant wrench which interacts between them. Figure 1.3 illustrates this compliantcoupling dependent relationship by suggesting that for a given resultant wrench applied to body 1 of a "small" intensity 6f on the screw $1, the corresponding twist of body 1 relative to body 0 is of a "small" angle 60 on the screw $2.1 The term "small" is introduced to convey that the intensity 6f of a wrench on the screw $1 is limited such that it produces a twist of intensity 60 on a screw $2 via a linear mapping of stiffness. Consider a spatial spring system consisting of the two bodies and the initially unloaded compliant coupling. Any work input to the system is positive, and from Figure 1.3, 6f 68 ((hl + h2) cos a12 a12 sin a12) > 0, (1.1) where h1 and h2 are the pitches of $1 and $2, and where a12 and a12 are the perpendicular distance and swept angle between the lines of the two screws. The expression in (1.1) is the summation of three products: the work done by the force on the translation (6f h2 60 cos a12), by the force on the rotation (a12 6f 60 sin a12), and by the couple on the rotation (h1 6f 60 cos a12). 'The wrench is illustrated in Figure 1.3 employing a Hunt "wrench applicator", which is a TPHT serial chain. A force 6f generated in the P joint applies force 6f to body 1 and simultaneously generates a couple h1 6f in the H joint, which in turn applies an equal and opposite couple h1 6f to body 1. In other words, a wrench of intensity 6f on screw $1 is applied to body 1. h, 8f a 12 (helical) Figure 1.3 A pair of screws are said to be reciprocal when the expression ((hl+h2)cos a12 a12 sinal2) vanishes. Accordingly, since the work input into the spatial spring system must be positive nonzero, (1.1), then the twist of the coupling cannot be reciprocal to the wrench that produces it. The transformation of screws illustrated in Figure 1.3 is represented here analytically in two geometrically different ways. Each of the two analytical representations requires the use of a 6x6 stiffness matrix to perform the mapping of stiffness. In comparison, these two stiffness matrices [K] and [k] differ in geometrical foundation: [K] is used to establish a potential energybased inner product for twists, while [k] defines the "eigenscrews" and "eigenstiffnesses" for the transformation, which are, in other words, the invariant properties of the mapping. A wrench on a screw will be represented as a general "ray" that is assigned an R6 vector *=[f; fio], where f is a force vector in the direction of the wrench and rio is a moment vector referenced to the origin. This ordering of screw coordinates is based on Pliicker's definition of a ray, which is a line formed from the join of two points. (See Plicker [1865] and [1866].) However, a twist on a screw will be represented using two different formats, one based on the ray, d=[O; io], and the other based on a general "axis" that is assigned an R6 vector, Dz=[Xo; 0]. Pliicker defined the axis to be dual to the ray, and the axis is thus a line dually formed from the meet of two planes. For a twist, 0 and 0 both represent the direction of a finite but "small" rotation, and Xi and Xo both represent a "small" displacement of the origin. Therefore, the axis and ray representations of the same twist are related by Xo 03 13 Xo xo 13 03 which can be more conveniently written as S= [A] b, (1.2) where [A] is the symmetric 6x6 matrix of above. Since [A]=[A], a reversal in the ordering of this same transformation yields D = [A] d. (1.3) The transformation described by (1.2) and (1.3) is an example of a more general transformation, defined as a correlation, that maps an axis to a ray (or a ray to an axis). It is important to distinguish this transformation from a collineation, which maps a ray to a ray (or an axis to an axis). 1.2 A PositiveDefinite Inner Product for Twists The first analytical representation [K] of the mapping of stiffness is a general correlation of screws that assumes the form, = [K] D, (1.4) where axial coordinates D are assigned to the twist, where ray coordinates are assigned to the wrench, and where [K] is a symmetric (nonsingular) 6x6 matrix. For instance, in Figure 1.3, the screw $2 with axial coordinates D)=[Xo; 6] is transformed into the screw $1 with ray coordinates W=[f; riio]. The potential energy stored in (or work input into) the spatial spring system can be expressed in terms of the axis coordinates of the twist and the ray coordinates of the wrench, and the same positivedefinite expression first given in (1.1) can alternatively be written in the form X0o o f 1 = ,' > 0. (1.5) Substituting from (1.4) yields that a given nonsingular matrix [K] is not only symmetric but also positivedefinite: DT'1 = T' [K] b > 0. (1.6) The expression in (1.6) introduces a quadratic form that can be used to establish a potentialenergybased, positivedefinite inner product for the axial coordinates of twists. The symmetric bilinear form for the axial coordinates D1 and D2 of a pair of twists is therefore, (11, D2) = T [K] )2, (1.7) the dimension of which is work. Accordingly, this inner product satisfies three necessary conditions that establish the R6 used for the axial coordinates of twists as a nondegenerate inner product space: (1^, 32)= (2, D1) (symmetry) (D1, A )2+ A D3) = A ( 1 2) + p (11, D3) (linear multiples) (1^, D) D 0, and (D, D) = 0 (positivedefiniteness) if and only if 1D = 0. Since the inner product given in (1.7) is positivedefinite, it cannot establish an invariant Euclidean metric for screws.2 (See Loncaric [1985], Brockett and Loncaric [1986], and Lipkin and Duffy [1985].) It would in fact induce an elliptic metric because it possesses a positivedefinite quadratic form that operates on homogeneous coordinates, i. e. only imaginary D would satisfy DT [K] D= 0. (Coordinates such as D=[0,0,0,0,0,0]T are not considered.) (See Lipkin and Duffy [1985] and [1988].) Because the mapping of stiffness (and its inner product) does not induce a Euclidean metric, it must undergo a change in representation whenever a change of 2For a Euclidean metric to be induced based upon an inner product represented by [K], then [K] must satisfy the relation [K]=[E]T[K] [E], where [E] describes a Euclidean transformation (collineation). (See Duffy [1990].) It will be shown however that the inner product described by [K] does not satisfy this relation. (See Equation (1.12).) coordinate system is encountered. For a general Euclidean change of coordinate system, the ray coordinates *=[f; ifo] of a wrench are transformed by the relation w=[e] w', (1.8) where *'=[f'; rii] are ray coordinates of the wrench in the "new" system, and where the 6x6 [e] takes the form, [e] R3 03 A3 R3 R3 Equation (1.8) represents a Euclidean collineation of screws, and it is specified by R3 and A3, which are, respectively, 3x3 rotation and 3x3 skewsymmetric translation matrices. Under the same change of coordinate system, i.e. the same Euclidean collineation of screws, the axial coordinates D)=[Xo; 0] of a twist are transformed by a similar relation, )D=[E] D', (1.9) where D'=[XS; 0'] are axial coordinates of the twist expressed in the new coordinate system, and where [E] takes the form, [E]= R3 A3R3 S03 R3 Substituting (1.8) and (1.9) into (1.4) and subsequently inverting [e] yields a new description of the same spatial spring system, viz. w=[e]1[K[E]D', and because [E]T=[e]1, this can further be expressed in the form = [E]T [K] [E] f)'. (1.10) Therefore, the representation of the mapping of stiffness in the new coordinate system is = [K'] D', (1.11) where from (1.10), [K']=[E]T[K][E]. (1.12) Equation (1.12) illustrates three important properties of the representation, [K], of the mapping of stiffness that describes a given spatial spring system: It is symmetric, regardless of coordinate system. It is likewise positivedefinite. Equation (1.12) is of the utmost importance because it illustrates that the eigenvalues (and eigenvectors) of a correlation have no invariant geometric meaning for Euclidean collineations, i. e. the equation does not relate similar matrices. Equation (1.12) does, however, relate congruent matrices that in general must have the same invariant signature (r, () which is defined as the number of positive (7r), negative (v), and zero (() eigenvalues. For (1.12), the congruent matrices [K'] and [K] both have six positive eigenvalues (7r=6) that are not equal (unless [E]T=[E]1). In general, it is not necessary that [K] be diagonalizable for some [E]. This latter property is also of the utmost importance since it reveals that in general, one must not use a model of stiffness based upon a centerof compliance. (See Figure 1.2.) In fact, it will be shown later that a proper model for general spatial stiffness is the Stewart Platform device illustrated in Figure 1.1. It is also important to realize that the inner product (D1, D2) is dependent upon the representation [K] of the mapping of stiffness. (See Equation (1.7).) Consider now that the inner product is defined in the "new" coordinate system by (I)1 1' = (D1) [K'] 6l, (1.13) where D'1 and ) are the axial coordinates of the pair of twists expressed in the new coordinate system. The scalar quantity, work, resulting from the inner product is preserved by substituting (1.12) into (1.13), regrouping, and subsequently substituting (1.9), which yields (^'1 D= (1 ) [E]T [K] [E] b' = ([E] D', [E] ID)' = (D), D2). (1.14) Here, D1 and D2 are the axial coordinates of the same two twists expressed in the old coordinate system. In the experimental implementation of this theory (Section 2.3), the inner product established here for twists is used to decompose in a meaningful way a general twist of a partially constrained gripper/workpiece into an allowable twist (twist of freedom) that is consistent with the environment together with a corrective twist (twist of compliance) that nulls an error due to the difference between a command wrench and the actual wrench acting upon the gripper/workpiece. 1.3 The EigenScrews of Stiffness The second analytical representation [k] of the mapping of stiffness is a general collineation of screws that assumes the form, S= [k] a. (1.15) This form differs from the correlation given by (1.4) because ray coordinates d are now assigned to the twist.3 For the example in Figure 1.3, the screw $2 that is now assigned the ray coordinates d=[O[; o] is transformed into the screw $1 with ray coordinates w=[f; rmo]. The characteristics of [k] can be determined by substituting (1.3) into (1.4), which yields w = [K][A] d, (1.16) and comparing with (1.15), [k] = [K][A]. (1.17) This effectively combines two successive correlations of screws, the first, ray to axis and the second, axis to ray. Therefore, the overall transformation is the collineation form of the mapping of stiffness, viz., ray to ray. Consider now that another twist of an angle 60 on a screw $i is chosen to be transformed under the mapping of stiffness, such that the wrench obtained acts upon the same screw $i with an intensity Sf. (See Figure 1.4.) This screw is accordingly defined as an "eigenscrew of stiffness" and the ratio of intensities of the wrench and the twist is defined as the corresponding "eigenstiffness", ni. All six such occurrences of these eigenscrews and their corresponding eigenstiffnesses can be determined from the eigenvectors and eigenvalues of [k]. It is important to recognize that the eigenvectors and eigenvalues of the matrix [k] are always real, and therefore, the eigenscrews and eigenstiffnesses of the mapping of stiffness are also always real. For a symmetric positivedefinite [K]1 and a symmetric [A], the solution to the "generalized" eigenvalue problem Ki [K]1 d = [A] d yields that the product [k]=[K][A] possesses real eigenvalues and eigenvectors. (See Korn and Korn [1968].) 3Equation (1.15) was in fact the form that Dimentberg [1965] used to describe the mapping of stiffness for two rigid bodies that were connected via a general compliant coupling. h8O 1s1 S8 H H T hi T $i O Figure 1.4 Consider that the six eigenscrews $i, $i, $iii, $iv, $v, and $vi, are described by the six eigenvectors of [k], which are the ray coordinates di, dii, diii, div, d , and dvi, respectively. (The eigenvectors are not necessarily unitized ray coordinates.) Consider also that the corresponding eigenstiffnesses, Ki, ii ,, iii, Kiv, v, and Kvi, are obtained as the corresponding eigenvalues of [k]. Then, an eigen screw, such as $i, transforms according to the relation Ki di = [k] di. (1.18) When comparing (1.15) with (1.18), it should be clear that the dimension of ti is force/rad and that Ki is the ratio of the intensities of the wrench and the twist acting on $i. From Figure 1.4, the positive work into the spatial spring system done by a wrench on a twist that is on an eigenscrew is expressed by 6f 6 (2 hi) > 0. (19) This result can also be deduced from the left side of (1.1), by substituting a12=0 and hl=h2=hi. Finally, substituting the relation 6f=Ki 60 yields 2 (60)2 i hi > 0, (1.20) which shows that the eigenstiffness and the pitch of the eigenscrew must have the same sign and must be nonzero. It should also be clear that the pitch of the eigen screw is finite, since work cannot be done by a couple on a translation. Invariance is always the key issue in geometry, and this section reflects this philosophy. With this in mind, consider now that a coordinate system has been changed, by for example (1.8). Then, in an analogy with the development of (1.10)(1.12), the new representation of the collineation form of the mapping of stiffness can be obtained by substituting [e] w' for and [e] d' for d in Equation (1.15) and subsequently inverting [e], and 17 = [e]1 [k] [e] 1', (1.21) which is equivalent to = [k'] d', (1.22) where the new representation of the collineation form of the mapping of stiffness is [k'] = [e]1 [k] [e]. (1.23) In contrast with Equation (1.12) which related congruent matrices, Equation (1.23) relates similar matrices. This reflects the fact that the eigenstiffnesses are invariant with a change of coordinate system. Although their representations certainly change, the eigenscrews in both (1.15) and (1.22) are the same, i.e. the eigenvectors of [k'] represent the same six screws as do the eigenvectors of [k], and these six pairs of eigenvectors are ray coordinates related by applications of (1.8). 1.4 The General Model of Spatial Stiffness It has been established that the mapping of stiffness can be represented in a correlation form (1.4) that uses a symmetric positivedefinite 6x6 matrix [K]. For this representation, axial coordinates D of a small twist of one body relative to another is transformed into the ray coordinates of the corresponding small change in the wrench that interacts between the two bodies through a real compliant coupling. This section provides the analytics that are necessary to model any real complicated compliant coupling, whether loaded or unloaded, by the general model of stiffness illustrated in Figure 1.1. The forward displacement analysis of this 33 Stewart Platform is known and reducible to an eighth degree polynomial (Griffis and Duffy [1989]). Consider that referenced to some coordinate system, the unitized ray coordinates, hni, i=l, 6, of the six lines illustrated in Figure 1.1 are known and form the columns of a 6x6 matrix [j]. These lines are in fact the lines of action of the six scalar forces, fi, i=l, 6, of the six linear springs that constrain the motion of the six prismatic joints of the Stewart Platform model. Firstly, it is necessary to consider a static analysis of the top platform. The 6 ray coordinates ( fi hi) of the resultant of forces that the top platform applies i=1 to the springs in the legs are equal to the ray coordinates Wo of the resultant of the remaining wrenches that are applied to the top platform by other bodies. This can be expressed in the form, o = [j] f, (1.24) where f is an R6 vector of the scalar forces (fi, i=l, 6) applied to the springs by the top platform. The force in a given leg, fi, is related to the difference between the loaded and unloaded spring lengths, (/iloi), by fi=ki(lil0o), where ki is the positive spring constant of the ith leg. In the following analysis, it is assumed that eventhough the legs deflect, they do not deflect considerably from their unloaded position, i. e. the ratio lo/li does not deviate far from unity.4 In order to obtain a representation of spatial stiffness, [K], this static analysis of the loaded model is now related to small deflections in the legs. A small deviation from a current given operating state of (1.24) can be accommodated by considering small changes in Wo and f to be respectively and Sf. Now, consider that the spring constants of the six legs form a diagonal matrix [ki] and that 6fi=ki Sli, where 61i is a small change in the spring (or leg) length. Repeating for all legs and substituting into (1.24) yields w = j] [ki] 5!, (1.25) 4Chapter 3 relies upon the dimensionless parameters p. = i/1. to illustrate that even the simplest compliant couplings become asymmetric as they deviate from their respective unloaded configurations. where 61 is an R6 vector of scalar leg deflections. The entries of the diagonal matrix [ki] are to be determined experimentally such that the Stewart Platform model represents accurately the spatial stiffness of the actual coupling between the two bodies. Now, the work done by a given leg force is fi l1i, and this is equal to (fi i )T f), where D is the twist of the top platform relative to the base. Equating these two expressions for work, dividing both sides by fi, and repeating for all legs yields the matrix expression 61 = [j] f), (1.26) which is the reverse kinematic solution. Substituting (1.26) into (1.25) gives the correlation form representation of the mapping of stiffness for the resultant model, S= [j] [ki] [j]T D. (1.27) Comparing (1.27) with (1.4) yields [K] = [j] [ki] []T. (1.28) The matrix [K] contains twentyone independent parameters (due to symmetry), whereas [ki] and [j] respectively contain six and eighteen independent parameters. Each column of [j] consists of the six ray coordinates of a line (four independent parameters) that joins a point in the base to a point in the top platform. Hence, the six columns of [j] contain separately a total of twentyfour independent parameters. However, the six lines meet pairwise at six points, which imposes six conditions and reduces [j] to containing a total of eighteen independent parameters, and these together with the six parameters of [ki] result in a total of twentyfour parameters for [ki] and [j]. Therefore, given a general [K] of twenty one independent parameters, it is necessary to specify three further conditions (i. e. to solve for [ki] and [j]). These additional free choices can ideally result in a virtually unchanged [j] for a wide range of relative motion between the two rigid bodies. In other words, the model should be designed such that changes in actual spatial stiffness ([K]), resulting from finite changes in relative position and orientation between the two rigid bodies, are accommodated as far as possible by changes solely in [ki]. 1.5 Parallel and Serial Arrangements Consider two bodies to be connected inparallel by a pair of Stewart Platform models, represented in the correlation forms [K1] and [K2]. (See Figure 1.5a.) It is required to determine the equivalent mapping of stiffness for an equivalent model, represented by [Ke]. This can be accomplished by considering that we = *1 + w2, (1.29) where We, r1, and w2 are ray coordinates of the resultant wrenches due to models [Ke], [K1], and [K2], respectively. Substituting (1.4) for each of the models yields [Ke] De = [K1] De + [K2] De, (1.30) where De are the axial coordinates of the twist that is the same for each of the three models. This result yields [Ke] = [K1] + [K2], (1.31) which is the spatial equivalent of two single degreeoffreedom springs connected in parallel. Consider now that two models are connected in series. (See Figure 1.5b.) Then, the base of the model [K2] is the top platform for the model [K1]. It is required to determine the equivalent mapping of stiffness for an equivalent model, L7J1 .^ . C\j t4 ~LJ [Ke], whose base is attached to the base of [K1] and whose top platform is attached to the top platform of [K2]. This can be accomplished by analogously considering that De = D1 + D2, (1.32) where De are the axial coordinates of the effective twist relating the relative motion between bodies 2 and 0, and D1 and )2 are the axial coordinates of the two intermediate twists, which relate the relative motions of bodies 1 and 0 and bodies 2 and 1, respectively. Substituting the inverse of (1.4) for each of the models yields [Kel1 We = [K1]1 e + [K211 We, (1.33) where We are the ray coordinates of the resultant wrench that is the same for each of the three models. Therefore, the effective mapping of stiffness for two models that are connected in series is obtained from the sum of the two spatial compliances: [Ke]1 = [K1]1 + [K21 (1.34) where the term "spatial compliance" is used to denote the inverse of spatial stiffness. Equation (1.34) is the spatial analogy of a pair of single degreeoffreedom springs connected in series. 1.6 Robotic Applications This representation of stiffness provides a novel and attractive way of properly utilizing spatial displacements to control general systems of forces/torques acting in combinations upon a body. When the rigid body is the gripper/workpiece of a general, six degreeoffreedom robotic manipulator, then, based on the guidelines set forth in this work, a new algorithm emerges for the simultaneous control of an allowable twist and a constraint wrench. The endeffector is considered to consist of two rigid bodies, one the gripper/workpiece and the other, the end link of the manipulator. In order to implement the new algorithm, the end link and the gripper/workpiece must be connected together by some compliant connection such as, for example, an instrumentedd) RemoteCenterofCompliance (iRCC) device.5 (A force/torque sensor is also needed in the connection.) The spatial stiffness of the iRCC can be easily quantified by an octahedral Stewart Platform model. This model of the iRCC is more attractive than the centerofcompliance model, since it is not necessary to operate the iRCC within bounded regions which were designed to contain a centerofcompliance. When unconstrained, the gripper/workpiece is considered to be connected to ground by two spatial springs connected in series. (Figure 1.5b also illustrates this when the gripper/workpiece is considered to be body 2, the end link considered to be body 1, and ground considered to be body 0.) One of the spatial springs is the manipulator itself, which connects the end link to ground, and its spatial stiffness can be modeled by a Stewart Platform and represented by [K1]. The other spatial spring is the iRCC that connects the gripper/workpiece to the end link of the manipulator. The mapping of stiffness of the second spatial spring can be analogously represented by [K2]. From (1.34), it is clear that the effective spatial stiffness relating the unconstrained gripper/workpiece to ground, [Ke], is obtained from the sum of the inverses of [K1] and [K2]. When the effects of the inverse of [K1] is minimal in comparison with the inverse of [K2], i. e. the iRCC is far more compliant than the manipulator, then the compliance of the manipulator can be neglected. In other words, [Ke] S [K2] when the manipulator is much stiffer than the endeffector's wrist (iRCC). sThe Charles Stark Draper Laboratory, Cambridge, MA. Consider now that the gripper/workpiece is fully constrained with respect to ground and that the iRCC is initially unloaded. Although the gripper/workpiece is fully constrained, the end link still possesses six degreesoffreedom due to the compliance in the iRCC. Since there can be no twist of the gripper/workpiece relative to ground, then the twists of the gripper/workpiece relative to the end link and of the end link relative to ground must be on the same general screw $, but their rotations, 602 and 691, must be in opposing directions. (See Figure 1.6.) This means that D1=D2, where I1 and D2 are the axial coordinates of the twists of the end link relative to ground and of the gripper/workpiece relative to the end link. Therefore, based upon a twist of the end link relative to ground, the iRCC is subsequently loaded with a wrench whose ray coordinates are w=[K2] D2. Substituting D52=D1 and neglecting a gravity wrench yields the small wrench that is applied to the gripper/workpiece by ground due to the twist of the end link, S= [K2] Dl. (1.35) Consider further that there is a general wrench with ray coordinates wr reacting between the gripper/workpiece and ground which is to be controlled. In order to begin to null an error between the desired wrench and a known (sensed) actual wrench with ray coordinates *s, a small corrective twist of the end link relative to ground is needed. The axial coordinates, Dc=D1, of the proper corrective twist can be obtained by inverting (1.35), and expressing it in the form, Dc = G [K2]1 We, (1.36) where G is a single dimensionless scalar gain that is used to limit 89c=691 of the corrective twist command, and where We=*r*s. This corrective twist is to be supplied by commanding the six joint motors of the manipulator. Therefore, the control of the twist of the end link relative to ground regulates the reaction wrench between the gripper/workpiece and ground. cIO _r *69>9 ! CJ 4LJ \a CL ( C L C O 60 Finally, consider the situation where the gripper/workpiece is only partially constrained with respect to ground via a stiff connection. Figure 1.7 illustrates an example of line contact between the gripper/workpiece and ground. Here, the possible screw of a reaction wrench must be an element of a 2system of screws, viz. the possible reaction wrench must be a force on a line parallel to and in the same plane as the lines $1 and $2.6 This system of wrenches is accordingly defined as the "wrenches of constraint" that constrain the motion of the gripper/workpiece relative to ground, and the ray coordinates wr of such a wrench are obtained via the linear combination Wr = A1 1 2 + A2 *2, (1.37) where W1 and W2 are ray coordinates of $1 and $2. The gripper/workpiece has four degreesoffreedom, and it can be considered to be initially motionless relative to ground. An error wrench due to the difference between the desired wrench and the actual wrench is nulled by a small corrective twist of the end link relative to ground, the axial coordinates of which, Dc, can be obtained from (1.36). This is because this corrective twist cannot move the gripper/workpiece, since the corresponding wrench that is induced to null the error in the iRCC is fully supported as a wrench of constraint between the gripper/workpiece and ground. Because the desired and actual wrenches of constraint can be written as linear combinations (1.37), it is clear that the error wrench can also be written as a linear combination, We = /:1 *1 + Y2 2, (1.38) where We='r*s are ray coordinates of the error wrench. It can be concluded from (1.36) and (1.38) that the corrective twist must also be an element of a 2 'The fact that the points of contact can only take compression is neglected. 4 0 O 4 0 <0 7E) O 4 0 0 ro  V (O 409 rLJ 6F) ~ system, and this system is defined as the "twists of compliance". Then, the axial coordinates 1)c of the corrective twist are obtained from the linear combination ]c = a1 I)* + a2 D), (1.39) where N^ and f* are axial coordinates of $ and $ which are the screws that correspond under the mapping of stiffness with the lines of force $1 and $2. (See Figure 1.7.) The axial coordinates of the the two twists of compliance can therefore be obtained by the relations, D^ = [K2]1 1 and Df^ = {K21 *2. (1.40) From (1.1), it follows that the two screws $* and $^ are not reciprocal to the screws $1 and $2, which span the wrenches of constraint, and from (1.6), (1* 1 0 and (D^)T 2 0. (1.41) Thus, it is clear that the screws spanning the twists of compliance are not elements of the 4system of screws which is reciprocal to the wrenches of constraint. Four screws that are each reciprocal to the two lines which span the wrenches of constraint do in fact quantify the twists of freedom of the gripper/workpiece relative to ground. These twists of freedom constitute all allowable twists that are consistent with the environment, and Figure 1.7 illustrates four such screws of zero pitch, namely the lines of rotation $3, $4, $5, and $6. Assigning the axial coordinates Di, i=3, 6, to these four twists and introducing them into the axialray coordinate form of reciprocity yields iT 1 = 0 and biT *2 = 0, (1.42) for i=3, 6. The twists of freedom are thus linearly independent from the twists of compliance. In fact, the two systems are orthogonal complements with respect to [K2]. This can be shown by firstly considering that the axial coordinates Db of a given twist of freedom of the gripper/workpiece relative to ground can be expressed by the linear combination, Db 1 D3 2 D4 + 3 5 + 4 D6. (1.43) From (1.37), (1.42), and (1.43), it can be deduced that any twist of freedom is reciprocal to any wrench of constraint and that it can be expressed by )b = 0, (1.44) where *=(*r=*e) are the ray coordinates of any wrench of constraint. Now, for each wrench of constraint, there is a unique twist of compliance, i. e. *=[K2] Dc, and therefore substituting into (1.44) yields bD 1 [K2] Dc = 0. (1.45) This expresses the condition of orthogonality between twists of freedom and twists of compliance with respect to the potentialenergybased inner product, and from (1.7), (Db, Dc) = ) [K21 Dc = 0. (1.46) Therefore, in a direct sum of orthogonal complements [B] D [C]=R6, where the matrix labels are used to denote the vector subspaces. The consequence of this direct sum is that a general D R6 can be written is a unique combination of a jb E [B] and a Dc E [C], so that D=)Db+Dc. It is important to realize that the twists of freedom and the wrenches of constraint are determined solely by the nature of the contact between the gripper/workpiece and ground. The twists of compliance are, however, dependent on the mapping of stiffness of the robot, shown here to be represented by [K2], together with the wrenches of constraint. It is important to further recognize that the screws $1 and $2 are not dependent on the choice of origin, and that these same two screws will always be obtained given a properly represented mapping of stiffness. The sole requirement of this mapping of stiffness is that it be positive definite. A centerofcompliance is not necessary, and a compliant coupling (such as the iRCC) need not be positioned and oriented in some special way to perform a given partially constrained task. Figure 1.7 essentially illustrates a virtual robot manipulator consisting of six serially connected screws, $, $ $3 $5, and $6. This virtual robot operates in parallel with the actual robot that controls the 6 d.o.f. displacement of the end link. In order to better visualize the decomposition, it is useful to consider that when the six virtual joints displace, the axial coordinates of the resultant twists producing ($1, $2) and ($3, $4, $5, $6) are respectively, Dc and Db. Finally, it can be concluded by superposition that these two twists, which together produce a general twist of the end link, provide a means to control the reaction wrench between the gripper/workpiece and ground, while simultaneously controlling the displacement of the gripper/workpiece relative to ground. This novel control strategy is defined as "Kinestatic Control." CHAPTER 2 IMPLEMENTING KINESTATIC CONTROL: USING DISPLACEMENTS TO NULL FORCES 2.1 Introduction Which way does a robot move itself in order to null the external contact forces that act on its gripper? In general, the contact forces that interact between the gripper and its environment are not nulled or even altered when the robot moves the partially constrained gripper in the unrestricted direction of a remaining freedom. Consequently, the efforts here are to document the investigations and implementations of other robot motions that though restricted do correctly null (or change) these contact forces. While addressing this question, this work establishes the property of robot stiffness to be the key and central issue. Robot stiffness is after all a property that is independent of the task required of the gripper. Succinctly, the robot is considered here to be a spatially deflecting spring that connects two rigid bodies together, viz. the gripper to ground. In the previous chapter, the author analyzed the geometrical properties of spatial stiffness and presented two analytical representations, one that defined a positivedefinite, potentialenergybased inner product and another that established the properties of spatial stiffness which are invariant for Euclidean motions. (See Chapter 1.) This work further used the positivedefinite inner product to facilitate a decomposition of the vector space of all twists'. A robotic example was given 1"Twist" is used here as a generic word to denote the infinitesimal displacement/rotation of one rigid body relative to another, while "wrench" is a similar generic word for the forces/torques that interact between two rigid bodies. 31 where a gripper (with workpiece) was in straight line contact with its environment, resulting uniquely in two degreesofconstraint (characterized by the wrenches of constraint) and four degreesoffreedom (characterized by the twists of freedom). Using the positivedefinite inner product, an orthogonal complementary twist space was reasoned to exist. This complementary twist space was defined as the "twists of compliance," and each twist contained therein possessed a working relationship with its own unique wrench of constraint. Accordingly, the twists of compliance are the restricted (nonallowable) twists that are reserved for nulling and/or correcting the contact forces which interact between the gripper and its environment. 2.1.1 The NonEuclidean Geometry of Stiffness While analyzing the geometrical properties of the inner product defined by spatial stiffness, it was concluded that it must be nonEuclidean, because it is positivedefinite and operates on twist coordinates.2 (See Section 1.2.) The use of a positivedefinite (nonEuclidean) inner product is clearly a necessity in the general case for determining a twist space that is an orthogonal complement to the twists of freedom (or a wrench space that is an orthogonal complement to the wrenches of constraint). 2Suppose [K] to be a positivedefinite 6x6 symmetric stiffness matrix. Then, the only twists that satisfy the quadratic form D^T[K] D=O are imaginary, where six notallzero homogeneous twist coordinates are given by D^=[6'T; 6T]T. In other words, for a nondegenerate coupling, there is no real twist about which no work is done. (An inner product that operates on homogeneous coordinates is considered to be nonEuclidean, specifically elliptic (Lipkin and Duffy [1985]) or virtual (Sommerville [1929]), when only imaginary elements satisfy the vanishing of its quadratic form. As far as a Euclidean inner product is concerned, mathematicians have demonstrated that one which operates on homogeneous coordinates is either positive semidefinite or indefinite. See (Semple and Kneebone [1979] and Porteous [1981].) The degree of freedom afforded to a gripper (its twists of freedom) is a purely geometrical notion, as well as its degree of constraint (its wrenches of constraint). In other words, the freedoms and constraints of the gripper are determinable through pure Euclidean geometry. Therefore, it cannot be meaningful to establish a nonEuclidean (positivedefinite) inner product based solely upon the nature of the freedoms and constraints acting between the gripper and its environment. In certain instances, using a "selection matrix" introduces a fallacy, as does defining a point where a "compliant frame" is to be located and subsequently used to perform decomposition. (See Duffy [1990].) The result of such an operation yields an orthogonal complementary twist space that is not necessarily the best twist space for nulling errors in the contact forces which interact between the gripper and its environment. In other words, it will not yield the correct twists of compliance. 2.1.2 An Example To further investigate this important statement, consider the planar two dimensional example illustrated in Figure 2.1. A wheel is connected to a platform via two translational springs, which are capable of compression as well as tension. The platform is then subsequently connected to ground via two actuated prismatic (slider) joints that are tuned for fineposition control. The wheel is to maintain contact with a rigid environment. The center point of the wheel thus has a single freedom, which is a displacement pt in the direction of Ui. Based on the constraint imposed upon the wheel, a normal contact force, fn acting through the center point along {in, may interact between the wheel and the environment. .1 Figure 2.1 It will be shown that the control of fn along un and pt along uit is accomplished by judicious choices of slider displacements 6d1 and 6d2, which together combine to move the platform in some direction. Intuitively, one may at the outset consider that the best motion for nulling errors in the force fn would be a displacement of the platform in the in direction, i. e. parallel to the normal force. However, this is not the case, and in general, the best displacement is in some other direction, Uc, which is canted to it at an angle not equal to 90. The best displacement is one that changes the force error in a desired way without affecting the control of the wheel's displacement in the uit direction. For example, displacing the platform in the Uc direction would not move an atrest wheel, but it would change the normal force. For this simple example, a positivedefinite Euclidean inner product mandates that a displacement along Un = cos(n) i+ sin(On) J is Euclidean orthogonal to a displacement along it = cost) i + sin(0t) j. However, these vectors are elements of a Euclidean vector space, which uses two nonhomogeneous coordinates to locate oo2 elements.3 (See Ryan [1989].) Even though there exists a purely geometrical positivedefinite inner product here in this simple case, it is not the best one with which to perform twist decomposition. It is proposed here that stiffness can be used to define a proper inner product with which to obtain the best fnerrornulling direction: the twist of compliance, ic = cos(0c) 1+ sin(Oc) J. In other words, for coordinates 3A Euclidean vector space does not possess the power of locating infinite elements with finite coordinates. Once a rotational coordinate is incorporated with the two translational ones (general planar rigid body motion), then a rotation about a point at infinity yields a pure translation, and accordingly, three finite homogeneous coordinates are used to describe general planar motion. Because the resulting homogeneous twist coordinates would no longer belong to a Euclidean vector space, a positivedefinite inner product acting on them must be non Euclidean. (See the previous footnote.) 36 Ut = [cst] and Uc= [csin( ut [K] c =0, where the 2x2 matrix [K] is symmetric, and its three elements have the dimension F/L. This matrix quantifies the stiffness of the mechanism consisting of a wheel connected to ground by two translational springs, a platform, and two actuated sliders. (It does not quantify its task, which is the surface to be tracked by the wheel). It is assumed that the actuated sliders are stiff (they are tuned for fineposition control), and since they act in series with the more compliant translational springs, their compliances can be neglected, and the stiffness of the overall mechanism is thus solely due to the translational springs. In order to obtain a mechanism stiffness mapping relating the small displacement of the wheel relative to the platform with the incremental change in external force acting on the wheel, it is necessary to express the force, f = fx i + fy j that the wheel applies to the springs: fx cl c2 ]k (l lo)121 y [ (2.1) fy l 2 k 2 (12 lo2), where ci = cos(0i) and si = sin(Bi), and where ki and (i loi) are respectively the positive nonzero spring constant and difference between the current and free lengths of the ith spring. When the wheel is in quasistatic equilibrium, then f is the force that the the environment applies to the wheel plus any other externally applied forces (gravity, for instance). Consider now that the two springs do not deviate too far from their free lengths. Then, from (2.1), for a small deviation, [ fx cl c2 kI 1 1 (22) 6fy s1 S2 i k2 612 where 6fx and 6fy are deviations away from fx and fy and where 61i is a deviation of the ith spring away from its current length. Substituting into the righthand side of (2.2) the inverse kinematic transformation, 611 c1 1l 6X (2.3) 612 C2 S by (obtained from a projection) establishes the stiffness relationship, 6fx =[K] 6x 1 sfx =[K] (2.4) bfy by where 6x and Sy are deviations of the center position of the wheel away from its current position measured relative to the platform, and where the stiffness matrix [K] is given by c C2 k 0 c s [K] = 1 2 1 i 1 1 (2.5) SSl s2 0 k2 c2 2 Consider a numerical example of 01 = 45, 02 = 90', and k1 = k2 = 10 kg/cm. Then, from (2.5), the stiffness matrix assumes the form, [K] = 5 5d. kg/cm. Now it remains to vary the task required of the wheel in order to demonstrate the concepts here that are new and of interest. (They are easily extendible to higher dimensions. Section 2.2 illustrates the extension to planar threedimensional and spatial sixdimensional cases.) Prior to analyzing the specific task illustrated in Figure 2.1, consider first that the wheel center point is fully constrained. This means that any force, f = fx i + fy j, acting through the center point may be applied to the wheel by the environment. It is clear that any small displacement of the platform due to slider displacements 5d1 and 6d2, 6d = SdI i + Md2 j, will cause via stiffness a small change in this force. Specifically, the change will be sfx 5. 5. 6d [ 1= 5. 1 (2.6) 6fy 5. 15. d2 The negative sign accounts for the kinematic inversion of specifying the displacement of the platform relative to the wheel. In other words, [dl = [ ' Conversely consider that certain desired changes in fx and fy are known and are denoted by 6fx and Sfy. The proper direction, 6dc = 6dcl i + 6dc2 to move the platform to accomplish this change can be obtained by inverting the spring matrix in (2.6), which yields Sd 0.3 0.1 6fx, dcl = 0. (2.7) 6dc2  0.1 0.1 fy Equation (2.7) can be used to control a timevarying general force, f = fx i + fy J, that interacts between the fully constrained wheel and its environment. At each instant, a force error is given, and (2.7) specifies the proper forceerrorreducing displacement Sdc. Consider now that the wheel is at rest but is loaded with an excessive normal force. (See Figure 2.1.) By application of (2.7), this force can be reduced in a desired way without moving the wheel as long as the change in force, [f is supported by the lone constraint imposed by the environment. Therefore, based on the figure, = 6f n [707 where 6fn is the desired change in the normal theJy fiue [u.Y]= j0"0 force, fn. Substituting into (2.7) yields the correct direction (with a magnitude) to move the platform: [dcl= fn 0.1414cm/kg This demonstrates that for this example a displacement of the platform in the negative x direction (positive 6fn) reduces the compressive normal force that acts on the wheel at an angle of 45. (The force fn is referenced negative for compression, and therefore, a positive 6fn reduces an excessively compressive force.) Because the wheel does not move when the platform displaces in this direction, this is inherently the best direction for correcting a normal force error, and consequently, it is the twist of compliance, Uc. The control of the normal force has been accomplished without displacing the wheel. It is important to recognize this since now, by superposition, a desired allowable displacement of the wheel, 6d = 6dtl + 6dt2 i = t( 0.707 + 0.707 j), can be accomplished by simply adding it to the forceerror correcting displacement, 6dc. (Because 6dt is an allowable displacement, adding it to 6dc does not affect the control of normal force.) Therefore, the following small slider displacements can be computed to control the normal force and the tangential displacement simultaneously: b6d1 0.707 + Gcf 0.1414 cm/kg (2.8) Gd6 = 0.707 =[+ Gc jn (2.8) 6d2 0.707 0.0 In (2.8), Gt and Gc are dimensionless scalar gains, and 6pt and 6fn are errors in wheel position and normal force. Successive applications of (2.8) based on updated errors in 6pt and 6fn provides the means for controlling both the tangential wheel position and the normal force. 2.1.3 The Justification for Stiffness to be the Inner Product The desired control algorithm has thus been formulated. A highlight of the example demonstrates precisely where the theory depends upon geometry and precisely where it is enhanced by the nongeometrical physical properties of the spring stiffnesses, ki. This establishes that stiffness can be used to define a physically meaningful inner product with which to perform twist decomposition. The constraint force, fn, does no work, as it acts in the direction Un canted at an angle On to the horizontal. The condition that the constraint force does no work may be written: (SPt it) (fn un) = 0, (2.9) where 6pt denotes a small displacement of the wheel in an allowable direction, Uit. As the wheel moves to accommodate the nowork condition (2.9), the center point of the wheel is restricted to a displacement bpt in the direction ui. Therefore, from (2.9), Ut is perpendicular to in (un ut = 0). Consequently, 9t On = 90'. (See Figure 2.1.) While this reaffirms the statement that the relationship between constraint and freedom is one of geometry, it often also conjures up the erroneous notion that an inner product for displacements has been established. The condition that the virtual work of a constraint force must vanish has been introduced solely for the purpose of obtaining the unrestricted freedom based on the constraint. (In Section 2.2, it becomes ever more apparent that (2.9) is not an inner product for displacements, and that in general, it is not an inner product at all, but rather, for example, a condition that a point lies on a line.) With the lone freedom established, it remains to determine the necessary complementary normalforcecorrecting displacement. Via stiffness, displacements are related to forces via a onetoone mapping. Therefore, for fs [cos(0n)l Sfn Un = fn [sin(n there exists the one correcting displacement, pc U = Pc [cos(c)1 Pc c = Lsin(c)j, and this relationship can be written, 6fn 'n = [K] 6pc Uc. Substituting into (2.9) and dividing by the nonzero magnitudes 6pt and SPc yields that the directions ut and 'c are "[K]orthogonal": cos() cos(oc) U [K] c = c [K] cos(c) = ( u, ) = 0. (2.10) SK] =sin(0t) sin(Oc) Since the normal force must do work on a displacement in the Uc direction, Ufe cannot be perpendicular to in. Therefore, it is not possible for u'c to be equal to it, and consequently, Uc must be independent of it. Equation (2.10) provides an orthogonal decomposition that allows for any displacement to be written as a linear combination of it and Ufc. (For example, iin = Al it + A2ic.) For the example illustrated in Figure 2.1, Ot = 135" which from (2.10) yields that Oc = 180". It is interesting to examine how 0c varies as the task of the wheel varies, viz. On and 0t vary. Figure 2.2 illustrates 0c as a function of 9t, for the same mechanism with taskindependent stiffness [K] =[ 155.] kg/cm. This solution for the twists of compliance (the best directions to move to null force errors) is by no means intuitive. The author submits the reason for this to be the nonEuclidean properties of stiffness.4 Simply considering all of the 4Variations in k1 and k2 cause 0c to vary, even when all (other) geometry is held fixed. This clearly illustrates that the solution of Oc is not fully dependent on geometry, which is another way of saying that the positivedefinite inner product (2.10) is nonEuclidean. The example given in Chapter 1 also explained that the complementary twists of compliance vary as the nongeometrical stiffness properties vary, even though the robot, gripper, workpiece, and environment are not moved. 42 0 u u1 L \ C O 0 0 0 4s E o L 0 1 Vc E, L n 0 0 IL L nTniTn1 i 1 TpltTTf mT T T rrrrn 0 0 ) 0 ) 0 ) 0 *M 10 .9 i4 available motions of the wheel does not immediately produce the correct force errornulling displacement direction. In order to make the solution intuitive, the engineer must incorporate into the thought process the appropriate mechanism properties, which at the very minimum must include its stiffness properties. In other words, "if I move the platform in this direction that is dependent on stiffness and the constraint, then there will be no displacement of the atrest wheel." While it has been recognized here that the use of a positivedefinite (and hence, a nonEuclidean) inner product is necessary, it only remains to justify the use of a chosen nonEuclidean inner product. Clearly, a more thorough examination includes properties affecting the dynamic (higher frequency) characteristics of the mechanism. Griffis [1988] investigated the use of a positive definite inner product defined by the kinetic energy of a rigid body. From a practical point of view, however, a meaningful algorithm to control force and displacement does not result from it. The author considers that most robots do not accelerate significantly and do not respond to inputs of higher frequency. (The position control loop for the robot used in this work had a 1 Hz. bandwidth, and for example, the robot responded to only 20% of a 2 Hz. sinusoidal input.) Therefore, it must be clear that the force vs. displacement (and wrench vs. twist) relations are inherently dominated by mechanism stiffness. Consequently, it has been the thrust of this work to examine the stiffness properties of the robot, and to use its knowledge to simultaneously regulate the force and the displacement of its endeffector. (Section 2.3 describes in detail the initial implementation of this theory, beginning with an empirical determination of robot stiffness and ending with proofofprinciple control examples.) 2.1.4 Relation to Previous Work In general, force/position control laws given in the literature depend upon the synthesis of matrices of control gains that effectively create commands of actuator torque (or force) increments.5 Figure 2.3 illustrates a general force/torque control loop. The author considers that this figure adapts itself in some form or fashion to all force control loops cited in the literature. For this loop, it remains to synthesize the control matrices [Kf] and [Kp]. The vast majority of the literature on this subject supports, I believe erroneously, that the matrix [Kf] is the designer's or programmer's choice and often relegates its synthesis to being dependent on robot task (for instance, making [Kf] diagonal for the Uit and iin directions in Figure 2.1). When robot stiffness is ignored, the author considers that the task dependent synthesis of [Kf] (and [Kp]) is necessary for these control laws to work, so that compatible forceerror and positionerror corrections can be combined. Excellent, insightful, and thorough investigations of onedimensional force control have been given in the literature, e. g. An et al. [1988], Chapter 8. Haefner, et al. [1986] effected twodimensional force control by employing a pair of one dimensional controllers acting inparallel (which does not offer much geometrical insight). The extension of Haefner, et al. is valid, but the extension of many other onedimensional applications to multidimensional ones has not been a natural one. It is common place for one to read that a robot is desired to have a "stiffness 'kx' in the 'x' direction." To coerce the closeloop robot system to exhibit this task dependent characteristic clearly requires the taskdependent synthesis of [Kf] and 5See Whitney [1987] for a survey of other force/position control laws, such as Hybrid Control (Raibert and Craig [1981]), Stiffness Control (Salisbury [1980], Kazerooni, et al. [1986]) and Impedance Control (Hogan [1985]). This survey is an excellent overview, and therefore, the control laws need not be repeated here. As far as torque control is concerned, see An, et al. [1988], which documented experimentation on a unique prototype direct drive serial robot that was specifically designed to facilitate the control of joint torques. + b c Dc + 1  0>1,  T t + Robot  System *W Nomenclature w. desired wrench wo actual wrench of constraint constraints (*a e [a]) wrench error Dc twist of compliance (wrencherror correction) DI desired twist of freedom > freedoms (Db E [B]) Do actual twist of freedom J [Kf] wrench gain matrix D general command twist to robot 60 actuator displacement bT actuator torque increment [Kp] actuator gain matrix [J] kinematic Jacobian relating 6Sp to ) [Robot System] robot actuators, permanently attached compliant devices and sensors, reacting against a rigid environment. Figure 2.3 JT [Kp], as well as possibly others. Some researchers deem the synthesis of the 36 elements in each of these 6x6 matrices necessary due to the robot system complexity, but it seems that this widespread synthesis itself introduces a complexity which is an order of magnitude greater than it needs to be, especially in a research lab where a graduate student likes to see the effects of initially implementing one scalar proportional gain, even if it is to control a sixdimensional system. Kinestatic Control establishes [Kp] and [Kf] whenever the backdrivable effects are minimal (shown with a dotted line in Figure 2.3), i. e., wrenches acting on the gripper do not affect the response of the actuators. For this case, the matrix [Kp] represents itself as no more than fineposition control gains, which are easily synthesized for a given robot while it is unconstrained. While the commands to the actuators are displacementbased, the matrix [Kf] must map a force/torque (wrench) error into a displacement/orientation (twist) correction, and consequently, [Kf] is a scalar times the inverse of stiffness (compliance). Under the provisions of Kinestatic Control, general sixdimensional simultaneous regulation of both force and position is accomplished via the synthesis and subsequent actions of only two single dimensionless scalar gains: one for force (wrench of constraint) and one for position (twist of freedom). (This is a generalization of (2.8).) The force gain essentially magnifies how far to move the endeffector along a wrenchcorrective twist of compliance, while the position gain magnifies how far to twist along a desired twist of freedom. Consequently, this control methodology depends on the knowledge of the openloop stiffness of the robot, which enables the actual control law to be simple yet responsive. 2.2 Twist Space Decomposition 2.2.1 Planar Twists Figure 2.4 illustrates a planar three revolute (3R) serial robot with a compliant device connecting its gripper to its endeffector (platform). The workpiece held fixed in the gripper touches the rigid environment in a single point, P. At the instant in this example, the environment has imposed one constraint on the gripper, which leaves it with two freedoms. In fact, a constraint force may be generated to act on the gripper along the line $a. (The point contact is considered to be a bilateral constraint here in the discussion, even though it is really a uni lateral one at the times of contact and withdrawal.) The revolute joints of the robot are actuated (41 642, and S03) and considered to be tuned for fineposition control so that the robot is nonback drivable while serving. The compliant device considered here is a parallel mechanism consisting of three extendible legs whose displacements are restricted via translational springs of known stiffnesses. The compliance of the stiff robot is neglected, and the stiffness of the robot system connecting gripper to ground is defined by the stiffness of the compliant device. For this planar example, force coordinates are initially defined, so that they can be used to determine the forces that the gripper applies to the springs. Obtaining a variational form of this is followed in turn with the definition of twist coordinates for the plane. This section continues by developing the mapping of stiffness for the compliant device (and hence, the robot system). It is shown that this mapping of stiffness relates a general point (of rotation) with a unique line (of force). This is finally used to obtain a twist of compliance that is necessary to complement the two freedoms of the gripper to effect kinestatic control. A 6'3 Figure 2.4 Lines of forces in the plane may be located with three homogeneous coordinates that are normalized, g=[c, s; r]T, where c and s are direction cosines and r the signed perpendicular distance of the line from the origin. A scalar multiple, f, of these coordinates varies the force acting along the line, and this can be represented by the three homogeneous coordinates w=f s=[fx, fy; mo]T. The force acting along a general line can be decomposed into a force acting along a parallel line through the origin together with a couple. The coordinates [fx, fy; 0]T denote a force acting through the origin, while the coordinates [0, 0; mo]T denote a couple (an infinitesimal force on the line at infinity whose homogeneous coordinates are [0, 0; 1]T). Adding the two sets of coordinates together reproduces the coordinates of the force. The homogeneous coordinates [fx, fy; mo]T of the force that the gripper applies to the springs can be written in the matrix form, fx cl c2 c3 (11 1) fy = s1 S2 3 k2 (12 o2) (2.11) mo 0 0 3 s3 k3 (13 o3) where the columns of the matrix contain the normalized homogeneous line coordinates (1', g2, and S3) of the lines (1, 2, and 3) of the three extensible legs shown in the figure, and where ki (li loi) is the leg force due to the extension of the ith spring. All coordinates are referenced to the coordinate system located on the platform at the intersection of the first two legs (hence the moment entries for those legs are zero). A variational form of (2.11) can be written as 6fx cl c2 c3 k1 611 fy = s S2 s3 k2 612 (2.12) 6mo 0 0 x3 s3j k3 613 provided the legs do not deviate too far from their unloaded positions. Equation (2.12) can be expressed in the compact form: S= [j]i]b, (2.13) where w = [6fx, 6fy; 6mo]T, where [j] replaces the 3x3 matrix of normalized line coordinates, and where [ki] is a 3x3 diagonal matrix of positive nonzero spring stiffnesses. The three elements of b6 are the small displacements of the legs, which are referenced positive for extension. Prior to defining an inverse kinematic transformation so that its incorporation into (2.13) will define the overall stiffness mapping, it is necessary to first define twist coordinates. A twist is a rotation about any point in the plane of motion.6 It can also be thought of as a pure translation, together with a rotation about the origin. The twist coordinates [6x, 6y; O]T denote the pure translation, while the coordinates [0, 0; S6]T denote the rotation about the origin. Adding these two sets of coordinates together produce the homogeneous coordinates of the general planar twist. These coordinates can also be written in the forms, D) = [6x, 6y; 6]T = [b6 y, 6_ x; 61]T= 6[ y, x; 1]T = 64 S, (2.14) where x, y are the Cartesian coordinates of the point of rotation. The three coordinates S = [y, x; 1]T are essentially a form of homogeneous point coordinates that are normalized. The twist coordinates ) = 64 S are accordingly a form of homogeneous point coordinates, the point being that about which one body rotates 64 relative to another. As such, a point that is infinitely distant, such as one with homogeneous coordinates [c, s; 0]T for instance, defines a pure translation. 6It is really a rotation about a line normal to the plane of motion, but for brevity, the point of intersection of this line and the plane is used in discussion. While defining an inverse kinematic transformation for the compliant device, consider that the gripper twists relative to the platform. In general, this is a rotation of an amount 6S about the point whose Cartesian coordinates are x, y. Then, it is straightforward to express, for example, the signed perpendicular distance, d3, between the point of rotation and the line of action of the third leg spring force as d3 = 53 S = c3Y 83x + x3s3, (2.15) where S = [y, x; 1]T are the normalized homogeneous coordinates of the point of rotation. While considering only those points of the gripper that instantaneously lie on the line of the third leg, one sees that, as they move tangent to their respective concentric circles, they all share the same component (6 d3) directed along the line. This component consequently defines the displacement of the third spring, and therefore, 6l3 = 6S d3. Multiplying (2.15) by 68 and substituting (2.14) yields 613 = 3 D. Repeating for the other two legs yields the inverse kinematic solution for the compliant device: 61 = U] f). (2.16) Substituting into (2.13) yields the overall mapping of stiffness, which is expressed here in the form, w = [K] D, (2.17) where [K] = j][ki]jT The stiffness mapping [K] now represents a linear geometric transformation that relates points with lines (specifically, a projective correlation). Equation (2.17) clearly shows that for a fixed [K], there is a onetoone correspondence between a point of rotation and a line of force: the point of rotation being that point in the platform about which the gripper turns an amount 64 and the line of force being that line along which a small force Sf acts to change the nominal force which the environment applies to the gripper. For the mechanism in Figure 2.4, the stiffness matrix [K] is considered known. (It is determined by j] and [ki].) The columns of [K] are scalar multiples of the coordinates of the forces necessary to cause twists respectively corresponding to a translation in the x direction, a translation in the ydirection, and a rotation about the origin. Considering the columns further as scalar multiplies of line coordinates, one sees that the intersection of the first two columns defines a unique point through which a force must act to cause a translation of the gripper relative to the platform. (If a force does not act through that point, then the gripper must rotate b6 about some point in the platform.) Prior to analyzing the specific task illustrated in Figure 2.4, consider first that the gripper is fully constrained relative to ground (bolted down, for example). This means that any nominal force whose coordinates are Wo = [fx, fy; mo]T is applied to the gripper by ground at a given instant with the robot in a stationary configuration. Clearly, any twist of the platform relative to ground (with coordinates Dc) will effect a change in this nominal force, and analytically, this can be expressed in the form, = [K] )c, (2.18) where a sign change has been installed in (2.17) because of the kinematic inversion of now specifying the twist of the platform relative to the grounded gripper. Because of (2.18), the force acting on the gripper changes to *0 + = [fx + 6fx, fy + fy; mo + 6mo]T. Alternatively, suppose that the desired change r is known, then Dc is computed by inverting (2.18): 1 )c = [K] (2.19) Equation (2.19) can be used to control a timevarying general force 0o = [fx, fy; mo]T, that interacts between the fully constrained gripper and its environment, because it specifies at each instant the best forceerrornulling point C about which to rotate the platform an amount 6q. It is interesting to consider the columns of [K] using superposition. They represent themselves as scalar multiples of the coordinates of twists that are necessary to change a nominal force by adding to it respectively a force along the xaxis, a force along the yaxis, and the couple normal to the plane. Considering the columns further as scalar multiples of point coordinates, one sees that the last column locates the only point for which a twist of the gripper relative to the platform generates a couple. (A twist of the gripper relative to the platform about any other point generates a force acting along some line.) Returning to the task at hand, namely that illustrated in Figure 2.4, it is clear that we must interest ourselves in specifically what point is related via (2.17) with the line of action $a of the constraint force. For it is about this point that the platform of the compliant device should rotate in order to null (or control) the constraint force. In other words, the point C that is sought is the twist of compliance, and from (2.19), its homogeneous coordinates are Dc = [K] 1 a, (2.20) where *a = fa [Ca, sa; ra]T are homogeneous coordinates of a force on the line $a. Figure 2.4 illustrates C, the precise location7 of which is only known after establishing [ki] and the geometry, i. e. [K] of the compliant device. 7The twist of compliance will usually not be the intuitive pure translation of the platform in the direction of the line $a. This would put point C at infinity to the left, which is only valid when (2.20) yields the homogeneous coordinates Dc = [ca, Sa; 0]T A rotation of the platform about point C cannot move the gripper, because the force, or change in force, applied to it is fully supported along $a. In other words, from (2.17), *a = [K] Dc. (2.21) Here, point C represents the best point to use to control force, because its use does not affect the control of the displacement of the workpiece (under ideal situations, e. g. no friction and a good stiffness model). A rotation of the platform about any other point does cause or affect the motion of the gripper. To visualize this, consider that because of Sb1, 42, and 603, the platform rotates relative to ground about a general point G. This twist decomposes itself into an unrestricted rotation of the gripper about point B together with a rotation about C, which causes a change in the contact force acting along $a. (Analytically, Dg = Db + Dc. Synthetically, the line $g containing G and C intersects $a in precisely point B.) It is the platform that twists Dg relative to ground, while the gripper twists Db = Dg Dc relative to ground. Imagining point G to place itself anywhere in the plane furthers the concept of decomposition of twist. Moving G, and hence $g, moves the point of intersection of $g and $a, causing B to slide up and down $a. That B lies on $a is in fact the condition that it is a twist of freedom. In other words, the linear restriction (or single constraint) on coordinates Db is the vanishing of the virtual work of a constraint force: T Db Woa = 6b fx + b f + 6b m = 0, (2.22) where Woa are coordinates of a nominal wrench acting on the gripper along $a. While it is clear that (2.22) is the point equation of the line $a, it should also be clear that it is valid for any scalar force acting along it. Supposing the magnitude to be small, Woa = Wa, then substituting (2.21) into the lefthand side of (2.22) and multiplying throughout by a minus sign gives the [K]orthogonal relationship between the twist of compliance and the twist of freedom, Db [K] Dc = (Db, Dc = 0, (2.23) which, after dividing throughout by nonzero rotation angles, 6bb and 64c, states clearly that the two points B and C are [K]orthogonal: Sb [K] Sc = ( Sb Sc ) = 0. (2.24) The concept of twist decomposition is extended by considering point B to be an element of the linear span of two points P and Q that lie on $a. In other words, analytically, the twist coordinates of B can be written as the linear combination, Db = bb Sb = 6 p Sp + 6q Sq, (2.25) where Sp and Sq are normalized coordinates of P and Q. Substituting (2.25) into (2.23) yields that, for general rotations 6bp and b6q, both points P and Q are [K] orthogonal to C. It can be said then that P and Q define a system of points, whose [K]orthogonal complement is C. Because C is the [K]orthogonal complement of P and Q and because it shares a working relationship with a constraint force acting along $a, it cannot lie on $a. This fulfills the concept of twist decomposition, since now the twist coordinates Dg of a general point G may be written, Dg = 6bg Sg = b6p Sp + b6q Sq + 6c Sc, (2.26) where the three linearly independent points P, Q, and C are used to span all points in the plane and represent all possible motions of the platform. Via the [K] orthogonal decomposition, the general platform twist is decomposed into a gripper twist (Db = 6p Sp + bjq Sq) and a constraintforceerror twist correction (Dc = 6Sc Sc), and this is illustrated in Figure 2.4 with the line $g. Kinestatic Control of the mechanism in Figure 2.4 is accomplished by an extension of Equation (2.8). It follows that the control law may take one of the following forms: Dg = Gb Db + Gc Dc, Dg = Gb Db Gc [K] 1 a, or Dg = Gb (6 pp S + 6q Sq) Gc Sfa [K] 1 Sa, (2.27) where [K] depends on the robot, Sp, Sq, and a on its task, and where Gb and Gc are dimensionless gains for position and force errors. The scalars bSp and 6q are errors in the rotations of the gripper about points P and Q, while the scalar 6fa is an error in the constraint force. Comparing (2.27) with Figure 2.3 shows that [Kf] = Gc [K] 1. (See Hennessey [1986] for an alternative planar algorithm that controls the displacement of the gripper in addition to the compliance levels of an iRCC which is attached to a PUMA robot.) Clearly, there are other ways of constraining the gripper of Figure 2.4, whether by just moving $a or by changing the kind of constraint altogether. An example of a different kind of constraint sees the incorporation of another point of contact and, hence, a second normal line of contact, $z. (See Figure 2.5.) This example leaves the gripper with a single freedom relative to ground, which is a rotation about the point of intersection (B) of the two lines $a and $z. Complementing this single twist of freedom must now be a pair of points (C1 and C2) denoting the twists of compliance. The pair corresponds (via [K]) with the lines $a and $z. The pair of points C1 and C2 themselves define another line, whose oo of points share a onetoone correspondence with the oo of lines that pass through B. It is this onetoone relationship defined by [K] that is used to control the constraint force passing through B. In summary, similar to (2.26), the Figure 2.5 coordinates of the general twist may be written as a linear combination of the normalized coordinates of the points B, C1, and C2. 2.2.2 Spatial Twists The purpose of this section is to establish twist decomposition for spatial applications involving a robot with six independent actuators. This is accomplished by i) defining twists and wrenches for sixdimensional spatial applications, ii) recognizing a taskindependent mapping of stiffness [K] for the robot, iii) investigating the taskdependent freedoms and constraints of the gripper relative to ground, which defines the twists of freedom and the wrenches of constraints, iv) determining the twists of compliance from the wrenches of constraints (via the inverse of [K]), and v) establishing that the twists of compliance and the twists of freedom are [K]orthogonal complements. In the spatial case, a twist is a rotation about a general unique line together with a translation in the same direction. It can also be thought of as a rotation about a line through the origin together with a general translation. Its homogeneous coordinates in either case are given by D=[6biT; boT]T, where 6~o is an infinitesimal displacement, and 6S is an infinitesimal rotation. (These coordinates are more conveniently expressed as D = [6io; 65], where the transposes are understood.) Its normalized coordinates, on the other hand, are given by S= [s'.; ST]T( = [soO; 9]), where l s'il = 1. The magnitude 65 of a twist is that scalar which scales all six normalized coordinates S to reproduce the homogeneous coordinates D of the twist (1D = 6 S). Exactly three kinds of twists exist. One body may move relative to another by a general twist, which is a rotation about a line together with a translation in the same direction. Analytically, this case is true when bXio b6 Z 0, where the pitch of the twist is given by h = s' so. A pure rotation is the case whenever Xo0 = 0 and 1165 0. Finally, a pure translation is the case whenever 51 = 0, for which twist coordinates may not be normalized (because I sll = 0). Accordingly, twist coordinates for translations are normalized differently, S= [so; 0], where nowll s'o = 1. Similarly in the spatial case, a wrench is a force acting along a general unique line together with a couple in the same direction. It can also be thought of as a force acting along a line through the origin together with a general couple. Its T T homogeneous coordinates in either case are given by i = fTT; mi ]T, where f is the force and riio is the moment. (This can also be more conveniently expressed as = [f; rio], where the transposes are understood.) Its normalized coordinates, on the other hand, are given by s = ['T; g ]T( = [g; o0]), where II gll = 1. The wrench magnitude, f, is that scalar which scales all six normalized coordinates A to reproduce the coordinates of the wrench r ( g = f s). A wrench increment (or a small change in a wrench) is defined by a wrench of small magnitude, e. g. Sf, which means w = [Sf; 61bio]. Exactly three kinds of wrenches exist. The resultant of the forces that one body applies to another may be a general wrench, which is a force acting along a line together with a couple in the same direction. Analytically, this case is true when f ri o 0, where the pitch of the wrench is given by h = So. A force is the case whenever f rio = 0 and II f 10 O. Finally, a couple is the case whenever fI =0, for which wrench coordinates may not be normalized (because II sl = 0). Accordingly, wrench coordinates for couples are normalized differently, i = [0; So], where nowi s'oII = 1. With twists and wrenches established for spatial applications, consider now that the spatial stiffness of the robot is known and that it provides a task independent relationship between twist and wrench, = [K] D, (2.28) where the magnitude of is small, and where now [K] is a 6x6 positivedefinite matrix denoting the spatial spring. Equation (2.28) relates the twist of the gripper (relative to the base of the spatial spring) to the change in wrench that the environment applies to the gripper. Equation (2.28) is considered to be a natural extension of (2.4) and (2.17). (See Chapter 1 and Equation (1.4).) Natural sixdimensional extensions of (2.9) and (2.22) provide the means to establish the general relationship between freedoms and constraints existing between a gripper and its environment: Db a = ob f a + b roa = 0, (2.29) where Db = [6xob; 6boi are coordinates of a twist of freedom and where Wa = [a; moa] are coordinates of a constraint wrench. Now, (2.22) is geometrically an incidence relation, specifying that a point lies on a line or a line contains a point. By analogy, (2.29) specifies, for example, that a line of a rotation must intersect a line of a force. In general, (2.29) states that a constraint wrench does no work. Clearly, (2.29) is valid regardless of the magnitude of the constraint wrench, and it may be finite (f) or incremental (6f). It is important to recognize that the sum of the number of freedoms and the number of constraints for the gripper relative to ground is six in a spatial application. (It may be recalled that this sum was two for the example in Figure 2.1 and three for the examples in Figures 2.4 and 2.5.) This means that when, for instance, four independent constraints are active, then two freedoms remain. In such a case, four sets of normalized coordinates si, i = 1,...,4, are used to denote the wrenches of constraints, while two sets of normalized coordinates S, j = 1,2, are used to denote the twists of freedom. All linear combinations of these two bases in conjunction with (2.29) mandate that ST sA = 0, for any i taken with either j. This essentially establishes vector spaces for both the twists of freedom and the wrenches of constraint. Recognizing that via (2.28) there exists for every wrench of constraint a unique twist essentially proves the existence of the twists of compliance. This can be expressed as *a = [K] Dc, (2.30) which to a sign change is an extension of (2.21). Because [K] has been specified as positive definite, which mandates that the constraint wrench with coordinates *a share a working relationship with the twist having coordinates Dc, it can be categorically stated that such a twist is a nonfreedom. Consequently, the twists of compliance vector space, whose normalized coordinates Si, i = 1,...,4, are obtained via an inverse mapping of (2.28), Ai Si = [K] 1 (i (2.31) for i = 1,...,4, where Ai is a scalar having dimensions L/F. (Refer to the example in Figure 2.1 where 1/6fn d = 0.1414cm/kg) Linear combinations of the Ic2 gure ..0 six linearly independent sets of twist coordinates, Sj, j = 1, 2 and Si, i = 1, ..., 4, span the coordinates of all twists. Substituting (2.30) into (2.29) yields the [K]orthogonal relationship between a twist of freedom and a twist of compliance. Succinctly, an extension of (2.10) and (2.23) is Db [K] Dc =( Db, c )= 0, (2.32) which is valid for all )c taken with any Db. Note that twists of freedom are on the left, while twists of compliance are on the right. This bookkeeping allows for asymmetric [K]s and is necessary for the general case of Kinestatic Control. Reconsider (2.30), where it was desired to obtain the twist of compliance (Dc) that was the only restricted twist capable of properly changing a given wrench of constraint (wa). Obtaining )c from (2.30) is not contingent on [K] being symmetric but rather only on its symmetric part ([K] + [K]T)/2 being positivedefinite. (That no real twist of freedom can be self [K]orthogonal, viz. D)b [K] Db = 0, ensures that no intersection exists between the two twist systems.) Examples thus far have been limited to symmetric stiffness matrices. In the next section, the theory is implemented employing a real empirically determined stiffness matrix that is not symmetric. 2.3 Implementing Kinestatic Control This section documents the sixdimensional implementation of this theory on a robot system comprising a modified General Electric P60 robot, two 386PC computers, three Creonics8 motion controller cards, a Lord ATI Corp. model 15/50 force/torque sensor, and a homemade uninstrumented compliant device. (See Figure 2.6.) Essentially, one of the computers (ROBOT) was employed to control the fineposition displacement of the robot, while the other (KINESTATIC) was employed to generate the twist commands of the endeffector platform. sCreonics Inc., Lebanon, NH, and Lord ATI Corp., Inc, Raleigh, NC. Figure 2.6 (Communications between the two asynchronously operating PCs was over Ethernet and consisted of a endeffector platform twist command sent one way and an actual position and orientation of the endeffector sent the other.) The end result of this assemblage of hardware and software provided a convenient test bed upon which to verify the theory. A liberal amount of compliance was introduced into the endeffector of the robot by the compliant device.9 Figure 2.6 illustrates this and the force/torque sensor that constitutes the last link of the robot. The arrangement robot force/torque sensorcompliant devicegripper is essentially a sixdimensional experimental extension of the slider actuatorsplatformspringswheel arrangement shown in Figure 2.1 and the robotplatformcompliant devicegripper arrangement shown in Figure 2.4. Therefore, with the force/torque sensor constituting the "platform" that was connected to ground by six nonbackdrivable actuators, it remained to twist it relative to ground (with six degreesoffreedom) in order to simultaneously control the constraint wrenches and twist freedoms of the gripper. For the implementation, the "gripper" was a three inch long bar of aluminum square stock, drilled and tapped. (See Figure 2.6 which shows the gripper bolted to a large immovable table.) 2.3.1 Empirical Determination of [K] Prior to simultaneously controlling force and displacement, it was necessary to first measure the mapping of stiffness for the robot, which established the relationship given by (2.28). This was done while the robot was in the 9Other researchers have installed compliance into their endeffectors: Whitney [1982], parts assembling aid, experimentally verified; Roberts, et al. 1985, fast responding one degreeoffreedom, experimentally verified; Xu and Paul 1988], for reasons of stability; Goswami, et al. [1990], programmable, task dependent stiffness and damping. configuration shown in Figure 2.6, where the gripper was bolted to the table. During the measurement the force/torque sensor was used to record the data on the forces/torques (wrenches) applied to the endeffector by the table. The six actuator encoders were used, and their sum total effect was representative of the twist of the force/torque sensor relative to ground, and hence of the grounded gripper relative to the force/torque sensor. From (2.28), it was clear that a minimum of six of these twists were necessary to measure [K], provided they were independent. Because the compliance of the serving robot was considered very low compared to that of the compliant device connected in series, the deflections of the robot were not measured during the determination of [K]. The four factors that were considered to have influenced the measurement of robot stiffness are given by the following: i) force/torque sensor (wrench measurement) resolution, ii) encoder (twist measurement) resolution, iii) linearity, and iv) robot configuration. Not only was the resolution in the individual coordinates considered, (fx for instance was 0.02 kg, and mox was 0.02 kgcm), but it was also necessary to consider the resolution in obtaining direction and location information from the measured wrench and twist coordinates. Therefore, one must displace the robot (command it to move) significantly enough to obtain a measurable (and more accurate) D, viz. it is necessary to accurately locate the twist together with its direction, pitch, and magnitude. This commanded twist must provide, through a reasonable amount of compliance, an accurate measure of wrench increment (*), viz. an accurate determination of its location, direction, pitch, and magnitude. It is important to note that the robot was not given too large a displacement (twist) for which the matrix [K] would lose its linearity. Such a set of six independent large twists taken together with their corresponding six wrench increments would yield a matrix [K] that would not map correctly a linear combination of the original six twists. In order to convince the investigator that the compliance of the robot was negligible, the robot configuration was changed, and measurements for [K] were repeated. A stiffness matrix [K] for each of two different robot configurations is presented here. Initially, a Remote CenterofCompliance (Lord ATI Corp. RCC) device was installed. However, this device was too stiff for axial displacements and cocking rotations.10 In other words, because the force/torque sensor tended to saturate, the robot could not be displaced far enough in these directions to obtain an accurate and repeatable measure of D. A second effort resulted in a spatial spring that was far too compliant, which meant that sufficient forces/torques could not be applied to the spring. A third and final assemblage (three compression springs each clamped at both ends) is shown in Figure 2.6. This yielded the repeatable and manageable stiffness matrix: 3.140 0.168 0.344 1.051 34.898 0.083 0.197 3.439 0.052 31.914 0.783 0.057 0.295 0.366 11.194 5.049 1.159 0.093 [K] = 1.381 28.511 2.082 394.018 5.979 2.235 25.660 1.342 2.008 2.243 377.047 5.944 0.959 0.087 0.073 8.484 8.377 76.698 10The RCC was not designed for force control, but rather it was designed to avoid it. The upperleft 3x3 has units of kg/cm, the lowerright 3x3 has units of kgcm, and the other two 3x3s have units of kg. It should be noted that [K] is asymmetric and that its symmetric part ([K] + [K]T)/2 is positivedefinite. The coordinate system used to express [K] was located at the center of the force/torque sensor. The zaxis was coaxial with the sixth joint of the robot, and the xaxis defined the reference for the last joint of the robot. (This coordinate system is referenced by the label F.) This choice of representation proved to be the most convenient, but another one could have just as well been used to establish a different representation, [K'], where [K'] = [E]T[K][E], and the 6x6 matrix [E] represents the transformation of twist coordinates. (In other words, the same twist needs a pair of twist coordinates b and D', one set for each coordinate system. Then, ) = [E] D'.) While the gripper was grounded and the compliant device was in an unloaded configuration, the matrix [K] was measured by sequentially moving the six individual actuators of the robot, and recording the changes in the forces and torques (wrench increment) applied to the endeffector. In other words, the six independent twists chosen were the individual actuator displacements themselves. (This choice ensured the directions and locations of the respective twists, while at the same time it enabled easy monitoring of twist magnitude.) Twists were referenced so to describe the gripper's motion relative to the platform (force/torque sensor). The following steps were taken to find [K] in terms of F: i) While holding the last five actuators fixed, the first actuator was moved 133 encoder edges, which corresponded to 0.25' degrees of the first joint. The twist and wrench increment were expressed in terms of F and recorded. The actuator was then returned to its original location, and the same command was given again. This process was performed ten times. The sum of the twist data was stored in D1, and the sum of the wrench data was stored in *1. ii) Step (i) was repeated for joints 2 through 6 in turn, and this generated the twist data iD and wrench data wi, (i = 2, ..., 6). The following summarizes the displacements commanded to the actuators: Actuator Command 1 133 4X encoder edges 2 230 3 139 4 277 5 185 6 150 Corresponding Joint Command 0.25 0.25 0.15 0.5 0.5 0.5 iii) The wrench data, *i, *.*' 6 was assembled as columns in a 6x6 matrix [W], and the twist data, )1, ..., D6 was likewise assembled as the columns of a 6x6 matrix [D]. From (2.28), these matrices are related [W] = [K] [D], and therefore [K] was determined by inverting [D]. iv) In order to check for linearity, twists that were not used to determine [K] were commanded, and the actual measured wrench increments were compared to those computed using [K]. The twists commanded were in fact the six cardinal twists of F. The endeffector A (force/torque sensor) was successively given small displacements in the x, y, and z directions, and then successive small rotations about the x, y, and z axes. These twists effected six wrench increments that corresponded respectively to scalar multiples of the six columns of [K]. v) In order to investigate the dependence of robot configuration on [K], the last joint was rotated 90, and steps (i) (iii) were repeated. This represented a significant change in the robot configuration as seen from the force/torque sensor and spatial spring, since the first five joints all moved relative to them. A stiffness matrix that was measured in the second robot configuration was 3.708 0.175 0.282 0.653 28.950 0.000 0.094 3.167 0.190 35.810 3.678 0.129 0.310 0.095 11.298 2.568 4.686 0.001 [K] = 1.841 26.456 0.373 385.484 2.320 8.246 30.373 1.016 0.751 2.486 352.869 9.167 0.791 0.181 0.046 0.949 13.464 76.315 where the units are the same as before. Finally, the following quantifies the repeatability of [K] (in terms of its 3x3 submatrices) by listing the average differences between all empirically determined [K]s. The first measure quantifies repeatability for [K]s measured while the robot remained in one of the two configurations. The second measure quantifies repeatability for [K]s measured while the robot was in either of the two configurations. 3x3 SubMatrix1 First Measure Second Measure upperleft 1% 8% lowerleft 1% 8% upperright 5% 13% lowerright 4% 7% 2.3.2 Implementation of 6 DOC Wrench Control Six degreeofconstraint wrench control was implemented on the experimental apparatus illustrated in Figure 2.6. A desired wrench was commanded to the KINESTATIC computer, which generated a corrective twist (Dc = [6Xoc; 6 c]) based on the control law, 1 c = G [K] 1 (2.33) where = [fx, fy, fz; mx, my, mz]T expressed in F is the difference between the desired and actual wrenches. (The scalar gain G was 0.03.) Equation (2.33) was performed approximately every 100 milliseconds, and each time it generated a new twist command. Before commanding Dc, it was necessary to first transform it into D = [6bo; 68], where 6xo = [R3] 6xoc and 6~ = [R3] 6 c, (2.34) and where [R3] is a 3x3 rotation matrix. The columns of [R3] are the direction cosines of the coordinate axes of F expressed in terms of a coordinate system G. The origins of F and 6 are the same, but the coordinate axes of G are parallel to "The values given for the upperleft 3x3 matrix are valid for elements in the 3 12 kg/cm range. The values given for the lowerleft and upperright 3x3 matrices are valid for elements with a magnitude in the 25 40 kg range, and the values given for the lowerright 3x3 matrix are valid for elements in the 75 400 kgcm range. those of the grounded system located at the shoulder of the GE Robot. (The coordinate system at the shoulder of the robot is labeled as R.) The result of (2.34) was communicated to the ROBOT computer, which controlled the position and orientation of the endeffector (force/torque sensor). The ROBOT computer performed its reverse and forward displacement kinematics every 20 milliseconds, and each time it communicated the results of the forward displacement analysis to the KINESTATIC computer. Figure 2.7 illustrates the response to a step input of i. = [0, 1 kg, 4 kg; 3 kgcm, 2 kgcm, 1 kgcm]T, given in terms of F. It is interesting to examine the response shown in Figure 2.7. This was a typical response the largest errors being in mx and my. This is because these coordinates measured torques in the x and y directions that, taken together with their corresponding twists, constituted the stiffest twistwrench combinations. (The coordinates of these twists are respectively the fourth and fifth columns of [K] 1.) Consequently, small errors in these twists were magnified as shown in Figure 2.7. The investigator realized that placing bounds on these coordinates was necessary. For instance, 8 kgcm 5 kg and used in (2.33) was no longer accurate when the compliant device was under loads outside of these bounds. (These bounds also affected where a controlled force could be located. For example, a 1 kg force in the zdirection could not be accurately controlled if it had more than an 8 cm moment about the xaxis.) A further examination of Figure 2.7 indicates that the lags were the same for all coordinates. (This was typical in the response plots of other, different 0 S0 N OE I  IL \ T vT '0 0o o o o o o o o in v mi w1 '4 0 01 r= Lrr r.1r  O 0  P o" 05 0 E O 0 0 0 0 0 0 0 0   0    u I 05 0 a) S0 OE IL t.i F0 F" f". ",, 1," Y  "...w   0O 0 0 0 0 0 0 00 If r IO J _ 0  )  05 01 61 N w r 0 0 0 0 0 1 (U 0 01 "T 0 0 0 0o L s X 0 SI 0 0 0 "o 0  U o OE  0lr I 0 VI I 0 0 " OE Ie ...., ., 1 ......r..t... 0 0 0 0 0 U 0 01 I ________ ii I wrench step changes.) It is important to recognize this, because this proves the principle that at each instant the wrench error is nulled by the best corrective twist. In other words, the wrench error coordinates are nulled at the same pace. 2.3.3 Implementation of 5DOC/1DOF Kinestatic Control Five degreeofconstraint/one degreeoffreedom kinestatic control was implemented on the experimental apparatus illustrated in Figure 2.8. The gripper was bolted to a slider (prismatic) joint that was in turn bolted to the level table. Relative to the slider, the gripper was positioned and oriented in a general way. The twist of freedom of the gripper was D)b = [ 1, 0, 0; 0, 0, 0]T, which was a translation in the negative xdirection of G. (It was also a translation in the negative xdirection of R, the grounded coordinate system located at the shoulder of the robot). The position of the slider was p, where 0
