UFDC Home  myUFDC Home  Help 



Full Text  
PAGE 1 1 MODELING AND CONTROL OF A PARALLEL ROBOTIC MANIPULATOR IN CONTACT WITH A HARD SURFACE By ALLEN D. NEASE, JR 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 201 2 PAGE 2 2 201 2 A llen D. Nease, J r. PAGE 3 3 To my family: Pat, Danny (deceased), Wyatt, and Katharine for the sacrifices that they made so that I could attend graduate school at the University of Florida, a nd to Dr. Joe Duffy for his support and encouragement. PAGE 4 4 ACKNOWLEDGMENTS I would like to thank Dr. Carl Crane, Dr. Mike Griffis and Dr. Joe Duffy for the original design of the Special 66 and the forward and reverse solutions. Dr. Crane deserves much credit for his encouragement, his patience, and his guidance throughout the process of executing this research and writing this document. Dr. Griffis provided invaluable advice and editorial assistance. Dr. Duffy was the mento r for all involved in this project, and his contributions will be forever remembered and appreciated. My thanks to Shannon Ridgeway for developing the original mechanical design of the 66 platform, the design of the metrology frame based on the Special 66 configuration, the design of the special compliant spring, and the kinematic analysis of the platform I would like to thank my committee for their hard work and support as well Special thanks to Mr. Walt Waltz, Dr. Phil lip Adsit, Dr. Arthur Morton, Mr. John Waltz, Mr. Chip Young, Mr. Darrell Stepp and the entire crew of the robotics prototype shop at AFRL Tyndall for their support and hard work in order for the project to succeed. Dr s. Griffis and Adsit devoted many hours to reviewing, advising, and providing timely technical advice for the work described herein. Dr. Al Rhodes provided me the freedom to pursue this research at AFRL, and I appreciate his confidence and commitment PAGE 5 5 TABLE OF CONTENTS page ACKNOWLEDGMENTS .................................................................................................. 4 LIST OF TABLES ............................................................................................................ 8 LIST OF FIGURES .......................................................................................................... 9 LIST OF ABBREVIATIONS ........................................................................................... 15 ABSTRACT ................................................................................................................... 16 CHAPTER 1 INTRODUCTION .................................................................................................... 18 Objectives ............................................................................................................... 20 High Level Assembly Task Supervisor ............................................................. 21 Mid Level Explicit Kinestatic Controller ............................................................ 22 Low Level Force Controller for a Hydraulically Actuated Compliant Leg .......... 23 Implementation ....................................................................................................... 2 4 Role of Screw Theory ............................................................................................. 25 Significance and Contributions ............................................................................... 26 2 LITERATURE REVIEW .......................................................................................... 30 Overview ................................................................................................................. 30 Contact ................................................................................................................... 31 Kinestatic Control and Screw Theory ...................................................................... 34 Force Control and Associated Control Theory ........................................................ 35 Hydraulic Systems .................................................................................................. 41 Parallel Robotic Mechanisms .................................................................................. 43 3 MO DELING CONTACT: THE ASSEMBLY TASK SUPERVISOR .......................... 45 Overview ................................................................................................................. 45 Wrenches of Constraint and Twists of Freedom ..................................................... 50 Contact Examples for Candidate Assembly Task ................................................... 59 Case 0: Free Space Motion .............................................................................. 60 Case 1a: Conventional Single Point Contact at Point F ................................... 61 Case 1b: Enhanced Point Contact at Point F ................................................... 63 Case 2a: Conventional Single Point Contact at Point G ................................... 64 Case 2b: Enhanced Point Contact at Point G ................................................... 65 Case 3: Two Point Contact ............................................................................... 65 Case 4: Fully Constrained Contact ................................................................... 66 Wrench Complements ............................................................................................ 67 PAGE 6 6 Heuristic Approach to the Assembly Task Supervisor ............................................ 69 Implementation: Candidate Assembly Task ............................................................ 72 4 EXPLICIT KINESTATIC CONTROLLER ................................................................ 85 Overview ................................................................................................................. 85 Resultant Wrench: A Function of Leg Forces ......................................................... 88 Reverse Analysis Summary .................................................................................... 89 Gravity Wrench Compensation ............................................................................... 91 Relating Twist to Leg Displacements ...................................................................... 92 Small Changes, Infinitesimal Changes, and Differential Calculus ........................... 95 Stiffness: Geometry and Physics ............................................................................ 95 Force Charges and Wrench Charges ................................................................... 106 Correcting for Constraint Wrench Error ................................................................. 112 Deflection Wrench: Correcting for Payload Location Error .................................... 134 Explicit Kinestatic Control: Combining Wrench Components to Generate Leg Force Setpoints ................................................................................................. 135 5 SINGLE LEG: FORCE CONTROL AND SYSTEM MODEL ................................. 152 Overview ............................................................................................................... 152 Physical Description o f Actuation Device .............................................................. 154 System Plant Model and Transfer Function .......................................................... 159 Minor Loop Analysis ............................................................................................. 171 Major Loop A nalysis ............................................................................................. 173 Actual Versus Modeled Controller ........................................................................ 180 Summary .............................................................................................................. 184 6 SYSTEM INTEGRATION AND IMPLEMENTATION ............................................ 207 Overview ............................................................................................................... 207 Leg Force Controller ............................................................................................. 210 Explicit Kinestatic Controller ................................................................................. 212 Question: Does the System Move While Controlling the Constraint Wrench? 214 Constraint Plus Deflecti on Translation Tests ............................................... 220 Constraint Plus Deflection Rotation Tests ................................................... 224 Assembly Task Supervisor ................................................................................... 226 Heuristic Test for Case 1 ................................................................................ 228 Heuristic Test for Case 2 ................................................................................ 230 Heuristic Test for Case 3 ................................................................................ 232 Modification to Heuristic ........................................................................................ 233 Summary .............................................................................................................. 234 7 SUMMARY AND CONCLUSIONS ........................................................................ 274 Summar y .............................................................................................................. 274 Assembly Task Supervisor ............................................................................. 274 Explicit Kinestatic Controller ........................................................................... 275 PAGE 7 7 Leg Force Controller ....................................................................................... 278 Conclusions .......................................................................................................... 280 Assembly Task Supervisor ............................................................................. 281 Explicit Kinestatic Controller ........................................................................... 281 Leg Force Controller ....................................................................................... 282 LIST OF RE FERENCES ............................................................................................. 285 BIOGRAPHICAL SKETCH .......................................................................................... 288 PAGE 8 8 LIST OF TABLES Table P age 3 1 Contents of the various system matrices for the determinator approach ......... 83 3 2 Contents of the various system matrices for the elliptic determinator approach ............................................................................................................ 84 4 1 Parameters associated with an actuation leg ................................................... 150 4 2 Summary of matrices ........................................................................................ 150 4 3 Typical usage for wrench notation .................................................................... 151 PAGE 9 9 LIST OF FIGURES Figure P age 1 1 Photograph of w eapon and a ircraft wing r eal w orld p ayload and t arget .......... 27 1 2 Planar example of an assembly t ask .................................................................. 28 1 3 Illustration of actuator and metrology p latforms .................................................. 29 3 1 Assembly task supervisor ................................................................................... 78 3 2 Contact states ..................................................................................................... 79 3 3 Twist in the plane ................................................................................................ 80 3 4 Coordinate system for payload ........................................................................... 80 3 5 Free space motion .............................................................................................. 81 3 6 Single point contact at p oint F ............................................................................ 82 3 7 Single point contact at p oint G ............................................................................ 82 3 8 Two point contact ............................................................................................... 82 4 1 Explicit kinestatic controller .............................................................................. 144 4 2 Actuation frame with independent metrology frame .......................................... 144 4 3 Resultant wrench and free body diagram of payload ........................................ 145 4 4 Payload frame with applied wrenches .............................................................. 146 4 5 Rotation of payload by small angle about the instantaneous center of rotation P .......................................................................................................... 147 4 6 Hydraulic actuation device with passive compliant spring ................................ 148 4 7 Explicit k i nestatic c ontrol for a parallel manipulator .......................................... 149 5 1 Leg f orce c ontroller ........................................................................................... 185 5 2 Hydraulic circuit for hydraulic cylinder actuation device .................................... 185 5 3 Photo of single leg test apparatus .................................................................... 186 5 4 Photo of singleleg compliant device ................................................................ 187 PAGE 10 10 5 5 Compliant device installed in line with linear actuator ...................................... 187 5 6 Applied load vs displacement of compliance spring #1 ................................... 188 5 7 Applied load vs displacement of compliance spring #2 ................................... 188 5 8 Applied load vs displacement of compliance spring #3 ................................... 189 5 9 Hydraulic circuit for hydraulic power unit .......................................................... 189 5 10 System plant block diagram for singleleg actuation device ............................. 190 5 11 Servo valve amplifier modeled response .......................................................... 191 5 12 Servo valve modeled response to different servov alve gains ......................... 191 5 13 Hydraulic cylinder modeled response to different payload weights .................. 192 5 14 modeled response ..................................................................................... 192 5 15 Velocity of single leg at = 500 ................................................................ 193 5 16 Velocity of single leg at = 1000 .............................................................. 193 5 17 Velocity of single leg at = 2700 .............................................................. 194 5 18 Unloaded maximum velocity/maximum flow rate vs system pressure ............. 194 5 19 measured vs modeled response ............................................................... 195 5 20 Compliant spring modeled response ................................................................ 195 5 21 modeled response .................................................................................... 196 5 22 measured vs modeled response ( = 500 ) ....................................... 196 5 23 Minor loop block diagram for singleleg actuation device ................................. 197 5 24 System plant (c ritical) modeled response ......................................................... 197 5 25 Minor loop modeled response to changes in feedback gains .................... 198 5 26 Major loop block diagram for singleleg actuation device ................................. 198 5 27 Controller modeled response to changes in with set gain ....................... 199 5 28 Controller modeled response to changes in with larger gain ................... 199 5 29 Major loop block diagram for singleleg actuation dev ice with = 0 ............... 200 PAGE 11 11 5 30 Major loop block diagram for singleleg actuation device with = 0 ............... 201 5 31 Major loop modeled response for / with varying ................................... 202 5 32 Major loop modeled response for / with varying .................................... 202 5 33 Major loop modeled response for /( ) with varying ......................... 203 5 34 Forward part of major loop ( = ) ........................................ 203 5 35 Modeled forward critical loop ( = ) .................................... 204 5 36 Closed loop force control, = 1000 = 0 4 = 150 .................. 204 5 37 Step response to force command with D/A control value ................................. 205 5 38 Step response to force command zoomed in to show 0 5 of data .............. 205 5 39 Step response to force command with D/A control value ................................. 206 5 40 Step response to force command zoomed in to show 0 5 data .................. 206 6 1 System integration and implementation ............................................................ 235 6 2 Planar platform response to force step input for l eg 1 ...................................... 236 6 3 Planar platform response to force step input for l eg 2 ...................................... 236 6 4 Planar platform response to force step input for l eg 3 ...................................... 237 6 5 Planar platform response to force step input for l egs 1, 2, 3 ............................ 237 6 6 Planar platform response to force step input for l egs 1, 2, 3 ............................ 238 6 7 Planar platform response to force step input for l egs 1, 2, 3 ............................ 238 6 8 Platform force response to external disturbance force ..................................... 239 6 9 Platform position response to external disturbance force ................................. 239 6 10 Case 1 showing a little drift in x direction, and slight angular change in payload angle ................................................................................................... 240 6 11 Case 1 magnitude and angle of commanded and actual constraint wrench ..... 240 6 12 Case 1 showing slight error on X component of constraint wrench and almost no error on Y component of constraint wrench ................................................. 241 6 13 Case 1 showing actual vs commanded leg forces ........................................... 241 PAGE 12 12 6 14 Case 1 showing leg forces for first 1 0 ........................................................ 242 6 15 Case 1 showing the error between the actual and commanded leg forces ...... 242 6 16 Case 1 position and angle change of point F ................................................... 243 6 17 Case 1 magnitude and angle of commanded and actual constraint wrench ..... 243 6 18 Case 1 leg force errors ..................................................................................... 244 6 19 Case 1 position and angle change of point F with disturbance force applied at approximately 2, 4, 38, 41, 43, and 48 seconds ............................................... 244 6 20 Case 1 magnitude and angle of commanded and actual constraint wrench during disturbance force application ................................................................. 245 6 21 Case 1 leg force errors during disturbance force application ............................ 245 6 22 Case 2 position changes of point G and angular change of payload axis ........ 246 6 23 Case 2 magnitude and angle of commanded and actual constraint wrench ..... 246 6 24 Case 2 X, Y components of constraint wrench ................................................. 247 6 25 Case 2 perpendicular distance to constraint wrench ........................................ 247 6 26 Case 2 leg force errors ..................................................................................... 248 6 27 Case 3 showing no movement of point F or rotation ........................................ 248 6 28 Case 3 magnitude and angle of commanded and actual constraint wrench ..... 249 6 29 Case 3 perpendicular distance to constraint wrench ........................................ 249 6 30 Case 3 leg force errors showing minimal magnitude of force errors ................. 250 6 31 Case 3 showing almost no movement of point F or rotation ............................. 250 6 32 Case 3 showing the magnitude and angle of the commanded and actual constraint wrench ............................................................................................. 251 6 33 Case 3 showing perpendicular distance to constraint wrench .......................... 251 6 34 Case 3 actual vs commanded leg forces ......................................................... 252 6 35 Case 3 showing leg forces for first 1 0 ........................................................ 252 6 36 Case 3 showing minimal force errors ............................................................... 253 PAGE 13 13 6 37 Case 1 showing position of point F after contact and during translation in direction perpendicular to contact normal ......................................................... 253 6 38 Case 1 showing magnitude and angle of command and actual constraint wrench throughout the sequence of periods ..................................................... 254 6 39 Case 1 showing leg force errors throughout the sequence of periods .............. 254 6 40 Case 2 showing position of point G after contact and during translation in direction perpendicular to contact normal ......................................................... 255 6 41 Case 2 showing magnitude and angle of commanded and actual constraint wrench during translation perpendicular to contact normal .............................. 255 6 42 Case 2 showing perpendicular distance to constraint wrench after contact and during translation perpendicular to contact normal .................................... 256 6 43 Case 2 showing position of point G after contact and during translation perpendicular to contact normal ....................................................................... 256 6 44 Case 2 showing magnitude and angle of commanded and actual constraint wrench during translation perpendicular to contact normal .............................. 257 6 45 Case 2 showing perpendicular distance to constraint wrench after contact and during translation perpendicular to contact normal .................................... 257 6 46 Case 3 showing position of point F after contact and during perpendicular to contact normal .................................................................................................. 258 6 47 Case 3 showing magnitude and angle of commanded and actual constraint wrench .............................................................................................................. 258 6 48 Case 3 showing perpendicular distance to constraint wrench .......................... 259 6 49 Case 1 position change of poi nt F, rotate for 5 seconds and then turn off deflection and constraint control modes ........................................................... 259 6 50 Case 1 magnitude and angle of wrench command and actual constraint wrench .............................................................................................................. 260 6 51 Case 2 position change of point G, sense contact with G, pause 5 seconds, rotate 5 seconds, then turn deflection and constraint control off ....................... 260 6 52 Case 2 magnitude and angle of commanded and actual constraint wrench rotate until contact with point F ......................................................................... 261 6 53 Case 2 perpendicular distance to constraint force, contact with G, rotate 5 seconds ............................................................................................................ 261 PAGE 14 14 6 54 Case 1 complete run ........................................................................................ 262 6 55 Case 1 showing different modes of control and other events ........................... 263 6 56 Case 1 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence .......................................................... 264 6 57 Case 1 showing constraint wrench perpendicular distance throughout sequence .......................................................................................................... 264 6 58 Case 1 showing leg force actual and setpoints throughout sequence .............. 265 6 59 Case 1 showing leg force errors throughout sequence .................................... 265 6 60 Case 2 complete run ........................................................................................ 266 6 61 Case 2 showing different modes of control and other events ........................... 267 6 62 Case 2 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence .......................................................... 268 6 63 Case 2 showing perpendicular distance to constraint wrench throughout sequence .......................................................................................................... 268 6 64 Case 2 showing leg force actual and setpoints throughout sequence .............. 269 6 65 Case 2 showing leg force errors throughout sequence .................................... 269 6 66 Case 3 complete run ........................................................................................ 270 6 67 Case 3 showing different modes of control and other events ........................... 271 6 68 Case 3 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence .......................................................... 272 6 69 Case 3 showing perpendicular distance to constraint wrench throughout sequence .......................................................................................................... 272 6 70 Case 3 showing leg force actual and setpoints throughout sequence .............. 273 6 71 Case 3 showing leg force errors throughout sequence .................................... 273 PAGE 15 15 LIST OF ABBREVIATION S AF Air Force AFRL Air Force Research Laboratory ANG ( a b ) The angle between a and b DOC Degrees of constraint DOF Degrees of freedom EKC Explicit kinestatic control FC Force control GRAV Gravity term IKC Implicit kinestatic c ontrol KC Kinestatic control LVDT Linear v ariable displacement trans former MAINT Maintenance force MET frame Metrology frame Six leg Six DOF device THOLD Threshold value TOC Twist of compliance TOF Twist of freedom WOC Wrench of constraint WOD Wrench of deflection PAGE 16 16 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 MODELING AND CONTROL OF A PARALLEL ROBOTIC MANIPULATOR IN CONTACT WITH A HARD SURFACE By Allen D. Nease, Jr. May 2012 Chair: Carl Crane Major: Mechanical Engineering This research addressed autonomous weapon loading onto combat aircraft. A planar platform was used to study force control between a manipulator and a hard surface target. The objectives were to develop an assembly task supervisor, an explicit kinestatic controller, and a force controller. An assembly task supervisor was developed to handle highlevel contact control. The supervisor generated a contact model and described the instantaneous kinestatics between the payload and the target. A heuristic was developed to sequence the payload so that the payload would reach a fully constrained (assembly complete) location. The heuristic was successfully demonstrated, moving the platform that originated from multiple initial locations to the goal of fully constrained contact with the target. The explicit kinestatic controller was developed to provide midlevel control of the parallel manipulator. This controller calculated desired leg forces given the desired constraint wrench and desired twist of freedom interacting between the payload and the target. Two important questions relating to the explicit kinestatic controller were addressed If a correction for the constraint wrench was made, did the payload PAGE 17 17 move? and If a deflection wrench was applied, did the deflection wrench affect the control of the constraint wrench? The leg force controller was developed to provide low level force control of each leg individually. Initial research was performed on a singleleg hydraulic test device for the force control algorithm development. A detailed study of the singleleg device provided useful insight into the challenges facing the development of the force control algorithm. Tests were performed to address controller questions and they confirmed that the payload did not move when a correction for the constraint wrench was applied. Decoupling between the maintenance constraint wrench and the twist of freedom deflection wrench was observed and it was shown that correction for the deflection wrench did not influence the control of the constraint wrench. The results of the research showed that the heuristic developed, along with the three level hierarchical control system, was used to success fully control the hydraulically actuated, parallel planar mechanism for constrained motion between the manipulator (payload) and the environment (target). PAGE 18 18 CHAPTER 1 INTRODUCTION If we knew what we were doing, it wouldnt be called research. Albert Einstein, 19 27 Solvay Conference As the Air Force has searched for better, more automated, and safer methods for the hot turnaround of combat aircraft, the use of robotic capabilities has begun to play a much larger role in what are now primarily human operated systems. In the current f ighter/bomber pilots operational mode, the aircraft is brought back from a mission, park ed, and then left for the ground crews to perform the tasks of refueling and rearming. A future paradigm has the aircraft controlled by the pilot and driven into a pre designated, specifically marked/sensed area, and all of the turnaround functions performed robotically in minimum time with minimum use of manpower. Two problem areas that need to be addressed immediately include automated aircraft refueling and the loading of weapons onto aircraft. This work has addressed the beginnings of a method to use robotics to effect the transformation in the specific area of munitions loading, but it could easily be extended to hot refueling and other logistical functions required to support combat aircraft as well as cargo aircraft and unmanned aerial vehicles Within the robotics field of study, this research focused on simultaneous control of force and position. The accomplishment of these tasks involved the use of t he geometrical and mathematical tenets of Screw Theory (Ball 1900) In s crew theory terminology this is termed simultaneous wrench and twist control. Th is research was particularly concerned with controlling the interface between a payload and a target Th e study included t wo scenarios (or modes) as useful PAGE 19 19 adjuncts to the pertinent USAF mission The first scenario unconstrained, was when there was no contact between the payload and the target, so that the payload was completely free to move in space. The second scenario, partially constrained, implied that the payload touched the target in such a way that contact forces existed between the two bodies. In this case, payload movement relative to the target was partially constrained. The top platfo rm was d efined as the payload of the robotic manipulator plus the object to be moved into position onto the target. The second important definition was the target that the payload engaged. An example of a payload was a bomb to be loaded onto the aircraft and an example of a target was the wing of an aircraft. This study used the term assembly task i n a high level way to describe a desirable contact sequence. A t any given instant, the assembly task describe d numerically and geometrically how the payload should touch the target. The description specifie d maximum (or desired) nonworking contact forces as well as subsequent, necessary assembly displ acements of the payl oad relative to the target. S uch a nonworking contact force was termed a wrench of constraint (WOC) and such an assembly displacement wa s termed a twist of freedom (TOF). A heuristic guided the analysis for the assembly task and was developed to prov ide a set of guidelines or criteria that determined the movement of the payload in relation to the target. Chapter 3 provides a detailed explanation of the heuristic The research required that contact describe how two touching bodies apply constraint wrenches to each other as well as how they freely twist relative to one another The study of the geometry for this contact is termed kinestatics. PAGE 20 20 Certain assumptions we re made in order to implement kinestatic control theory. First, the payload actually start ed in the unconstrained contact mode. Secondly, payload and target geometry we r e known, but the target location (position and orientation) wa s not known. The details of contact between the payload and target we re not known a priori i.e. the system discover ed the contact mode during operation. Friction between the payload and target was minimized during design, and, for this effort, the interaction between these objects was considered frictionless. The robot carrying the payload wa s capable of force control in its actuators (or conventional position control as necessary). Force control of a given actuator did not affect force control in the other actuators (decoupling of force application). Finally, it wa s given that a means exist ed for the system to obtain the position and orientation of the payload as well as the current wrench acting on the payload. O bjectives The overarching motivation for this research was to develop a robotic system capable of loading a bomb (payload) onto a fighter jet wing (target ) A key requirement of such a system was to accomplish this assembly task without damaging the jet s wing or weapon due to excessive contact forces. As such, the fundamental objective of the research wa s to establish a theory for a multi level, hierarchical control system that wa s capable of determining the details of contact between a payload and its target and subsequently controlling the assembly sequence. The specific objectives of this research we re to : A. Develop an assembly t ask supervisor B. Develop an explicit kinestatic controller and C. Develop a force control ler for hydraulic ally actuated, compliant leg s. PAGE 21 21 High L evel A ssembly T ask S upervisor An assembly task supervisor was conceived to handle contact at the highest level. A key role was to generate a model of contact (Chapter 3) Suc h a model was generated in real time to describe the instantaneous kinestatics between the payload and the target. This established subspaces for the wrenches of constraint and the twists of freedom. A second key role was to use this model to smartly sequence through the assembly task. T he assembly task was based on the assumption that at initiation, an unconstrained contact configuration existed. The assembl y task dictated an initial dire ction of motion for the payload in search of contact with the target. The supervisor required the determination of the contact mode (unconstrained or partially constrained) When contact was detected, the system entered the partially constrained mode. W hen the partially constrained mode was determined, the supervisor quantif ied the nature of the contact by establishing the kind of contact and the location of the contact. This established the subspaces for wrenches of c onstraint and twists of freedom The supervisor then used this knowledge to discover what the current step was in the assembly task. Next, i t selected a desired wrench of constraint from all available wrenches of constraint and a desired twist of freedom from all available twists of free dom in such a way as to further progress in the assembly task. The supervisor updated this continuously in order to successfully sequence through the assembly task. A planar example was used to illustrate this concept and is shown in Figure 1 2 In order to complete this particular assembly task, the following steps were required: PAGE 22 22 1. Assume that the payload was not in contact with the target. Start movement of the payload in the Y direction. 2. Move to a first point of contact. Sense contact and establish the commanded contact force. 3. Rotate to line or two point contact, unless two point contact wa s made initially. Continue the task by m a intain ing 4. Translate (slide) into final position while maintaining contact force ( ). 5. The assembly task completed Note that in step 2, either point of contact may have been made initially and that distinction should be determined by the system M id L evel E xplicit K inestatic C ontroller An explicit kinestatic controller was conceived to implement the desired twist of freedom and wrench of constraint (Chapter 4). A key role of the controller was to determine what commands to send to the actuators. The approach taken here was an explicit control strategy that meant the actuators were commanded to achieve a given force. (This was in contrast to implicit force control that would have commanded the actuators to achieve a certain displacement ) T he controller generated two error wrenches. In order to control a wrench of constraint, the controller generated a constraint wrench error that was the difference between the command ed wrench of constraint and the actual sensed wrench of constraint. In or der to implement a desired twist of freedom, the controller derived a second error wrench that was uniquely related. A stiffness mapping of the robotic manipulator was used to establish the relationship between the second error wrench and the desired twis t of freedom. Effectively, the second error wrench, when applied, provided the PAGE 23 23 desired twist of freedom. Therefore, the second error wrench was dubbed the wrench of deflection The explicit kinestatic controller (EKC) summed the constraint wrench error with the d eflection wrench. The controller used this wrench and other applicable additional wrenches (for example gravity and offset) to generate a general wrench of command. This wrench ( using the geometry of the actuation device) was decomposed into desired individual leg actuator forces. Low L evel F orce C ontroller for a H y draulically A ctuated C ompliant L eg A force controller was conceived to provide the low level feedback control of one actuation device or leg (Chapter 5) The key role of the controller was to decouple feedback of individual actuation devices from the geometry specific to a platform A single actuation device or leg provided a test bed from which the feedback model and control law were developed. To faci litate control law development, a transfer function (TF) was acquired through testing on the single leg actuation device. Different methodologies were used to obtain several TFs and a comparison was made to determine the best TF. The force control algor ithm was developed and tested on the single leg actuation device. A known compliance was introduced into the single leg actuation device to facilitate the ability to effectively control force in the hard contact scenario. For actuating the leg, this research employed hydraulic power. Although there are other ways to power such a leg (for example: pneumatically, electromechanically, or using linear electrical actuators), none of these technologies satisfied the requirement for rigidity and high load carrying capacity without the attendant problems associated with electromechanical devices such as high heat buildup. Hydraulically powered PAGE 24 24 systems have thre e advantages : high power and force density interactions with the envir onment when compared to other power modes; fast response time; and the ability to maintain force indefinitely without the heating issues that plague electric drive systems. Hydraulics was chosen in spite of two apparent disadvantages: highly nonlinear an d inherently complex mathematical analysis, and being physically messy if a system failure occur r ed ( for example a leakage o r hose breakage) I mplementation The robotic manipulator used in this research relied on two parallel mechanisms, one for actuating (heavy lifting) and another for metrology (accurate location) and is discussed in Chapter 6 and shown in Figure 1 3 A parallel mechanism, as opposed to a serial mechanism, lent itself well to the accomplishment of several tasks, such as those required by the mission of combat aircraft turnaround. These key attributes of a general parallel mechanism were investigated: 1. Legs acting in parallel so one end of a given leg was connected to a base platform while the other end of that leg was connected to a top or moveable platform (payload). 2. Legs acting in parallel provided a heavy payload capacity making the parallel mechanism much more suitable for heavy lift applications s uch as loading ordnance onto an aircraft. 3. Legs acting in parallel meant errors accrued in parallel rather than in series, and the result was better accuracy. 4. Unmodeled compliance was minimized, e.g. joint and structural flexing. 5. The geometry of the parall el mechanism wa s and is very amenable to the use of screw theory for analysis, particularly for static wrench analysis. This in turn simplified the geometric, modeling, and control issues associated with the robotic manipulator. PAGE 25 25 The parallel mechanisms were employed for the above reasons in spite of the apparent disadvantages, which include limited range of motion and static singularities. A singularity may lead to a configuration in which the stiffness of the mechanism was lost. Several methods have bee n developed to avoid the singularity issue. The method used in this research was to plan the motion of the manipulator so as to avoid the singularity. The parallel mechanism used for metrology, the socalled metrology frame (MET frame), w ould have very ac curately measured the location, both position and orientation, of the payload. The data from the MET frame could have been used to assist in the control of the robotic manipulator. The MET frame data sets were designed specifically to calculate the Jacobian or [ ] matrix, and the stiffness mapping the [ ] matrix, of the actuator platform. The MET frame was designed for application to the 6 6 spatial mechanism and was not utilized in this planar mechanism research. R ole of Screw Theory It is important to note that this research relied heavily on screw theory. This was done for two primary reasons: 1. Simplifications from the general spatial case to simpler planar, spherical, etc cases could be made while maintaining validity and consistent terminology. 2. The kinestatics c ould be described even in the most complex, general spatial case using reciprocal screw systems to model the subspaces for the wrenches of constraint and the twists of freedom. One of the appealing aspects of using screw theory was that the g eometry facilitates convenient and meaningful simplifications from the general, spatial (six degreeof freedom) examples to more tangible, simpler, and more conveniently illustrated examples. In light of thi s, a planar parallel mechanism was chosen as the PAGE 26 26 actuator platform. The primary challenge addressed was that of partially constrained motion, and the planar examples used here (planar actuator platform and planar assembly task s were shown in Figure 1 2 ) provided a valuable and illustrative demonstration that was easily scalable to spatial examples. Successfully solving the challenges stated above using the planar examples provided the theoretical background for transition to the more general Six Leg or Stewart type platform. Reciprocal screw systems have a firm geometrical foundation, as witnessed in Ball ( 1900) Lipkin ( 1985) Griffis ( 1991) Crane ( 2010) Gr iffis ( 2009) and Hunt ( 197 8), and were applied to the study of partial constraint and kinestatics. Th e references identified the wrenches of constraint and the twists of freedom as reciprocal screw systems and further demonstrated how the many different cases exist ed in kinestatics. The results were found to be exhaustive but manageable. Significance and Contributions This research d eveloped the mathematical background for s imultaneous force and position control (wrench and twist control) of a parallel, hydraulic robotic manipulator. The mathematical tools that were used in the research were origina lly developed by Sir Robert S. Ball in his treatise on the Theory of Screws (Ball 1900). The mathematical develop ment was then prove n by experimentation with the actual c ontrol of a parallel, hydraulic robot in hard contact with a target This, in turn, p ro ved the viability of kinestatic control in effecting e xplicit (vs. i mplicit) kinestatic for ce control (EKC) The final goal of the research was to develop and prove that the mechanical and control methods described herein have form ed the foundation for automation of a critical PAGE 27 27 task to support the USAF ( w eapons loading and hot air craft turnaround) This goal was achieved. There is a great deal of additional research remaining regarding the planar manipulator. Other areas for consideration might be : human interface with the platform, i.e. manin the loop; and research on a vision based controller. The Air Force Research Laboratory has built a full six degree of freedom, heavy duty manipulator that will be transitioned to UF for further research. The possibilities for expanding the research on this platform are virtually limitless. Figure 1 1 Photograph of w eapon and a ircraft w ing r eal w orld p ayload and t arget. Photo courtesy of the United States Department of Defense. PAGE 28 28 Figure 1 2 Planar example of an a ssembly t ask. A) Starting configuration. B) Initial approach to target. C) Rotate to line contact (two point contact). D) Slide to fully constrained location. D ) Slide to fully constrained location A ) Starting configuration C ) Rotate to line contact (two point). B ) Initial approach to target Rotation about Z Axis PAGE 29 29 Figure 1 3 Illustration of a ctuator and m etrology p lat forms along with payload and t arget PAGE 30 30 CHAPTER 2 LITERATURE REVIEW Life is not easy for any of us. But what of that? We must have perseverance and above all confidence in ourselves. We must believe that we are gifted in something and that this thing must be obtained. Marie Curie, Nobel Prize winning physicist letter to her brother, 1894 O verview The literature review described documents that support ed this research in the following areas : C ontact, K inestatic C ontrol and S crew T heory, F orce C ontrol a n d As sociated Control Theory H ydraulic S ystems, and P arallel R obotic M echanisms. The five areas listed above formed the core of the material required to provide the necessary background information for the completion of this project. Constrained motion of a parallel robot moving from the unconstrained to partially constrained and then to the fully constrained mode employed the study and use of screw theory and the study of contact, that is hard, controlled contact against a rigid surfa ce (target) Force control was required in order to close the loop around the Explicit Kinestatic Controller (EKC) In addition to the force control algorithm developed, a great deal of study was devoted to other control techniques that leant themselves to force control of the parallel robotic manipulator. Since the device was hydraulically powered, the study of hydraulic systems and controls played a large role in the research as well Modeling of the system was an essential issue as well. Finally, t he inclusion of the study of parallel robotic devices was required as the basis for the design, kinematics, and dynamics of the manipulator arrangement PAGE 31 3 1 C ontact Ayre s (1962) outline on matrices provided background for fundamental matrix theory used during this project. There were probably more current editions of this book, but the basics of matrix manipulation have n o t changed since Heisenb e rg rediscovered the rules of matrix multiplication in 1921 in his quest to establish quantum mechanics as the basis for quantum theory. Jennings text (1977) on matrix computation was another older but useful adjunct to the matrix analysis involved in this research. Jennings treatment of matrix multiplication, transposition, and the inverse were very va luable during contact modeling, the development of the assembly task supervisor, and controller design. Drs Crane and Duffy (1998) wrote an excellent text on the kinematic analysis of robotic manipulators. Though the authors focus was on serial manipulators, the section on mechanism mobility, or the degrees of freedom (DOF), was very useful for understanding the kinematic model for a manipulator This work established the equations and parameters for determining the degrees of freedom of both serial and parallel robotic manipulators This reference also contained a detailed discussion of quarternions and quarternion algebra. Professors Gibson and Hunt (1990) succinctly characterized the classification of screw systems in total agreement with Hunts earl ier work (1978). This paper provided a very clear picture of the screw system classification. The assembly task supervisor development relied heavily on screw theory. The primary focus of the paper was on onesystems (points), two systems (lines), and t hree systems (planes). This was essential in qualifying which type of system this research used and which aided in modeling efforts. PAGE 32 32 Dr. Mike Griffis (2009) wrote a detailed explanation of explicit vs. implicit force control. His paper provided a great deal of detail on how screw theory could and should be employed in the design and analysis of parallel robotic mechanisms. Dr. Griffis paper has proved invaluable in supporting the research associated with the work involved in this dissertation. Especially important was the information pertaining to contact as well as e xplicit kinestatic control presented later in this work Dr. Harvey Lipkin (1985) established an im portant part of the groundwork for the use of screw theory in the development of kinestatics and hybrid control. In fact, the first use of the term kinestatics was attributed to Dr. Lipkin. Lipkin developed a meaningful and detailed description and insig ht into the understanding of homogenous coordinates. This information became a useful adjunct to that obtained from Brand (1947). In Chapter 5 of his dissertation, Lipkin discussed the application of screw theory to hybrid control and specifically addres sed the case of constrained motion. Lipkin matrix matrix provided a means to convert from ray co ordinates to axis coordinates. By extension, the information in this chapter was extremely valuable in this research for a nalyzing partially constrained motion. It should be kept in mind that m otion and control of the parallel robotic manipulator with partial constraints was the key issue in this project. Professor Tom Neff (1961) put together a very useful compilation of basic rules and examples for dealing with vectors. Neff used this material in his course on Statics at the University of Florida back in the early 1960s a s a refresher for new engineering student s in the study of Statics PAGE 33 33 Professor Pl cker (1865) established the ground rules for the use of ray and axis coordinate systems in his paper presented by Cayley to the Royal Society of London in 1865. Pl cker also established the use of the now famous notation for describing in general terms the direction and moment of a line : { } Later references by Hunt (1978) used the terminology { } to reference the same direction and moment of a line and called them the Pl cker coordinates The use of this notation requir ed that the coordinates be normalized and is discussed later in this research. In an 1866 paper Pl cker (1866) discussed in detail the concept of reciprocity and its value to the analysis of the geometry of mechanisms. Pl cker also elaborated quite well on forces and their analys e s. He introduced a term or name for the resultant: dyname. He then explored and analyzed the dyname and the geometrical meaning of the term The information provided by Schimmels and Peshkin ( 1994) ha d special application to the friction/stiction issue. This article summarized developments in actuators with friction and stiction Compliant actuators allow deviation from the equilibrium position with little regard for friction/stiction in the actuators themselves Huang and Schim mels (2001) provided an excellent example and method of determining the system stiffness matrix using the principles of screw theory. The author did disagree with the analysis done by Dr s. Duffy and Griffis that general spatial stiffness c ould be modeled with an equivalent parallel (Stewart platform) mechanism. A key issue was whether the method for determining the stiffness matrix put forth in this paper was limited to small deflections from the unloaded or equilibrium position of the compliant system. PAGE 34 34 K inestatic Control and S crew T heory Sir Robert Ball (1900) literally wrote the book on screw theory. His exposition on the cylindroid is critical to a complete unders tanding of the theory of screws. The basics of screw theory wrenches, twists, and pit ch were presented early in the volume and were essential reading for any one interested in the application of screw theory Also of interest was the section on reciprocal screws. The sections on Equ i librium of a Rigid Body and The Plane Presentation were of particular importance in this research. Modeling, development of the assembly task, and controller design depended heavily on Balls work and was absolutely necessary for a thorough understanding of screw theory. Brand (1947) presented a well written description of homogeneous coordinates His definition of homogeneous coordinates laid the groundwork for a good portion of the analysis performed in this research: Coordinates of a point, line, or plane are called homogeneous if the entity they determine is not altered when the coordinates are multiplied by the same scalar. Th is information was also used for development of m otor a lgebra as part of the background research for explicit control Dr. Mike Griffis (1991) posed and proved the effectiveness of the use of screw theory in simultaneously controlling force and position. This research was focused on implicit force control, i.e. control of the position of the actuators versus control of force. Key components of this research were the concepts of wrench of constraint (WOC) and twist of freedom (TOF). Though the focus was on Implicit Kinestatic Control (IKC), the screw theory analytical methods employed were of great value in constructing the explicit kinestatic controller employed in this research. Griffis also originated the concepts of wrench of deflection (WOD) and twist of compliance (TOC) PAGE 35 35 Johnson and Kiokemeister (1960) provided the basic calculus reference for the work done in this project The defin ition of a total differential was a useful math resource, as was the information on the chain rule. This paper provided a refresher on the derivatives of trigonometric functions, and the product of a derivative. Pigoski Griffis, and Duffy (1998) laid further groundwork for the study of screw theory and its use in the analysis of parallel mechanisms. This paper was very useful since it addressed a planar parallel device. Though the device in the paper was physically different from the planar mec hanism used in this research, it was possible to glean much useful information from the contents. Of special importance was the focus on stiffness and contact with the environment. Interestingly, Pigoski began his paper with the same useful equation tha t formed the basis for the explicit kinestatic controller study in this research: = [ ] This was the equation that map ped the applied wrench to the twist of freedom via the stiffness matrix, [ ] F orce Control and Associated Control Theory Chatzakos and Papadopoulos (2003) studied the problem of contact utilizing a single hydraulic cylinder in contact with a hard surface. There are many problems associated with the use of hydraulics, not the least of which were non linearitys and static an d dynamic load parameters. The authors developed a very simple equation = + + + where was the desired force. The second order system error equation was + + = 0 The equation provi ded the benefit of being able to control the operation of the robot both in free space and in contact using the same equation. The paper described a model based control approach PAGE 36 36 that appeared to be very effective. Although not directly useable in this st udy the information proved to be well worth the study. Chatzakos and Papadoupolous (2003) stated unequivocally that, due to the inherent nonlinearities of any hydraulically powered robotic system, the use of proportional plus integral plus derivative ( PID ) proportional plus integral ( PI ) or proportional plus derivative ( PD ) control was not an acceptable alternative. It is possible, in special cases, to linearize around a particular operating point and use PID or PD only for that operating point. The following is a quote from Chatzakos: Electrohydraulic servo systems exhibit highly nonlinear behavior to the effect that classical linear controllers, e.g. PD, usually achieve a limited performance. This statement, al though probably true in general, was proven not applicable in this research. In their Schaums Outline S eries, DiStefano et al (1967) provided a very condensed accounting of the basics of control theory. Different sections discussed the basics of block diagram algebra and provided a good review of the topic. The analysis of the Bode plot and operations in the frequency domain were useful during modeling of different elements of the actuation system as well as sensor modeling Overall, this reference provided a good review of the topics that are essential to any frequency domain control system description. Dorfs (1993) text provided a sound reference for the basics of control theory. He d iscussed the develop ment of the system transfer function as well as Bode plot analysis The presentati on of the Routh Hurwitz stability criterion w as also useful in characterizing the control system. PAGE 37 37 Dunnigan et al (1996) developed an apparently effective hybrid position/force controller for a hydraulically operated manipulator. Though their research foc used on undersea applications, the analysis and the lessons learned proved valuable in a surface environment as well. Gorinevsky, Formalsky, and Schneider ( 1997) present ed an excellent formulation of force control for robotic systems. The most important portion was the description of and differentiation between explicit force control and implicit force control. The beginning premise is = From there they buil t on t he entire force control problem The implicit vs. explicit issues were presented in this text and are discussed in this dissertation. Also presented were several very useful force control algorithms, several of which were modified and adapted to control both the singleleg device as well as the planar mechanism The most tractable equation for this particular application was = ( ) As with any robotic controller, force feedback control has inherent instabilities which must be overcome in the design of the model controller. The model force controller was presented in Chapter 5 as: = ( ) ( 2 1 ) where = ( 2 2 ) = ( 2 3 ) and > 0 ( 2 4 ) The resulting equation became = ( ) ( 2 5 ) PAGE 38 38 w here this relation holds: ( 2 6 ) This model controller worked well as a precursor to the actual controller also described in Chapter 5. Mr. Kennedys (2009) thesis demonstrated a w orkable method of experimentally determining the servo system Bode diagram and, from that, the open loop system tr ansfer function. The primary input signal was the chirp signal This signal was a varying frequency input to the system in locked mode with known compliance. Kennedy used a chirp signal in a single experiment t o sweep through the frequencies, plotted the output over the input, and produced a Bode diagram of the results. This was useful as a check on the method used in this research in which tests were run at many different frequencies, the output/input plots were created for each case, and from these the Bode magnitude and phase plots were generated. Kennedy, in essence, used a useful shortcut to obtain similar frequency domain results. Dr. Tommy Pool (1989) did an excellent job of describing the details of a h ydraulically powered robot manipulator for a citrus picking robot. Pools focus was on a serial manipulator rather than a parallel manipulator, but, as in several other instances, the hydraulic control issues were very similar regardless of robot geometr y. Pools contr ol system analysis utilized the frequency domain and explained the controller design using Bode diagrams. Pool went into great detail in the description of the Bode diagrams and their significance in the control system design. Robinson and Pratt s ( 2000) article described the modeling, control, and testing of a single leg apparatus equipped with a compliant spring. Many simplifying assumptions PAGE 39 39 were made, but the conclusion of the authors was that high bandwidth meant a large spring constant, and a low bandwidth (minimum impedance) required a low spring constant The results of th e paper meshed well with th is research and the use of explicit force control as the backbone of the system control. In order to achieve a better, workable cont roller, a compliant device was placed inline with the linear actuator to provide a more robust force controller. Gorinevsky et al ( 1997) mentions the need for and the usefulness of adding a compliant device when executing force control in contact. Kenn edy (2009) on the other hand did not include known compliance in his experiments. This research shows that may have been a mistake that cost Kennedy in the overall performance of the hydraulic system. Robinson and Pratt (2000) went to great lengths to incorporate a known compliant device in their design of a similar Massachusetts Institute of Technology ( MIT ) Leg Lab single leg device which included experiments with a single hydraulic cylinder in hard contact with an unyielding surface. They discovered that a well designed, compliant spring will possibly decrease the force control bandwidth. Even if bandwidth w as not an issue, the known compliance has other advantages. By controlling and measuring the deflection of the spring, a simple algorithm can be utilized that would reduce the bandwidth, but provide excellent force control of the system. In addition, Robinson and Pratt were able to use a PI controller very successfully for the single leg in their experiment. The MIT device was very similar to th e single leg used in this research, with the difference being the size of the test platform. The platform used to provide data for this document was an order of magnitude larger than the one used by the MIT team. PAGE 40 40 Th e paper by Niksefat, Wu, and Sepehri (2001) discusse d the use of a back stepping Lyapunov based controller that addressed the nonlinearities associated with servo valve control of hydraulic systems. The authors discuss ed the issues of load on the hydraulic system as well as servovalve dynamics. The information provided in this paper was used during the development of the singleleg force controller as well as in the development of the system model Vossoughi and Donath (1995) examined in depth a new approach to electrohydraulic system control. The authors introduced an interesting concept of feedback dynamic inversion in order to linearize the system controller. This approach could possibly provide an alternative to linearization over a limited range or about a fixed operating point. Dr. Whitney (1987) present ed a historic al view of robotic force control, and his writing was very relevant to current research developments Of special interest was his division of force control into two regimes: Logic Branching and Continuous Force Feedback. The former was term ed static, and the latter was termed dynamic. He lai d out the steps involved in force control at that time, and they are pertinent in todays control world: task description, forcemotion relations, task execution strategy, comma nd logic, control, and stability considerations. Alt hough this work focused on serial manipulators parallel manipulation in the late 1980s was in its infancy the concepts were very applicable to the research described in this project In the referenc e by Zheng et al (2011) the authors state d that the I in PID control cannot be used to successfully control a robotic manipulator. There were two reasons why this viewpoint did not apply to this research. The f irst was the friction PAGE 41 41 issue. If friction was neglected, then the PID or ID control will work as was proven herein. Secondly, Zheng et al (2011) were experimenting with a serial, three axis, gimbaled device with no known passive compliance in series with the end effector The parallel device used in this research presented a different paradigm from a kinematic standpoint as well as from the compliance standpoint. The paper by Ferr e tti et al (1995) qualified the above observation by stating that inclusion of a known compliance would all ow the use of integral control in the case of contact with a hard or stiff surface. The authors specifically addressed the problem of explicit force control. They went on to state that force control in the case of a very stiff or hard contact would be im practical unless accompanied by the inclusion of a known, passive compliance. H ydraulic Systems Th e book by Johnson (1977) provided an elementary description of electromechanical/hydraulic servovalves and systems. T he information was very pertinent to a nyone work ing in the area of control of hydraulic systems. Extensive detail was provided on the construction and operation of servovalves Johnson als o discussed the use of the openloop control block diagram as a means to determine hydraulic servo syst em gains and instabilities. Probably the most extensive work presented in the area of hydraulic control systems was by Merritt (1967). Merritt established the basics of hydraulic control and provided flow and valve equations that have proven to be extremely accurate. Information from Merritt was used extensively during the system model development and the singleleg force controller PAGE 42 42 Manring (2005) updated Merritts classic book on hydraulic systems but focused more on pumps and hydraulic power sources. This was an excellent update to a genuinely classic text. Manring stressed one of the key advantages of hydraulic systems: the high torqueto inertia ratio provided by fluid power in his introduction. Development of the singleleg force control levied M anring s information, especially the section on fluid bulk modulus and servovalve control When dealing with only a first order hydraulic system, the time constant was the determining factor for system analysis. On the other hand, a second order system required that one consider the damping ratio as well as the natural frequency. If a simple spring, mass, damper system was considered, the linear differential equation was + + = 0 This equation was easily converted to the more standard form + 2 + = 0 where = and = At this point, a solution of the form = = and = was sought where is the system eigenvalue. By a process of substitution it was possible to determine that + 2 + = 0 was the characteristic equation for the hydraulic system. I t was found that the solution to the unforced equation was = + where and were the system eigenvalues. At this point, pole placement techniques could have be en used to ensure control stability. McCloy and Martin (1980) published the basics of fluid power control. Though the text covered both hydraulic and pneumatic systems, the hy draulic information was covered separately which made the reference very easy to use for the purposes of this research. The bulk modulus information was useful as was the line pressure loss i nformation, model information, the servovalve coverage, Boolean algebra information and the listing of hydraulic circuit symbol s. PAGE 43 43 Merritt (1967) Johnson (1977) Manring (2005) and McCloy and Martin ( 1980) all provided valuable information regarding the use of hydraulics for control and actuation. Merritt was by far the richest in useful material, but these references provided valuable information on hydraulic systems and components of the systems Merritt did not focus on one single type of hydraulic system, but rather studied and described many of the most common s ystems. His studies pointed to the fact that the slowest device in the system was the limiting factor in the closed loop response. This conclusion necessitated the determination of the hydraulic systems natural frequency. Once this was determined, it w as evident that the active hydraulic components must be chosen to provide large natural frequencies or, in other words, fast response times. This led to the further conclusion that the limiting factor in the performance of the system was the rate of response of the actuator, spring, and sensors. Obviously the actuator/load (spring) combination was proven to provide the slowest response and was the real world limiting factor for the systems response in the system under investigation in this project Merr itt explained in detail that by selecting highspeed control devices, the closed loop transfer function reduced to a fifth order system that incorporat ed an integrator term. The open loop controller reduced to a second order system by the same reasoning P arallel Robotic Mechanisms Dr. Joe Duffy (1996) published an outstanding text on statics and kinematics as they relate to the screw theory analysis of robotic manipulators. The section on infinitesimal displacement analysis proved to be extremely useful in the development of quantifying contact and the assembly task controller The key to the movement or the differential in this research was the infinitesim al motion of the manipulator In this PAGE 44 44 study, small motions were the independent variable as opposed to the use of time as in most studies of robotic control. Dr. Ken Hunt (1978) wrote what has proven to be a classic text on mechanisms, geometry, and the role of screw theory in the description of kinematic systems. The classification of screw systems into seven categories and the detailed description of each category proved useful in the description of contact, the assembly task controller, and the explicit kinestatic controller. Hunt also clarified the use of Pl cker coordinates in the definition of control space. PAGE 45 45 CHAPTER 3 MODELING CONTACT: TH E ASSEMBLY TASK SUPERVISOR Nothing in the world can take the place of persistence. Talent will not; nothing is more common than unsuccessful men with talent. Genius will not; unrewarded genius is almost a proverb. Education will not; the world is full of educated derelicts. Persistence and determination alone are omnipotent. The slogan press on has solved, and always will solve, the problems of the human race. President Calvin Coolidge, Fail Up: 20 L essons on Building Success F rom Failure, Tavis Smiley Overview The ultimate goal of the assembly task supervisor was to get the payload into its final position with respect to its target, i.e. complete the assembly. As the assembly task supervisor played a high level role it had the responsibility of providing an instantaneous model of contact. As stated in the introduction, contact not only described how two touching bodies apply constraint wrenches to each other, but it also described how the bodies may freely twist relative to one another. The term kinestatics was used to describe the geometry of contact and was consistent with Lipkin (1985). Figure 3 1 i dentifies the upper level (black box) overview of the assembly task supervisor. The assembly task supervisor used geometrical information about the target and payload along with other system information regarding force and torque thresholds to establish the initial state and status of the controller. The output to the user included the current contact case information, the location of the payload, and the location of the target when discovered. The current contact case identified where and how the payload touched the target at any given moment PAGE 46 46 An important given for the assembly task supervisor was a task dependent set of contact cases [] that quantified and qualified all possible contact scenarios. One special contact case represented when the payload and target were not in contact (disengaged). Another special contact case was when the payload and the target were fully mated (engaged) and the assembly was complete. In general, the set of contact cases [] contained all manners and locations where the payload may possibly touch the target. To define these cases, an [] table was generated for a given assembly task and is presented later. The assembly task supervisor relied on the collection of contact scenarios ( []) in order to generate a sequence of output commands to steer the assembly process toward the aforementioned goal. Output commands from the supervisor to the kine static controller included the current contact model and control set points for the force feedback algorithm in terms of the desired constraint wrench and the desired twist of freedom. The desired constraint wrench was considered a maintenance wrench for the purpose of maintaining contact and the desired twist of freedom was considered a forward twist for the purpose of steering the assembly process toward completion. Feedback from the kinestatic controller to the supervisor included the sensed wrench, the current location of the payload, and the current stiffness matrix [ ] of the actuation platform. The motion of objects in space, whether a point, a line, a plane, or a three dimensional object, can be depicted using a multitude of descriptors. One of the most robust methods of describing this motion was developed by Sir Robert Stawell Ball (1900) in a work entitled: A Treatise on The Theory of Screws. Over the years, screw PAGE 47 47 theory has proven to be a rather elegant way to express rigid body displacements, velocities, forces, and torques in a unified, compact manner ( Hunt 1978) In the simplest terms, a screw is a line with an associated pitch, where the pitch has a dimension length ( ). From the kinematics viewpoint, a twist about a screw represents how a rigid body rotates about a line and simultaneously translates in the direction of the line. The ratio of the two components is the pitch. The twist amplitude (a scalar) denotes the amount of rotation about the screw. In the special case of the three legged planar mechanism the movement was in the plane which simplified the screw systems. In the planar case, the movement occurred in one of either two cases: 1. For the case of pure rotation, the pitch is zero. 2. For the case of pure translation, the pitch is infinite. The same result is achieved geometrically as the axis of a pure rotation is moved to infinity. In this work, twist amplitude is an infinitely small quantity (Ball, 1900); or in other words, infinitesimal. As it will be shown, compliance is a time independent relationship between force and displacement. Hence twists in this research take the form of infinitesimal displacements rather than instantaneous velocities. From the statics viewpoint, a wrench on a screw represents the resultant of a system of forces that act on a rigid body. This can be resolved into a force acting along a certain line together with a couple that acts in a plane perpendicular to the given line. The ratio of the two components is the pitch. The wrench intensi ty (a scalar) denotes the magnitude of the force. PAGE 48 48 In the special case of the planar mechanism wrenches act in the plane which simplifies the screw systems. In the planar case, the wrench occurs in one of either two cases: 1. For the case of a pure force, the pitch of the wrench is zero. 2. For the case of a pure couple, the pitch is infinite. The same result is achieved geometrically as the line of action of a pure force is moved to infinity. It was important to delve into the concept of contact, as it described a critical relationship between two bodies in space. First of all, a body in space can be characterized by both physical and spatial parameters. Examples of physical parameters include size, perimeter geometry, mass, and density while spatial parameters include position and orientation (called the location of the body). When there is physical separation between two bodies in space, the bodies are obviously not in contact. This implies that there is a finite distance separating each surface particle of the bodies, and there are no force interactions between the bodies. If the bodies are manipulated in such a way that at least one point of one body contacts another point in the other body, then contact has been made and can be defined (qualified and quantified). It should be noted that contact can be characterized by the reaction force ( ) acting at the point of contact between the bodies, where can have a negative, positive, or a zero value. A negative force implies that the two bodies are st uck together or somehow can resist being pulled apart. If one body pulls away from the other body, the contact state is identical to the first state where there is a physical separation between the two bodies. These contact states are shown in Figure 3 2 When multiple points on two bodies are in contact with each other, additional contact states can be identified. These additional states can be desc ribed as line or PAGE 49 49 planar contact, for example. Each of these can likewise possess one of the three conditions described previously, i.e. no force, positive force, or negative force. When referencing either a positive or negative force, the choice of coordinate systems comes into play. As long as the establishment of the coordinate system is consistent and uniformly applied throughout, then the sense of positiveness is maintained. In general, the force applied by the payload to the target is taken as positive. For example, in Figure 3 2 when payload A is in positive contact with target B then a positive force is applied to B (say a force to the right). Potentially complicating matters is the fact that this type of contact is actually unidirectional in typical practice, where the stuck state is disregarded. As the above referenced positive contact is reduced, contact becomes decoupled ( Figure 3 2 ). Lipkin (1985) referred to the unidirectional effect as unilater al and noted the difficulty of its study. To avoid that difficulty in this study, a bidirectional or bilateral methodology was employed as the following describes. In controlling, but also maintaining, contact, the tactic employed here relied on a mai ntenance force which was a force that exceeded a minimal threshold force. The maintenance force, or maintenance wrench say, ensured contact in such a way that the bodies were kept from decoupling and the forces were kept from being excessive ( Figure 3 2 ). When contact was established and maintained between two bodies, then relative twist between the two bodies was restricted, and the bodies were constrained or at least partially constrained. The set of all relative twists that were allowable and compatible with a given contact scenario was defined as the twists of freedom. In the view of the PAGE 50 50 assembly task supervisor, such a twist denoted an assembly displacement or assembly twist. The term forward twist denoted an assembly twist that nudged the assembly sequence toward completion. Wrenches that interact between two bodies in maintained contact may take on two forms: working and nonworking. Working contact wrenches such as those generated by friction were not considered in this study. The set of all nonworking wrenches that may be transmitted between two bodies in such contact was defined as the wrenches of constraint or constraint wrenches. Consider that a single point of contact existed between two bodies, such as shown in Figure 3 2 In such a situation, consider that there was also a plane tangent to the area of contact. An example of a nonworking wrench was a pure force that was perpendicular to this tangent plane through the point of contact, i.e. the contact normal. Wrenches of Constraint and Twists of Freedom This section formally introduces the concepts of wrenches of constraint and twists of freedom. In order to apply screw theory, it was first necessary to introduce the concepts of wrench coordinates and twist coordinates. The following describes the concepts of h omogeneous coordinates, scalar multiples, and normalized coordinates. This section also includes the concept of reciprocity and the work of a twist on a wrench. Pl cker ( 1865, 1866) identified rays for wrenches and axes for twists and identified coordinat es for each. Ray coordinates for a wrench had evolved into the form , ( 3 1 ) while axis coordinates for a twist had evolved into the form PAGE 51 51 , ( 3 2 ) When a Cartesian coordinate system was employed, represented a force along the axis, represented a moment about the axis, represented a displacement along the axis, and represented an angular rotation about the axis. An infinitesimal change, identified by is defined as a very small change for the purpose of this analysis. Thus, is an infinitesimal change in position and is an infinitesimal change in force Over the years, it became customary to scale these homogeneous coordinates in such a way that each coordinate in , was divided by where = + + ( 3 3 ) so that the resulting ray coordinates took the form ( , ) where + + = 1 ( 3 4 ) This determined as the direction cosines of the associated screw and as the moments of the screw. The coordinates for such a screw were thus normalized. Likewise, it became customary to scale these axis coordinates in such a way that each coordinate in , was divided by where = + + ( 3 5 ) so that the resulting axis coordinates took the form ( , ) where again + + = 1 This again determined as the direction cosines of the associated screw and determined as the moments of the screw. In order to switch between axis and ray coordinates, Lipkin (1985) conceived the following which becomes a matrix eq uation : PAGE 52 52 = [ ] ( 3 6 ) where = ( , ) ( 3 7 ) are the ray coordinates of a screw, where = ( , ) ( 3 8 ) are the axis coordinates of the same screw, and where [ ] = 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 ( 3 9 ) Given ray coordinates it is well known (Lipkin 1985) that the coordinates are normalized when [ ] = 1 ( 3 10 ) where [ ] = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ( 3 11 ) Equation 3 10 along with E quations 3 11 and 3 7 is equivalent to E quation 3 4 Given axis coordinates it is well known that the coordinates are normalized when [ ] = 1 ( 3 12 ) where PAGE 53 53 [ ] = 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ( 3 13 ) Given normalized ray coordinates it is also well known that pitch is given by = [ ] ( 3 14 ) or in axis coordinates it is also well known that pitch is given by = [ ] ( 3 15 ) Given the condition that = 0 it is known that the associated coordinates locate a line. These Pl cker coordinates so identified served as a foundation to simplify screw theory applied to the plane. So, in the plane, for Equation 3 1 = = = 0 or = = = 0 which means ( 0 0 0 ) or 0 0 0 ( 3 16 ) A shorthand version of the above for the planar case was written as ( ) or So, in the plane, for Equation 3 2 = = = 0 or = = = 0 which means ( 0 0 0 ) or ( 0 0 0 ) ( 3 17 ) A similar shorthand version of the above for the planar case was ( ) or ( ) The following equation defines the wrench coordinates, in terms of screw theory as applied in the plane: PAGE 54 54 = = [ ] ( 3 18 ) where = = ( 3 19 ) such that + = 1 ( 3 20 ) and as defined in Equation 3 3 It should be noted that the coordinate normalization process for the planar case requires that + = 1 As used in Equation 3 19 above, the scalar is the magnitude of the force. Here [ ] are normalized coordinates of the line of action of the force, i.e. the wrench in general. This means that and are direction cosines in the and directions respectively, and is the moment of the line about the axis (the perpendicular distance to the origin in the plane). As a force, [ ], can be called a unit force (or unit wrench). Here, the line of action is finitely located. As 0 and as the line of action of the force moves towards (or as ), then the result is an infinitesimal force acting at infinity or a pure torque. With these assumptions, Equation 3 19 becomes 0 0 = 0 0 1 ( 3 21 ) such that = 0 = 0 = 0 = 0 ( 3 22 ) PAGE 55 55 Here scalar is the magnitude of the pure torque. In this case [ 0 0 1 ] are the line coordinates of the line at infinity in the plane and are called a unit torque. Technically, these coordinates are not normalized, since + 1 However, the coordinates, with = 1 are used to denote a pure unit torque. The twist coordinates as applied in the plane (i.e. a planar twist) are denoted as follows: = = [ ] ( 3 23 ) where = = ( 3 24 ) such that = 1 Here, scalar is the amplitude of the rotation (twist), and the units are radians. In practice, this is an infinitesimal rotation as previously noted. The normalized line coordinates of the axis of rotation are given by [ ] with = 1 and the axis of rotatio n is parallel to the axis. The values and denote the moments of the axis of rotation about the and axes, respectively. As a rotation, the normalized coordinates [ ] denote a unit rotation (or unit twist). In the plane, again wit h = 1 the coordinates of the axis of rotation can be related to the point which is called the instant center (IC) of rotation ( Figure 3 3 ). As such, consider the unit rotation given by the coordinates: = 1 ( 3 25 ) PAGE 56 56 The coordinates 1 = = ( 3 26 ) demonstrated the idea that the moment components = and = are the infinitesimal and displacements of the point of the body that is instantaneously at the origin. In other words, + = = ( ) ( 3 27 ) Now let the I.C. go toward so that and/or and let tend even more towards 0. With this, consider that the products and are nonzero and finite. In this case, the coordinates can be scaled such that = 1 + 0 = 1 + 0 ( 3 28 ) which requires that + = 1 and = 0 ( 3 29 ) This twist then denotes a pure translation in the and direction corresponding to and i.e. 0 = 0 = 0 ( 3 30 ) where is an infinitesimal displacement of dimension length. In summary, = 1 denotes unit rotation while + = 1 and = 0 denotes unit translation. It should be noted that the above coordinates are adapted for use in the plane from Pl ckers implementation of 3space. PAGE 57 57 It is important to recall and appreciate that the preceding wrench and twist coordinates are homogeneous. This aspect holds fixed a given line of action of a force while its intensity varies, or holds fixed a given axis of rotation of a planar twist while its amplitude varies. At this point, it is desirable to introduce the concept of work in the planar system : = = [ ] = + + ( 3 31 ) In E quation 3 31, represents a planar twist of one body with respect to another body, say body A with respect to body B. Also, represents a wrench applied to body A or by body A. Consider a system c omprising bodies A and B together with mechanical connections between them. When represents a wrench applied to body A, then the work is considered an input into the system. When represents a wrench applied by body A, then the work is considered an output from the system. It should be pointed out that, technically, is a small change in work. It can be seen from the previous discussion that and are infinitesimally small numbers. Note that and are scalar numbers in E quation 3 31 This is an important concept throughout the mathematical analysis of contact using screw theory. When considering constraint forces, the first rule is that a wrench of constraint cannot do any work on any twist of freedom. Thus, the governing equation is = 0 ( 3 32 ) As the coordinates are homogeneous, it can be shown that the above is a geometrical relationship that says the line of action of a force is reciprocal to the axis of rotation of a planar twist. Equation 3 32 is the condition of reciprocity PAGE 58 58 In light of the preceding consider that in such a reciprocal relationship, the wrench is a wrench of constraint and the twist is a twist of freedom. Notatio nally then, a wrench of constraint will be identified with a subscript and a twist of freedom will be identified with a subscript so that Equation 3 32 takes t he form = 0 ( 3 33 ) Consider now that the wrench of constraint belongs to a vector subspace or screw system, i.e. the wrenches of constraint, and that the twist of freedom belongs to its own vector subspace or screw system, i.e. the twists of freedom. Such vector subspaces, or, more commonly, screw systems, are represented mathematically in the plane as follows. First, for a wrench of constraint, = [ ] ( 3 34 ) where matrix [ ] (Jennings 1977 and Ayres 1962) has dimension 3 and where has dimension 1 In this, the is an array of scalar multipliers of the columns of matrix [ ] which describes the constraints. The columns of [ ] are the unit forces and unit torques of the wrenches of constraint. Any linearly independent set of unit forces and unit torques in the subspace may be chosen. The columns of [ ] are the basis elements for the wrenches of constraint. Secondly, for a twist of freedom, = [ ] ( 3 35 ) where matrix [ ] has dimension 3 and where has dimension 1 In this, the is an array of scalar multipliers of the columns of matrix [ ] which describes the freedoms. The columns of [ ] are the unit rotations and unit translations of the twists of freedom. Any linearly independent set of unit rotations and unit translations in the PAGE 59 59 subspace may be chosen. The columns of [ ] are the basis elements for the twists of freedom. For the case of planar analysis, + = 3 ( 3 36 ) and for spatial analysis, + = 6 ( 3 37 ) where is degrees of constraint, and mobility is degrees of freedom. The matrices [ ] and [ ] are (or locate) reciprocal screw systems. In order to guarantee that the screws are reciprocal, the rate of work betw een them must be zero, regardless of the intensity of the applied wrench or of the twist amplitude. Expanding Equation 3 33 further yields 0 = = [ ] [ ] = ( [ ] [ ] ) ( 3 38 ) which must be true for all or This means that [ ] [ ] = [ 0 ] ( 3 39 ) where [ 0 ] is an matrix of zeros. From this, it follows that [ ] [ ] = [ 0 ] ( 3 40 ) where [ 0 ] is likewise an matrix of zeros. Contact Examples for Candidate Assembly Task The candidate assembly task described in Chapter 1 involved a useful planar example of a payload and a target. The following section will examine the various contact cases during movement of the payload toward the target, tracing the progression from moti on in free space to the fully constrained case. Figure 3 4 depicts the planar payload with an attached coordinate system. The payload consisted of tw o contact points labeled F and G, where the distance between the points was A PAGE 60 60 coordinate axis was placed on the payload such that the axis was aligned along the line FG, and the axis intersected point F and resided in the plane of actuation. It is important to note that the coordinate system used for this work was chosen for convenience and the overall results were not geometrically altered by a different location of the coordinate system. Contact examples for the kinestatic cases listed in items 0 4 below will be presented in the following analysis. In each case the type of motion is specified, the constraints and freedoms are listed, a diagram is provided where appropriate, and the case specific, [ ] and [ ] matrices are presented: 0. Movement in free space, 1. Conventional point contact and enhanced point contact at point F in the plane, 2. Conventional point contact and enhanced point contact at point G in the plane, 3. Line contact, 4. Fully constrained contact. Case 0: Free Space Motion The first case considered was that of free space motion. It was assumed that the assembly process would begin with the payload not in contact with the target. Given this unconstrained starting location, a twist of freedom was applied in the global vertical ( Y) dir ection to effect initial movement of the payload. It was also assumed that the general location of the target was known, that its location was somewhere above the payload. Figure 3 5 demonstrated the general layout of the payloadto target relationship. PAGE 61 61 For this case, the constraints are zero, = 0 and the freedoms are three, = 3 The [ ] matrix describing the system constraints is simply the null matrix, [ ] = ( 3 41 ) This statement should be self evident by observing that in free space, the system has zero constraints. The [ ] matrix for this case is the identity matrix, [ ] = 1 0 0 0 1 0 0 0 1 ( 3 42 ) Again, this statement should be self evident from the fact that there are three freedoms. The columns of [ ] identify unit translations in the and directions as well as a unit rotation at the origin about the axis. Case 1a : Convent ional Single Point Contact at Point F When the target touched the payload in a single point of contact, whether at point F or G, then it was generally accepted that there was one degreeof constraint and two degrees of freedom. This was referred to as conventional single point contact. Figure 3 6 shows an illustration of this contact at point F. Consider a line tangent to the target at the point of contact. The constraint force acted perpendicular to this tangent, namely along the contact normal. The contact normal was elevated at angle relative to the axis. Determining the line coordinates of the contact normal provided the necessary information for the formation of the wrench of constraint [ ] = cos ( ) sin ( ) 0 ( 3 43 ) PAGE 62 62 In order to ensure reciprocity in the plane, Equation 3 40 must be satisfied and is repeated below for reference: [ ] [ ] = [ 0 ] ( 3 44 ) A valid first column of the [ ] matrix describes the unit translation perpendicular to the contact normal, and takes the form sin ( ) cos ( ) 0 ( 3 45 ) The second column of [ ] characterizes the unit rotation about point F: 0 0 1 ( 3 46 ) Combining both columns generates a [ ] matrix that satisfies Equation 3 44: [ ] = sin ( ) 0 cos ( ) 0 0 1 ( 3 47 ) The reciprocity can be demonstrated by completing Equation 3 44 for this particular case. sin ( ) 0 cos ( ) 0 0 1 cos ( ) sin ( ) 0 = 0 0 ( 3 48 ) When the matrix multiplication is completed, the first column of Equation 3 48 leads to sin ( ) cos ( ) 0 cos ( ) sin ( ) 0 = 0 ( 3 49 ) and the second column leads to 0 0 1 cos ( ) sin ( ) 0 = 0 ( 3 50 ) PAGE 63 63 Case 1b : Enhanced Point Contact at Point F Enhanced point contact (EPC) was termed sticky point contact due to the addition of a self imposed constraint as the following describes. The goal in EPC was to eliminate sliding and to rotate the payload about the first point of contact, whether point F or G, until contact with t he target by the other point had been achieved. In order to maintain the contact at point F and rotate the payload such that point G contacted the target, it was desirable to alter [ ] of Equation 3 47 by removing the first column consisting of the unit translation in the direction of the tangent and keeping the second column consisting of the unit rotation passing through the origin. This effectively makes the contact emulate a revolute joint at the origin, thereby providing one degreeof freedom and two degrees of constraint. In light of this, the [ ] takes the following form: [ ] = 0 0 1 ( 3 51 ) Using Equation 3 40 yields the following [ ] : [ ] = 1 0 0 1 0 0 ( 3 52 ) The first column of [ ] represented a unit force along the axis and the second column represented a unit force along the axis through point F. The column of [ ] represented the unit rotation about the point F. The geometrical significance of reducing freedom and adding constraint in this way will be discussed later PAGE 64 64 Case 2a : Conventional Single Point Contact at P oint G This case describes conventional single point contact at point G ( Figure 3 7 ). The complexity of this contact is the same as presented in Case 1a, that is a one degree of constraint, two degree of freedom example. (However, Case 1a was able to employ a canonical representation. ) So, consider that point G was touching the target in such a way that the contact normal was canted at an angle The wrench of constraint can be easily identified as [ ] = cos ( ) sin ( ) sin ( ) ( 3 53 ) since these are the line coordinates of the contact normal. In the plane, this means that the twists of freedom can be identified by a 3 2 matrix [ ] that satisfies Equation 3 40: [ ] [ ] = [ 0 ] ( 3 54 ) One column of [ ] is potentially a translation perpendicular to the contact normal: or sin ( ) cos ( ) 0 ( 3 55 ) A second column of [ ] is a rotation about point G 0 1 ( 3 56 ) Putting the columns together yields a valid [ ] that satisfies Equation 3 40, or [ ] = sin ( ) 0 cos ( ) 0 1 ( 3 57 ) The following shows this reciprocity: PAGE 65 65 sin ( ) 0 cos ( ) 0 1 cos ( ) sin ( ) sin ( ) = 0 0 ( 3 58 ) which leads to sin ( ) cos ( ) 0 cos ( ) sin ( ) sin ( ) = 0 ( 3 59 ) and to 0 1 cos ( ) sin ( ) sin ( ) = 0 ( 3 60 ) Case 2b : Enhanced Point Contact at Point G Analogous to Case 1b, the addition of a self imposed constraint was made to eliminate sliding along the target and to rotate the payload about point G until contact with point F was made. In order to accomplish this, it was again necessary to determine both the [ ] and [ ] matrices. For this case, the number of constraints increased to two, and the number of freedoms, decreased to one. Equation 3 36 for the planar case still applied. For EPC contact at G, the [ ] and [ ] matrices are [ ] = 1 0 0 1 0 and [ ] = 0 1 ( 3 61 ) The first column of [ ] represented a unit force along the axis and the second column represented a unit force parallel to the axis through point G. The column of [ ] represented the unit rotation about the point G Case 3: Two P oint C ontact In this case, contact is maintained at both points F and G simultaneously. From a kinestatics standpoint, this is equivalent to line contact. In terms of the candidate PAGE 66 66 assembly task example, it is desirable to translate the payload in the direction of the contact line, i.e. along the payload axis ( Figure 3 8 ). The constraints, are two, and the freedoms, are one, where Equation 3 36 remains valid. Specifically, the [ ] matrix is a 3 2 matrix and the [ ] matrix is a 3 1 matrix This is referred to as two point contact or prismatic type contact. The [ ] and [ ] matrices for this case are given by [ ] = 0 0 1 1 0 ( 3 62 ) and [ ] = 1 0 0 ( 3 63 ) The columns of [ ] denote unit forces parallel to the axis through points F and G. As desired, the column of [ ] denotes a unit translation in the direction of the axis. Case 4: Fully C onstrained C ontact The end result of the assembly task was to achieve the case of full constraint. This meant that the objective of finding t he target, engaging the target, moving in contact with the target, and finally reaching the goal of full mating had been achieved. In this case, Equation 3 36 applied, where the parameters = 3 and = 0 The [ ] matrix is a 3 3 matrix with 3 degrees of constraint [ ] = 1 0 0 0 1 0 0 0 1 ( 3 64 ) The [ ] matrix is a null matrix with zero degrees of freedom. [ ] = ( 3 65 ) PAGE 67 67 Wrench Complements The objective of this section was to determine a second wrench space ( [ ]) that complements the given wrenches of constraint. This second wrench space was defined as the deflection wrenches. This wrench space had the same dimensionality as the twists of freedom. Using the two wrench spaces together, it was possible to deco mpose a general wrench, such as a sensed wrench, into a unique constraint wrench and a unique deflection wrench. Specifically, the constraint wrench controls the nonworking contact forces and it is desirable that the deflection wrench controls the requi red permissible twist. The use of the term deflection wrench here was consistent with Griffis (2009). In order to obtain the essential elements of kinestatic control and, more specifically, explicit force control, the concept of wrench complements, name ly the [ ] matrix, must be introduced. The following describes the role of the [ ] matrix. For a given contact scenario, where constraints [ ] = and freedoms [ ] = are provided, such that [ ][ ] = [ 0 ] it is required to have a matrix [ ] = such that [ ] is non singular, and where the columns of [ ] denote the deflection wrenches. A non singular matrix is, by definition, invertible. So, for a given general wrench, and the given [ ] [ ] and [ ] the general wrench can then be decomposed onto a unique element of [ ] = [ ] ( 3 66 ) PAGE 68 68 and a unique element of [ ] = [ ] ( 3 67 ) as the following describes. The equation = = [ ] ( 3 68 ) yields the scalar arrays and where the dimension of is 1 and the dimension of is 1 The scalar arrays set the unique elements using Equations 3 66 and 3 67 such that = + ( 3 69 ) A matrix [ ] which satisfies the above requirement is said to complement [ ] It means any force in the plane can be written from the combined column space of the two matrices taken together, i.e. the column space of [ ] In terms of subspaces, this can be written as [ ] [ ] = In general, for a given [ ] the complement [ ] is not unique. However, if a symmetric, positive definite matrix [ ] can be used, then the [ ] that satisfies [ ] [ ] [ ] = [ 0 ] ( 3 70 ) is unique, with respect to the specified [ ] In such a case, the resulting [ ] complements [ ] and is called the [ ]orthogonal complement. In order to contemplate a viable [ ], a generic stiffness mapping was introduced such that = [ ] ( 3 71 ) where a twist of freedom was mapped to a desired deflection wrench using a stiffness matrix [ ] For the purposes of this work, [ ] was a symmetric, positive PAGE 69 69 definite ( 3 3 ) matrix. With Equations 3 35 and 3 67, the stiffness mapping can be used to establish the columns of [ ] by the relation [ ] = [ ] [ ] ( 3 72 ) Recall that [ ] has the same dimensionality as [ ] Using Equation 3 72 the [ ] matrix can be written as [ ] = [ ] [ ] = [ ] [ ] ( 3 73 ) where [ ] = [ ] is also a symmetric, positive definite, ( 3 3 ) matrix which means that [ ] = [ ]. In other words, [ ] is a compliance matrix and [ ] is a stiffness matrix. Equation 3 73 can be substituted into Equation 3 40 yielding [ ] [ ] [ ] = [ 0 ] ( 3 74 ) In this way, [ ] is the [ ]orthogonal complement of [ ] and this yields the desired result. Note that further details of [ ] and [ ] are provided in Chapter 4. Heuristic Approach to the Assembly Task Supervisor A comprehensive fully robust assembly task sequencer for a general task was beyond the scope of this research. The approach taken here was to establish a heuristic based strategy that was applied to a candidate planar assembly task. Future efforts would ex tend this basic planar task sequencer to a six (6) degree of freedom example where the constraints are more general or more complex. Given a multitude of possible contact cases, the Assembly Task Supervisor heuristic determined the correct contact case. O nce the correct contact case was determined, it remained to command a desired, associated forward twist in order to sequence through the assembly task. The following identifies aspects of the heuristic: a. Preliminarily, the heuristic required all possible c ontact cases given by the [ ] and [ ] matrices. Included as the generic case 0 was the PAGE 70 70 unconstrained case. The final case is the fully constrained case, which identified the completed assembly task. Cases in between those two identified all possible partially constrained contact cases. b. Also, preliminarily, the description identified a forward twist for each contact case, [], which provided direction on how to move forward towards completion of the assembly task c. Next, given a symmetr ic, positive definite stiffness matrix [ ], obtain the orthogonal complement [ ] one for each partially constrained contact case. d. Create the square, invertible matrix, [ ] one for each partially constrained case. e. Obtain sensed wrench which s hould be the resultant of contact forces exerted between the payload and target. f. Use the inverse of [ ] to decompose onto a constraint wrench and a deflection wrench ( Equations 3 66 through 3 69 .) For the unconstrained case, = And for the fully constrained case, = g. Calculate the intensity of which is identified by When the following is true: < ( 3 75 ) then the correct contact case is unconstrained (case 0). h. Otherwise, the correct contact case is determined as follows. Calculate the intensities of the deflection wrenches, one for each partially constrained contact case. Calculate the ratios = again one for each partially constrained contact case. The minimum ratio identifies the partially constrained contact case that most closely matches the actual sensed wrench. If the associated deflection wrench intensity is below a certain threshold, < ( 3 76 ) then that partially constrained case is the correct contact case. If the associated deflection wrench intensity is above that threshold, then the correct case is fully constrained and the assembly task is complete. In the above, the scalar identifies the intensity of and the scalar identifies the intensity of The intensity of a wrench can be calculated for example using Equation 3 3 PAGE 71 71 Several assumptions have been made with respect to the design of the heuristic in order for the heuristic to function properly: a) The sensed wrench is being provided by the kinestatic controll er (Figure 3 1 ), and it is assumed that the wrench reported represents the resultant of contact wrenches interacting between payload and target. The kinestatic controller internally compensates for all other effects such as payload, gravity, dynamics, etc. b) Friction is minimal at the contact interface. Given the sensed wrench and the contact model and stiffness model, the sensed wrench is decompos ed into a nonworking (constraint) wrench and a working (deflection) wrench ( T he basis of Equations 3 69 and 3 71.) The deflection wrench component or a portion of it can be used to represent friction. The heuristic works when this deflection wrench component is not substantial, for if it were substantial (say excessive friction) then the heuristic could report the assembly wa s completed before it really was completed. c) When the deflection wr ench is determined substantial, it is assumed that the assembly task actually is completed versus just stuck. d) The heuristic assumes that contact wrenches (and resultants) are forces which exclude couples (or infinitely located lines of action). The heuristic relies on wrench intensities, which can be zero for infinitely located forces (couples). In threedimensional space, the heuristic assumes that contact wrenches are proper having nonzero intensities. Two aspects of the approach taken here make this plausible. First, unidirectional or unilateral contact occurs ( Figure 3 2 ) and resultant wrenches cannot be couples when contact points are arranged as shown in Figure 3 5 Secondly, the use of maintenance forces are believed to reduce the likelihood of sensed wrenches being couples. A more comprehensive heuristic could resolve (include) the case when a sensed wrench was a couple. e) The heuristic assumes that no intersections exist between the column spaces of [] from different contact cases. If two contact cases had the same unit for ce as a possible constraint wrench, then the above heuristic could not determine which case was correct. A more comprehensive heuristic may examine the history of contact in order to resolve such an issue for a wider range of assembly tasks. f) For the purposes of the heuristic, higher order changes in contact models are ignored. In other words, [] does not change as a function of the twist of the payload with respect to the target. However, in Chapter 4, first order changes in [] were required. PAGE 72 72 g) The target was fixed to ground at an unspecified location. (In this study, the target was located somewhere above the payload.) Implementation: Candidate Assembly Task This heuristic was adapted and applied to the candidate as sembly task originally presented in Chapter 1. The contact cases [] for the candidate assembly task w ere essentially presented in this chapter. The following discusses in more detail how the heuristic plays the role identified in Figure 3 1 while satisfying the candidate assembly task. The following makes reference to Table 3 1 This table relies on the contact cases and uses the same indices as presented and used in this chapter. For the baseline example, use the conventional point contact versions for cases 1 and 2. In this vein, T able 3 1 lists cases 0 through 4 where case 0 is unconstrained and case 4 is fully constrained. The remaining cases 1, 2, and 3 identify the partially constrained cases. The results of the s ection e ntitled Contact Examples for Candidate Assembly Task were used in this way to populate the first three columns of Table 3 1 These columns relied on a contact normal angle as the following also describes. Figure 3 1 illustrate d the assembly task supervisor inputs and outputs and how the supervisor interact ed with the user and the next lower level kinestatic controller. One of the feedbacks from the kinestatic controller was the sensed wrench, which was provided in terms of the global coordinate system ( Fig ure 3 5 ). Since the kinestatic controller also provided as feedback the current payload location (position, orientation), it was a straightforward transformation of the global sensed wrench into the sensed wrench given with respect to the payload coordinate system. It was the wrench PAGE 73 73 that was used to calculate basically the angular elevation of the sensed wrench with respect to the payload axis. Given the assumption that in the initial state (i.e. case = 0 ) the payload was not in contact with the target, the first move consisted of a movement upward in the global Y direction. In terms of the local payload coordinate system, the equivalent unit translation has the coordinates [ sin ( ) cos ( ) 0 ], where is the angular elevation of the payload axis with respect to the global axis. This movement was implemented using a twist of freedom, which is the forward twist with = 0 as shown in the fourth column of Table 3 1 The scalar multipliers and were provided by the user to the assembly task supervisor as nominal displacements for payload ( Figure 3 1 ). For control purposes, the assembly task supervisor was required to send commands to the kinestatic controller ( Figure 3 1 ). With case = 0 being unconstrained, the desired wrench of constraint was the zero wrench, namely [0 0 0 ]. Also sent to the kinestati c controller was the current contact case, shown as [] and [] in Table 3 1 Finally sent to the kinestatic controller was the next twist of fr eedom which is the forward twist = [ sin ( ) cos ( ) 0 ]. In terms of the global system this was [0 1 0 ]. It should be noted that twists and wrenches sent and received from the kinestatic controller were given in terms of the global coordinate system. As noted, straightforward transformations of wrench and twist coordinates can be employed to convert to and from the local payload system. As such, the remaining discussion uses payload referenced coordinates, except as noted. PAGE 74 74 When the assembly task supervisor determines the contact case is = 1 contact occurs at point F. In this case, the commands to the kinestatic controller are wrench of constraint [cos ( ) sin ( ) 0 ] and the twist of freedom [0 0 1 ]. Also sen t are the contact case data [] and [] The twist of freedom is the desired forward twist that rotates the payload in the desired direction such that point G moves closer to the target. Note that is provided by the user to the supervisor, nominal forces and torques for maintaining contact in Figure 3 1 When the assembly task supervisor determines the contact case is = 2 contact o ccurs at point G. In this case, the commands to the kinestatic controller are wrench of constraint [ cos ( ) sin ( ) sin ( ) ] and the twist of freedom [0 1 ]. Also sent are the contact case data [] and [] The twist of f reedom is the desired forward twist that rotates the payload in the desired direction such that point F moves closer to the target. When the assembly task supervisor determines the contact case is = 3 contact occurs at points F and G simultaneously. In this case, the commands to the kinestatic controller are wrench of constraint 0 1 and the twist of freedom [1 0 0 ]. Also sent are the contact case data [] and [] The twist of freedom is the desired forward twist that slides the payload in the desired direction, namely the positive axis direction (right) towards the intended target endstop. Note the commanded wrench of constraint is a maintenance wrench that balances contact between points F and G. When the a ssembly task supervisor determines the contact case is = 4 contact is fully constrained, and the candidate assembly task is completed. In this case, the PAGE 75 75 commands to the kinestatic controller are 0 1 for wrench of constraint and [0 0 0 ] for twist of freedom, i.e., a zero twist. Also sent are the contact case data [] and [] The intention of the zero twist is to hold the payload in place, and using the above wrench of constraint provides a maintenance wrench to hold the assembly together. For each partially constrained case, it is necessary to determine the complementary [] in order to assemble an associated 3 3 matrix [] The required stiffness matrix [ ] is fed back by the kinestatic controller ( Figure 3 1 ). For a given [], the associated [] is easily determined from Equation 3 72 Table 3 1 shows these results in the 5th and 6th columns, where the fully constrained case is taken as the identity matrix. For Table 3 1 the next to last column examines the decomposition of a given sensed wrench onto the two wrench spaces and provides for each case the representative constraint wrench and deflection wrench c omponents. ( T he basis of Equations 3 69 and 3 71.) The last column in Table 3 1 represents the internal workings, the decision tree, for the heuristic. The user provides to the assembly task supervisor certain force levels, and which indicated minimum force levels required to declare contact made and to declare assembly done. (Refer to Figure 3 1 nominal thresholds.) The following description of the last column in Table 3 1 f ollows the (g) and (h) of the heuristic given in the section entitled Heuristic Approach to the Assembly Task Supervisor PAGE 76 76 The heuristic declares current case is = 0 (unconstrained) when < When this determination is false, then the heuris tic examines the ratios = = 1 3 The minimum designates as a case to consider in the next step in the heuristic. When the ratio of the deflection wrench intensity to the sensed wrench intensity is minimum or tends towards zero, the best possible match between the constraint wrench and the sensed wrench is found, i.e. the following tends to be true: ( 3 77 ) For the case when < then the heuristic declares that the current case is = in other words one of the partially constrained cases. Otherwise, the heuristic declares that the current case is = 4 (fully constrained) and the assembly task is complete. The preceding strategy relied on determining from the sensed wrench and is referred to as the socalled determinator approach. This method of analysis was Euclidean in nature, and it was not origin dependent. The determinator strategy provided one option f or finding a geometrically meaningful method for finding the constraint wrench in the contact cases [] that most closely matched the sensed wrench. From Lipkin (1985), this is a least squares minimization problem that relies on the geometrically meaningful symmetric bilinear form = [ ] ( 3 78 ) where [ ] is given by Equation 3 11 and the wrenches are given in ray coordinates. Briefly, [ ] identifies what Lipkin refers to as the Euclidean Absolute. A slightly different implementation strategy was also considered, one which was nonEuclidean and relied on an elliptic polarity Lipkin (1985). The approach was PAGE 77 77 referred to as the elliptic determinator strategy. This is origin dependent as the following describes. In comparison with the determinator strategy, the choice of origin adds extra columns in [ ] and deleted certain columns of [ ] for = 1 2 Table 3 2 identifies this and uses the en hanced point contact versions for cases 1 and 2 from the section entitled Contact Examples for Candidate Assembly Task The concept augments using [ ] to map [ ] into [ ] by artificially adding additional constraints, creating the sticky point contact scenario. For case = 1 Table 3 1 shows the following kinestatic model [ ] = cos ( ) sin ( ) 0 [ ] = sin ( ) 0 cos ( ) 0 0 1 ( 3 79 ) while Table 3 2 shows the kinestatic model [ ] = 1 0 0 1 0 0 [ ] = 0 0 1 ( 3 80 ) It should be appreciated that the Table 3 2 version of the kinestatic model is equivalent to (same column space as) [ ] = sin ( ) cos ( ) cos ( ) sin ( ) 0 0 [ ] = 0 0 1 ( 3 81 ) for any value of This second version clearly shows that the first column of [ ] of Equation 3 79, say creates the first column of [ ] say Geometrically, this results from a mapping of twist to wrench that can be expressed as = [ ] ( 3 82 ) where [ ] is a 3 3 identity matrix. This is equivalent to an elliptic polarity as discussed by Lipkin (1985). The unit force combines with the single column [ ] of PAGE 78 78 Equation 3 79 to create [ ] It should be noted that the elliptic polarity in this case depends on point F being the origin. Likewise, a second elliptic polarity for case = 2 depends on point G being t he origin. Using a congruency transformation as Lipkin (1985) described, this second elliptic polarity could have been reexpressed using the point F as the origin. The elliptic determinator strategy provided a very interesting option though it was not as geometrically viable as the predominately used determinator approach. Figure 3 1 Assembly t ask supervisor. Assembly Task Supervisor Given: Payload and target geometries Desired endmating specification for payload and target Nominal threshold forces & torques for determining contact Nominal forces and torques for maintaining contact Maximum work volume for payload Nominal displacements for payload (to scale twist of freedom) Contact cases (the [ ] table) Feedback from kinestatic controller: Current sensed wrench Current pa yload location Current stiffness matrix [ ] Command to u ser: Current contact case Target location estimate Payload location Command to kinestatic controller: Desired wrench of constraint Next twist of freedom Current contact or kinestatic model PAGE 79 79 Figure 3 2 Contact s tates. 2 bodies approaching Force = 0 2 bodies i n contact Initial force = 0 Contact established: > Contact maintained : > ensures that contact is maintained A A B B B A 2 bodies separating (decoupled) Force = 0 A B 2 bodies separating (but stuck together) Negative contact force A B B A Positive contact force Unidirectional or unilateral Bidirectional or bilateral PAGE 80 80 Figure 3 3 Twist in the plane. Figure 3 4 Coordinate system for payload. F G Instantaneous center of rotation (I.C.) P PAGE 81 81 Figure 3 5 Free s pace m otion. F G Global coordinate axes Payload PAGE 82 82 Figure 3 6 Single point c ontact at p oint F Figure 3 7 Single point c ontact at p oint G. Figure 3 8 Two p oint contact. payload F G t angency line target payload F G T angency line Target payload F G T angency line Target PAGE 83 83 Table 3 1 Contents of the various system matrices: [ ], [ ], [ ] [ ], and [ ], in the context of each of the five contact cases for the determinator approach. Case number [ ] Wrenches of constraint = angle between and [ ] Twists of freedom Forward Twist 3 3 [ ] Symmetric, (+) definite, yields [ ] 3 3 [ ] rank 3 Project onto [ ] and [ ] is the sensed wrench Heuristic decision tree Case = 0 : Unconstrained [ ] = [ ] = [ ] s c 0 < Case = 1 : Conventional point contact at F [ ] = cs0 [ ] = s0 c0 0 1 0 0 1 [ ] = [ ] [ ] = [ ] = c s0 = [ ] = [ ] = [ ] [ ] and is minimum of set = = 1 3 and < Case = 2 : Conventional point contact at G [ ] = cs s [ ] = s0 c 0 1 0 1 [ ] = [ ] [ ] = [ ] = cs s = [ ] = [ ] = [ ] [ ] and is minimum of set = = 1 3 and < Case = 3 : Two point contact at F,G Line contact [ ] = 0 0 1 1 0 [ ] = 1 0 0 1 0 0 [ ] = [ ] [ ] = [ ] = 0 0 1 1 0 = [ ] = [ ] [ ] = [ ] and is minimum of set = = 1 3 and < Case = 4 : Fully constrained [ ] = [ ] [ ] = [ ] = [ ] = 1 0 0 0 1 0 0 0 1 = and is minimum of set = = 1 3 and Note: s= sin ( ) c= cos ( ) s= sin ( ) and c= cos ( ) PAGE 84 84 Table 3 2 Contents of the various system matrices: [ ], [ ], [ ] [ ], and [ ], in the context of each of the five contact cases for the elliptic determinator approach. Case number [ ] Wrenches of constraint [ ] Twists of freedom Forward Twist 3 3 [ ] Symmetric, (+) definite, yields [ ] 3 3 [ ] rank 3 Project onto [ ] and [ ] is the sensed wrench Heuristic decision tree Case = 0 : Unconstrained [ ] = [ ] = [ ] s c 0 < Case = 1 : Enhanced point contact at F [ ] = 1 0 0 1 0 0 [ ] = 0 0 1 0 0 1 [ ] = [ ] [ ] = [ ] = 1 0 0 1 0 0 = [ ] = [ ] [ ] = [ ] and is minimum of set = = 1 3 and < Case = 2 : Enhanced point contact at G [ ] = 1 0 0 1 0 [ ] = 0 1 0 1 [ ] = [ ] [ ] = [ ] = 1 0 0 1 0 = [ ] = [ ] [ ] = [ ] and is minimum of set = = 1 3 and < Case = 3 : Two point contact at F,G Line contact [ ] = 0 0 1 1 0 [ ] = 1 0 0 1 0 0 [ ] = [ ] [ ] = [ ] = 0 0 1 1 0 = [ ] = [ ] [ ] = [ ] and is minimum of set = = 1 3 and < Case = 4 : Fully constrained [ ] = [ ] [ ] = [ ] = [ ] = 1 0 0 0 1 0 0 0 1 = and is minimum of set = = 1 3 and Note: s= sin ( ) and c= cos ( ) PAGE 85 85 CHAPTER 4 EXPLICIT KINESTATIC CONTROLLE R Live It, Breath e It. Mike Griffis, advice about ones own research, personal communication Overview The role of the kinestatic controller was to calculate desired leg forces for the parallel robotic mechanism given the desired constraint wrench and twist of freedom interacting between the payload and the target. This involved performing calculations to determine the current payload position and orientation, the current stiffness matrix [ ] of the parallel mechanism, and the resultant wrench exerted by the payload on the target. In this study, the parallel robotic mechanism used for payload actuation cons isted of three legs acting in a plane. A parallel mechanism was used because of the increased load carrying capability, as opposed to the more familiar serial or anthropomorphic device. The kinestatic controller accepted information from the assembly task supervisor, the metrology frame, and the leg force controllers and performed intricate calculations to determine desired leg force set points. Additionally, the controller provided feedback of the current payload location, the current sensed wrench, and c urrent stiffness matrix to the assembly task supervisor. A black box view of the kinestatic controller is provided in Figure 4 1 The assembly task supervisor specifically provided the desired constraint wrench, the desired twist of freedom, and the current kinestatic model. These data identified geometrically how the payload was currently touching the target (i.e. kinestatic model). Given that the payload was partially constrained with respect to the target, a wrench of PAGE 86 86 constraint interacted between the two bodies. The kinestatic controllers first responsibility was to control this wrench, making sure that it tracked the desired constraint wrench. Given that there was also a desired twist of freedom provided, the kinestatic controllers second responsibility was to control (or effect) the desired payload displacement. It satisfied both responsibilities simultaneously by providing explicit for ce set points, or desired leg forces, to the leg force controllers. The transformation of the supervisor inputs into desired leg forces required detailed calculations relying on the kinestatics of contact and the geometry and physics of stiffness of the parallel mechanism. The following analysis describes this process as well as introducing and describing the term explicit kinestatic control. Figure 4 2 illustrates the planar parallel mechanism, also known as the planar mechanism which was used for accomplishing the explicit kinestatic control. Since a parallel mechanism was used, there was a complexity involved in executing a forward analysis, i.e calculating the position and orientation of the payload given the leg lengths. To simplify this aspect but maintain the desired use of leg forces acting in a parallel mechanism, a metrology frame was employed. The metrology frame independently provided direct measurement of the position and orientation of the payload (shown as independently acting in Figure 4 2 ). In other words, the metrology fram e simply provided the coordinates of point F, ( ), and the orientation of the payload, ( ), as measured in the global coordinate system located at the base of the actuation frame. In what follows, the instantaneous values of and w ere therefore known. PAGE 87 87 In the special case of unconstrained motion (noncontact), the explicit kinestatic controller operated in an alternate mode ( Figure 4 1 ). Instead of commanding leg forces, the kinestatic controller commanded coarse leg velocities to the leg force controllers. Implementation of this special case was straightforward. In what follows, the more common, general case of partial constraint is presented, which results in the determination of explicit leg force setpoints. In the broadest sense, explicit kinestatic control used a general wrench in order to correct for an error in the constraint wrench and to simultaneously effect the desired twist of freedom. The constraint wrench error was combined with a deflection wrench, which was uniquely related to the desired twist of freedom. This approach was in contrast to an alternate approach, implicit kinestatic control, which involved the use of a general twist to simultaneously implement a twist of freedom and a desired chang e in the constraint wrench. The explicit kinestatic controller additionally incorporated wrench offsets into the general wrench, e.g. for a static offset or for gravity compensation. In the following, details for how the explicit kinestatic controller functions are presented. Griffis (2009) first presented the concepts for explicit kinestatic control, deflection wrench, force charge, and wrench charge. The following extends those concepts in a consistent manner. While developing the theory set forth her e, two objectives were identified. One objective was to verify that the correction for constraint wrench error did not cause a displacement of the payload. A second objective was to verify that the deflection wrench did not cause a change in the constrai nt wrench. For the controller to properly function, and hence be physically and geometrically meaningful, it was important that PAGE 88 88 these two objectives were satisfied. When satisfied, the two corrective actions could be combined by a principle of superposit ion. In this way, the verifications qualified the explicit kinestatic controller as meaningful, and they qualified the resulting leg force setpoints as meaningful as well. Resultant Wrench: A Function of Leg Forces A resultant wrench was defined as t he combination of the three leg forces = 1 2 3 acting along the three lines of the legs. ( Figure 4 3 .) The coordinates of the unit forces o f the three legs were given by = [ ] ( 4 1 ) where = cos ( ) and = sin ( ) where is the orientation of the leg with respect to the axis, where = where = = = = 0 = and = and finally where and are the coordinates of the points D and E. The resultant can then be written as = 0 + + ( 4 2 ) This can be reorganized to yield = 0 ( 4 3 ) or more briefly = [ ] ( 4 4 ) where PAGE 89 89 [ ] = 0 ( 4 5 ) = ( 4 6 ) and = [ ] ( 4 7 ) As the above resultant wrench was the combination of the forces applied by the platform to the legs the wrench given by was taken as the wrench applied to the payload by the legs A free body diagram (Neff 1961) of the payload revealed that with the payload in equilibrium, + ( ) = 0 ( 4 8 ) where the wrench was taken as the combination of all wrenches applied to the payload excluding leg forces. For example, is the combination of constraint wrenches, gravity wrenches, or DAlembert wrenches etc. ( Figure 4 4 ). Rewriting Equation 4 8 yields = and this illustrates that the resultant wrench of all forces applied by the platform to the legs was equal to the combination of all wrenches applied to the payload excluding leg forces. Reverse Analysis Summary The preceding development required the sine and cosine of the leg angles with respect to the base coordinate frame. Calculation of the leg angles required intermediate calculations of the individual leg lengths. Referring to Figure 4 3 it was apparent that several distances were constant. Namely, the distances between the global coordinate axis (O) and cylinder pins D and E. Distances between cylinder pins PAGE 90 90 A to B and A to C were also constant as well as the distance between points F and G. Reflecting the coordinates of cylinder pins A, B, and C to the global coordinate system involved a transformation from the payload coordinate system. Namely, = + ( 4 9 ) = + ( 4 10 ) and = + ( 4 11 ) Note that = = since points A, B, and C have the same offset in the payload coordinate system from point F. In the above = cos ( ) and = sin ( ) Each leg length was calculated using the Pythagorean Theorem, namely, = + ( 4 12 ) = ( ) + ( 4 13 ) and = ( ) + ( 4 14 ) Calculations of the sines, cosines, and angles were also straightforward. Namely, = = = 2 ( ) = ( ) = = 2 ( 4 15 ) and = ( ) = = 2 ( ) PAGE 91 91 where 2 ( ) is the four quadrant arctangent function, = cos ( ) and = sin ( ) Gravity Wrench Compensation Figure 4 3 further illustrates a wrench due to gravity, given by This wrench is given by = 0 1 ( 4 16 ) where is the weight of the payload, and where is given by = + c s ( 4 17 ) where and are the payload local coordinates for the center of mass of the payload. The kinestatic controller compensated for this wrench by subtracting its effect from the resultant wrench in the following way: = ( 4 18 ) which yields the sensed wrench given by It was this sensed wrench that represented the resultant of all contact wrenches applied by the target to the payload. The wrench was given in terms of the global coordinate system illustrated in Figure 4 3 The wrench actually used by the assembly task supervisor was a transfor med version, which was related to by = [ ] ( 4 19 ) where [ ] = c s s c s c s + c 0 0 1 ( 4 20 ) PAGE 92 92 This transformation was required because the assembly task supervisor used the payload coordinate system at point F. The negative sign in Equation 4 19 results from the sign convention presented in Chapter 3 where a positive force represented a payload force applied to the target. The in Equation 4 19 is conceptually equivalent to the in Chapter 3. Relating Twist to Leg Displacements From the given location of the payload with respect to the fixed reference frame, i.e. ( ) a nd it was straightforward to determine the instantaneous orientations and lengths for the three Legs = 1 2 3 by means of a reverse kinematic analysis. The lengths corresponded respectively with the distances to to and to and are shown in Figure 4 2 With denoting how the payload twists relative to the fixed reference frame, the objective was to determine corresponding changes in leg lengths as well as corresponding changes in leg orientation angles In Chapter 3, it was shown that the axis coordinates in the plane of a unit rotation were given by = [, 1 ], where were the global system coordinates of the instant center of rotation. Also in Chapter 3, it was shown that the ray coordinates in the plane of a unit force were given by = [ ] ( 4 21 ) where and were the direction cosines of the line of action of the force and where was the perpendicular distance of the line to the origin. It is well known that the shortest (or perpendicular) distance between the line and the instant center is given by PAGE 93 93 = + ( 4 22 ) which can be written as = ( 4 23 ) Predicated on the unit rotation, Equation 4 23 was subsequently scaled by to yield = = ( 4 24 ) where the substitution for infinitesimal twist = [, 1 ]= was used. Chapter 3 identified the as an infinitesimal rotation of the payload with respect to the target. Chapter 3 further identified the infinitesimal twist using the homogeneous coordinates = [ ], and consequently identified a pure translation when = 0 This made = of Equation 4 24 remain valid, regardless of whether the infinitesimal twist was a pure rotation or a pure translation. Scaling to create the above generic Equation 4 24 provided a means to project an infinitesimal twist onto a generic line, and this created the desired component of infinitesimal displacement of points on that line. Equation 4 24 was applied multiple times as the following describes in order to obtain the desired relationships. The points and of the payload shown in Figure 4 2 were designated as moving pivot points for the Leg s 1, 2, and 3 respectively. The desired relationships were obtained by considering the displacements of these moving pivot points as the p ayload rotated about the instant center of rotation located at ( ) in terms of the global coordinate system. The leg had the line of action given by from Equation 4 1 Based on the twist given by any point in the payload lying on this line will displace with a component in the direction of the line. The value is shown in Figure 4 5 as the PAGE 94 94 perpendicular distance of the line of action to the instant center. One such point lying on this line of action was of course the moving pivot point and as such yielded the desir ed relationship = Using the generic Equation 4 24 this yield ed the relationship = = ( 4 25 ) Figure 4 5 further show ed a second line at the moving pivot point for the leg given by = + ( 4 26 ) which was perpendicular to the line of the leg. (Note the significance of the first two coordinates of Equation 4 26 with respect to Equation 4 1 .) Based on the twist given by any point in the payload lying on this second line will displace with a component in the direction of the second line, where is the perpendicular distance of the second line to the instant center. One such point on the second line was of course the moving pivot point and as such this yields the second desired relationship in the form = Again, using Equation 4 24 this yields the relationship = = ( 4 27 ) Applying Equations 4 25, 4 26, and 4 27 to the three legs yields the following desired relationships: = [ ] = [ ] ( 4 28 ) and PAGE 95 95 = [ ] ( 4 29 ) where [ ] = + + ( 4 30 ) and where [ ] was given by Equation 4 5 Small Changes, Infinitesimal Changes, and Differential Calculus It was important to appreciate the view taken in this research, regarding infinitesimal changes, infinitesimal displacements, and the like. These changes were small, non zero, finite and actually used that way in practice. Mathematically, as they tend to zero, they may be regarded as differentials or, more precisely, total differentials (Johnson and Kiokemeister 1960) For example, Equation 4 28 may be regarded as the total differential of leg lengths with respect to displacement and orientation. Specifically, the first row of Eq uation 4 28 relates the differential to the differentials or infinitesimal changes and In light of this, elements of [ ] were viewed as der ivatives, or more precisely, partial derivatives. (For example, the upper left component of [ ] was ) In what follows, the term change is often used interchangeably with differential or total differential and this should be clear from t he context. Stiffness: Geometry and Physics The stiffness mapping produced the change in resultant wrench which corresponded to a provided twist: = ( 4 31 ) PAGE 96 96 where is the twist, is the change in resultant wrench, and where [] is the 3 3 stiffness matrix. The stiffness matrix [] possessed the properties of being nonsingular and positivedefinite. However, it is well known to be asymmetric in general. Pigoski et al (19 9 8 ) demonstrated that the mapping Equation 4 31 depended on the frame of reference. This meant for a given twist, the change in resultant wrench depended on which reference frame was used. The mapping Equation 4 31 was initially derived for the fixed frame of reference, which resulted in an asymmetric matrix. Subsequently, the mapping was reconfigured so that a symmetric matrix was obtained, whi ch corresponded with what was called a symmetric body reference frame1. This yields the more familiar presentation = [ ] ( 4 32 ) where [ ] is the familiar 3 3 symmetric matrix, taken as invertible and positivedefinite. While this research emphasized geometry, it was important from time to time to review mathematically the basis of these relationships. For example, in the previous sections, the geometrical and mathematical meanings of Equation 4 28 were discussed. In Equation 4 32, th e geometrical mapping related an infinitesimal twist (a point) to a change of wrench (a line), thereby establishing a projective correlation. (Griffis 199 1, Crane et al 1995, Cran e and Duffy 1998, Duffy 1996, and Gibson and Hunt 1990 .) On the other hand, mathematically, the Equations 4 31 and 4 32 may be regarded as total differ entials and this actually revealed [ ] as a matrix of partial derivatives. 1 For the purpose here, consider the symmetric body as a fictitious body or a virtual body that provides the analytical relationship given by Equation 4 32. PAGE 97 97 The objective of what follows was to fully determine the above relationships of Equations 4 31 and 4 32. Each of the legs was considered to be a hydraulically operated cylinder in line with a passi ve, compliant linear spring of stiffness As the cylinder was much stiffer than the spring, the stiffness of each leg was taken as which yield ed the relationship = ( 4 33 ) where is the differential change in leg force. To obtain the stiffness mapping Equation 4 31 a differential of Equation 4 4 was taken (using Equation 4 7 for ) which yields = [ ] + [ ] ( 4 34 ) where [ ] = 0 ( 4 35 ) The above technique for defining [ ] and embedding the into the column vector on the right handside was appreciated when contemplating the differential of [ ] namely [ ] which was [ ] = 0 ( 4 36 ) or [ ] = 0 ( 4 37 ) or PAGE 98 98 [ ] = [ ] [ ] ( 4 38 ) where [ ] = ( 4 39 ) The elements of [ ] were then incorporated into the column vector on the right handside of Equation 4 34 Based on the preceding discussions of differentials and derivatives, the matrix [ ] contained components that were partial derivatives. ( Equation 4 38.) Note that the columns of [ ] locate lines perpendicular to the legs at the fixed pivot points (O, D, and E in Figure 4 2 ). In other words, the lines of [ ] were actually viewed as the geometric derivatives of the lines of [ ] A more convenient representation of Equation 4 34 was used: = [ ] [ ] + [ ] [ ] ( 4 40 ) where [ ] = ( 4 41 ) [ ] = ( 4 42 ) and where Equation 4 33 was used. Using the kinematic substitutions from Equations 4 28, 4 29 and 4 30, Equation 4 40 was written in the desired form = [ ( 4 31) ] PAGE 99 99 where = [ ] + [ ] ( 4 43 ) and [ ] = [ ] [ ] [ ] ( 4 44 ) and [ ] = [ ] [ ] [ ] ( 4 45 ) It was important to note that as given above was asymmetric due to the asymmetric term [ ] since [ ] [ ]. It was straightforward to decompose [ ] into a symmetric part [ ] and a sk ew symmetric part [ ] : [ ] = [ ] + [ ] ( 4 46 ) where [ ] = [ ] [ ] [ ] + [ ] ( 4 47 ) [ ] = 1 2 ( 4 48 ) [ ] = 1 2 ( [ ] + [ ] ) ( 4 49 ) and [ ] = 1 4 ( 4 50 ) where substitutions for were made from Equation 4 2 The above was verified using [ ] = ( [ ] + [ ]) and [ ] = ( [ ] [ ]) which relied on using well known relationships for extracting symmetric and skew symmetric parts of an asymmetric matrix. PAGE 100 100 It should be noted that the columns of [ ] locate lines that are perpendicular to the lines of the legs at the midpoints of the legs. This was found to be consistent with Pigoski (1998 ) and produced an assoc iated symmetric body stiffness mapping given by = [ ] [ ( 4 32 ) ] where [ ] was defined as the symmetric part of [], which was [ ] = + Using Equations 4 43 and 4 46, this was written as [ ] = [ ] + [ ] ( 4 51 ) It was instructive to further investigate the genesis of [ ] First, the matrix [ ] was written in terms of [ ] [ ] = [ ] + [ ] ( 4 52 ) where [ ] was a sparse matrix with leg lengths in the last row, [ ] = ( 4 53 ) Using [ ] = ( [ ] + [ ]) together with Equation 4 45, the matrix [ ] was written as [ ] = ( [ ] [ ] [ ] + [ ] [ ] [ ] ) = ( [ ] [ ] ( [ ] + [ ] ) + ( [ ] + [ ] ) [ ] [ ] ) = [ ] [ ] [ ] + ( [ ] [ ] [ ] + [ ] [ ] [ ] ) ( 4 54 ) Using Equation 4 52 Equation 4 49 was written as [ ] = [ ] + [ ] ( 4 55 ) which was then used in the following: PAGE 101 101 [ ] [ ] [ ] = [ ] + [ ] [ ] [ ] + [ ] = [ ] [ ] [ ]+ ( [ ] [ ] [ ]+ [ ] [ ] [ ]) + [ ] [ ] [ ] = [ ] + [ ] [ ] [ ] ( 4 56 ) where Equation 4 54 substitution was made. Rearranging Equation 4 56 yields [ ] = [ ] [ ] [ ] 1 4 [ ] [ ] [ ] ( 4 57 ) which was equivalent to Equation 4 47 It was further instructive to investigate the skew symmetric part [ ] which was given by [ ] = 1 2 ( [ ] [ ] ) ( 4 58 ) and using Equation s 4 45 and 4 52, Equation 4 58 can be re written as [ ] = ( [ ] [ ] [ ] [ ] [ ] [ ] ) = ( [ ] [ ] ( [ ] + [ ] ) ( [ ] + [ ] ) [ ] [ ] ) = ( [ ] [ ] [ ] [ ] [ ] [ ] ) ( 4 59 ) Making substitutions for [ ] [ ] and [ ] and then simplifying yields [ ] = 1 2 ( 4 60 ) where Equation 4 2 was also used. Using Equations 4 43 4 46, 4 51, the mapping Equation 4 31 was written as = [ ] ( 4 61 ) w here Equation 4 32 was substituted and where the operator [ ] was a 3 3 skew symmetric matrix given by PAGE 102 102 [ ] = ( 4 62 ) The expression [ ] was a planar equivalent version of a screw cross product as the following describes. In a brief excursion into threedimensional space, it is well known that the ray ray (read as ray cross ray) screw cross product of two screws, = and = where are direction cosines and where are moments, is given by = = + ( 4 63 ) where for example is the well known cross product for vectors. Note that Brand (1947) refers to this as a motor product. This is equivalent to the matrix form: = ( 4 64 ) where was a 6 6 matrix partitioned into four 3 3 submatrices. For example, the operator is a wellknown skew symmetric 3 3 matrix given by = ( 4 65 ) and where = [ ]. The operator in Equation 4 65 functions in such a way that = ( 4 66 ) which was employed three times in the generation of Equation 4 64 Using the axis to ray coordinate transform, as used in Equation 3 6 namely using the operator PAGE 103 103 [ ] = ( 4 67 ) where was a 3 3 identity matrix, Equation 4 64 was manipulated into a form that allowed the second screw to be provided in axis coordinates, : = = = ( 4 68 ) The preceding notation for a screw cross product equates a ray ray screw cross product with the matrix product of a 6 6 skew symmetric matrix (created from the first screw) times the axis coordinate representation of the second screw Consider that Equation 4 68 is applied to the screw cross product of the resultant wrench and the infinitesimal twist, where the resultant wrench ( ) was given in ray coordinates and the twist ( ) was given in axis or equivalent ray ( = [ ] ) coordinates. Recall, from the plane, we have: = 0 0 0 and = [ 0 0 0 ]. Substituting these into Equation 4 68 yields =       ( 4 69 ) PAGE 104 104 As was typical in the plane, due to sparseness, the short hand notation was used where zero components were not shown resulting in the identical, three component planar version: [ ] ( 4 70 ) where Equation 4 62 was used. Equation 4 70 demonstrated that the second term of Equation 4 61 was identical to or + In light of this, Equation 4 61 was written as = + ( 4 71 ) As discussed in Brand (1947), an equation such as Equation 4 71 relates the differential as seen by an observer on a first reference frame to the differential as seen by another observer located on a second reference frame, where the second re ference frame twists relative to the first by the amount or, in the planar notation, Here the differentials and specifically denoted reference framedependent changes in resultant wrench, where the resultant wrench was given in Equation 4 71 by which was the same as of Equation 4 2 The assembly task supervisor actually used a transformed version of Equation 4 32, as it employed the payload as the reference body: = [ ] ( 4 72 ) where the double daggers denoted that the elements were expressed in terms of the payload coordinate system. Chapter 3 further identified a compliance matrix, [ ] say, which being consistent with Equation 4 72 was identified as [ ] = [ ] PAGE 105 105 It was considered that typical wrenches (or change in wrenches) transform according to = [ ] ( 4 73 ) where [ ] is given in Equation 4 20, and that typical twists transform according to = [ ] ( 4 74 ) where [ ] was related to [ ] as the following describes. When a given wrench and twist pair are reciprocal, then = 0 As they would remain reciprocal in the transformed coordinates, the relation ( [ ] ) [ ] = ( ) [ ][ ] = 0 holds. In order for ( ) = 0 to be true for any reciprocal wrench and twist pair, then [ ] [ ] = [ ] ( 4 75 ) or [ ] = ( [ ] ) = [ ] ( 4 76 ) Substituting Equation s 4 73 and 4 74 into Equation 4 32 and using Equation 4 76 yields Equation 4 72 where [ ] = [ ] [ ] [ ] ( 4 77 ) set s the familiar congruency transformation to provide the desired relation. In summary, the two mappings Equation s 4 31 and 4 32 identify the changes in resultant wrench as seen from two different reference frames: the fixed reference frame and a second reference frame, referred to as a symmetric body reference frame in Pigoski (19 9 8 ). Pigoski demonstrated that the symmetric mapping Equation 4 32 was useful for relations based on potential energy. As identified in Equation 4 71 the symmetric body reference frame twists with respect to the fixed frame (or in the planar notation). One should recall that the payload (moving body) twists the full PAGE 106 106 with respect to the fixed frame. It is instructive that the symmetric body reference frame twists half as much. Force Charges and Wrench Charges In the applic ation of a given leg of the parallel mechanism, it was necessary to consider the changes in leg length and leg force as the hydraulic cylinder extended or retracted. An effective free length was defined as where = ( 4 78 ) and was taken again as the pinto pin distance. In this case was the force carried in the leg, and was the stiffness of the passive spring. The value was then considered an adjustable length in the equation = ( ) ( 4 79 ) Equation 4 79 took into consideration the fact that the cylinder could be extended or retracted. The following presents how the value of was related to the active aspect required to explicitly cont rol force. Figure 4 6 identifies such a leg with a cylinder in line with a passive spring. The actual length of the passive spring was designated as and the free or unloaded passive spring length was designated Thus, the leg force was also written as = ( ) ( 4 80 ) Considering a change in Equation 4 80 yields the relation = ( 4 81 ) From Equation s 4 79 and 4 80 the following was derived: = ( ) ( 4 82 ) PAGE 107 107 Small changes in yields the relation (remember that the passive spring length was constant): = ( 4 83 ) Multiplying Equation 4 83 by yields the relation = ( 4 84 ) where Equation 4 81 was used and where the force charge was defined as = ( 4 85 ) Equation 4 85 demonstrates how the active adjustment of leg length directly provided the desired active adjustment of leg force (referred to herein as a force charge). It was noted that when there was no force charge ( = 0 ), Equation 4 84 reduced to Equation 4 33. This signified that when no active force component was applied, the result was only a passive response. From the perspective of explicit force control, it was important to introduce the concept of force charge since the application of force charges will be necessary in each leg of the parallel mechanism in order to make the necessary corrections for both con straint wrench error as well as payload location errors, effecting a desired twist of freedom. Taking the differential of Equation 4 79 yields = ( 4 86 ) Figure 4 6 further identifies the environment with a stiffness and, broadly speaking, this represented what the leg was pushing against. Since a positive leg displacement tended to compress the environment, there was a change in sign such that = ( 4 87 ) PAGE 108 108 Equating Equation s 4 86 and 4 87 yield s = ( 4 88 ) and this can be rearranged such that = + ( 4 89 ) Substituting the results of Equation 4 89 into Equation 4 87 yield s a second instructive relationship: = + ( 4 90 ) where Equation 4 85 was used. A summary for the various leg parameters is provided in Table 4 1 The abov e was used to explain a few instructive scenarios regarding First, if the leg was pushing against a very soft environment (or nonexistent target), then 0 In this case, Equation 4 89 showed that = and Equation 4 90 showed that = 0 Starting from an unloaded situation, this meant that = and = 0 or the pin to pin distance tracked the effective free length. On the other hand, if the leg was pushing against a very stiff environment, for example then Equation 4 89 showed that 0 and Equation 4 90 showed that = This meant that a changing effective f ree length (or a force charge) was met with a constant pinto pin length for the leg coupled with an increasing force internal to the leg. (The hydraulic cylinder change in length was met with an equal and opposite change in spring length.) Returning to t he actuator platform and applying the above concept to all legs in Equation 4 4 yields the relation PAGE 109 109 = [ ] = [ ] [ ] ( 4 91 ) where Equation 4 41 was used and where Equation 4 79 was applied to each of the three legs. Taking the differential of Equation 4 91, as seen by an observer on the fixed reference, yields = [ ] [ ] + [ ] [ ] ( ) ( ) ( ) ( 4 92 ) or = [ ] [ ] + [ ] ( 4 93 ) or = [ ] [ ] [ ] [ ] + [ ] [ ] ( 4 94 ) or = ( 4 95 ) In the above, Equations 4 28, 4 29 and 4 42 through 4 45 were used. Also, in the above, wrench charge was defined as = [ ] ( 4 96 ) where are the individual force charges for the leg. Using Equation 4 85, this was alternatively written as PAGE 110 110 = [ ] [ ] ( 4 97 ) In preparation for the sections that follow, it proved useful to consider a scenario where the location of the payload was specified ( were given) and also the resultant wrench was specified ( was given). Using Equations 4 9 through 4 15 and Equation 4 5 the [ ] and of Equ ation 4 91 could be calculated. With [ ] also specified, the following relationship was determined: = [ ] [ ] ( 4 98 ) E quation 4 98 identified the unique set of that would be necessary to provide the specified resultant wrench at the specified payload location. In further preparation for the sections that follow, i t proved useful to rearrange and modify Equation 4 95 as the following describes. First, Equation 4 95 was rearranged such that the wrench charge was on the left hand side: = ( 4 99 ) It was appreciated that the differentials on the right hand side were seen by an observer on the fixed reference frame. In the first term of the right hand side, the following matrix factor was determined using Equations 4 43 4 46, 4 51, 4 60 and 4 62: = [ ] [ ] ( 4 100 ) Regarding the second term on the right hand side, Equation 4 61 retained its relevance by relating the differential of the resultant wrench as seen from two different reference frames: the fixed and symmetric body references. Equation 4 61 is repeated here for convenience : PAGE 111 111 = [ ] [ ( 4 61) ] Substitution of these results into Equation 4 99 yields = [ ] ( 4 101 ) where the cross product operator terms cancel. A useful outcome of this cancellation was that the wr ench charge was independent of the reference frame used to calculate differentials. It was noted that when there was no wrench charge ( = 0 ), Equations 4 99 and 4 101 reduced to Equations 4 31 and 4 32 respectively. This signified that when no active wrench component was applied, the result was only a passive response. In summary, t he wrench charge defined in Equation 4 96 represented a coordinated adjustment of actuator leg force charges The coordinated aspect was further appreciate d when Equation 4 101 was compared to Equation 4 84 : = [ ( 4 84 ) ] = [ ] [ ( 4 101 ) ] since Equation 4 101 was a wrench equivalent expression of the scalar Equation 4 84 One could think of and as the control inputs to the physical system that actively adjusts force and displacement simultaneously. Broadly speaking each relation equated a charge with a balance of some displacement and some change in load. The balance that results, i.e. how much displacement and how much change in load, depended on the nature of the target environment, or how the payload was parti ally constrained, what the payload was pushing against and where. In terms of how the force charge balanced, the results PAGE 112 112 were given in Equation s 4 89 and 4 90 The specifics of how the wrench charge balanced are provided in the following sections. Correcting for Constraint Wrench Error Consider the partially constrained system (i .e. payload touching target) to be in static equilibrium at a particular instant, that is = [ ] where 0 [ ] being a 3 matrix, and being a 1 matrix ( E quation 3 34 and the associated discussion in Chapter 3 illustrate d partial constraint examples where [ ] and were defined.) This section discusses controlling a constraint wrench, specifically correcting for constraint wrench error. As such, a reference constraint wrench was = [ ] ( 4 102 ) where the second subscript denoted reference. Next, a sensed constraint wrench was = [ ] ( 4 103 ) where the second subscript denote d sensed. The error in constraint wrench was then defined as the difference between the reference and sensed constraint wrenches, or = ( 4 104 ) where the second subscript denoted error and where the represented the constraint wrench error. For the purposes of this section, constraint wrench errors were considered small, i.e. infinitesimal, or could be rescaled to meet this requirement. Equation 4 104 was found to be equivalent to = [ ] [ ] ( 4 105 ) or PAGE 113 113 = [ ] ( ) ( 4 106 ) or = [ ] ( 4 107 ) where = ( 4 108 ) In the above, were associated scalars for the constraint wrench error. Now consider the case where the wrench charge was proportional to a constraint wrench error, that is = [ ] ( 4 109 ) where the subscript identified a constraint wrench error wrench charge. The following discusses how this wrench charge was used to effect a correction for constraint wrench error Consider the actual constraint wrench to be = and recall Equation 3 34 for the constraint wrench as previously introduced in Chapter 3. This yields = = [ ] ( 4 110 ) The differential of the actual constraint wrench was taken as follows: = [ ] + [ ] ( 4 111 ) In E quation 4 111 denotes an infinitesimal change in constraint wrench, denotes an infinitesimal change in constraint wrench scalars, and [ ] denotes an infinitesimal change in the [ ] matrix due to movement (or a twist) of the payload with respect to the target. (Recall that infinitesimal change or change was considered interchangeable with differential.) PAGE 114 114 More specifically, the product [ ] was found to be equivalent to a second produc t: [ ] = [ ] ( 4 112 ) where the were the scalars of an adapted Equation 3 35 = = [ ] ( 4 113 ) where [ ] was a 3 matrix. Substituting the results from Equation 4 112 into Equation 4 111 yields the relationship = [ ] + [ ] ( 4 114 ) The matrix [ ] is presented as follows. In order to determine [ ] it was necessary to first assume that the column of [ ] was The represented the total differential2 of where was the unit force (or unit torque) of the column of [ ]. The following was then considered: = [ ] ( 4 115 ) where [ ] was a 3 matrix, and was the number of freedoms. Consider that Equation 4 115 holds for = 1 where represented the number of constraints. More particularly, the column of [ ] was the geometric derivative for how the unit force changed based on the infinitesimal change (In other words, the 2 One can appreciate the notion of total differential in the present context when considering a general vector f unction, say = ( ) ( ) The total differential can be written = which is given in matrix form. Now, conceptually replace with replace with [ ] and replace with The result of doing so conceptually yields Equation 4 115. PAGE 115 115 column of [ ] was .) In light of this and applying Equation 4 115 to all = 1 we have [ ] = = [ ] = [ ] ( 4 116 ) where [ ] = [ ] ( 4 117 ) The above identifies [ ] as a weighted sum of geometric derivatives, where the weighting factors are Since [ ] corresponded to a derivative, it was necessary to acknowledge that it was reference frame dependent. Likewise, [ ] was reference frame dependent, since it relied on [ ] being components. In this respect, [ ] acted like a derivative. A short excursion into generic wrench differentials as seen from different reference frames is presented below. Relevant examples for [ ] are then presented. It was useful to consider a generic wrench as seen from three different reference frames: fixed (base or target) moving (payload), and symmetric (Footnote 1 on page 96). A generic wrench in the plane, was seen changing in the three different reference frames, where the moving frame twisted relative to the fixed frame by amount the fixed frame twisted relative to the symmetric frame by the amount an d the moving frame twisted relative to the symmetric frame by the amount The differentials as seen from those reference frames were respectively and say. Those differentials were related to one another by the following: = [ ] ( 4 118 ) PAGE 116 116 = + [ ] ( 4 119 ) = [ ] ( 4 120 ) where [ ] is the previously discussed cross product operator. E quations 4 118 through 4 120 can be generated by generically applying = [ ] i.e. Equation 4 61. Consider = [ ] and = [ ] where [ ] and [ ] are changes in as seen by observers on the moving and symmetric frames, respectively. Recall that = [ ] Substituting into Equation 4 120 and using in the cross product operator yields [ ] = [ ] [ ] [ ] ( 4 121 ) which for nonzero yields [ ] = [ ] [ ] [ ] ( 4 122 ) Similarly, consider = where denotes changes in as seen by an observer on the fixed frame. Using Equation 4 119 yields [ ] = + [ ] [ ] ( 4 123 ) For the example Case 1a discussed in Chapter 3, the [ ] and [ ] were given as [ ] = [ ] = c s 0 [ ( 3 43) ] and [ ] = s 0 c 0 0 1 [ ( 3 47) ] PAGE 117 117 which were specified in terms of the moving body reference frame and where for convenience, = cos ( ) = sin ( ) and was the first (only) column of [ ] Then, changes in as seen by a moving reference frame observer were given by [ ] = 0 s 0 c 0 0 ( 4 124 ) where are the scalars of Equation 3 35. Equation 4 124 can then be written as [ ] = [ ] ( 4 125 ) where [ ] = 0 s 0 c 0 0 ( 4 126 ) This was determined by examining the freedoms represented by the [ ] matrix, as the same scalars operated in Equation s 3 35 and 4 125 In this case, there were two degrees of freedom, which mean t = The first column of [ ] was all zeros, as the following describes. The corresponding column of [ ] was a pure translation perpendicular to the contact normal. The inclination of the contact normal remained unchanged during translation. Also during translation, the location of the contact normal remained through the origin of the moving reference frame. (In other words, an observer on the moving frame would not see the contact normal move because of the pure translation.) Thus, one could consider the angle as a constant and changes in were zeros. The second column of [ ] represents a pure rotation about the axis of the moving reference frame. The second column of [ ] corresponds to the effects of this rotation. A positive rotation of the payload with respect to the target about the axis PAGE 118 118 resulted in a decreasing contact normal angle This resulted in the negated derivative of namely ( [ s, c, 0 ]) as given in the second column of Equation 4 126 Examining Equation 4 122, it was necessary to generate the cross product operator, which using Equation 4 62 for [ ] resulted in [ ] = s c s c ( 4 127 ) It was then necessary to develop the cross product operator term given by [ ] [ ] = s c s c s 0 c 0 0 1 ( 4 128 ) or [ ] [ ] = 0 s 0 c 1 0 ( 4 129 ) Subsequently, as seen from the symmetric reference frame, applying Equation 4 122 for = 1 yields [ ] = 0 s 0 c 0 0 1 2 0 s 0 c 1 0 ( 4 130 ) or [ ] = 1 2 0 s 0 c 1 0 ( 4 131 ) The [ ] of Equation 4 131 times is the change in that an observer on the symmetric body would see at this instant given in terms of the moving coordinate system. It was instructive to reconsider Case 1a of Chapter 3 but this time as seen from the fixed reference frame. In this situation, the contact normal remained passing PAGE 119 119 through the moving point F but was canted in terms of the global coordinate system by say, where = + In light of this, Equati on s 3 43 and 3 47 may be rewritten [ ] = [ ] = ( 4 132 ) and [ ] = 0 1 ( 4 133 ) where again for convenience, = cos ( ) and = sin ( ) and where ( ) again denoted the location of point F on the payload in terms of the global coordinate system. From this perspective, the change in was written as = 0 0 0 0 1 0 ( 4 134 ) or = [ ] ( 4 135 ) where [ ] = 0 0 0 0 1 0 ( 4 136 ) The above was determined by considering how changed as seen from the fixed reference, namely as the payload translated per the first column of [ ] due to This resulted in the moment of decreasing correspondingly while maintaining constant orientation for contact normal. (Hence the first column of [ ] was given by [ 0 0 1 ].) Subsequently, the second column of [ ] was reported as all zeroes since the fixed reference frame did not see the contact normal change directions or otherwise PAGE 120 120 move as the payload rotated about the axis through point F that was parallel to the global axis. A more thorough examination of the first column of [ ] was conducted, specifically to address how the 1 was determined for the change in moment. The moment coordinate in was or The translation due to the first column of [ ] and due to the scalar displacement resulted in the coordinates of point F cha nging at the next instant to where = ( 4 137 ) and = + ( 4 138 ) This resulted in the moment at the next instant changing to In light of this, the desired change in moment was determined by the following: = = ( 1 ) ( 4 139 ) which showed definitively that the moment decreased correspondingly with the pure translation (first column of [ ] ). Examining Equation 4 123, it was necessary to generate the cross product operator, which using Equation 4 62 for [ ] resulted in [ ] = ( 4 140 ) It was then necessary to develop the cross product operator term given by PAGE 121 121 [ ] [ ] = 0 1 ( 4 141 ) or [ ] [ ] = 0 0 1 ( + ) ( 4 142 ) Subsequently, as seen from the symmetric reference frame, applying Equation 4 123 yields [ ] = 0 0 0 0 1 0 + 1 2 0 0 1 ( + ) ( 4 143 ) or [ ] = 1 2 0 0 1 ( + ) ( 4 144 ) The [ ] of Equation 4 144 times is the change in that an observer on the symmetric body would see at this inst ant given in terms of the fixed (global) coordinate system. In order to compare Equation 4 131 with Equation 4 144 it was necessary to consider the [ ] of Equation 4 131 as [ ] in the relation [ ] = [ ] [ ] ( 4 145 ) which is comparable to Equation 4 73 where [ ] is given in Equation 4 20, and where [ ] was used to denote the Equation 4 131 expression as seen from the global coordinate system. Substituting Equation s 4 20 and 4 131 into Equation 4 145 yields PAGE 122 122 [ ] = 1 2 + 0 0 1 0 0 1 0 ( 4 146 ) or [ ] = 1 2 0 0 1 ( + ) ( 4 147 ) where + = sin ( + ) = sin ( ) = ( 4 148 ) + = cos ( + ) = cos ( ) = ( 4 149 ) and ( ) + = ( + ) ( 4 150 ) The above shows that [ ] is identical to [ ] of Equation 4 144 For the example Case 2a discussed in Chapter 3, the [ ] and [ ] were given as [ ] = [ ] = c s s [ ( 3 53) ] and [ ] = s 0 c 0 1 [ ( 3 57) ] which were specified in terms of the moving body reference frame and where again for convenience, = cos ( ) and = sin ( ) Then, changes in as seen by a moving reference frame observer were given by [ ] = 0 s 0 c 0 c ( 4 151 ) where are the scalars of Equation 3 35, which can be written PAGE 123 123 [ ] = [ ] ( 4 152 ) where [ ] = 0 s 0 c 0 c ( 4 153 ) This was determined by examining the freedoms represented by the [ ] matrix, as the same scalars operated in Equations 3 35 and 4 152 In this case, there were two degrees of freedom, which meant = The first column of [ ] was all zeros, as the following describes. The corresponding column of [ ] was a pure translation perpendicular to the contact normal. The inclination of the contact normal remained unchanged during translation. In other words, an observer on the moving frame would not see the contact normal move because of the pure translation. The second column of [ ] represented a pure rotation about an axis through point G parallel to the axis of the moving reference frame. The second column of [ ] corresponds to effects of this rotation. A positive rotation of the payload with respect to the target about this axis resulted in a decreasing contact normal angle This resulted in the negated derivative of namely ( [ s, c, c]) as given in the second column of Equation 4 153 Examining Equation 4 122, it was necessary to generate the cross product operator, which using Equation 4 62 for [ ] resulted in [ ] = s c s c ( 4 154 ) It was then necessary to develop the cross product operator term given by PAGE 124 124 [ ] [ ] = s c s c s 0 c 0 1 ( 4 155 ) or [ ] [ ] = 0 s 0 c 1 c ( 4 156 ) Subsequently, as seen from the symmetric reference frame, applying Equation 4 122 yields [ ] = 0 s 0 c 0 c 1 2 0 s 0 c 1 c ( 4 157 ) or [ ] = 1 2 0 s 0 c 1 c ( 4 158 ) The [ ] of Equation 4 158 times is the change in that an observer on the symmetric body would see at this instant given in terms of the moving coordinate system. It was instructive to reconsider Case 2a of Chapter 3 but this time as seen from the fixed reference frame. In this situation, the contact normal remained passing through the moving point G but was canted in terms of the global coordinate system by where = + In light of this, Equation s 3 53 and 3 57 may be rewritten as [ ] = [ ] = ( 4 159 ) and PAGE 125 125 [ ] = 0 1 ( 4 160 ) where = + ( 4 161 ) = + ( 4 162 ) and where ( ) denoted the location of point F and ( ) denoted the location of point G on the payload in terms of the global coordinate system. From this perspective, the change in was written as = 0 0 0 0 1 0 ( 4 163 ) or = [ ] ( 4 164 ) where [ ] = 0 0 0 0 1 0 ( 4 165 ) The above was determined by considering how changed as seen from the fixed reference, namely as the payload translated per the first column of [ ] due to This resulted in the moment of decreasing correspondingly while maintaining constant orientation for contact normal. (Hence the fir st column of [ ] was given by [ 0 0 1 ], and this can be verified in a manner similar to Case 1a above using a modified Equation 4 139 .) Subsequently, the second column of [ ] was reported as all zeroes since the fixed reference frame did not see the contact normal change PAGE 126 126 directions or otherwise move as the payload rotated about the axis through point G that was parallel to the global axis. Examining Equation 4 123, it was necessary to generate the cross product operator, which using Equation 4 62 for [ ] resulted in [ ] = ( 4 166 ) It was then necessary to develop the cross product operator term given by [ ] [ ] = 0 1 ( 4 167 ) or [ ] [ ] = 0 0 1 ( + ) ( 4 168 ) Subsequently, as seen from the symmetric reference frame, applying Equation 4 123 yields [ ] = 0 0 0 0 1 0 + 1 2 0 0 1 ( + ) ( 4 169 ) or [ ] = 1 2 0 0 1 ( + ) ( 4 170 ) The [ ] of Equation 4 170 times was the change in that an observer on the symmetric body would see at this instant given in terms of the fixed (global) coordinate system. PAGE 127 127 In order to compare Equation 4 158 with Equation 4 170 it was necessary to consider the [ ] of Equation 4 158 as [ ] in the relation [ ] = [ ] [ ] ( 4 171 ) which is comparable to Equation 4 73 where [ ] is given in Equation 4 20, and where [ ] was used to denote the Equation 4 158 expression as seen from the global coordinate system. Substituting Equation s 4 20 and 4 158 into Equation 4 171 yields [ ] = 1 2 + 0 0 1 0 s 0 c 1 c ( 4 172 ) or [ ] = 1 2 0 0 1 ( + + ( c + ) ) ( 4 173 ) or [ ] = 1 2 0 0 1 ( + ) ( 4 174 ) where Equations 4 148 4 149 4 150 4 161 and 4 162 were used and where = c + ( 4 175 ) The above shows that [ ] is identical to [ ] of Equation 4 170 For the example Case 3 discussed in Chapter 3, the [ ] and [ ] were given as [ ] = [ ] = 0 0 1 1 0 [ ( 3 62) ] and PAGE 128 128 [ ] = 1 0 0 [ ( 3 63) ] which were specified in terms of the moving body reference frame Then, changes in and as seen by a moving reference frame observer were given by [ ] = [ ] ( 4 176 ) and [ ] = [ ] ( 4 177 ) where is the lone scalar of Equation 3 35 and where [ ] = 0 0 0 ( 4 178 ) and [ ] = 0 0 0 ( 4 179 ) The above trivial results were determined by examining the freedom represented by the [ ] matrix, as the same single scalar operated in Equation s 3 35 4 176 and 4 177 The columns of [ ] and [ ] were zeros, as the following descr ibes. The corresponding column of [ ] was a pure translation perpendicular to the contact normals at points F and G. The contact normals remained perpendicular to the direction of translation, as translation progressed. Also during translation, the locations of each contact normal remained fixed in the moving frame. (In other words, an observer on the moving frame would not see either contact normal move because of the pure translation.) PAGE 129 129 Examining Equation 4 122, it was necessary to generate the cross product operator, whi ch using Equation 4 62 for [ ] resulted in [ ] = 1 0 1 0 ( 4 180 ) where in this special case [ ] = [ ] = [ ] It was then necessary to develop the cross product operator term given by [ ] [ ] = 1 0 1 0 1 0 0 ( 4 181 ) or [ ] [ ] = 0 0 1 ( 4 182 ) Subsequently, as seen from the symmetric reference frame, applying Equation 4 122 yields [ ] = 0 0 0 1 2 0 0 1 ( 4 183 ) or [ ] = 1 2 0 0 1 ( 4 184 ) where Equation 4 184 was true for = 1 2 It was instructive to reconsider Case 3 of Chapter 3 but this time as seen from the fixed reference frame. In this situation, the contact normals remained passing through the moving points F and G, but the payload was canted in terms of the global coordinate system by In light of this, Equation s 3 62 and 3 63 may be rewritten PAGE 130 130 [ ] = [ ] = ( 4 185 ) and [ ] = 0 ( 4 186 ) where again for convenience, = cos ( ) and = sin ( ) where ( ) and ( ) again denoted the locations of points F and G on the payload in terms of the global coordinate system, and where Equation s 4 161 and 4 162 were used. From this perspective, i.e. the fixed reference frame, the change in was written = 0 0 1 ( 4 187 ) or = ( 4 188 ) where = 0 0 1 ( 4 189 ) where in this special case [ ] = [ ] = It was determined that the moment of changed in proportion to the displacement A more thorough examination of was conducted, specifically how the + 1 was determined for the change in moment. For example, take = 1 The moment coordinate in was or + The translation due to the column of PAGE 131 131 [ ] and due to the scalar displacement resulted in the coordinates of point F changing at the next instant to where = + ( 4 190 ) and = + ( 4 191 ) This resulted in the moment at the next instant changing to In light of this, the desired change in moment was determined by the following: = = ( + 1 ) ( 4 192 ) which showed definitively that the moment increased correspondingly with the pure translation (column of [ ] ). The same result applied for case = 2 Examining Equation 4 123, it was necessary to generate the cross product operator, which, using Equation 4 62 for [ ] resulted in [ ] = ( 4 193 ) where again in this special case [ ] = [ ] = [ ] It was then necessary to develop the cross product operator term given by [ ] [ ] = 0 ( 4 194 ) or [ ] [ ] = 0 0 1 ( 4 195 ) PAGE 132 132 Subsequently, as seen from the symmetric reference frame, applying Equation 4 123 yields [ ] = 0 0 1 + 1 2 0 0 1 ( 4 196 ) or [ ] = 1 2 0 0 1 ( 4 197 ) The [ ] of Equation 4 184 is identical to the [ ] given in Equation 4 197 It is left to the reader to see that this is consistent with analogies of Equation s 4 145 and 4 171 i.e. the coordinates [ 0 0 1 ] are mapped to themselves The above thorough examination of the matrix [ ] was useful in answering the fundamental question of what happens to the constraint wrench and displacement of the payload whenever the wrench charge Equation 4 109 was applied: = [ ] [ ( 4 109 ) ] Specifically the [ ] matrix played a component role of the change in wrench as given in Equation 4 114 : = [ ] + [ ] [ ( 4 114 ) ] The above two equations were used in the wrench balance Equation 4 101 : = [ ] [ ( 4 101 ) ] It should be noted that the [ ] and [ ] matrices presented above and used in the following were those seen by an observer in the symmetric reference frame. Substituting Equation s 4 114 and 3 35 into Equation 4 101 yields = [ ] [ ] [ ] [ ] ( 4 198 ) PAGE 133 133 E quation 4 198 was also written as = [ ] + [ ] ( 4 199 ) where [ ] (a 3 matrix) and [ ] (a 3 matrix) were given as [ ] = [ ] [ ] ( 4 200 ) and [ ] = [ ] ( 4 201 ) In the above, [ ] was given by Equation 3 72 or [ ] = [ ] [ ] Equation 4 199 was an alternative form of the balance equation and showed how the wrench charge balanced to give a displacement and constraint wrench change It was also equivalent to the following equation: = [ ] ( 4 202 ) where [ ] = [ ] [ ] ( 4 203 ) (This was explained in a more colloquial fashion by saying that [ ] was [ ] glued to [ ].) It was assumed that the [ ] (a 3 3 matrix) was invertible. The reader should recall that originally was an 1 array of scalar multipliers of the columns of the freedom matrix [ ] in the twist of freedom Equation 3 35 and was a 1 array of scalar multipliers of the columns of the constraint matrix [ ] in the constraint wrench change Equation 4 114 Putting these together yields which was the 3 1 matrix in Equation 4 202 Rearranging Equation 4 202 yields: PAGE 134 134 = [ ] ( 4 204 ) Substituting Equation 4 109 for or = = [ ] yields important relationships : = and = 0 The critical bottom line was if solely a correction for constraint wrench error were made, i.e. = [ ] then = and the payload did not move, i.e., = 0 This was one of the two fundamental concerns discussed in the overview of this chapter. Table 4 2 provides definitions as well as array sizes for various matrices analyzed and discussed in this section. Deflection Wrench: Correcting for Payload Location Error The complementary case for Equation 4 204 was considered where the wrench charge was taken from the column space of [ ] in the relationship = [ ] ( 4 205 ) where together with the relation = [ ] denoted a location error for the payload. In other words, was a twist error and was the associated deflection wrench. From Equation 4 204 it was clear that a wrench charge such as that given by Equation 4 205 or = yields = and = 0 In other words, the short story was that the payload corrected for position error as desired, without changing constraint wrench scalars. This condition represented the second of the two fundamental concerns discussed in the overview of this chapter. An alternative approach to the foregoing would consider how the payload moves in response to the wrench charge given by Equation 4 205 so that the payload finds PAGE 135 135 (retains) a minimum potential energy condition. This could be accomplished by starting with the equation = ( ) ( 4 206 ) differentiating it, setting the result to zero, and making key substitutions. The alternative approach was undertaken by Griffis (2009) in a twodimensional example. Explicit Kinestatic Control: Combining Wrench Components to Generate Leg Force Setpoints Figure 4 7 identifies the Explicit Kinestatic Controller used in this research. The purpose of this section i s to describe how the controller operated. As discussed in the overview of this chapter, the Assembly Task Supervisor provided the desired twist of freedom, shown as in Figure 4 7 the desired wrench of constraint, shown as in Figure 4 7 and the current kinestatic model. It was discussed in Chapter 3 that the kinestat ic model or contact model was given by [ ] and [ ] In light of the results of preceding sections, it was straightforward to consider that the Assembly Task Supervisor further provided [ ] where = 1 For the purposes of this section, the fixed, global coordinate system was used ( Figure 4 2 ). Transformations between the payload coordinate system and global coordinate system were straightforwar d and documented previously by Equations 4 19 4 73, and 4 74. Finally, for the purposes of this section, it was taken that the [ ] and [ ] were specified as seen by an observer in the symmetric body. To aid the reader, Figure 4 7 additionally shows reference and actual locations of the payload as and The Assembly Task Supervisor was responsible for combining these to generate the desired corrective twist given by used herein. PAGE 136 136 The upper loop of the controller utilized as the input. Filter matrix [] yields the representative freedom twist Filter matrix [ ] subsequently yields the component of the constraint wrench change. The freedom twist was then scaled by [ ] and the result was combined with This yields That combination wa s consistent with Equation 4 205 That result was finally scaled by the scalar gain to yield the corrective deflection wrench As such, the upper loop of the controller made corrections for the error in payload location. On the other hand, the lower loop of the controller made corrections for constraint wrench error as the following describes. Inputted was the desired constraint wrench given by This was combined with the gravity wrench as given by Equation 4 16, which yields the that was subsequently used in the main summing junctio n of the controller. In the lower feedback loop, was the resultant of all actual leg forces. (In other words, = of Equation 4 4 ) The difference was then generated in the feedback path, and this difference was consistent with of Equation 4 18 Subsequently, the difference was generated as the constraint wrench error ( An example can be found in Equation 4 104 .) Subsequently, the constraint wrench error was filtered by [] to yield This provided a corrective component that was consistent with Equation 4 109. Finally, was scaled by the scalar gain to yield which was the component that was actually used to correct for constraint wrench error. In the main summing junction, three wrenches were added in order to generate the general wrench given by PAGE 137 137 = + + ( 4 207 ) Note that the above + was consistent with the concept of wrench charge in the preceding sections. Finally, the general wrench was multiplied by the inverse of the Jacobian, or = [ ] ( 4 208 ) which yields (consistent with Equation 4 4 ) that was the array of desired leg forces which needed to be controlled by the Robot System shown in Figure 4 7 While the preceding describes the operation of the explicit kinestatic controller, it remains in the following to present the definitions of the filter matrices [], [] and [ ]. Synthesis of the scalar gains and will be discussed in Chapter 6. In what follows, it was assumed that a general twist could be written as a unique combination of a freedom twist and a compliance twist, or = [ ] + [ ] ( 4 209 ) where is the general twist, where [ ] is the freedom twist, and where [ ] is the compliance twist (Griffis 1991) Rewriting Equation 4 209 and representing the freedom twist by = [ ] and representing the compliance twist by = [ ] yields = + ( 4 210 ) In the above, [ ] was a 3 matrix, was a 1 matrix, [ ] was a 3 matrix, and was a 1 matrix, where here again + = 3 Also, in the above, [ ] was defined as the following: [ ] = [ ] [ ] = [ ] [ ] ( 4 211 ) where [ ] = [ ] PAGE 138 138 Rewriting Equation 4 209 and applying it to the provided yields the relationship [ ] = ( 4 212 ) where the 3 3 matrix [ ] = [[ ] [ ]] was generated from the columns of [ ] and [ ] and where the scalars of together with the scalars formed the 3 1 matrix. Multiplying Equation 4 212 first by [ ] and then by [ ] yields the relationship [ ] [ ] [ ] = [ ] [ ] ( 4 213 ) which can be written [ ] [ ] = [ ] [ ] [ ] [ ] ( 4 214 ) where [ ] = [ ] [ ] [ ] ( 4 215 ) and [ ] = [ ] [ ] [ ] ( 4 216 ) In the above, the partitioning of Equation 4 214 was made possible because [ ] [ ] [ ] = [ 0 ] ( 4 217 ) and [ ] [ ] [ ] = [ 0 ] ( 4 218 ) where [0] is an matrix of zeroes. This was because substitution of Equation 4 211 and simplification yields the known true statement [ ][ ] = [0] for example, which depends on the reciprocity of freedom twists and constraint wrenches. PAGE 139 139 ( Equation 3 4 0 .) Furthermore, the Equations 4 217 and 4 218 declare that [ ] and [ ] are [ ] orthogonal complements. Expanding Equation 4 214 yields the desired equations for two of the filter matrices. The first was calculated using [ ] = [ ] [ ] ( 4 219 ) Substituting for [ ] yields ( [ ] [ ] [ ] ) = [ ] [ ] ( 4 220 ) or by multiplying both sides by ( [ ] [ ] [ ]) yields = ( [ ] [ ] [ ] ) [ ] [ ] ( 4 221 ) Multiplying both sides of the equation by [ ] yields [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 222 ) Recognizing that = [ ] Equation 3 35, and substituting into E quation 4 222 yields = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 223 ) By defining [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 224 ) one could write Equation 4 223 as = [ ] ( 4 225 ) and this specifies one branch of the upper loop in Figure 4 7 Taking Equation 4 221 and multiplying by [ ] yields [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 226 ) Examining Equation 4 114, one could write it as PAGE 140 140 = + ( 4 227 ) where = [ ] and = [ ] remembering that denotes an infinitesimal change in constraint wrench and the subscript denotes the particular component of constraint wrench change. Thus using = [ ] Equation 4 226 becomes = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 228 ) Letting [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 229 ) one gets = [ ] ( 4 230 ) and this specifies the other branch of the upper loop in Figure 4 7 In what follows, it was assumed that a general wrench change could be written as a unique combination of deflection wrench and constraint wrench changes, or = [ ] + [ ] ( 4 231 ) where is the general wrench, where [ ] is the deflection wrench, and where [ ] is the constraint wrench. (Chapter 3.) Rewriting Equation 4 231 and representing the deflection wrench by = [ ] and representing the constraint wrench by = [ ] yields = + ( 4 232 ) In the above, [ ] was a 3 matrix, was a 1 matrix, [ ] was a 3 matrix, and was a 1 matrix, where here again + = 3 Also, in the above, [ ] was defined as the following: [ ] = [ ] [ ] = [ ] [ ] ( 4 233 ) Rewriting Equation 4 231 and applying it to the calculated yields the relationship PAGE 141 141 [ ] = ( 4 234 ) where the 3 3 matrix [ ] = [[ ] [ ]] was generated from the columns of [ ] and [ ] and where the scalars of together with the scalars formed the 3 1 matrix. Multiplying Equation 4 234 first by [ ] and then by [ ] yields the relationship [ ] [ ] [ ] = [ ] [ ] ( 4 235 ) which can be written [ ] [ ] = [ ] [ ] [ ] [ ] ( 4 236 ) where [ ] = [ ] [ ] [ ] ( 4 237 ) and [ ] = [ ] [ ] [ ] ( 4 238 ) In the above, the partitioning of Equation 4 236 was made possible because [ ] [ ] [ ] = [ 0 ] ( 4 239 ) and [ ] [ ] [ ] = [ 0 ] ( 4 240 ) where [0] is an matrix of zeroes. This was because substitution of Equation 4 233 and simplification yields the known true statement [ ] [ ] = [0] for example, which depends on the reciprocity of freedom twists and constraint wrenches. ( Equation 3 40.) Furthermore, the Equations 4 239 and 4 240 declare that [ ] and [ ] are [ ] orthogonal complements. PAGE 142 142 Expanding Equation 4 236 yields the desired equations for the filter mat rix [ ] This was calculated using [ ] = [ ] [ ] ( 4 241 ) which were the last rows of Equation 4 236 Substituting for [ ] yields ( [ ] [ ] [ ] ) = [ ] [ ] ( 4 242 ) or by multiplying both sides by ( [ ] [ ] [ ]) yields = ( [ ] [ ] [ ] ) [ ] [ ] ( 4 243 ) Multiplying both sides of the equation by [ ] yields [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 244 ) Recognizing that = [ ] based on Equation 4 109 and substituting into Equation 4 244 yields = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 245 ) By defining [ ] = [ ] ( [ ] [ ] [ ] ) [ ] [ ] ( 4 246 ) one could write Equation 4 245 as = [ ] ( 4 247 ) and this specifies the lower loop in Figure 4 7 It was convenient that Equation 4 246 also could be written [ ] = [ ] [ ] ( [ ] [ ] [ ] ) [ ] ( 4 248 ) since [ ] = [ ][ ] In order to complete the specification of the controller of Figure 4 7 it rem ained to identify how [ ] of Equation 4 229 was determined. As it was noted in the foregoing, it PAGE 143 143 was straightforward that the assembly task supervisor provided the [ ] Thus, it remained to use Equation 4 117 to determine [ ] : [ ] = [ ] [ ( 4 117) ] This required calculation of where = 1 which was written as an 1 array. Using the above approach, it is straightforward that this array can be written = [ ] ( 4 249 ) where [ ] = ( [ ] [ ] [ ] ) [ ] [ ] ( 4 250 ) (Hint: adapt Equation 4 243.) This is equivalent to [ ] = ( [ ] [ ] [ ] ) [ ] ( 4 251 ) It should be noted that, for the sake of clarity, Equation 4 249 is not explicitly shown in Figure 4 7 However, an additional filter could be added in Figure 4 7 that takes as input and applies the filter matrix [], i.e., Equation 4 249 The output of such a filter could be shown to adaptively change [ ]. Table 4 3 provided definitions of the various wrench terms and the equations in which they appeared. PAGE 144 144 Figure 4 1 Explicit kinestatic c ontroller. Figure 4 2 Actuation f rame with independent m etrology f rame G F Leg 1 Leg 3 Leg 2 D O C B A E payload Metrology Frame Explicit Kinestatic Controller From a ssembly t ask s upervisor: Desired wrench of constraint Next twist of freedom The current contact model Feedback from f orce c ontroller: Current actual leg forces Current actual leg lengths (redundant) Feedback to a ssembly t ask s upervisor: Current payload location Current sensed wrench Stiffness matrix [ ] Command to f orce c ontroller: Leg force setpoints Leg velocity setpoints (noncontact mode) From m etrology f rame: Payload location PAGE 145 145 Figure 4 3 Resultant w rench and f ree b ody d iagram of p ayload. G F Leg 1 Leg 3 Leg 2 D O C B A E payload target = = PAGE 146 146 Figure 4 4 Payload f rame with a pplied w renches. Payload = + + = wrench applied to payload by target = DAlembert wrench applied to payload = wrench applied to payload, mass by gravity = wrench applied to payload by the legs = resultant of wrenches (excluding leg forces) PAGE 147 147 Figure 4 5 Rotation of payload by small angle about the instantaneous center of rotation P. Leg O P (line of action) Payload moving pivot point (original starting location) fixed pivot point Payload moving pivot point (next location after rotation) Instantaneous center (I.C.) of rotation P PAGE 148 148 Figure 4 6 Hydraulic actuation device with passive compliant spring. Hydraulic cylinder = l Hydraulic cylinder = 0 = ( ) < 0 > 0 = cylinder extended cylinder retracted PAGE 149 149 Figure 4 7 Explicit kinestatic c ontrol for a p arallel m anipulator. + [ ] [ ] [ ] [ ] + [ ] + Robot System + + + PAGE 150 150 Table 4 1 Parameters associated with an actuation leg. Parameter for leg Description True or actual pinto pin distance for leg Pin to pin distance when leg is unloaded ( = 0 ); an adjustable value tied to the hydraulic cylinder True or actual length of spring, leg Length of unloaded spring, leg Force in leg Small change in leg length (pin to pin distance) Small change in Force charge in leg : = Table 4 2 Summary of matrices Matrix Dimension Description [ ] 3 Twists of freedom [ ] 3 Wrenches of constraint 1 Twist scalars. 1 Constraint wrench scalars 1 Constraint wrench scalar changes [ ] 3 Change in [ ] [ ] 3 Derivative of [ ] with respect to twist scalars, e.g. [ ] = [ ] [ ] = [ ] [ ] [ ] 3 Matrix that contributes to displacement component of wrench charge. [ ] = [ ] 3 Matrix that contributes to constraint wrench change component of wrench charge. [ ] = [ [ ] [ ] ] 3 3 Combined matrix made up of columns of [ ] and [ ] 3 1 Combined matrix made up of the twist scalars and the constraint wrench change scalars PAGE 151 151 Table 4 3 Typical usage for wrench notation. Wrench Usage Equation Resultant Wrench; a combination of the three leg forces, 4 7 Sensed Wrench. 4 18 Wrench of Constraint (Chapter 3) 3 34 Resultant Wrench which is a combination of the forces applied to the payload by the legs 4 8 Commanded Wrench or a typical wrench in Ray Coordinates 3 18 Gra vity Wrench = 0 1 where = coordinate of the center of gravity, and is the weight of the payload. 4 16 General Wrench; = + where is the Constraint Wrench and is the Deflection Wrench 3 69 4 207 Transformed version of the sensed wrench 4 18 The change in the resultant wrench. 4 31 The change in the basic wrench. 4 32 Deflection Wrench. 3 67 Wrench Charge 4 102 4 109 Wrench at the next step (or next instant of time) in the process Constraint Wrench Error 4 104 Reference Constraint Wrench 4 102 Sensed Constraint Wrench. 4 103 PAGE 152 152 CHAPTER 5 SINGLE LEG: FORCE CONTROL AND SYSTEM MODEL Almost everything is more complicated than it seems, but almost nothing is hopeless. Vincent Barnett Red Planet Mars the movie, 2006 Overview The goal of this chapter was to examine the basic actuation device used to complete the assembly task. The basic actuation device used in this study was a double acting hydraulic cylinder that acted as one leg in the parallel mechanism. The f ollowing provided a description of the multiple components of the leg A mechanism feedback control algorithm was used that focused on each leg individually Thus, a detailed study of the single leg proved instructive since it provided a system to test and refine the basic force control algorithm as well as provide data for development of components for the system model. The following discussion of the single leg force control ler would have been much less meaningful if the issue of decoupling of all three legs in the planar mechanism had not been resolved through experimentation. Chapter 6 explained coupling and decoupling in some detail and proved that decoupling of the legs was a valid assumption. Electro hydraulic servo systems are inherently nonlinear. The nonlinearities stemmed from two primary sources: the actuation subsystem, and the loading subsystem (Vossoughi 1995 and Whitney 1987). Included in the actuation subsystem nonlinearities are: servovalve hysteresis, asymmetric actuation resulting from unequal piston actuation areas, nonlinear servovalve pressure and flow characteristics, variations in the amount of fluid trapped between the servovalve and the cylinder, and PAGE 153 153 the effect of flow forces on the servovalve. The load subsystem nonlinearities include items which were primarily application dependent: inertial coupling and associated nonlinear dynamics of the system hardware and friction and stiction in the hy draulic cylinders. There were numerous analytical techniques available for accommodating the system non linearities. The method used in this analysis will be explained in the following, but, in general, it involved a combination of careful selection of s ensors and methods that ensured that the system operated in the linear portion of the available operating space. It should be noted that the transfer function for any hydraulically powered system is based on an inherently nonlinear system There always existed the risk that incorrect assumptions might be made in attempting to linearize the system A linear representation of the system was, at best, only an approximation (MOOG 1965). That stated, there was no question that there was great usefulness in establishing a linear TF for approximating hydraulic system performance for both the single leg device as well as the planar mechanism Figure 5 1 identifie d the upper level (black box) overview of the force controller. The leg force controller was conceived to provide the low lever force control of each leg of the parallel manipulator. The force controller was designed specifically for decoupling the feedback of the individual activation devices from the platform specific geometry. This design coupled with the overall design of the system and verified by the system model clearly placed the control of the parallel robotic manipulator in the linear range of motion for this application. PAGE 154 154 Chatzakos and Papadopoulis (2003) proposed a potentially workable alternative to the force control used in this research. Their work was studied as a possible alternative but not used other than as a reference Input from the Explic it Kinestatic Controller (Chapter 4) included individual leg parameters such as the force or velocity set points depending on the control mode selected (contact vs. noncontact ). Output from the Force Controller to the Explicit Kinestatic Controller inclu ded actual (sensed) values from each leg. This information consisted of the actual leg forces, leg lengths, and other parameters from the physical device including precise measurement of the compliant spring deflection. Output from the force controller t o the individual actuation device was implemented as a servovalve command. The system model of a single leg actuation device, the hydraulic model, and a host of sensors to quantify characteristics of the single leg were internal to the leg force controller box in Figure 5 1 Physical Description of Actuation Device The actuation device was a double acting hydraulic cylinder that was controlled using a precision servov alve. A hydraulic circuit diagram of the actuation device is shown in Figure 5 2 while Figure 5 3 shows a photograph of the single leg actuation device. The servovalve provided the capability to very precisely meter hydraulic fluid to either port on the hydraulic cylinder allowing for movement in either direction. The hydraulic cylinder used in this study was an EatonVickers Series NZ50 with a 1 5 bore and a stroke of 25 The servovalve used was a Schenck Pegasus model 122A with a nominal flow rate of 1 6 (gallons per minute) rated for 1 000 drop across PAGE 155 155 the servovalve. The actual servovalve flow rate varied as a function of the system operating pressure, the load pressure drop, and the servovalve command. The manufacturers figure of 1 6 was provided for a load pressure drop of 1 000 Sensors incorporated onto the single hydraulic actuation device included two position sensors one used for safety considerations and one used to determine the cylinder extension/retraction length. The safety position sensor was a Celesco series SP1 compact string pot that provided potentiometric output used to determine the cylinder extension and provide a warning when the cylinder was reaching minimum or maximum limits established for safe operations. The second sensor used for precision length measurement was a Newall series MHG incremental linear encoder. In order to measure the force generated by the linear actuator, a high accuracy S beam load cell was placed in series with the cylinder rod of the actuation device. Omega model LCCA 1k sensors were initially used to measure force. During the research, it was determined that a compliant device was required in series with the hydraulic cylinder to more effectively manage closedloop force control (Robinson and Pratt, 2000). The compliant device chosen was a specially engineered linear spring that not only provided the compliance required for force control but could also be used as a force sensor through the incorporation of a linear position transducer in parallel with the spring3 (Ferretti et al 1995). The actual spring is shown in the photograph in Figure 5 4 The spring was installed inl ine with the hydraulic linear actuator and is shown in Figure 5 5 The sensor selected to measure the compression 3 The author would like to thank Shannon Ridgeway (University of Florida) for designing the spring used in this research. PAGE 156 156 of the spring was an LVDT (linear variable differential transformer), a transducer manufactured by Measurement Specialties, DC EC Accusens Series, model 250 DC EC, with a 0 25 range of travel. The LVDT was powered by 15 and produced an output signal of 10 Testing of the compliant springs was accomplished on an MTS (Material Test Systems) compression/tension testing machine, model 322.31, load frame 162, with a maximum capacity of 55 000 when in the compression mode. The load cell on the MTS was manufactured by Eaton, model 622.341, with a rated capacity of 50 000 T he spring constant s w ere accurately determine d by the testing of the specially engineered springs. The average spring constant for all three springs was determined to be 3 658 The most important attribute of the special compliant spring was its linearity ( Figure 5 6 Figure 5 7 and Figure 5 8 ). Hydraulic power was supplied to the cylinder by a R exroth hydraulic power unit (HPU) consisting of a pressurecompensated hydraulic pump powered by an electric motor rated at 20 208 /460 and 1760 The hydraulic circuit for the HPU is shown in Fi gure 5 9 The HPU was capable of supplying constant pressure of 3000 However, different system pressure values were used during characterization of the single leg model. The range of system pressures included 500 1000 2000 and 2700 Reduced pressures were used during this study so that forces generated were reduced to protect system hardware during initial development and operation. In a real world application involving contact with a very expensive aircraft, the lowest workable pressure may in fact prove to be the more prudent path to take, but the system pressure setting must be commensurate to handle the anticipated loads. PAGE 157 157 The hydraulically powered r obot described in this work also included hydraulic pressure sensors that were placed to read system pressure, on both sides of the hydraulic ports, and and the return pressure, The value for return pressure was generally taken as zero but depending on pressure drop through the hoses back to the hydraulic power unit tank, the return pressure c ould influence the system operation. The pressure differen tial between the hydraulic cylinder ports was called the load pressure difference: = Although pressure information provided useful information during system setup, pressure information was not employed in the control of the system. Pressure fluctuations and the lack of sensor bandwidth resulted in pressure data that was not s uitable for control. Friction and stiction on the piston and on the seals added greatly to the uncertainty involved wit h using cylinder pressure as a control input. By focusing more on the spring deflection, it was possible to reduce the effects of hydraulic system friction issues (Schimmels et al 1994, Huang and Schimmels 2001) Since hydraulic systems are inherently nonlinear, it was important to lessen the effects of nonlinearities as much as possible. The next step in the system analysis involved choosing a model or modeling method for the system under study. Initial m odeling efforts employed MATLAB and the National Instruments graphical programming and modeling techniques. A great deal of time and effort were expended in attempts to develop a wor kable system model that was also stable in the closed loop control mode. After repeated attempts to characterize the system using the tools listed above, the decision was made to restrict the control system development to the frequency domain and thereby simplify the analysis. Time domain analysis involving the system differential equations was PAGE 158 158 investigated, but the drawbacks to this approach included difficulty in solving the sets of equations for systems greater than second order and the fact that relat ive stability is difficult to study in the time domain (Dunnigan et al 1996, Niksefat et al 2001) The frequency response of a system was defined as the steady state response to a sinusoidal input. In addition, the Laplace transform operator can be s et equal to or = and substituted into the characteristic equations to generate magnitude and phase plots for varying frequencies This transformation relates the Laplace operator to the Fourier operator which in turn provides a direct correlation between the system transfer function and the amplitude and phase char acteristics of the system (Dorf 1993). Say the open loop transfer function of some system is ( ) Then, Bode plots normally consist of two graphs (DiStefano 1967), the magnitude of ( ) and the phase angle of ( ) More specifically, the magnitude plot was generated by calculating = 20 log  ( )  ( 5 1 ) where is given in as a function of frequency In the same way, the phase angle plot was generated by calculating the phase angle ( ( ) ) versus the frequency In the above, the transfer function ( ) is implicitly dimensionless, oftentimes being a ratio of values of same units. In this case, the result of Equation 5 1 yields the typical value having units of decibels or In the case where a transfer function ( ) has units, e.g. a corresponding adaptation of Equation 5 1 gives = 20 log  ( )  ( / ) ( 5 2 ) PAGE 159 159 which yield ed a number identified with units ( / ) Such a number is an absolute measure referenced in this case to 0 ( / ) = 1 In this example, represents inches per second and stands for volt. System Plant Model and Transfer Function Studying and carefully characterizing the system block diagram began with the development of the transfer function for each system component (DiStefano 1967, Merritt 1967, McCloy 1980, and Dorf 199 3 ). The individual blocks for the elements of the system are shown in Figure 5 10. Control of the singleleg actuation device was performed using a computer system tha t used analog to digital and digital to analog functionality (A/D, D/A). To control the servo valve, a voltage was sent to an amplifier that converted the voltage to amperage before sending this command to the valve. A 16bit D/A converter was used to co nvert a desired control signal for the servovalve to a 10 signal. This voltage was then sent to an EatonVickers power amplifier for servovalves series EHA PAM 291A 20. This power amplifier was designed for controlling servovalves requiring up to 200 current and had the capability to not only adjust gain, but provide a signal offset and apply a dither (both frequency and amplitude) to the signal sent to the servovalve. A 16bit A/D converter was used to read and convert the LVDT positi on sensor data for the compliance spring. A linear encoder was used to measure the hydraulic cylinder length (exclusive of the spring). The resolution of the encoder was 5 (microns), read in quadrature, and counted using dedicated edge detection har dware. The block diagram model of the system plant is shown in Figure 5 10 In this diagram, the single leg is represented as the combination of a servovalve amplifier, a PAGE 160 160 servo valve, a hydraulic cylinder, and a compliant spring. These components were represented in individual blocks in the figure. The c ontrol ler for the device used a commanded voltage to the servo amplifier which, through the system plant effected extension or retraction of the hydraulic cylinder. The servovalve hydraulic cylinder combination is inherently a velocity device (when motion is unconstrained) since an opening of the servovalv e generated a flow rate, and the hydraulic cylinder rod moved generally speaking at a rate proportional to the flow rate. It is important to once again note the criticality of using compliance in force control when contacting a hard surface. The lack of a passive, but readable, compliant element could prove to be very unproductive (Kennedy 2009). A model of each element in the single leg was estimated and combined to form a system model. The bode magnitude and phase angle plots, a common tool used for f requency analysis, were plotted for each element of the system model (Pool 1989) Due to parameters varying in each element, some of the graphs show a family of curves to illustrate the effect of different parameters. Some bode plots are presented as a combination of the transfer functions of several system model elements where applicable. The first item to be addressed was the servovalve amplifier transfer function defined as From the operating data sheet for the servoamplifier, the 3 ( ) frequency was 1 500 ( H ertz). Due to this frequency being higher than the response frequencies for both the servovalve and hydraulic cylinder, the transfer function was modeled as a constant gain given by = Bo de magnitude and phase angle plots are presented in Figure 5 11. The Bode magnitude PAGE 161 161 plot was a straight line and represented the magnitude of the cons tant expressed in decibels, The Bode phase angle plot was a horizontal line located at 0 showing that there was no phase lag for all frequencies. The next element in Figure 5 10 was the servovalve transfer function given by The dynamics of the servovalve were modeled as a second order system with a conversion factor to convert milli amperes ( ) to a flow rate ( ). The servovalve transfer function was given by = + 2 + ( 5 3 ) The servovalve conversion factor ( ) had the nominal value = when the full flow through the servovalve was realized. Different system pressures resulted in a percentage of flow of the nominal. This was represented by as a percentage that ranged between 0 100% and depended on the pressure drop across the servovalve. Control flow to the load changes with load pressure drop and spool stroke and follows the square root relationship = 1000 ( 5 4 ) where is the servovalve output flow, is the servo valve rated flow at 1 000 pressure drop across the valve (or often called the pressure drop across the load), is the servovalve current, is the servo valve rated current, is the supply pressure in and is the load pressure in The other servovalve transfer function parameters were taken directly from the valve manufacturers data sheet. The characteristic frequency was given by = 60 and the damping rati o was modeled PAGE 162 162 as = 1 / 2 Bode magnitude and phase angle plots are presented in Figure 5 12. The magnitude plot is horizontal until the natural fr equency is reached, where it falls off at the rate of 40 where represents decade ( a factor of 10 change in frequency ) The Bode phase angle plot at low frequencies has a phase angle of 0 progressed to 90 at the natural frequency and asymptotically approached 180 at high frequencies. One of the obvious advantages of using Bode plots (that are plotted using logarithmic scales) was the simplification of the math involved when several transfer functions were multiplied together. Combining two or more Bode plots of the transfer functions required only that they be added together. However, the most useful advantage of the Bode plots for control system analysis lies in the capability that they facilitated toward the investigation of the phase and gain margins. This, in turn, allowed the control system researcher to rapidly assess the various gains and identify critical maximum and minimum values of those gains that would provide adequate system response when evaluated against system stability. The third block in Figure 5 10 was the hydraulic cylinder transfer function The transfer function was modeled as a second order system where = + 2 + ( 5 5 ) The conversion factor was used to convert hydraulic fluid flow rate ( ) into cylinder rod velocity ( or ). Depending on the direction of cylinder movement, the conversion factor ranged from 2 18 to 3 92 due to the hydraulic cylinder being single ended. It should be noted that for a certain servovalve opening (for an ideal precision servovalve), the flow through the extension port is identical (but in the PAGE 163 163 opposite direction) to the flow through the ret raction port. This idealized servovalve condition meant that a limiting factor in piston movement was the servovalve port opening rather than the direction of the flow. If flow rate through the valve is used, the range of the linear velocity conversion factor depended on the cross sectional area of the piston or piston/rod end of the cylinder. The cross sectional area of the pistononly side (diameter of piston was 1 50 ) of the cylinder was 1 77. The crosssectional area of the rod end was 0 98. The nominal maximum extension velocity was 3 48 and the nominal maximum retraction velocity was 6 27 for the nominal servo valve flow rate of 1 6 In practice, the maximum retraction velocity could not be realized because of a limiting flow returning through the servovalve extension port (i.e. the flow was choked). The natural frequency of the hydraulic cylinder was defined as The equation for the cyli nder natural frequency (Merritt 1967) was = 4 ( 5 6 ) where was the effective bulk modulus of the hydraulic fluid, was the cross sectional area of the cylinder, was the total volume of fluid under compression in both cylinder chambers, and was the total mass of the piston and the load. The following discusses how each of these parameters affects the hydraulic cylinder natural frequency. The bulk modulus of hydraulic fluid was a parameter provided by the manufacturer. For the particular fluid used in this research, the value was given as 270 000 Entrained air in the fluid play ed a major factor in reducing the bulk PAGE 164 164 modulus. A reliable effective system bulk modulus, used by Merritt (1967) was 100 000 The compressibility of the fluid, characterized by the bulk modulus, acted as a spring within the system. This spring effect contributed to system resonance and increased the complexity of the characterization of the system dynamic performance. In general, the effective bulk modulus was derived as a function of flexibility in the hydraulic tubing, the amount of entrained air in the system, and flexibility in structural components such as the hydraulic pump (Manring 2005). The components immediately und er study in this research included only the servovalves and the cylinders. All flexible hydraulic lines were upstream and were not included in this research. Every effort was made by the researcher to ensure that the hydraulic system was bled of air and any other entrained gases. Given the above, it should be stated that since in the calculation of is a square root, the difference between using 100 000 and 270 000 would change by about 60%. The value in the calculation was the effective cross sectional area of the hydraulic cylinder where hydraulic pressure acted. This value differed from the full cylinder bore, 1 77 to the area of the cylinder minus the area of the rod, 0 98 Differences in using the lesser crosssectional area to the maximum cross sectional area would change by about 80%. For this analysis, an average cross sectional area = 1 38 was used at times in order to simplify the analysis of the singleended, doubleact ing cylinder. The total cylinder volume ( ) varied depending on where the piston was located between zero to full extension. Due to the differing cross sectional areas, the total volume of the hydraulic cylinder ranged from 24 5 to 44 2 To sim plify calculations, PAGE 165 165 a cylinder rod extension was calculated in order to center the piston volumetrically. When the cylinder rod was thus centered, the volumes on each side of the piston were equalized to a value of 31. 6 Using different volumes i n the calculations, could change approximately 34% depending on the position of the cylinder Generally speaking, the hydraulic cylinder natural frequency can be written as = 1 + 1 ( 5 7 ) When the hydraulic cylinder was volumetrically centered, this expression reverts to Equation 5 6 In the more general case, the i ssue of the two trapped oil springs (Merritt 1967), where the volume of oil, and on either side of the piston, was addressed by considering that each spring had a gradient of which interacted with the mass to give the hydraulic natural frequency. Merritt determined through experimentation that the springs could be added even when the initial chamber volumes differed. It should be noted that the lowest natural frequency occurred when = = The t otal mass of the piston and the load, ( ), also varied depending on the mass of the payload. Nominally, values for represented a total piston and load weight of 250 Values ranging from this weight to a weight of 1 000 were used to examine ranges for Using minimum values for and and maximum values for and values were established as of 9 W hile using maximum values for and and minimum values for and a maximum of 73 was established. In order to PAGE 166 166 establish an average estimate for values for = 100 000 = 1 37 = 31 6 and = 250 were used to calculate 31 The damping ratio, was defined as = + 4 ( 5 8 ) In E quation 5 8 is the valve total flow pressure coefficient where = 0 00616 and represents the nominal servovalve flow rate ( 1 6 ) at maximum spool displacement with t he rated 1000 drop across the valve. The values used for and were presented above. The value was the viscous damping coefficient of the piston and load, which was approximated as = 0 1 Merritt (1967) a nd Manring (2005) posited an inequality that should be satisfied through proper selection of and this was used as a guide in the selection of for these calculations: 1 Given the values for and from the previous par agraph, it was obvious that could range in value from 0 1 10 while maintaining the inequality. In light of the above, calculated values for ranged from 0 13 to 0 17 Figure 5 13 show s the Bode magnitude and phase angle plots for the nominal values and alternate masses chosen for the hydraulic cylinder model. These plots show how the magnitude and phase angle plots v ary when the mass parameter was varied. The Bode plots again were typical of the second order equations used to generate the plots, where the natural frequency of the component and the damping ratio determined the shapes of the curves. Examination of the magnitude plots overlaid in Figure 5 13 showed that as the load increased, the resonant peak was damped significantly as PAGE 167 167 would be expected. The Bode phase angle plot becomes less steep and smoother as the load is increased. These three transfer function blocks comprised the system plant, This was written as = ( 5 9 ) Combining these transfer functions using block diagram algebra provided a composite transfer function that was 4th order in the Laplace transform operator The Bode magnitude and phase angle plots for the system plant are shown in Figure 5 14 with g= 0 18 The magnitude plot was reasonably well behaved with a horizontal run at the 14. 7 point and a resonant peak at the approximate break frequency of 30 where the break frequency marked where the magnitude dropped by 3 After the break frequency, the magnitude dropped at the rate of 80 This rate of descent indicated a fourth order system. The Bode phase angle plot followed a predictable pattern with an asymptotic rise from 0 rising steeply through the break frequency, and asymptotically approaching 360 The plot of the free space or unclamped m otion ( Figure 5 15, Figure 5 16, and Figure 5 17) of the single leg at different system pressures produced an interesting phenomenon t hat was observed in both the = 500 and the = 2 700 experimental runs. The plots show velocity as / as a function of command voltage ( ). A very pronounced nonlinearity was observed at low command voltages during the extension mode of cylinder operation. On the other hand, the companion plot of cylinder retraction appeared acceptably linear in nature throughout. The nonlinearity at PAGE 168 168 low voltage extension can be attributed to the friction and stiction associated with the cyli nder operation at low command voltages. The same trend was observed, but to a much lesser degree, in the retraction mode as the commanded voltage fell below approximately 2 The = 500 experiment ( Figure 5 15) produced extension rod/piston velocities that started at approximately 0 18 and then climbed to a peak of 0 32 before settling at 0 30 Again, the low initial veloc ity was attributed to friction and stiction in the cylinder. The = 2700 experiment ( Figure 5 17) produced rod/piston velocities in extension of approximately 0 68 The motion began slowly at 0 52 then increased rapidly to the aforementioned value. The retraction mode exhibited a somewhat different behavior with an initial motion of 0 70 which then tapered downward to a fairly steady state motion of 0 55 This initial movement in retraction was attributed to the higher flow acting on the smaller inner piston area and proceeded until the larger fluid volume in the lower chamber essentially choked the motion, i.e. the larger volume could not be dumped quickly enough through the return cylinder port. It should be recalled that the testing described above was done in the unloaded mode of operation. Figure 5 18 showed the actual plot of the leg operating in the unloaded or free motion condition with flow rate in plotted as a function of system pressure in Measurements were taken between = 500 and = 2700 for 90% commanded servovalve opening. The measured flow rate increased as system PAGE 169 169 pressure increased, from 1 0 at 500 to 2 25 at 3000 This was in excellent correspondence with the model given in Equation 5 4 for = 0 9 and = 0 1 This is also shown in Figure 5 18. The maximum and average piston velocity was the measured value which proved the datas veracity. Figure 5 19 showed the plot of measured and corrected data collected in the unloaded mode and compared to a second order system mode. Figure 5 19 also showed the model parameters. The measured versus modeled data plots were in very close agreement. The testing was done at low frequencies, between 0 1 and 50 so the phase lag did not go bey ond approximately 90 Ideally, if the frequency had been increased to 1000 the phase lag would have asymptotically approached 360 The break frequency occurred, as expected, at approximately 30 The fourth block in Figure 5 10 addressed the compliant spring transfer function, = ( 5 10 ) where was the Laplace transform operator, and an average value used for the compliant spring constant was 3659 The compliant spring constant given above is an average for all three of the specially engineered springs. The individ ual spring constants were: Spring #1: 3734 Spring #2: 3803 Spring #3: 3439 The numbering of the spring s corresponded to each of the numbered legs of the planar mechanism on which they were installed from left to right. Figure 5 20 show ed the magnitude and phase angle plots of the compliant spring transfer function. The Bode magnitude plot descended in a straight li ne at the rate of 20 which PAGE 170 170 signified a first order system. The Bode phase angle plot was also typical for a first order system, that is a straight line at 90 With the spring placed in series with the actuator, the complexity of the system increased to fifth order. This was given by the transfer function The specially engineered spring provided many benefits, but also added complexity to the system This resulted in a reduction in large force bandwidth, which was seen as a positive event in this case. That is an anomaly in conventional thinking The anomaly could be explained by the development of a more controllable system through more precise m easurement of spring deflection. In other words, the loss in bandwidth was more than compensated for by an increased sensing accuracy which was provided by the compliant spring and the associated linear transducer This in turn led to a more controllable system through the precise measurement of spring deflection. The added complexity was a result of the fact that the spring increased the order of the system mathematical analysis. An additional benefit was the fact that, since the spring deflects much more than a strain gauge or load cell, this enhanced measurement fidelity For t his reason, the load cells described previously were not used as a source of data for force measurement. The load cells were re tained as a serial part of the overall actuation and sensing system in the event that they might be required during future research. This was possi ble due to the fact that the spring constant of the load cell, 78, 000 was more than an order of magnitude greater than that of the specially engineered spring and hence could be ignored as a contributor to load deflection. PAGE 171 171 The or sy stem plant model, included plus the spring transfer function, This modeled transfer function is shown in Figure 5 21 for frequenci es between 0 1 and 1000 These plots were representative of a 5 order transfer function. The loaded system model of measured versus corrected data, was plotted in Figure 5 22. These data were collected for a fully restrained spring with a system pressure of = 500 The data was collected at frequencies that rang ed between 0 1 and 50 This did not allow data to be collected at higher frequencies due to measurement limitations. At low frequencies (less than 10 ), the system appeared to be first order 20 This was due to the addition of the spring with its associated Laplace integrator, If the capability had been available to record data at higher frequencies, it was speculated that the graph would have exhibited higher order tendencies consistent with the modeled data shown in Figure 5 21. Minor Loop Analysis The minor loop block diagram is shown in Figure 5 23 The input to this loop was the commanded voltage, and the output was cylinder velocity, in The minor loop consisted of the plant together with the feedback gain The minor loop transfer function then became = 1 + ( 5 11 ) where the open loop part is Figure 5 24 shows the magnitude and phase plot for the open loop part with gains of = 0 and 2 22 The following PAGE 172 172 discusses why the feedback gain was introduced, how it was selected, and why the 2 22 was a critical maximum. Figure 5 23 shows the relationship of the various blocks and transfer functio ns in the minor loop. The feedback gain, was introduced to provide damping in the overall system, to enhance stability, and to permit higher major loop control gains (Gorinevsky et al 1997). Varying the values of from 0 0 to 2 16 changed the shape of the magnitude and phase angle plots of as shown in Figure 5 25 The Bode magnitude plot shows an increasing resonant peak at the break frequency of close to 30 as the value of was increased. The Bode magnitude plot shows the system transfer function as a 4th order system, with a magnitude slope of 80 at high frequencies. The Bode phase angle plot also shows characteristics of the 4th order system where the phase changed from 0 to 360 as the frequency increased. Increasing lowered the magnitude at lower frequencies (less than 10 ). Increasing also reduced the phase lag at frequencies less than the break frequency ( 30 ). (Figure 5 25.) These key aspects provided damping and hence stability in the major loop. However, could only be increased to a critical value, as the following describes. It should be noted that the systems ability to trac k an input was a function of the hydraulic systems sensitivity and depended on the magnitude of the open loop gain (Johnson 1977). In this case, the open loop gain is actually the feedback gain Since each component of the hydraulic system co ntributes some amount of phase lag and amplitude gain, the magnitude gain of the open loop part should not be PAGE 173 173 greater than one (1) at the frequency where the phase shift is 180 If the gain exceeds one (1) under those conditions, t hen the system will oscillate uncontrollably at that frequency. In this case the unity magnitude was identified as 0 The feedback signal in the minor loop must be removed or disconnected from the summing junction, and this was the rationale for usi ng the open loop part to determine a critical maximum Figure 5 24 shows that the critical value of as approximately 2 22 It was critical because it placed the magnitude of the open loop part at 0 for the 180 phase lag frequency ( 30 ). Major Loop Analysis The major loop block diagram is shown in Figure 5 26 There were two inputs: desired force given in and load displacement given as The output was the actual force exerted on the leg, given in It should be noted that the load displacement was the variation in pinto pin length as defined in Chapter 4. The major loop consisted of the controller transfer function, the plant transfer function, the feedback gain, the compliant spring transfer function, and the differentiator which acted on Recall that the minor loop consisted of and The controller was defined as = 1 + ( 5 12 ) where is defined as the controller proportional gain, is defined as the controller integral gain, and is the Laplace operator. The plots in Figure 5 27 and Figure 5 28 demonstrated the frequency response of the controller as modeled. These two figures show the differences when various PAGE 174 174 feedback g ains were implemented in the major loop. In each case the proportional gain was held constant. Figure 5 27 showed the effects of varying the integral gain with the proportional gain held constant at 0 18 This was only apparent at low frequencies, that is frequencies below 0 1 As expected, the phase angle plot showed an asymptotic rise to zero phase angle and the magnitude showed an asymptotic decline to 16 The integral gain, was varied from 0 to 0 2 The difference between Figure 5 27 and Figure 5 28 involved holding the proportional gain constant while varying the int egral gain. Figure 5 28 was plotted with held constant at 0 4 The integral gain was again varied from 0 to 0 2 but the fa milies of magnitude curves were shifted upward by approximately 6 This corresponds to the ratio of the s that were used in the plots. The following provides additional background related to the structural design of the controller. Subsequently details were given regarding how the controller gains were selected. Finally, the actual response of the major loop was presented and compared to the derived model. Closed loop feedback control was implemented using several different feedback modes. The simplest closedloop control implemented was position control and was used to move the hydraulic cylinder to a desired position (extension or retraction of the cylinder rod). Another controller implemented was velocity control, where the velocity of the hydraulic cylinder extension or retraction was used in the feedback loop. In order to control force, most effort went into development of the closedloop force control PAGE 175 175 algorithm around the singleleg actuation device. At the singleleg control level, th e controller used an algorithm to generate a signal to the servovalve using the desired leg force and the actual sensed leg force. Closedloop feedback control in this chapter was represented by the box labeled Robot System in Figure 4 7 where input to the Robot System box was the desired leg force. Different feedback control algorithms were studied and considered for this project. The first was t he simplest and represented proportional feedback by using a single gain value that was multiplied by the force error. It was determined that the PD (proportional derivative) algorithm did not perform as well as the algorithm presented above. Gorinevsky e t al (1997) provided details on force control of robotic manipulators. Though this work was generic, that is it did not focus on either serial or parallel robotic manipulators; it provided excellent insight into the complexities of force control and contact for a robotic manipulator. The text also focused largely on explicit force control rather than implicit force control Explicit force control was the primary area of interest in this work. The focus in this reference was of great assistance in the efforts to gain insight into the force control strategy. Gorinevskys term for constrained motion was guarded contact which was a very apt description regarding the work described in this document. In Gorinevskys terms: A guarded motion allows one to establ ish manipulator contact with an object where position of the object is a priori unknown. After experimenting with numerous PD, PI, PID (P is proportional, D is derivative, and I is integral) and other control algorithms, a version of the controller proposed by Gorinevsk et al ( 1997) was modified to meet the requirements of the single leg device PAGE 176 176 as well as the planar mechanism The modified result that was used in the model for this research was given by the following: = + ( 5 13 ) where is the voltage to the servovalve amplifier, = is force error, is cylinder velocity, and and are the control gains as described above. This algorithm has been dubbed PIFB (proportional, integral, and feedback) It should be made clear once again that this control algorithm simply closed the loop around the screw theory based explicit kinestatic controller Equation 5 13 worked quite well in the model of the system response. As always with any control algorithm containing variable gains, tuning those gains was essential to the success of the project. Equation 5 14 describe d the leg force relationship to the desired force and the spring constant. In this case i s the transfer function with = 0 where = 1 + ( 5 14 ) and = 1 + ( 5 15 ) The additional transfer function, is given by the following = 1 1 + ( 5 16 ) where is the transfer function with = 0 Equation 5 17 is given as : PAGE 177 177 = + ( 5 17 ) where is the actual sensed force on the leg and is the desired force. The terms in the transfer functions above were defined previously. was the compliant spring constant used at the average value of 3658 and is the leg length or displacement of the load as measured by the L VDT Figure 5 29 showed the closed loop block diagram for the system with = 0 that is the device is in the clamped or locked configuration. Only the force was allowed as an input in this configuration. In this case Equation 5 14 bec ame = or = Figure 5 30 showed the closed loop block diagram for the system with the desired force set to zero, or = 0 In this configuration the device was free to move but with no load applied to it. In this case Equation 5 14 became = Figure 5 31 and Figure 5 32 showed the Bode magnitude and phase plots for several different values of the gains: and with the system in the = 0 mode. The magnitude plot fell off past the break frequency at a rate of 100 which is consistent with a 5 order system. This was very consistent across all of the different gain combinations. The break frequency also consistently held at approximately 30 despite the varying gains. The magnitude plot showed a resonant peak above 1 despite changes in the gains. This problem was solved in a redesigned controller which was described in the following sections. The phase plot was well behaved with differences in the plot provided primarily by changes in while the other gains, and were held PAGE 178 178 constant ( Figure 5 31). In Figure 5 32 the re was very little variation in the phase plot in which and were held constant, and was varied from 0 1 to 0 4 The phase angle predictably followed a smooth curve from 0 to 450 with the break or vertical porti on occurring at 30 When was held to zero or = 0 the closed loop Bode magnitude and phase angle plot was shown in Figure 5 33. This plot showed for the modeled data where was defined above in Equation 5 16. In this p lot, the equation from Figure 5 30, = was used in the form: = /( ) The Bode magnitude plot rose at the average rate of + 20 The change in sign in the numerator caused the rise as seen in Equation 5 16 (Ogata 1970). The + 20 rise in the magnitude plot indicated a first order system for The Bode phase angle plot for produced a well behaved curve which originated at approximately 20 and terminated at 180 Two of the gains were held constant for all three cases: = 0 and = 0 05 was again varied from 0 1 to 0 4 for the case described above was 0 1 the second used = 0 2 and the third case used = 0 4 For the second case, the p hase angle plot originated at 10 and terminated at 180 and for the third case, the curve originated at 5 and again terminated at 180 It was obvious that as was increased, the origination point of the phase angle curv e approached 0 Figure 5 34 showed the data plotted on a Bode diagram for the forward loop of the controller where the transfer function of int erest is = The PAGE 179 179 controller design was facilitated by the generation of this plot. The transfer functions and were defined above. was the transfer function for the inner loop feedback whic h combined and The data plotted in Figure 5 34 applied for the case of = 0 For this data set, = 1 0 = 1 0 and = 0 After the break frequency of 30 the magnitude plot fell off in a straight line at a rate of 100 which indicated a fifth order system. The magnitude plot included a small, but manageable, resonant frequency rise at the break frequency. The phase angle plot rose smoothly through the break with the lower asymptote beginning at 90 at a frequency of 0 1 and then ascended to 450 at a frequency of 1000 for the upper asymptote. Note that in this case the peak magnitude occurred at + 20 indicating that a gain adjustment was required. Figure 5 35 show ed the same tra nsfer function with the gains used in Figure 5 34 overlaid with a new data plot using = 0 8 with = 1 0 as in the previous case. The change in shifted the peak magnitude downward to 0 which was the desired goal Note that the change in gain, did not change the phase, so the phase angle plots in both cases were a direct overlay of one another. Figure 5 36 showed the Bode diagram plot of the experimental data obtained from operation of the single leg device. T he data plotted in the figure were obtained using the following test parameters: system pressure = 1000 proportional gain = 0 04 the desired force = 150 and with the frequencies varied from PAGE 180 180 0 1 10 System bandwidth constrained most operation of the device to low frequencies. The Bode phase and magnitude plots were well behaved with the magnitude originating at 0 and breaking at 4 The phase angle originated at 0 and moved smoothly past the break fr equency through approximately 80 at 10 Figure 5 37 and Figure 5 38 showed the forced response from an applied step input from 0 to 250 Again, the plots of the data showed excellent correlation with the modified force controller. This data was obtained in the locked or clamped position of the single leg device as opposed to free motion. Figure 5 39 and Figure 5 40 demonstrated the response to a step input from 250 to + 250 load in the locked or clamped position. The gain was = 0 02 and the operating pressure was = 1000 The forced step response tracked the force command extremely well. Actual Versus Modeled Controller The actual controller, or modified controller, used to obtain this data was slightly different from the controller used to obtain the data for the modeled plots. The workable controller for this research actually evolved into the modified controller. The following section will provide a more detailed explanation. After extensive modeling of the control system using Equation 5 13 it was found that the control algorithm worked quite well as a model controller. During actual experimentation, the model controller was modified to produce results that were more accurate and would provide better performance in the real world setting. The actual controller hardware that was readily available for testing helped drive this modification in PAGE 181 181 controllers. Initial consideration indicated that the hardware would evolve into a configuration that rese mbled the model control law, but actual test results for the modified controller proved that this was untrue. A description of the modified controller follows. The minor loop was controlled primarily by the position of the mechanism and used a proportional gain. was the modified proportional gain where ~ 0 13 1 5 1000 1 25 4 1 = 660 ( 5 18 ) In E quation 5 18, is the encoder count and is voltage. The first term ( 0 13 ) in E quation 5 18 was derived from the actual proportional gain, multiplied by the D/A converter conversion factor of This gain multiplied the position error, given in a nd outputted a short integer D/A command. T he minor loop operated at 1000 and the voltage sent to the servovalve was given by = ( ) ( 5 19 ) where was the commanded position, and was the actual position. Both and were recorded in It was noted that the zeroorder hold (ZOH) was neglected for the minor loop i n the higher frequencies for the purposes of this controller. T his was due to the effects of the ZOH data points falling much closer together at the higher frequencies, and, in effect smoothing the curve. In this slightly different construct, the major loop operated on a force control error of = Thi s was the difference between the commanded and the actual forces PAGE 182 182 recorded in The major loop controller calculated the desired change in commanded position, at a rate of 100 This was based on the relation = ( 5 20 ) where = 0 4 ~ 79 10 ( 5 21 ) Because the calculations were occurring at each = then followed the relationship: ( ) = ( 0 ) + = ( 5 22 ) where, without a loss of generality, the initial condition ( 0 ) = 0 was assumed. The Laplace Transform of the control law became = + ( 5 23 ) where is the actual velocity with units of In this case = ( ) ( 5 24 ) where ( ) was the ZOH transfer function and = ~ 0 13 0 4 0 01 = 5 2 ( 5 2 5 ) where = and ~ 660 It was noted that = 0 during the actual testing, and the ZOH transfer function was ( ) = 1 2 2 + ( 5 26 ) where = 0 01 PAGE 183 183 The gain set used in this discussion and previously in this chapter followed the convention { } where, as previously defined, was the proportional gain, was the integral gain, and was the feedback gain. The actual gain set used with the modified controller was 0 5 2 At this point, it was useful to compare the actual, modified controller with the theoretical controller that was modeled previously The theoretical model examined the gain margi n for the minor loop and then for the major loop of the overall control scheme. For the model, the gain set 0 1 was considered a good or stable option which yielded a positive gain margin in both the minor and major loops. The g ain set 1 0 1 was used in the model as a beginning point or reference for the determination of a stable gain margin for the major loop. The transfer function for the open loop portion of the major loop was given previously as = K + K s 1 + ( 5 27 ) This was the transfer function used in developing the data shown in Figure 5 34 and Figure 5 35. There was excellent agreement between the theoretical gain set, 0 1 and the actual or modified gain set, 0 5 2 at low frequencies. At higher frequencies, the actual gain set produced a desirable lower phase lag The magnitude PAGE 184 184 was also less. The theoretical an d the actual gains were in reasonable agreement with the reference gain set 1 0 1 It should be noted that some researchers have posited that the integral portion of the feedback would not work for the case of hard contact (Zheng et al 2011). Once the friction and compliance issues were resolved, it became obvious that integral gain would be a valuable contributor to the force control scheme (Schimmels and Peshkin 1994). Summary This chapter presented the development of the force control algorithm that formed the basis of control of each individual leg of the hydraulic, robotic manipulator. A detailed description of the hydraulic components was provided first and was then followed by the dev elopment of the hydraulic system plant model and the related transfer function. Finally the minor control loop algorithm was discussed followed by an analysis of the major control loop. Each set of data collected from the experiments was plotted in the accompanying figures and discussed in the appropriate section. T he single leg force control research was begun at the most basic level E xperiments were conducted while the end of the single leg actuation device was clamped rigidly to a mechanical structure Once data had been collected in the clamped or locked mode, final experiments were run to effect free space movement until contact was made with the mechanical structure. C ontact was sustained while a preset maintenance force was maintained. The prel iminary data was collected using the model control algorithm Equation 5 13 PAGE 185 185 = + [ ( 5 13 )] The actual controller developed after a great deal of experimentation with the model controller used to collect the final data was given as: = + [ ( 5 23 )] The work performed in Chapter 5 formed the third block in the hier archical explicit kinestatic force control algorithm. Figure 5 1 Leg force c ontroller. Figure 5 2 Hydraulic circuit for hydraulic cylinder actuation device. 3 way servo valve double acting hydraulic cylinder 1.5 in bore, 25 in stroke to return manifold (see hydraulic power unit diagram) from pressure manifold (see hydraulic power unit diagram) 5 micron filter dump valve solenoid preset to 3125 Leg Force Controller From e xplicit k inestatic c ontroller: Leg force setpoints Leg velocity setpoints (noncontact mode) Feedback from actuation devices: Actual leg forces Actual leg lengths Derived leg velocities Feedback to e xplicit k inestatic c ontroller: Current actual leg forces Current actual leg lengths (redundant) Command to actuation devices: Hydraulic servovalve commands PAGE 186 186 Figure 5 3 Photo of single leg test apparatus. Photo courtesy of Al Nease. PAGE 187 187 Figure 5 4 Photo of single leg compliant device. Photo courtesy of Al Nease. Figure 5 5 C ompliant device installed inline with linear actuator Photo courtesy of Al Nease. PAGE 188 188 Figure 5 6 Applied l oad vs d isplacement of c ompliance spring #1. Figure 5 7 Applied l oad vs d isplacement of c ompliance spring #2. y = 3733.5x + 1956.9 1000 800 600 400 200 0 200 0.80 0.75 0.70 0.65 0.60 0.55 0.50 Applied Load (lb f ) Displacement (in) Applied Load vs Displacement (Spring #1) y = 3803.3x + 103.32 1000 800 600 400 200 0 200 0.30 0.25 0.20 0.15 0.10 0.05 0.00 Applied Load (lb f ) Displacement (in) Applied Load vs Displacement (Spring #2) PAGE 189 189 Figure 5 8 Applied l oad vs d isplacement of c ompliance spring #3. Figure 5 9 Hydraulic circuit for hydraulic power unit. y = 3439.2x + 187.09 1000 800 600 400 200 0 200 0.35 0.30 0.25 0.20 0.15 0.10 0.05 0.00 Applied Load (lb f ) Displacement (in) Applied Load vs Displacement (Spring #3) 20 HP motor 1760 RPM 5 micron filter 40 gal reservoir HPU dump valve solenoid preset to 3125 ball valve pressure manifold 1 liter accumulator charged to 2100 fluid cooler 10 micron filter with pressure release return manifold to hydraulic actuator #3 to hydraulic actuator #1 to hydraulic actuator #2 from hydraulic actuator #3 from hydraulic actuator #1 from hydraulic actuator #2 3000 psi (500 psi for initial tests) PAGE 190 190 Figure 5 10. System plant block diagram for singleleg actuation device. S ervo amplifier gain + 2 + S ervo valve conversion factor + 2 + H ydraulic cylinder conversion factor C ompliant spring constant S ervo valve damping ratio S ervo valve natural frequency C ylinder damping ratio H ydraulic cylinder natural frequency Laplace transform operator F orce applied to load PAGE 191 191 Figure 5 11. Servo v alve a mplifier modeled response. Figure 5 12. Servo v alve modeled response to different servovalve gains. 90 45 0 45 90 0 20 40 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (mA/V)) Frequency (Hz) Servo Valve Amplifier Bode Magnitude and Phase Angle Plot Mag Phase gsva= 20.0 mA/V 180 135 90 45 0 120 100 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (GPM/mA)) Frequency (Hz) Servo Valve Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 g sv = 0.00800 GPM/mA 2 g sv = 0.00720 GPM/mA 3 g sv = 0.00327 GPM/mA PAGE 192 192 Figure 5 13. Hydraulic cylinder modeled response to different payload weights. Figure 5 14. modeled response. 180 135 90 45 0 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (ips/GPM)) Frequency (Hz) Hydraulic Cylinder Bode Magnitude and Phase Angle 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 Wt = 250 lb f z = 0.13, w = 209 rad/sec 2 Wt = 500 lb f z = 0.19, w = 148 rad/sec 3 Wt = 1000 lb f z = 0.27, w = 104 rad/sec 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (ips/V)) Frequency (Hz) System Plant Bode Magnitude and Phase Angle Plots 1 Mag 1 Phase g sva *g sv *g cyl = 0.18 (ips/V) PAGE 193 193 Figure 5 15. Velocity of single leg at = 500 Figure 5 16. Velocity of single leg at = 1000 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0 1 2 3 4 5 6 7 8 9 10 Velocity (ips/V) Voltage (V) Velocity of Single Leg at P s = 500 psi Retract Extend 0 0.1 0.2 0.3 0.4 0.5 0.6 0 1 2 3 4 5 6 7 8 9 10 Velocity (ips/V) Voltage (V) Velocity of Single Leg at P s = 1000 psi Retract Extend PAGE 194 194 Figure 5 17. Velocity of single leg at = 2700 Figure 5 18. Unloaded maximum velocity/ maximum flow rate vs system pressure. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0 1 2 3 4 5 6 7 8 9 10 Velocity (ips/V) Voltage (V) Velocity of Single Leg at P s = 2700 psi Retract Extend 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.0 0.5 1.0 1.5 2.0 2.5 0 500 1000 1500 2000 2500 3000 Maximum Average Velocity (ips/V) Flow Rate (GPM) Pressure Supply (psi) Velocity and Flow vs. System Pressure Flow Rate vs Pressure Supply theoretical Actual Flow Rate (GPM) Maximum Average Velocity (ips/V) PAGE 195 195 Figure 5 19. measured vs modeled response. Figure 5 20. Compliant s pring modeled response. 135 90 45 0 20 15 10 0.1 1.0 10.0 100.0 Phase (deg) Magnitude (dB (ips/V)) Frequency (Hz) Unloaded System Plant (Measured/Corrected vs 2nd Order Model) Mag measured (Corrected) Mag modeled Phase measured (Corrected) Phase modeled 2 nd order model used: 0.97 GPM ave. area = 1.37 in 2 z c = 0.707 w c = 38 Hz 135 90 45 0 20 0 20 40 60 80 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) Compliant Spring Bode Magnitude and Phase Angle Plot Mag Phase K sp = 3658.7 psi PAGE 196 196 Figure 5 21. modeled response. Figure 5 22. measured vs modeled response ( = 500 ) 450 405 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 20 40 60 80 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (mA/V)) Frequency (Hz) G splt Bode Magnitude and Phase Angle Plot Mag Phase Ksp= 3658.7 psi 225 180 135 90 45 0 0.0 20.0 40.0 60.0 80.0 0.1 1.0 10.0 100.0 Phase (deg) Magnitude (dB (lb f /V)) Frequency (Hz) Loaded System Plant (Measured vs 2 nd Order with Integrator Model) Mag measured (Corrected) Mag modeled Phase measured (Corrected) Phase modeled 2 nd order model used: 0.85 GPM ave. area = (1.76+0.98)/2 = 1.37 in 2 z = 0.707 w c = 38 Hz K spr (spring constant) = 3658 lb f / in 2 nd order plus additional K spr /s integrator PAGE 197 197 Figure 5 23. Minor loop block diagram for singleleg actuation device. Figure 5 24. System p lant (c ritical) modeled response. 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (ips/V)) Frequency (Hz) System Plant (Critical) Bode Magnitude and Phase Angle Plots 1 Mag 2 Mag 1 Phase 2 Phase 1 g sva *g sv *g cyl = 0.18 (ips/V) 2 g sva *g sv *g cyl *2.22 = 0.41 (ips/V) = + S ervo valve amplifier transfer function (TF) Laplace t ransform operator S ervo valve TF M inor loop feedback gain H ydraulic cylinder TF S ystem plant TF Comm and input to servovalve ( ) Cylinder velocity inches per second TF of system plant with minor loop feedback = 1 + PAGE 198 198 Figure 5 25. Minor l oop modeled response to changes in feedback gains. Figure 5 26. Major loop block diagram for singleleg actuation device. 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 20 40 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB (ips/V)) Frequency (Hz) Minor Loop (1) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 4 Mag 1 Phase 2 Phase 3 Phase 4 Phase 1 K FB = 1.7 2 K FB = 1.0 3 K FB = 2.0 4 K FB = 2.16 = 1 + + + + D esired force C ontroller proportional gain A ctual force C ontroller integral gain S ervo valve amplifier TF Laplace transform operator S ervo valve TF M inor loop feedback gain H ydraulic cylinder TF D isplacement of load C ompliant spring TF Controller TF = 1 + PAGE 199 199 Figure 5 27. Controller modeled response to changes in with set gain. Figure 5 28. Controller modeled response to changes in with larger gain. 135 90 45 0 45 20.0 0.0 20.0 40.0 60.0 0.001 0.010 0.100 1.000 10.000 Phase (deg) Magnitude (dB (V/lb f )) Frequency (Hz) PI Controller (1) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 (K p = 0.18, K I = 0.0) 2 ( K p = 0.18, K I = 0.1) 3 ( K p = 0.18, K I = 0.2) 135 90 45 0 45 20.0 0.0 20.0 40.0 60.0 0.001 0.010 0.100 1.000 10.000 Phase (deg) Magnitude (dB (V/lb f )) Frequency (Hz) PI Controller (2) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 (K p = 0.4, K I = 0.0) 2 (K p = 0.4, K I = 0.1) 3 (K p = 0.4, K I = 0.2) PAGE 200 200 Figure 5 29. Major loop block diagram for singleleg actuation device with = 0 = + + TF of servo valve amplifier, servo valve, and hydraulic cylinder Inner (minor) loop feedback gain TF of controller = 1 + TF of inner loop feedback with and TF of compliant spring TF of forward loop with and TF total for / with = 0 = 1 + + = 1 + + = 1 + PAGE 201 201 Figure 5 30. Major loop block diagram for singleleg actuation device with = 0 TF of controller TF of system plant with inner loop feedback TF of compliant spring TF total for / ( ) with = 0 Laplace transform operator = 1 1 + + = 1 1 + 1 + 1 + PAGE 202 202 Figure 5 31. Major l oop modeled response for / with varying Figure 5 32. Major l oop modeled response for / with varying 450 405 360 315 270 225 180 135 90 45 0 180 160 140 120 100 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) Major Loop (1) ( x l =0 ) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 K FB = 0, K p = 0.05, K I = 0.02 2 K FB = 0, K p = 0.07, K I = 0.02 3 K FB = 0, K p = 0.08, K I = 0.02 450 405 360 315 270 225 180 135 90 45 0 180 160 140 120 100 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) Major Loop (2) ( x l =0 ) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 K FB = 0, K p = 0.05, K I = 0.1 2 K FB = 0, K p = 0.05, K I = 0.2 3 K FB = 0, K p = 0.05, K I = 0.4 PAGE 203 203 Figure 5 33. M ajor l oop modeled response for /( ) with varying Figure 5 34. Forward part of m ajor l oop ( = ). 225 180 135 90 45 0 80 60 40 20 0 20 40 60 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) Major Loop (3) ( F d =0 ) Bode Magnitude and Phase Angle Plot 1 Mag 2 Mag 3 Mag 1 Phase 2 Phase 3 Phase 1 K FB = 0, K p = 0.05, K I = 0.1 2 K FB = 0, K p = 0.05, K I = 0.2 3 K FB = 0, K p = 0.05, K I = 0.4 450 405 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 20 40 60 80 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) G fl =G ctl *G mplt *G spr Bode Magnitude and Phase Angle Plots 1 Mag 1 Phase K FB = 1.0, K p = 1.0 PAGE 204 204 Figure 5 35. Modeled forward critical loop ( = ). Figure 5 36. Closed loop force control, = 1000 = 0 4 = 150 450 405 360 315 270 225 180 135 90 45 0 140 120 100 80 60 40 20 0 20 40 60 80 0.1 1.0 10.0 100.0 1000.0 Phase (deg) Magnitude (dB) Frequency (Hz) G fl (Critical) Bode Magnitude and Phase Angle Plots 1 Mag 2 Mag 1 Phase 2 Phase 1 K FB = 1.0 K p = 1.0 2 K FB = 1.0 K p = 0.056 90 45 0 20 10 0 10 20 0.1 1.0 10.0 Phase (deg) Magnitude (dB) Frequency (Hz) Bode Magnitude and Phase Plots for P s = 1000 psi, Gain = 0.4, F d = 150 lb f Magnitude Phase PAGE 205 205 Figure 5 37. Step response to force command with D/A control value. Figure 5 38. Step response to force command zoomed in to show 0 5 of data. 10000 8000 6000 4000 2000 0 2000 300 250 200 150 100 50 0 50 0.0 0.5 1.0 1.5 2.0 2.5 D/A control value Force (lb f ) Time (sec) Force step response (0 to 250 lb f ) P s = 1000 psi, Gain = 0.2 measured force force setpoint D/A control value 10000 8000 6000 4000 2000 0 2000 300 250 200 150 100 50 0 50 0.0 0.1 0.2 0.3 0.4 0.5 D/A control value Force (lb f ) Time (sec) Force step response (0 to 250 lb f ) P s = 1000 psi, Gain = 0.2 measured force force setpoint D/A control value PAGE 206 206 Figure 5 39. Step response to force command with D/A control value. Figure 5 40. Step response to force command zoomed in to show 0 5 data. 5000 0 5000 10000 15000 20000 300 200 100 0 100 200 300 0.0 0.5 1.0 1.5 2.0 2.5 D/A control value Force (lb f ) Time (sec) Force step response ( 250 to 250 lb f ) P s = 1000 psi, Gain = 0.2 measured force force setpoint D/A control value 5000 0 5000 10000 15000 20000 300 200 100 0 100 200 300 0 0.1 0.2 0.3 0.4 0.5 D/A control value Force (lb f ) Time (sec) Force step response ( 250 to 250 lb f ) P s = 1000 psi, Gain = 0.2 measured force force setpoint D/A control value PAGE 207 207 CHAPTER 6 SYSTEM INTEGRATION AND IMPL EMENTATION If a man has done his best, what else is there? General George S. Patton, War as I Knew It memoirs, 1975 Overview This chapter present s the synthesis of the work described in Chapters 3, 4, and 5 and the culmination of the heuristic force controller as applied to the planar mechanism. Figure 6 1 dem onstrates the interrelationship among the various controller components shown previously in Figure 3 1 Figure 4 1 and Figure 5 1 This chapter begins with the force control of the single leg device presented in Chapter 5. The single leg work was then extended to the three legs of the planar mechanism. The research performed in Chapter 4 on the explicit kinestatic controller (EKC) was then applied to the planar mechanism. The work developed in Chapter 3, the assembly task supervisor, finalized the overall control heuristic. This information was used to guide the planar mechanism through a commanded sequence of movements in order to accomplish the assembly task. Each level of the control architecture depended on input and output from the levels above or below it as shown in Figure 6 1 A decision was made at the outset of this research to go with explicit force control versus implicit force control. Explicit force control addressed the forc e applied to the actuator. Implicit for ce control addressed the displacement of each actuator For the system investigated in this research consisting of legs connected in parallel, explicit force control provided a better fit to obtain the goals of simultaneously controlling constraint wrench and freedom twist in partially constrained scenarios. PAGE 208 208 The primary issue regarding force control was the ability to control the interaction force between the manipulator and the environment and effect movement of the payload while controlling this force. This goal was accomplished by development of three levels of controllers arranged in the hierarchy shown in Figure 6 1 The first controller, the low level controller was the leg force controller that was applied to the three hydraulic legs of the payload mechanism. One of the primary results of data taken from the payload mechanism using individual leg force control was that the force control was decoupled. In oth er words, the force control in one leg did not affect force control in any of the other legs. The mid level controller was the explicit kinestatic controller. Its role was to be able to take the constraint wrench and the twist of freedom (deflection wrenc h) and generate leg force setpoints. There were three items that required validation. It was required to show that by controlling the constraint wrench, this did not cause the payload to move. Secondly, it was required to show that the addition of the deflection wrench did indeed cause the payload to move in the desired direction, that is along the direction of the desired twist of freedom. Lastly, it was required to show that the deflection wrench did not affect the control of the constraint wrench. Th e high level controller was the assembly task supervisor. Its function was to sequence the payload through the developed heuristic for the assembly task. Implementation of the assembly task supervisor required that the payload be correctly sequenced through the heuristic independent of the starting position and orientation (location) of the payload or target PAGE 209 209 These three levels of controllers are represented in Figure 6 1 and illustrate the interconnectivity of the controllers. A brief discussion of each of these controllers is presented in the following. Throughout this chapter, several conventions were maintained as keys to the description of the operation of the planar mechanism. All displacements were presented in the global coordinate system while all wrench coordinates were referenced to the payload coordinate system. During the tests where the point G coordinates were plotted, the coordinates were calculated from the location of point F along with the knowledge of the distance between points F and G ( 34 709 along the payload axis ) and the gamma angle, given as the inclination of the payload from the global horizontal axis. This approach, together with the following concepts, demonstrated that the calibration of the kinematic model was good and workable. The target was initially placed at 0 with respect to the global axis Within the range of reasonable accuracy, the coordinate of point F when point F was in contact agreed with the coordinate of point G when point G was also in contact. This test provided the nec essary confidence that the kinematic model was viable and that the forward analysis was dependable. It should be noted that having the target located at 0 with respect to the global axis did not represent a simplification that would jeopardize the research results or conclusions. Rather than move the target to various starting angles with respect to the global coordinate system, the payload mechanism was rotated prior to initiation of data tests. For example, if the payload mechanism was rotated about the payload axis PAGE 210 210 coordinate frame in a negative direction, point F would be higher than point G in the global coordinate system. Then when the test was initiated and the payload was lifted in the global + axis direction, point F would make con tact with the target first. If the payload mechanism was rotated about the payload axis coordinate frame in a positive direction, point G would be higher than point F in the global coordinate system. When the test was initiated and the payload was lif ted in the global + axis direction, the point G would make contact with the target first. If the payload mechanism was not rotated initially, points F and G would be at the same global coordinate system location. When the test was initiated and the payload was lifted in the global + axis direction, the points F and G would make contact with the target at virtually the same time. Leg Force Controller The control of the single leg described in Chapter 5, was used as the force controller for the indi vidual legs of the planar mechanism From Figure 6 1 it was seen that the data input to the leg force controller from the system sensors were the actual leg forces, leg lengths, and the derived leg velocities. The leg force controller returned the real time data on the leg forces and the leg positions to the EKC. The EKC calculated the leg force setpoints and the leg velocity setpoints. Leg velocity setpoints were only used when the device was in the free space motion or noncontact mode. Th e leg force controller used the force setpoints to generate commands to the res pective electrohydraulic servovalves that controlled each leg. This section described the results obtained when the force controller was applied to the planar mechanism consis ting of three leg devices or actuators The data plots of PAGE 211 211 experiments with the payload clamped or locked to the target were presented in Figure 6 2 through Figure 6 7 In general, the step response of each leg was extremely well behaved with each leg reaching the force setpoint within 250 Figure 6 2 show s the response of the planar mechanism to a command set given in units of where the force commands to Legs 1 2 and 3 were given in that order: ( 100 0 0 ) respectively. The plot thus represented a step from 0 to 100 in L eg 1 only. Figure 6 3 and Figure 6 4 represented individual step responses in L eg 2 and L eg 3 of 100 respectively. The ordered force sets for these plots were ( 0 100 0 ) and ( 0 0 100 ) As was the case with L eg 1 the force applied to Leg 2 represented a step from 0 to 100 with 0 having been applied as setpoints to the other two legs. The same force setpoint was applied to Leg 3 as shown in Figure 6 4 These three plots clearly show that there was no cross coupling between force control of the individual leg controllers. This justifies the assumption of decoupled leg force control presented in Chapter 5 Figure 6 5 showed the response of the planar mechanism to a 0 to + 100 step in all three legs simultaneously and was represented by ( 100, 100 100 ) Figure 6 6 showed the results when a negative force step input was applied to all three legs and was given by ( 100, 100 100 ) The results were again well behaved with the same approximate rise ti me and no cross coupling present Figure 6 7 showed the results when Legs 1 and 3 responded to a force step input of 0 to 100 and L eg 2 to a 0 to + 100 step input. This case was represented by ( 100 100 100) As in the previous experiments, the results were well behaved with the actual force measured in each leg reaching the setpoint within 250 PAGE 212 212 Cross coupling, due to the unique design and the kinematics of the planar mechanism, proved to be of no significance from a control or an operational standpoint. This was shown clearly in Figure 6 2 Figure 6 3 and Figure 6 4 In these plots, L egs 1 2 and 3 were operated individually and in coordinated fashion with no demonstrable sign of crosscoupling. Force control of L eg 1 did not affect the control of either L eg 2 or L eg 3 and the same phenomena were observed with other combinations of leg force control. Because of these results, cross coupling was not subsequently addressed in this research. During an initial test of individual force control of the planar mechanism, a nominal force setpoint of 50 was applied to each leg. This experiment was performed on the payload in an unconstrained (or unclamped) mode. After contact with the target and the control stabilized, an external disturbance force was applied to the payload in the global + payload direction to observe the response of the leg controllers. The response of the force controllers are shown in Figure 6 8 It can be seen that during the application of the disturba nce force, the sensed force in Legs 2 and 3 incre ased while the force sensed in L eg 1 decreased. After the disturbance force was removed, the controller was able to bring the forces in each leg back to the 50 setpoint. Figure 6 9 shows the response of the individual leg lengths during the disturbance force and, as expected, the lengths of L egs 2 and 3 become shorter while L eg 1 increases in length. Explicit Kinestatic Controller The explicit kinestatic controller (EKC) discussed in detail in Chapter 4, provided the means for decomposing a general wrench generated by the leg forces of the planar mechanism into a unique constraint wrench applied to the target together w ith a PAGE 213 213 deflection wrench that caused a desired freedom twist of the payload. This was all accomplished using screw theory and the concept of wrenches of constraint (WOC) and twists of freedom (TOF) interacting between the payload and the target. The EKC performed calculations to determine the current stiffness matrix [ ] of the planar mechanism, the current payload position and orientation, and the resultant wrench or resultant force to be applied to the target. The explicit kinestatic controller used real time leg force and leg length data from the leg force controller to calculate the current wrench, the payload location, and the stiffness matrix [ ]4. The EKC made these data available to the assembly task supervisor (ATS). The assembly task supervisor performed the necessary calculations and then returned the desired wrench of constraint, the next desired twist of freedom, as well as the current kinestatic model. The EKC then calculated the leg force setpoints and provided these to the leg force controller. The EKC was implemented according to Figure 4 7 where the resultant wrench was calculated using Equation 4 4 During the feedback loop shown in Figure 4 7 the constraint wrench was obtained and controlled. This required that the assembly task supervisor provide the [ ] and [ ] matrices developed in Chapter 3 and shown in Table 3 1 to the next level controller The EKC compensated for gravity as shown in Figure 4 7 Collectively, the [ ] and [ ] matrices form the viable kinestatic model. 4 During this research, the data from the metrology frame was not used to provide the payload location. Actual individual leg lengths were used along with the kinematic model of the planar mechanism to establish the payload location. Future efforts will utilize the metrology frame in order to establish the payload location for simpler and easier extension to the six legged spatial platform. PAGE 214 214 Operation of the explicit kinestatic controller required that two objectives be satisfied. These were presented in Chapter 4 and are repeated below: While developing the theory set forth here, two objectives were identified. One objective was to verify that the correction for constraint wrench error did not cause a displacement of the payload. A second objective was to verify that the deflection wrench did not cause a change i n the constraint wrench. For the controller to properly function, and hence be physically and geometrically meaningful, it was important that these two objectives were satisfied. When satisfied, the two corrective actions could be combined by a principle of superposition. In this way, the verifications qualified the explicit kinestatic controller as meaningful, and they qualified the resulting leg force setpoints as meaningful as well. In addition, a third objective was to show that the application of a deflection wrench would cause payload movement in the desired direction. The realization of these objectives comes from analysis of the experimental data collected during testing as the following describes. Question: Does the System Move While Controlling the Constraint Wrench? In the first series of tests, the explicit kinestatic controller was programmed to raise the payload upward in the global + axis direction until contact with the target was determined. During this series of tests, the EKC and th e low level leg force controllers were both active throughout. Prior to the start of each test, the payload was placed at either + 5 0 or 5 with respect to the global coordinate system and at some generous vertical distance below the target. The first test performed was to examine movement of the payload after contact with point F was determined. Figure 6 10 plotted the change in payload point F coordinates and the change in the payload angle with respect to the global coordinate system after contac t with point F was determined (C ase 1). Contact was detected and data logged beginning at 0 in the plot. There was some change noted in the PAGE 215 215 location of point F with the drifting approximately 0 02 and drifting between 0 and 0 01 over a period of 50 The angle of the payload did change from 0 to nearly 1 during the 50 test duration. Leg 3 consistently lagged the other two legs in reaction time which was possibly due to a slight offset error in calibration of the third leg or stiction related to the force transducer/spring component. Figure 6 11 showed plots of the magnitude and angle of the commanded and actual constraint wrenches. It can be seen that the magnitude of the constraint wrench was equal to the maintenance force of 225 The angle o f the constraint wrench was about 82 The wrench angle was expected to be approximately 85 in the payload system since the payload was originally positioned at 5 in the global coordinate system. The small difference can be attributed to a slight angular drift prior to contact. Also, slight calibration errors coupled with stiction errors in the legs may have contributed to the wrench angle disparity. Figure 6 12 presented the components of the constraint wrench. Since the target was nominally at 82 the component of the constraint wrench should be 225 sin ( 82 ) and the component should be 225 cos ( 82 ) This meant that the loadings were approximately 223 and 30 respectively. It can be seen that the component started around 25 and gradually increased over time to 30 Figure 6 13 showed the commanded leg forces versus the actual leg forces for the Case 1 contact test. The actual leg forces followed the commanded values very well. Figure 6 14 provided a zoomedin view of the commanded leg forces versus the actual PAGE 216 216 measured leg forces presented in Figure 6 13 In each case, after about 0 4 the commanded and actual leg forces were in close agreement with each other. Figure 6 15 showed the commanded leg force errors. This figure showed the differences more clearly between commanded and actual leg force errors. Legs 1 and 2 varied by about 2 but L eg 3 had an initial offset of about 5 and over time drifted toward a 3 error off set. The magnitude of error oscillations in L eg 3 were consistent with that observed in Legs 1 and 2 It is worthy to note that the resolution of the force measurements were on the order of 1 demonstrating that errors shown in Figure 6 15 were genuinely small. Over the course of the 50 tes t, the small error recorded in L eg 3 explained the cause of the slight rotation shown in Figure 6 10. In light of this, it can be said the control of the substantial 225 constraint force did not cause demonstrable displacement of the payload. Another tes t for contact with point F first is shown in Figure 6 16 through Figure 6 18. Figure 6 16 showed a displacement of point F illustrating a drift in the global axis direction upon contact at point F. There was no drift in the global dir ection. Figure 6 17 showed the constraint wrench magnitude, 225 and angle, 90 Both tracked the commanded values well across the 50 duration of the test. In Figure 6 18, the force errors were plotted and again, the same response as previously shown was apparent. However, the offset in the leg error for Leg 3 was higher at 15 initially and fell to around 9 before contact between the target and point G could be seen at approximately 45 Clearly some offset error in Leg 3 caused the coordinate of point F to drift more in this run than in the previous run. However, the PAGE 217 217 basic conclusion was the same: no demonstrable displacement could be attributed to the control of the substantial 225 constraint force. The slight errors in the leg forces were again due to calibrati on and/or stiction causes. A third data test for contact with point F (Case 1 contact) is shown in Figure 6 19 through Figure 6 21 During this test, external disturbance forces were randomly applied to the payload. Figure 6 19 present s the response of the payload point F to these disturbances. This test was performed to investigate the stiction or offset effects in Leg 3 The times of the application of the external disturbances were: 2 4 38 41 43 48 and were indicated by black vertical lines. Depending on the magnitude (not measured) of the disturbances, changes were noted in the payload axis rotation displacement as well as changes in the global displacement of point F. These data were proof that the minimal moveme nt during the early runs was caused by a combination of stiction and slight offsets in the individual leg setpoint of Leg 3 This information was critical to showing that the objective of control of the constraint wrench did not cause the payload to move. It could be argued that more attention should have been paid to the nonlinear issue of stiction, however, it will be shown that through proper choice of the gains in the force controller, that this was not necessary. While the magnitudes of the disturba nces were not measured, it is useful to note that the disturbances were applied using a handheld rubber mallet. The external disturbances were applied in the general global direction, and a given disturbance (if large enough) seemed to counteract or reverse slight drifts while the test run was underway. PAGE 218 218 Figure 6 20 plot s the magnitude and angle of the commanded and actual constraint wrench during the test where external disturbances were applied to the payload. As can be seen, slight changes in the magnitude and angle were observed at the application of the external disturbances. Figure 6 21 shows the leg force errors during the test and shows that Legs 1 and 2 were behaving well, but Leg 3 showed the effects of stiction or offset calibration. Figure 6 22 through Figure 6 26 show s the results of another run, but this one started with point G at a higher global elevation than point F. This represente d Case 2 (contact with point G first). Figure 6 22 showed the response of the payload point G and the payload axis rotation displacement. From this plot, it was observed that point G initially moved in the global direction but then did not move. Changes in the direction were also minimal. The payload coordinate system showed a slight rotation change from 0 to 0 2 during the test duration of 50 The order of magnitude of these changes was very small and, it could be argued due to the resolution ( 1 ) of the force sensors, negligible. Figure 6 23 show s the magnitude and angle of the constraint wrenches and show s that the magnitude of the actual constraint wrench tracked well to the 225 threshold setpoint, and the actual wrench angle tracked well to 90 Figure 6 24 showed the components of the magnitude of the constraint wrench, and here the component tracked well to the 225 value, and the component tracked well to the 0 val ue. The perpendicular distance to the constraint wrench was plotted in Figure 6 25 and showed that the distance from the payload coordinate origin was 34 7 Th is represented the distance between points F and G showing that the constraint wrench PAGE 219 219 did in fact go through point G. Figure 6 26 showed a plot of the leg force errors and showed each leg was well behaved with the leg errors running between 2 The next set of data was collected to illustrate the lack of movement of the payload during control of the constraint wrench for Case 3 (points F and G in contact at the same time ). Figure 6 27 plot s the global change in the payload point F and the global change in the payload axis coordinate system. For contact with both points F and G on the target, the values changed very little, and the angle change was minimal as well. Figure 6 28 show s that the ma gnitude and angle of the actual constraint wrench were 225 and 91 respectively which showed good correlation to the desired values. The perpendicular distance to the actual constraint wrench is shown in Figure 6 29 and demonstrated that the perpendicular distance to the constraint wrench was located midway between points F and G (distance FG/2). Figure 6 30 showed a plot of the leg force errors, and, as in a previous plot, the leg force errors were within 2 Another data run was performed for Case 3, and the results are shown in Figure 6 31 through Figure 6 36. Figure 6 31 showed that point F did not move and the angle of the payload coordinate system did not change during the test. The magnitude and angle of the constraint wrench plot ( Figure 6 32) again showed that the magnitude of the constraint wrench was maintained ( 225 ) and the constraint wrench angle was 90 The perpendicular distance to the constraint wrench ( Figure 6 33) was close to 17 4 which was, again, halfway between points F and G. Figure 6 34 and Figure 6 35 show s good correlation between the actual and commanded leg forces. The plot of leg force errors in Figure 6 36 show that the control of the leg force errors was well behaved with less than 2 errors. PAGE 220 220 Constraint P lus Deflection Translation Tests The next series of tests w as conducted to illust rate that the deflection wrench performed as desired and that the deflection wrench did not affect control of the constraint wrench. The approach for these test s was to position the payload mechanism in an initial configuration of 5 0 or + 5 with respect to the global coordinate system. The payload mechanism was then raised in the global direction until contact was determined. After contact was determined, and its case was determined, the explicit kinestatic controller was turned on and for p eriod one ( in this case, for a time of 5 ) the constraint wrench was controlled. After period one, the explicit kinestatic controller was enabled for constraint wrench plus deflection wrench control for a duration of period two ( in this case, for a time of 5 ). The deflection commanded for these data runs was a translation tangent to the sensed constraint wrench direction in the general global direction. After period two, the explicit kinestatic controller was turned off, but the leg for ce controller remained active and used the last force setpoint from the explicit kinestatic controller. In the graphs of these data this was shown as period three. Recall that the leg force controller was active for the entire duration of these tests; however, the leg force controller received force setpoint updates from the explicit kinestatic controller when the explicit kinestatic controller was active. In each case, the controller determined the contact angle as it was seen from the payload frame of reference. From this information, the tangent translation angle was determined and was used to calculate the proper direction for movement. PAGE 221 221 The first test illustrated Case 1 (point F contact first). Figure 6 37 showed the response of point F and the payload axis during the test. Contact with point F created the reference time = 0 For the next 5 period one, the kinestatic constraint only control was utilized. For the ensuing 5 period two, the kinestatic constraint plus deflection translation control was active. During period three, the kinestatic controller was turned off and the leg force controller remained active using the last force set points for each leg. During the first 5 in which kinestatic constraint only control was operational, it was seen that the payload drifted only slightly along the global axis, there was no movement in the global direction, and the angular change of the payload coordinate system drifted slightly by 1 During the next 5 interval, the payload mechanism was translated along the target in the general global direction by use of the kinestatic constra int and deflection translation controller. It was observed that the payload point F did move in the direction showing that the deflection wrench did move the payload in the desired direction. The payload coordinate system also continued rotation by another 1 5 Figure 6 38 showed the magnitude and angle of the commanded and actual constraint wrench during the test. As noted previously, the magnitude was w ell behaved ( 225 ) during the entire test showing that while the deflection wrench was being applied, the constraint wrench was maintained; i.e. the deflection wrench did not affect the constraint wrench. The wrench angle during the kinestatic cons traint control was close to 90 When the kinestatic constraint and deflectiontranslation control were active, the wrench angle dropped to 85 This was probably due to the effects of friction between point F and the target. The kinestati c controller did not take PAGE 222 222 friction into consideration and movement of the payload mechanism in the direction would tend to reduce the wrench angle. The leg force error plot in Figure 6 39 showed the leg force errors during the different controller modes. Leg 3 again demonstrated a larger than usual leg force error response that was probably due to the effects of stiction observed in earlier tests and the resulting plots. Figure 6 37 provided information and a key conclusion on the operation of the controller during period three. The leg forces retained their final setpoints which had been determined in period two. Period three is important because it shows tangentially that the general wrench naturally decomposed onto the deflection wrench (continuing the desired translation) and the desired constraint wrench. Though period three was quite brief when considering the total length of the test, it was shown that neither of the se wrenches adversely affected the other. For this reason, period three was absolutely necessary to rule out the possibility that the kinestatic controll er was somehow compensating for the interaction between the two types of wrench errors. The next test consisted of the payload mechanism in the + 5 rotation so that point G would touch the target initially (Case 2). Figure 6 40 show s the response of the payload point G during the test and the payload coordinate axis rotation. This test showed that point G did not move in either the or global directions during period one, and then again showed that the deflection wrench moved the payload in the desired direction during period two. During the kinestatic constraint and deflectiontranslation controller phase, period two, the coordinate of point G moved in the global coordinate direction while the global coordinate remained fixed. The rotation of the payload coordinate axis system was minimal with no rotational change during period PAGE 223 223 one, and a slight 0 1 change was observed during the deflecti on phase, period two. During period three, the previous rates of changes continued during the leg force controller phase. Figure 6 41 showed the response of the magnitude and angle of the constraint wrench. During the constraint only control phase, period one, the magnitude tracked the setpoint of 225 and the wrench angle of 90 During the constraint and deflection control phase, the magnitude drifted almost 8 and the wrench angle was 85 These data results were almost identical to the previous run for Case 1. The perpendicular distance to the constraint wrench plot ( Figure 6 42) showed a result between 34. 6 and 34. 7 demonstrating that the constraint wrench was passing through point G. Another Case 2 run is shown in Figure 6 43 through Figure 6 45 Again, almost identical results from the previous Case 2 results were observed. The point G movement shown in Figure 6 43 was minimal in the directions, and the angular change of the payload coordinate system was close to 0 during period one. During period two, the coordinate of point G moved linearly in the global direction showing that the deflection wrench moved the payload along the target Magnitude and angle of the constraint wrench was shown in Figure 6 44 with similar results to those s hown in Figure 6 41 which demonstrated that the deflection wrench did not affect control of the constraint wrench. The perpendicular distance to the constraint wrench ( 34. 7 ) was constant and showed that the constraint wrench passed through point G ( Figure 6 45 ). During p eriod three, the natural decomposition of the general w rench continued. Figure 6 43 demonstrated the manifestation of the deflection wrench where the PAGE 224 224 displacement remained virtually unaltered between periods two and three. Figure 6 44 and Figure 6 45 further demonstrated t hat the constraint wrench i s likewise unaltered between periods two and three. It again show s that the kinestatic controller was not in any way correcting for constraint wrench errors due to deflection wrenches in the previous period. A test was performed to illustrate Case 3 (contact with points F and G simultaneously ). These da ta are shown in Figure 6 46 through Figure 6 48. For this test, the desired displacement phase was tangential to the target Figure 6 46 showed that the position of point F did not move during period one, showing that the constraint wrench di d not cause the payload to move. During period two, the position of point F moved in the + global direction showing that the deflection wrench did move the payload in the desired direction. Figure 6 47 showed the magnitude and angle of the constraint wrench throughout all three phases of control maintained at 225 and 90 respectively. This demonstrated particularly in period three results that the deflection wrench did not affect the control of the constraint wrench. The perpendicular distance to the constraint wrench was plotted in Figure 6 48 and showed that the constraint wrench passed through a location midway between points F and G, as expected. Constraint P lus Deflection Rotation Tests In order to more fully answer the question of whether the deflection wrench affect ed control of the constraint wrench, it was also necessary to test deflection wrenches that produced rotations. Figure 6 49 and Figure 6 50 show s results from a Case 1 run (point F contacts the target first). Figure 6 49 demonstrated how the point F PAGE 225 225 moved and how the payload rotated during the analogous three periods of k inestatic control. At time = 0 the kinestatic controller sensed contact with point F. During a 5 interval of period one, it was again observed that the coordinate drifted slightly ( 0 1 ) and the coordinate did not move. There was a slight rotation of the payload axis of about 1 These results are consistent with previous data runs where point F came into contact with th e target first. During the next 5 interval of period two, it can be seen that the coordinate of point F continues to drift, the coordinate stays at 0 and the angle of the payload rotates in a positive sense about the payload axis sho wing that the rotation deflection wrench moved the payload in the desired rotation direction. After this 5 interval, the kinestatic controller was turned off and the leg force controller remained active, that is period three. Approximately 0 5 later, point G made contact with the target. During this 0 5 period, Figure 6 50 showed that the magnitude and angle of the actual constraint wrench did not change appreciably showing that the kinestatic controller had not somehow compensated in the previous period two. This was further proof that the rotation deflection wrench did not affect the constraint wrench. Another data run; this time with point G contacting the target first; can be seen in Figure 6 51 through Figure 6 53. Figure 6 51 show s point G movement after contact with the target and during a rotation for 5 During period one, point G did not move in the global direction showing that control of the constraint wrench did not move the payload. During period two, the location of point G decreased a small amount ( 0 2 ) and the payload angle decreased, as desired. After period two ended (at approximately 10 2 ), the leg force controller was the only controller active during period three. It PAGE 226 226 can be noticed that during period three the payload continued to displace at the same rate, and the constraint wrench was nominally maintained until contact with both points F and G was detected at approximately 12. 5 Prior to this contact, the same conclusions were drawn from the previous discussion regarding why the deflection wrench did not affect control of the constraint wrench. At the instant of contact and afterwards, Figure 6 52 showed where the magnitude of the constraint wrench increased over two fold. Since the constraint wrench was not being controlled, this was expected. Up to the time when contact with points F and G was made, the constraint wrench was not affected by the control of the deflection wrench. Figure 6 53 s how s that the wrench of constraint acted through point G until contact with both points F and G was sensed at 12 5 The perpendicular distance to the wrench then moved midway between points F and G Assembly Task Supervisor The assembly task superv isor (ATS) was discussed in Chapter 3 and provided the high level control for the entire process of moving the parallel robotic manipulator into contact with an unknown, hard target. The assembly task supervisor was designed to ensure that the payload or top platform of the planar mechanism was positioned correctly with respect to the target. At the high level the assembly task supervisor provided the instantaneous model of contact. Input to the assembly task supervisor from the explicit kinestatic contr oller consisted of the current sensed wrench, the current payload location, and the current stiffness matrix, [ ] Output from the assembly task supervisor to the user or system PAGE 227 227 operator was the current contact case, the estimated target location, and the payload location. Input to the ATS was provided as a command from the system operator or user and included payload and target geometries, the desired endmating specifications for payload and target, the nominal threshold forces and torques for determining contact, the nominal forces and torques for maintaining contact, the maximum work volume of the payload, the nominal payload displacements, and the contact cases which were presented in Table 3 1 The role of the ATS was to determine when contact was made, determine the type of contact, and generate a sequence of output commands for movement to complete the assembly task. The assembly task supervisor provided the wrench of constraint, the twist of freedom, and the current contact model to the explicit kinestatic controller. In the implementation example em ployed in this research, the contact types included single point contact at either or and two point contact when both points were in contac t with the target. If single point contact had been the case, then a rotation would be commanded in order to progress toward twopoint contact. When both points had made contact, then the next step was a translation command to a hard stop while maintaini ng the given maintenance force. This series of moves completed the required assembly task. As these twists of freedom of the payload are implemented, it is important that contact i s ensured and controlled within reasonable bounds of the controller and of the test apparatus. In order to meet these goals, the ATS generated contact wrench commands, specifically constraint wrench commands. It was essential that the PAGE 228 228 constraint wrench commands maintain minimal, or maintenance, contact without excessive force a pplication. (Chapter 3.) In the test setup the target was arbitrarily set approximately to 0 from the horizontal axis ( axis) with respect to the global coordinate system. The heuristic was not provided with this data. Since this was an arbitrary setting, the target could have been positioned at any angle or location, and the heuristic would have calculated the target orientation during tests. The execution of the heuristic explained in Chapter 3 was confirmed by the data shown in Figure 6 54 through Figure 6 71 for three different payload orientations. Figure 6 54, Figure 6 60 and Figure 6 66 each represented a complete data run for the three different cases of contact (Case 1, Case 2, and Case 3) that could occur during mating of the payload to the target. Heuristic Test for Case 1 Figure 6 54 provided the overview for the first test series where point F (Case 1) came into contact with the target first. This data run started with the payload in the horizontal position. Between 30 and 36 a manual rotation was performed to effect a 5 rotation about point F. This positioned the payload with the point F higher than point G in the global direction. After this rotation had been completed, the heuristic control test was initiated at 37 6 During this period of no contact (Case 0 ) point F moved in the global direction, with no change in the global direction. At 56 contact with point F was determined. The heuristic was then paused f or 2 to show that there was no movement of the payload before the heuristic continued (period one) During this time between 56 and 58 it can be seen that the location of PAGE 229 229 point F did not change and there was only a slight change in the rotation of the payload axis. Between 58 and 60 the payload platform was rotated in order to bring point G into contact with the target (period two (a) where the (a) was used to indicate the time interval used for kinestatic control constraint plus a rotation deflection) Contact between point G and the target occurred at time = 60 6 Then a translation deflection was commanded to move the payload in the payload + axis direction until hard contact was determined (period two (b) wh ere the (b) was used to indicate the time interval used for kinestatic control constraint plus a translation deflection) This translation can be seen between times 60 6 and 83 5 After the fully constrained Case 4 was detected, the kinestatic controller was continued for another 3 before the kinestatic controller was turned off and only leg force controller remained active (period three) Figure 6 55 shows a zoomed in view of Figure 6 54 so that the details of the run can be examined. After contact was sensed at point F, the slight drift in the global axis direction can be seen. After contact with both points F and G, it can be also seen that there was a slight push of the point F in the global axis dir ection; probably the result of Leg 3 stiction problems. The rest of the run was well behaved and the behavior was as expected. Figure 6 56 show s the magnitude and angle of the commanded wrench versus the actual constraint wrench. As expected, the wrench magnitude remained close to the 225 threshold with spikes observed during transition between cases. Figure 6 57 showed the perpendicular distance to the constraint w rench. During the initial part of the run when contact was with point F, the perpendicular distance to the wrench was PAGE 230 230 0 This indicated that the constraint wrench passed through the point F. When both points F and G made contact, this perpendicular distance to the constraint wrench moved and indicated that the wrench acted midway between points F and G Figure 6 58 show s the actual versus the commanded leg forces and Figure 6 59 show s a plot of the leg force errors. Remember that during all tests, the leg force controller was active at all times what was different during k inestatic control was the fact that the kinestatic controller calculated the force setpoint and delivered this setpoint to the leg force controller. In general, each of the three legs was able to track its setpoint except for Leg 3 during periods one and two (a). Here, the magnitude of the leg force error was fairly large at 300 indicating that, during this period the leg was pushing the payload to effect the rotation. After two point contact was made, Leg 3 force errors fell below 2 Heuristic Test for Case 2 The next test was to illustrate the success of the heuristic if point G contacted the target first (Case 2). The next series of plots mimic the previous section and in general, similar results were obtained. Figure 6 60 showed the complete second run. Initially, the payload was positioned in a horizontal ( 0 ) orientation. At time = 20 the payload orientation was rotated to a + 5 angle with respect to the global coordinate system. This move ended at approximately 27 After a slight pause, the payload was moved in the global direction to start the run. At about 38 contact with point G was determined (s tarting period one) After a 2 pause, the payload was rotated in a direction to affect two point contact (period two (a)) Contact at both points F and G was determined at approximately 43 starting period two (b) Then PAGE 231 231 translation along the payload + axis was initiated to move the payload into a fully constrained case. At about 63. 5 the fully constrained case (Case 4) was determined. After a 3 delay, the kinestatic controller was turned off and the leg force controller re mained active. Figure 6 61 shows the same run with the time scale expanded from 35 to 70 It can be seen that a similar response occurred as in the previous t est. Figure 6 62 shows the magnitude and angle of the constraint wrench during the test. As shown in the previous test, the magnitude tracked the 225 setpoint well during all of the kinematic control periods, and the angle of the constraint wrench was close to 90 The perpendicular distance to the constraint wrench is shown in Figure 6 63. As expected, during the nocontact phase, the perpendicular distance was set to 0, then when contact with point G was made, the perpendicular distance was calculated to be through point G ( close to 34 7 ). When contact was made by both points F and G, the perpendicular distance moved to a location between points F and G. Figure 6 64 and Figure 6 65 show the actual and commanded leg forces and the leg force errors respectively throughout the test. It was observed during this run that the magnitu de of the leg force errors for L egs 1 and 2 were higher during periods one and two (a). These magnitudes were similar to that of Leg 3 on the previous (C ase 1) test and indicated that Legs 1 and 2 were required to rotate the payload. After contact with points F and G was determined, the force errors for all legs returned to 2 as expected. PAGE 232 232 Heuristic Test for Case 3 The last run tested the heuristic for the case in which both points F and G contacted the target at essentially the same time. This run is presented in Figure 6 66 through Figure 6 71 In this test, the payload platform was kept at 0 orientation with respect to the global coordinate system and then moved up toward the target. The complete data set for the test run is shown in Figure 6 66 where the payload moves upward toward the target at approximately 42 Since the target in the test run shown in the figures mentioned above was artificially set at 0 and since the payload was kept at 0 during its accent toward contact, the only possible conclusion would be that points F and G on the payload would make contact with the target simultaneously. However, the heuristic did not have t his knowledge, but was able to make the determination of simultaneous contact independently. At approximately 60 it was determined that contact with both points F and G was made and period one control was initiated. Since both points were in contact, after a two second paus e, the payload was translated in the payload + axis direction until the fully constrained case was determined (at = 82. 5 period two (b) ). Again, the next figure, Figure 6 67 shows a closer view of the heuristic run between 60 and 90 The primary difference between this plot and the previous two case runs was the scale factor on the payload angle rotation axis. The previous two case runs used 6 whereas this plot used 0 05 As can be seen in this figure, the response of this case is almost identical to the same portions in the previous two case plots. Figure 6 68 showed the magnitude and angle of the constraint wrenches and showed PAGE 233 233 the magnitude tracking the setpoint of 225 and the wrench angle tracking 90 The perpendicular distance to the constraint wrench shown in Figure 6 69 illustrated that the constraint wrench acted midway between points F and G. The actual and commanded leg forces are shown in Figure 6 70 and the leg force errors are shown in Figure 6 71. It can be noted that the actual leg forces tracked well to the commanded leg forces with minimal errors throughout the entire run. Modification to Heuristic The heuristic implemented in the experiments described above was based on the logic established in Chapter 3. This logic worked well for the most part and provided the heuristic with the means to correctly report the various contact cases. In order to ensure that all of the contact cas es were correctly reported, a modification was made to the heuristic as described below. The specific challenge which plagued the originally proposed heuristic involved determining the correct contact case when the payload first touched the target. Say point F was the first point touching the target. This would invoke Case 1 as the reported case. But if the angle of the sensed wrench in the payload coordinate system closely approached 90 then the original heuristic would occasionally report Case 3 in error. In order to address this challenge, additional logic was incorporated into the heuristic whenever Case 3 was detected. The logic change involved the specific recalculation of the perpendicular distance of the sensed wrench in the assembly tas k supervisor. If the perpendicular distance was less than one third of the distance between points F and G but closer to point F, then Case 1 was r eported instead of Case PAGE 234 234 3. On the other hand, if this distance was closer, by analogy, to point G, then Cas e 2 was reported instead of Case 3. The conclusion to be drawn from this was that the heuristic had trouble differentiating between contact cases when there was an intersection between the wrench of constraint subspaces. An example would be when the colum n spaces of [ ] and [ ] share a contact wrench through point F. The analogous case could also occur at point G. (Chapter 3.) This change or tweak to the heuristic formed the beginning of an inquiry that requires further research into the geometric ramif ications, especially when considering contact in three dimensional space. Summary This chapter presented a series of data tests performed on the three legged parallel platform to verify analytical work presented in Chapters 3, 4, and 5. Of primary importance, was the determination that leg force control was shown to be decoupled, controlling a constraint wrench did not cause the payload to move, that the deflection wrench moved the payload in the desired direction, the deflection wrench did not affect the constraint wrench, and that the heuristic developed in Chapter 3 functioned as designed. Initial tests of appli cation of the single leg force controller to the three legs of the payload mechanism showed that no cross coupling occurred between the legs. The next series of tests showed that a correction for constraint wrench error did not cause a displacement of the payload. Although slight movement of the payload was observed during some tests where the kinestatic constraint only controller was active (per iod one), this was attributed to stiction in L eg 3 A test was performed th at illustrated the PAGE 235 235 stiction in L eg 3 or the offset error in calibration. The next two series of tests were performed to prove that a deflection wrench (either a rotational deflec tion wrench or a translational deflection wrench) did not adversely affect control of the constraint wrench. This was proved by observing the magnitude and angle of the constraint wrench in the transitions between periods two and three. The last three data runs proved that the heuristic was functional; and it did not matter if point F, point G, or points F and G contacted the target first Figure 6 1 System integration and i mplementation. Assembly Task Supervisor Feedback from kinestatic controller: Current sensed wrench Current payload location Current stiffness matrix [ ] Command to u ser: Current contact case Target location estimate Payload location Command to k inestatic c ontroller: Desired wrench of constraint Next twist of freedom Current contact or kinestatic model Command to f orce c ontroller: Leg force setpoints Leg velocity setpoints (non contact mode) Feedback from f orce c ontroller: Current actual leg forces Current actual leg lengths Explicit Kinestatic Controller From m etrology f rame: Payload location Given: Payload and target geometries Desired endmating specification for payload and target Nominal threshold forces & torques for determining contact Nominal forces and torques for maintaining contact Payload maximum work volume Payload nominal displacements Contact cases (the [ ] table) Leg Force Controller Command to actuation devices: Hydraulic servovalve commands Feedback from actuation devices: Actual leg forces Actual leg lengths Derived leg velocities PAGE 236 236 Figure 6 2 Planar p latform response to force step input for l eg 1. Figure 6 3 Planar p latform response to force step input for l eg 2 20 0 20 40 60 80 100 120 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Leg 1 Force measured leg 1 Force measured leg 2 Force measured leg 3 20 0 20 40 60 80 100 120 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Leg 2 Force measured leg 1 Force measured leg 2 Force measured leg 3 PAGE 237 237 Figure 6 4 Planar p latform response to force step input for l eg 3. Figure 6 5 Planar p latform response to force step input for l eg s 1 2 3. 20 0 20 40 60 80 100 120 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Leg 3 Force measured leg 1 Force measured leg 2 Force measured leg 3 20 0 20 40 60 80 100 120 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Legs 1, 2, 3 Force measured leg 1 Force measured leg 2 Force measured leg 3 PAGE 238 238 Figure 6 6 Planar p latform response to force step input for l eg s 1, 2, 3. Figure 6 7 Planar p latform respons e to force step input for l eg s 1, 2, 3. 120 100 80 60 40 20 0 20 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Legs 1, 2, 3 Force measured leg 1 Force measured leg 2 Force measured leg 3 120 100 80 60 40 20 0 20 40 60 80 100 120 0.0 0.5 1.0 1.5 2.0 Force (lb f ) Time (sec) Force Step Input for Legs 1, 2, 3 Force measured leg 1 Force measured leg 2 Force measured leg 3 PAGE 239 239 Figure 6 8 Platform force response to external disturbance force applied along + axis Figure 6 9 Platform position response to external disturbance force applied along + axis. 0 10 20 30 40 50 60 70 80 90 0 10 20 30 40 50 60 70 80 90 0 5 10 15 20 25 30 35 Force Legs 2 and 3 (lb f ) Force Leg 1 (lb f ) Time (sec) Disturbance Force Applied to Payload along +X payload axis Force measured leg 1 Force measured leg 2 Force measured leg 3 Legs 2 and 3 Leg 1 0 10000 20000 30000 40000 50000 60000 70000 80000 0 5 10 15 20 25 30 35 Leg length (encoder counts) Time (sec) Disturbance Force Applied to Payload along +X payload axis Leg length measured Leg 1 Leg length measured Leg 2 Leg length measured Leg 3 PAGE 240 240 Figure 6 10. Case 1 (contact with point F) showing a little drift in x direction, and slight angular change in payload angle. Figure 6 11. Case 1 (contact with point F) magnitude and angle of commanded and actual cons traint wrench. 1.20 1.00 0.80 0.60 0.40 0.20 0.00 0.20 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 0 10 20 30 40 50 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench PAGE 241 241 Figure 6 12. Case 1 (contact with point F) showing slight error on X component of constraint wrench and almost no error on Y component of constraint wrench. Figure 6 13. Case 1 (contact with point F) showing actual vs. commanded leg forces 300 250 200 150 100 50 0 0 5 10 15 20 25 30 35 0 10 20 30 40 50 Y Magnitude of Wrench (lb f ) X Magnitude of Wrench (lb f ) Time (sec) X and Y Components of Constraint Wrench X component of commanded constraint wrench X component of actual constraint wrench Y component of commanded constraint wrench Y component of actual constraint wrench 0 50 100 150 200 250 0 10 20 30 40 50 Leg Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 Commanded Force Leg 2 Commanded Force Leg 3 Commanded PAGE 242 242 Figure 6 14. Case 1 (contact with point F) showing leg forces for first 1 0 Figure 6 15. Case 1 (contact with point F) showing the error between the actual and commanded leg forces 0 50 100 150 200 250 0.0 0.2 0.4 0.6 0.8 1.0 Leg Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 Commanded Force Leg 2 Commanded Force Leg 3 Commanded 10 8 6 4 2 0 2 4 6 8 10 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) PAGE 243 243 Figure 6 16. Case 1 (contact with point F) position and ang le change of point F (showing drift in X direction but not Y) Figure 6 17. Case 1 (contact with point F) magnitude and angle of commanded and actual constraint wrench. 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 350 0 10 20 30 40 50 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench PAGE 244 244 Figure 6 18. Case 1 (contact with point F) leg force errors Figure 6 19. Case 1 (contact with point F) position and angle change of point F with disturbance force applied at approximat ely 2, 4, 38, 41, 43, and 48 seconds 25 20 15 10 5 0 5 10 15 20 25 30 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) 2.0 1.5 1.0 0.5 0.0 0.5 1.0 1.5 2.0 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Time of application of disturbance force PAGE 245 245 Figure 6 20. Case 1 (contact with point F) magnitude and angle of commanded and actual constraint wrench during disturbance force application. Figure 6 21. Case 1 (contact with point F) leg force errors during disturbance force application. 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 0 10 20 30 40 50 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Time of application of disturbance force 25 20 15 10 5 0 5 10 15 20 25 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors During Application of Disturbance Force Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) Time of application of disturbance force PAGE 246 246 Figure 6 22. Case 2 (contact with point G) position changes of point G and angular change of payload axis Figure 6 23. Case 2 (contact with point G) magnitude and angle of commanded and actual constraint wrench. 0.25 0.20 0.15 0.10 0.05 0.00 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point G linear displacement (in) Time (sec) Point G Linear and Payload Axis Rotational Displacement Point G global change in x direction Point G global change in y direction Global change in payload X axis angle (gamma) 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 0 10 20 30 40 50 Wrench Angle (deg) Wrench Magnitude(lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench PAGE 247 247 Figure 6 24. Case 2 (contact with point G) X, Y components of constraint wrench. Figure 6 25. Case 2 (contact with point G) perpendicular distance to constraint wrench. 300 250 200 150 100 50 0 15 10 5 0 5 10 15 0 10 20 30 40 50 Y Magnitude of Wrench (lb f ) X Magnitude of Wrench (lb f ) Time (sec) X and Y Components of Constraint Wrench X component of commanded constraint wrench X component of actual constraint wrench Y component of commanded constraint wrench Y component of actual constraint wrench 40 35 30 25 20 15 10 5 0 0 10 20 30 40 50 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Perpendicular distance to actual constraint wrench (calculated) PAGE 248 248 Figure 6 26. Case 2 (contact with point G) leg force errors Figure 6 27. Case 3 (contact with point F and G) showing no movement of point F or rotation 10 8 6 4 2 0 2 4 6 8 10 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) 0.10 0.08 0.06 0.04 0.02 0.00 0.02 0.04 0.06 0.08 0.10 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) PAGE 249 249 Figure 6 28. Case 3 (contact with point s F and G) magnitude and angle of commanded and actual constraint wrench. Figure 6 29. Case 3 (contact with point s F and G) p erpendicular distance to constraint wrench. 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 350 400 450 500 0 10 20 30 40 50 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench 20 18 16 14 12 10 8 6 4 2 0 0 10 20 30 40 50 Perpendicular distance to constraint wrench (in) Time (sec) Perpendicular distance to actual constraint wrench (FG/2) Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench PAGE 250 250 Figure 6 30. Case 3 (contact with point s F and G) leg force errors showing minimal magnitude of force errors Figure 6 3 1 Case 3 (contact with point s F and G) showing almost no movement of F or rotation 10 8 6 4 2 0 2 4 6 8 10 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) 0.10 0.08 0.06 0.04 0.02 0.00 0.02 0.04 0.06 0.08 0.10 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 0 10 20 30 40 50 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) PAGE 251 251 Figure 6 32. Case 3 (contact with point s F and G) showing the magnitude and angle of the commanded and actual constraint wrench. Figure 6 33. Case 3 (contact with point s F and G) showing perpendicular distance to constraint wrench. 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 0 10 20 30 40 50 Wrench angle (deg) Wrench magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench 20 18 16 14 12 10 8 6 4 2 0 0 10 20 30 40 50 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench PAGE 252 252 Figure 6 34. Case 3 (contact with point s F and G) actual vs. commanded leg forces Figure 6 35. Case 3 (contact with point s F and G) showing leg forces for first 1 0 50 0 50 100 150 200 250 300 350 0 10 20 30 40 50 Leg Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 commanded Force Leg 2 commanded Force Leg 3 commanded 50 0 50 100 150 200 250 300 350 0.0 0.2 0.4 0.6 0.8 1.0 Leg Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 commanded Force Leg 2 commanded Force Leg 3 commanded PAGE 253 253 Figure 6 36. Case 3 (contact with point s F and G) showing minimal force errors. Figure 6 37. Case 1 showing position of point F after contact and during translation in direction perpendicular to contact normal 10 8 6 4 2 0 2 4 6 8 10 0 10 20 30 40 50 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) 2 1 0 1 2 3 4 5 6 0.50 0.40 0.30 0.20 0.10 0.00 0.10 0.20 0.30 0.40 0.50 0 5 10 15 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 254 254 Figure 6 38. Case 1 showing magnitude and angle of com mand and actual constraint wrench throughout the sequence of periods Figure 6 39. Case 1 showing leg force errors throughout the sequence of periods 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 0 5 10 15 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 60 40 20 0 20 40 60 0 5 10 15 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 255 255 Figure 6 40. Case 2 showing position of point G after contact and during translation in direction perpendicular to contact normal. Figure 6 41. Case 2 showing magnitude and angle of com manded and actual constraint wrench during translation perpendicular to contact normal 1.5 1.3 1.0 0.8 0.5 0.3 0.0 0.3 0.5 2.0 1.5 1.0 0.5 0.0 0.5 1.0 1.5 2.0 0 5 10 15 Payload axis rotational displacement (deg) Point G linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point G global change in x direction Point G global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 0 5 10 15 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 256 256 Figure 6 42. Case 2 showing perpendicular distance to constraint wrench after contact and during translation perpendi cular to contact normal Figure 6 43. Case 2 showing position of point G after contact and during translation perpendicular to contact normal 40 35 30 25 20 15 10 5 0 0 5 10 15 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Perpendicular distance to actual constraint wrench (calculated) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 1.0 0.8 0.6 0.4 0.2 0.0 0.2 0.4 0.6 0.8 1.0 2.0 1.5 1.0 0.5 0.0 0.5 1.0 1.5 2.0 0 5 10 15 Payload axis rotational displacement (deg) Point G linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point G global change in x direction Point G global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 257 257 Figure 6 44. Case 2 showing magnitude and angle of commanded and actual constraint wrench during translation perpendicular to contact normal Figure 6 45. Case 2 showing perpendicular distance to constraint wrench af ter contact and during translation perpendicular to contact normal 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 0 5 10 15 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 40 35 30 25 20 15 10 5 0 0 5 10 15 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 258 258 Figure 6 46. Case 3 showing position of point F after contact and during perpendicular to contact normal Figure 6 47. Case 3 showing magnitude and angle of commanded and actual constraint wrench. 0.5 0.4 0.3 0.2 0.1 0.0 0.1 0.2 0.3 0.4 0.5 2.5 2.0 1.5 1.0 0.5 0.0 0.5 1.0 1.5 2.0 2.5 0 5 10 15 20 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 350 400 0 5 10 15 20 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 259 259 Figure 6 48. Case 3 showing perpendicular distance to constraint wrench. Figure 6 49. Case 1 p osition change of point F, rotate for 5 seconds and then turn off deflection and constraint control modes 20 18 16 14 12 10 8 6 4 2 0 0 5 10 15 20 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 4.0 3.0 2.0 1.0 0.0 1.0 2.0 3.0 4.0 5.0 6.0 0.25 0.20 0.15 0.10 0.05 0.00 0.05 0.10 0.15 0.20 0.25 0 5 10 15 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Sense c ontact at F and G Sense c ontact at F rotate 5 sec slight drift in payload axis angle pause 5 sec Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 260 260 Figure 6 50. Case 1 m agnitude and angle of wrench command and actual constraint wrench. Figure 6 51. Case 2 p osition change of point G, sense contact with G, pause 5 seconds, rotate 5 seconds, then turn deflection and constraint control of f 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 0 5 10 15 Payload axis rotational displacement (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Sense contact at F and G Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 6 5 4 3 2 1 0 0.4 0.3 0.2 0.1 0.0 0.1 0.2 0.3 0.4 0 5 10 15 Payload axis rotational displacement (deg) Point G linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point G global change in x direction Point G global change in y direction Global change in payload X axis angle (gamma) Mode change or other event Sensed contact at F & G Very small drift in Xpayloadaxis rotate for 5 seconds Sensed c ontact point G pause for 5 seconds Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 261 261 Figure 6 52. Case 2 m agnitude and angle of commanded and actual constraint wrench rotate until contact with point F Figure 6 53. Case 2 p erpendicular distance to constraint force, contact with G, rotate 5 seconds 100 90 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 300 350 400 450 0 5 10 15 Payload axis rotational displacement (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Contact at F & G Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 40 35 30 25 20 15 10 5 0 0 5 10 15 Perpendicular distance to wrench (in) Time (sec) Perpendicular distance to wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Mode change or other event Contact at F & G Leg Force Control A lways on Kinestatic Control 1 constraint only 2 constraint and deflection 3 no kinestatic control 1 2 3 PAGE 262 262 Figure 6 54. Case 1 complete run. Start with horizontal, rotate to 5 with respect to global coordinate system, then start test 6 4 2 0 2 4 6 0 10 20 30 40 50 60 0 10 20 30 40 50 60 70 80 90 Angle of payload axis coordinate frame X axis (deg) Position of payload axis coordinate frame (point F) (in) Time (sec) Location of the Payload Coordinate Axis X location of point F Y location of point F payload X axis angle (gamma) Mode change or other event translate along Xpayloadaxis fully constrained start payload "up" manually rotate point G down sense contact at F rotate to get contact with point G case 0 (no contact) case 1 (contact at F) case 3 (contact at F&G) case 4 (fully constrained) pause sense contact at F&G sense fully constrained pause 2 sec PAGE 263 263 Figure 6 55. Case 1 showing different modes of control and other events 6 4 2 0 2 4 6 6 4 2 0 2 4 6 55 60 65 70 75 80 85 90 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event sense contact at F sense contact at F & G sense fully constrained Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 264 264 Figure 6 56. Case 1 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence. Figure 6 57. Case 1 showing constraint wrench perpendicular distance throughout sequence. 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 700 800 55 60 65 70 75 80 85 90 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b 20 18 16 14 12 10 8 6 4 2 0 55 60 65 70 75 80 85 90 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 265 265 F igure 6 58. Case 1 showing leg force actual and setpoints throughout sequence Figure 6 59. Case 1 showing leg force errors throughout sequence 100 0 100 200 300 400 500 600 55 60 65 70 75 80 85 90 Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 commanded Force Leg 2 commanded Force Leg 3 commanded Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b 400 300 200 100 0 100 200 300 400 55 60 65 70 75 80 85 90 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 266 266 Figure 6 60. Case 2 complete run. Start with horizontal, rotate to + 5 with respect to global coordinate system, then start test 6 4 2 0 2 4 6 0 10 20 30 40 50 60 0 10 20 30 40 50 60 70 Angle of payload axis coordinate frame X axis (deg) Position of payload axis coordinate frame (point F) (in) Time (sec) Location of the Payload Coordinate Axis X location of point F Y location of point F payload X axis angle (gamma) Mode change or other event fully constrained start payload "up" manually rotate point G up sense contact at G rotate to get contact with point F case 0 (no contact) case 2 (contact at G) case 3 (contact at F&G) case 4 (fully constrained) p a u s e pause 2 sec sense contact at F&G translate along Xpayloadaxis sense fully constrained PAGE 267 267 Figure 6 61. Case 2 showing different modes of control and other events 6 4 2 0 2 4 6 6 4 2 0 2 4 6 35 40 45 50 55 60 65 70 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event sense contact at F&G sense contact at G sense fully constrained Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 268 268 Figure 6 62. Case 2 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence. Figure 6 63. Case 2 showing perpendicular distance to constraint wrench throughout sequence. 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 700 800 35 40 45 50 55 60 65 70 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b 35 30 25 20 15 10 5 0 35 40 45 50 55 60 65 70 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 269 269 Figure 6 64. Case 2 showing leg force actual and setpoints throughout sequence. Figure 6 65. Case 2 showing leg force errors throughout sequence 300 200 100 0 100 200 300 400 500 600 700 35 40 45 50 55 60 65 70 Leg Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 commanded Force Leg 2 commanded Force Leg 3 commanded Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b 400 300 200 100 0 100 200 300 400 35 40 45 50 55 60 65 70 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 2a 3 2b PAGE 270 270 Figure 6 66. Case 3 complete run. Start with horizontal with respect to global coordinate system, then start test 6 4 2 0 2 4 6 0 10 20 30 40 50 60 0 10 20 30 40 50 60 70 80 90 Angle of payload axis coordinate frame X axis (deg) Position of payload axis coordinate frame (point F) (in) Time (sec) Location of the payload coordinate axis X location of point F Y location of point F payload X axis angle (gamma) Mode change or other event fully constrained start payload "up" sense contact at F&G case 0 (no contact) case 3 (contact at F&G) case 4 (fully constrained) pause 2 sec translate along Xpayloadaxis sense fully constrained PAGE 271 271 Figure 6 67. Case 3 showing different modes of control and other events 0.05 0.04 0.03 0.02 0.01 0.00 0.01 0.02 0.03 0.04 0.05 6 4 2 0 2 4 6 60 65 70 75 80 85 90 Payload axis rotational displacement (deg) Point F linear displacement (in) Time (sec) Payload Axis Linear and Rotational Displacement Point F global change in x direction Point F global change in y direction Global change in payload X axis angle (gamma) Mode change or other event sense contact at F&G sense fully constrained Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 3 2b PAGE 272 272 Figure 6 68. Case 3 showing magnitude and angle of commanded and actual constraint wrench throughout movement sequence. Figure 6 69. Case 3 showing perpendicular dis tance to constraint wrench throughout sequence. 100 90 80 70 60 50 40 30 20 10 0 0 100 200 300 400 500 600 700 800 60 65 70 75 80 85 90 Wrench Angle (deg) Wrench Magnitude (lb f ) Time (sec) Magnitude and Angle of Commanded and Actual Constraint Wrench Magnitude of commanded constraint wrench Magnitude of actual constraint wrench Angle of commanded constraint wrench Angle of actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 3 2b 35 30 25 20 15 10 5 0 60 65 70 75 80 85 90 Perpendicular distance to wrench (in) Time (sec) Perpendicular Distance to Constraint Wrench Perpendicular distance to commanded constraint wrench Perpendicular distance to actual constraint wrench Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 3 2b PAGE 273 273 Figure 6 70. Case 3 showing leg force actual and setpoints throughout sequence Figure 6 71. Case 3 showing leg force errors throughout sequence 200 100 0 100 200 300 400 500 600 700 60 65 70 75 80 85 90 Force (lb f ) Time (sec) Actual vs Commanded Leg Forces Force Leg 1 Actual Force Leg 2 Actual Force Leg 3 Actual Force Leg 1 commanded Force Leg 2 commanded Force Leg 3 commanded Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 3 2b 400 300 200 100 0 100 200 300 400 60 65 70 75 80 85 90 Leg Force Error (lb f ) Time (sec) Leg Force Errors Leg 1 force error (actual force commanded force) Leg 2 force error (actual force commanded force) Leg 3 force error (actual force commanded force) Mode change or other event Leg Force Control A lways on Kinestatic Control 1 constraint only 2a constraint and deflection (rotation) 2b constraint and deflection (translation) 3 no kinestatic control 1 3 2b PAGE 274 274 CHAPTER 7 SUMMARY AND CONCLUSIONS The credit belongs to the man who is actually in the arena, whose face is marred by dust and sweat and blood; who strives valiantly And who at the worst, if he fails, at leas t fails while daring greatly, so that his place shall never be with those cold and timid souls who nei ther know victory nor defeat. President Teddy Roosevelt Citizenship in a Republic speech at the Sorbonne, Paris, 23 April, 1910 Summary The overarching objective for this research was to develop a robotic sy stem that was capable of loading a bomb onto a United States Air Force combat aircraft. This overarching objective was divided into several specific objectives that were the focus of t his research. These specific objectives included: develop an assembly task supervisor, develop an explicit kinestatic controller, and develop a force controller for hydraulically actuated, compliant legs. Assembly Task Supervisor An assembly task supervisor was developed to handle contact at a high level of control. The supervisor generated a model of contact and used this model to describe the instantaneous kinestatics between the payload and the target. A heuristic was developed to sequence the payload mechanism through a series of motions to perform the defined assembly task. It should be noted that a comprehensive, fully robust assembly task sequencer for a general task was beyond the scope of this research. The assembly task heuristic was applied to a candidate planar assembly problem Given a multitude of possible contact cases, the Assembly Task Supervisor heuristic determined the correct contact case. The methodology embodied in the heuristic was summarized in the [] table in Chapter 3, Table 3 1 PAGE 275 275 The heuristic was developed primarily as a means for the ATS to sequence the payload through a series of movements in order for the payload to reach the fully constrained position, in other words, the assembly complete position. For the planar case studied in this research, there existed three possible paths through the heuristic. The first two involved single point contact between the payload and the target at either point F or point G. The third path involved twopoint contact between the payload and the target. After single point contact was made, the payload was rotated as required until twopoint contact was obtained. The payload was then translated unt il the fully constrained position with respect to the target was sensed. The heuristic was successfully demonstrated as a means of guiding the movement of the payload through a series of steps, originating from multiple initial positions and orientations, in order to attain the goal of fully constrained motion in contact with the target. It should be recalled that both the location and orientation of the target with respect to the payload was a priori unknown, and the sequencing of the heuristic did not depend on that information. To prove the point, the assembly task supervisor was employed through extensive experimentation and testing and successfully commanded the payload to the fully constrained location in every case. Explicit Kinestatic Controller Th e Explicit Kinestatic Controller (EKC) was developed to provide a midlevel control of the parallel robotic manipulator. The EKC calculated the desired leg forces given the desired constraint wrench and desired twist of freedom interacting between the pay load and the target once contact had been determined. PAGE 276 276 The EKC provided the intermediate step between the assembly task supervisor and its associated heuristic and the leg force controller. Constraint wrench and twist of freedom information provided by the assembly task supervisor was used by the EKC to generate force control setpoints for the leg force controller. Decoupling between the maintenance constraint wrench and the twist of freedom was demonstrated in several tests performed on the planar mechanis m. For this to be clearly demonstrated, the heuristic was executed in a stepwise fashion. During these tests, the lowlevel leg force controller was always active. At different times during the tests, either kinestatic constraint only control was used to generate the force setpoints, or a combination of the kinestatic constraint control plus deflection control was used. It should be noted that there were two different types of deflection control implemented during the experiments : rotation and translat ion. Chapter 4 posed two important questions relating to the EKC and d uring the development of the control architecture using screw theory (Ball 1900; Griffis 2009) as the mathematical basis for this research. The first question: If a correction for the constraint wrench is made, does the payload move? The investigation involved a series of tests to investigate this question. These tests demonstrated the response of the system to different starting locations of the payload with respect to the target. Whe n contact was detected and confirmed between the payload and the target, the kinestatic controller was deactivated and only the low level leg force controller was active. In general, the payload did not move appreciably, however some movement was detected when point F was the first to make contact. PAGE 277 277 This was attributed to stiction in the spring assembly This is discussed in more detail below. When point G or both points F and G made contact, there was indeed very little movement of the payload in positio n or orientation. These tests illustrated that the payload did not move when a correction for the constraint wrench was applied. The second question posed in Chapter 4: If a deflection wrench was applied, then did it affect the control of the constraint wrench? A second series of tests w ere performed to answer this question. In this series of tests, the heuristic was paused during execution at different points in the c ontrol sequence. This was done to provide evidence of the decoupling between the deflection wrench and the constraint wrench. In multiple test runs using different initial locations of the payload, it was shown that the corrections for the deflection wrench did not influence the control of the constraint wrench. A more detailed explanation of this experiment and the results thereof involved the comparison of the data collected at the end of period two ( when the EKC was calculating the leg force setpoints) with the beginning of the period three ( when the EKC was not calculating leg force setpoints) From this comparison, it was clear that the general wrench continued to decompose into a part that effected displacement, the deflection wrench, and a part th at effected contact intensity, the constraint wrench. This threshold transition, since the displacement continued without discontinuity and since the constraint wrench also continued without discontinuity, provoked the conclusion that the EKC was not corr ecting during period two for constraint wrench error effects due to PAGE 278 278 deflection wrench commands. This demonstrated through experimentation that the two were decoupled. Another important concept that was successfully shown was that the wrench of deflection worked as advertised and pushed the payload in the commanded direction. It was decided early in the design of the planar mechanism to utilize two points on the payload for contact between the payload and the target. These two points were labeled F and G. The reason for two points instead of one or three or more points was due to the nature of the weapons loading mission. Normal stores are attached to the aircraft wing through two hard connections fixed on the wing. This led to the selection of the two p oints in order to emulate the real world environment as closely as possible. The same principle was applied to the selection of the distance separating the two points. Leg F orce C ontroller The leg force controller was developed to provide low level force control of each leg individually. Initial research was performed on a single leg hydraulic test device as a first step in developing the force control algorithm. A detailed study of the single leg device provided useful insight into the challenges facing the development of the force control algorithm. The single leg device also provided a platform to test and refine the basic force control feedback algorithm. As part of this research and testing, individual components of the single leg hydraulic actuator and control system were modeled and tested to determine overall system performance. A model of the force controller was developed, and compared to the results of testing of the single leg PAGE 279 279 The general single leg model resulted in a fifth order system. H owever, for control frequencies between 0 1 10 a second order system with an integrator provided an accurate model. The single leg models were compared with actual system frequency response data obtained from the test apparatus, and these were used to verify the basic assumptions that had been made at the outset of the testing. Proof of the success of the system model was shown by comparing modeled response data to the actual data obtained from the single leg test device for different system componen ts. Since the primary focus of this research was on force control of a robotic manipulator in contact, the comparison of the modeled controller to the actual closed loop force controller proved to be extremely important. There was good agreement througho ut the low frequency range. Higher frequency ranges could not be measured due to the resolution of the leg length and leg force sensors selected for this project. It was worth noting that the experimental apparatus used in this research employed state of the art technology in manufacturing, in sensors, in actuators, and in computational capability. In support of this research, the USAF provided great assistance over the past several years. As the research progressed, it was determined that the hydraulic servo valves had less than optimal flow rate capability, 1 6 actual versus 5 desired. It was also determined that the leg length sensors or LVDTs had an actual resolution of 0 5 when a resolution of 0 1 would result in better resu lts Lastly, it was determined that the leg force sensor resolution, 1 was coarse when compared to a preferred resolution of 1 16 Since all of these PAGE 280 280 components were very expensive, it was impossible, due to fiscal constraints, to replace t hem once the preferred values for resolution had been determined. The above mentioned shortcomings of the experimental apparatus did not prevent this research from meeting its objectives. All of the objectives were met and the viability of the technical approach was clearly demonstrated. Due to time and fiscal constraints, optimization was left for future research. Force step commands were applied to the single leg platform to demonstrate the response of the system to a step input of the force setpoint. In each case, the actual force in the single leg approached the force setpoint within 0 25 No over shoots or oscillations were noted. The information devel oped in the single leg research was transitioned to the parallel or planar mechanism. Force command step tests were performed on the individual legs of the planar mechanism to obtain single leg response in the planar mode. The key observation from that t esting was that cross coupling among the legs was not an issue. Multiple tests were performed that used different force setpoints for each leg. In each case, the other legs did not show cross coupling effects, and the same response times were observed as in the single leg device tests. Conclusions In general, all three specific objectives of this research were successfully demonstrated using both the single leg device and the planar mechanism. Due to time and resource limitations, a demonstration of the explicit force control method was not performed on the six leg, six degree of freedom platform. The groundwork has been laid for extension of this methodology to the three dimensional case. PAGE 281 281 Assembly Task Supervisor The assembly task supervisor was success fully proven to provide the required high level control of the parallel planar mechanism. The heuristic was developed as a functional part of the ATS and provided highlevel control of the payload mechanism. The ultimate goal of the ATS was to ensure that the payload was moved into contact with the target and thence to its final, fully constrained location with respect to the target, in other words, the assembly complete position. The key responsibility of the ATS was to provide the ongoing, instantaneo us model of contact with the target together with the commands relating to how much contact to maintain (magnitude of wrench of constraint) and how to tweak the payload toward final assembly through manipulation of the twist of freedom. In order for this research to have real value, it should be extendable to the spatial case: the six legged manipulator. This was certainly a critical part of the research, and one of the key elements in that extension would be the development of the wrenches of constraint and the twists of freedom for different contact cases. While the complexity of these elements would expand, the development presented in this work could easily be extended to the spatial case. The ATS also demonstrated that the principle of superpositio n is viable for this and similar parallel robotic manipulator analyses In addition, this research validated the concept of a hierarchical approach to parallel robotic manipulator control: assembly task supervisor, explicit kinestatic controller, and sin gle leg force controller. Explicit Kinestatic Controller The explicit kinestatic controller was proven more than adequate to perform the mid level control of the parallel manipulator. The advantages of explicit versus implicit PAGE 282 282 force control were discussed in previous chapters. For the parallel connected legs of the system investigated in this research, explicit force control provided a better fit to obtain the goals of contact and translation while in contact As the center piece of the force control schema, the EKC as developed herein is readily extendable to the spatial platform. The overall control scheme would apply as well to the spatial 6 6 mechanism as it did to the planar threelegged mechanism. The first example of required modifications would be the stiffness matrix shown in the EKC control diagram. Instead of the current 3 x 3 matrix, the stiffness matrix would become a 6 x 6 matrix. The same degree of complexity would be added to the Jacobian matrix where a 3 x 3 matrix must be reconstitute d as a 6 x 6 matrix. This is added complexity, but it is all within the realm of accomplishment. Additional layers of complexity would be required to the input. The input in the 6 6 case would have to include an angular component as well as the Several other changes would have to be made to the overall control scheme for the parallel robotic manipulator in order to accommodate motion in threedimensional space, but the work has been done in this research to accomplish those tasks. Leg Force C ontroller Force control for both the single leg device and the planar mechanism was successfully accomplished using data from the system model and by applying that data to develop the individual leg force controllers. The single leg force controller proved to be a particularly difficult part of this research. A great deal of information existed regarding force control and contact while in the force control mode. Gorinevsky (1997) provided a starting point, but PAGE 283 283 experimentation led t o the development of a truly unique force control algorithm. This algorithm, once combined with screw theory in the assembly task supervisor and the explicit kinestatic controller, proved to be an important breakthrough in the force control realm. It sho uld be noted that the force control algorithm developed in this research proved that the integral portion of proportional integral derivative control w ould work for a parallel robotic mechanism. One last conclusion was the success of the specially engineer ed, linear spring. The spring was proven very effective in providing the required degree of known compliance for the leg force controller system. The stiction noted in the leg assemblies, especially with regard to Leg 3 was attributed to the spring guides rather than to the cylinder itself. Each of the three springs was enclosed in a spring guide ( Figure 5 5 ) which also contained the linear variable displacement transformer (LVDT). There is a good deal of improvement to the design of the guide that should be accomplished as part of future work. Since the LVDT motion must be coupled directly to the spring deflection, some sort of guide mechanism will be required in order to prevent misleading displacement measurements from the LVDT if the spring bends rather than deflects in a strictly linear manner. Potential improvements to the current design include the addition of a third guide leg instead of the two guide legs currently used and replacement of the bronze bushing with a linear bearing to reduce or eliminate stiction as the guide legs displace linearly in line with the hydraulic leg movement. PAGE 284 284 This research met every objective identified at the outset. Also of importance was the fact that this research has paved the way for extension of the system model and controller from the twodimensional planar mechanism to a threedimensional platform. PAGE 285 285 LIST OF REFERENCES Ayres, Frank, Jr. (1962) Matrices Schaums Out line Series in Mathematics McGraw Hill Book Co., New York, NY, 2 29, 3963 Ball, Sir Robert S. (1900) A Treatise on the Theory of Screws ; Cambridge University Press Brand, L. (1947) Vector and Tensor Analysis John Wiley and Sons, New York, NY, 5154, 6582 Chatzakos, P, and Papadopoulos, E (2003) On Model based Control of Hydraulic Actuators, 12th International Workshop on Robotics in AlpeAdria Danube Region, Cassino, Italy Crane, Carl D., III and Duffy, J. (1998) Kinematic Analysis of Robot Manipul ators Cambridge University Press, Cambridge, UK, 5359 Crane, Carl D., III, Duffy, J, and Rico, J. M. (1995) Screw Theory and Its Application to Spatial Robot Manipulators University of Florida, 5 12 DiSteffano, J. J., III, Stubberud, A. R., and Willia ms, I. J. (1967) Feedback and Control Systems, Schaums Outline Series McGraw Hill Book Company, New York, NY, 97134, 275312 Dorf, R. C. (1993) Modern Control Systems AddisonWesley Publishing Company, Menlo Park, CA, 116130, 170175. Duffy, J. (1996) Statics and Kinematics with Applications to Robotics, Cambridge University Press, Cambridge, UK, 148 151 Dunnigan, M. W., Lane, D.M., Clegg, A. C., and Edwards, I. (1996) Hybrid Position/Force Control of a Hydraulic Underwater Manipulator, IEEE Proceedi ngs on Control Theory Applications 143: 101 120. F erretti, G., Magnani, G., and Rocco, P. (1995) On the Stability of Integral Force Control in Case of Contact with Stiff Surfaces, Transactions of the ASME Journal of Dynamic Systems, Measurement, and Control 1 17 : 547 553 Gibson, C. G., and Hunt, K. H. (1990) Geometry of Screw Systems 2: Classification of Screw Systems, Mechanism and Machine Theory 25 : 11 27 Gorinevsky, D. M., Formalsky, A. M., and Schneider, A. Y. (1997) Force Control of Robotics Systems CRC Press, Boca Raton, FL,120, 113 166, 193201. Griffis, M. W. (1991) Kinestatic Control: A Novel Theory for Simultaneously Regulating Force and Displacement Doctoral dissertation presented to the University of Florida College of Engineering, 20 30. PAGE 286 286 Griffis, M. W., (2009) Kinestatic Control: Important Features Last modified July 2009, http://screwtheory.org/griff/papers/2009/kinestatic1.pdf Huang, S. and Schimmels, J. M. (2001) A Classification of Spatial Stiffness Based on the Degree of Translational Rotational Coupling, ASME Journal of Mechanical Design, 123: 353 358. Hunt, Kenneth H (1978) Kinematic Geometry of Mechanisms Oxford University Press, London, UK, 331402 Jennings, A. (1977) Matrix Computation for Engineers and Scientists John Wiley and Sons, London, UK, 1134. Johnson, J. E. (1977) Electrohydraulic Servo Systems Penton/IPC, Cleveland, OH, 2942, 109116, 129141 Johnson, R. E. and Kiokemeister, F. L. (1 960) Calculus with Analytic Geometry Allyn and Bacon, Boston, MA, 533 536 Kennedy, J. L. (2009) Force Control of a Hydraulic Servo System ; University of Missouri Masters thesis Lipkin, H. (1985) Geometry and Mapping of Screws with Applications to the Hybrid Control of Robotic Manipulators University of Florida doctoral dissertation,1938, 205284. Manring, Noah D. (2005) Hydraulic Control Systems John Wiley and Sons, Inc., Hoboken, NJ McCloy, D., and Martin, H. R. (1980) Control of Fluid Power: Anal ysis and Design, Ellis Horwood Limited, Chichester, UK, 3136, 203 209, 261265 Merritt, H. E. (1967) Hydraulic Control Systems John Wiley and Sons, New York, NY MOOG, Inc. (1965) Technical Bulletin 103, MOOG Controls Division, East Aurora, NY Neff, T. O. (1961) An Introduction to Vector Notation, monograph published by University of Florida Department of Mechanical Engineering Niksefat, N., Wu, C. Q., and Sepehri, N. (2001) Design of a Lyapunov Controller for an Electro hydraulic Actuator During Contact Tasks, ASME Journal of Dynamic Systems, Measurement, and Control, 123 : 299 307. Pigoski, T., Griffis, M., and Duffy, J. (1998) Stiffness Mappings Employing Different Frames o f Reference, Mechanism and Machine Theory, 33: 825838 Pl cker, J (1865) On a New Geometry of Space, Philosophical Transactions of the Royal Society of London, 155 : 725 791. PAGE 287 287 Pl cker, J (1866) Fundamental Views Regarding Mechanics, Philosophical Transactions of the Royal Society of London, 155: 361 380 Pool, Thomas A. (1989) Motion Control of a Citrus Picking Robot Doctoral dissertation, University of Florida College of Agricultural Engineering Robinson, D. W., and Pratt, G. A. (2000) Force Controllable Hydro Elastic Actuator, MIT Leg Laboratory, IEEE International Conference on Robotics and Automation, San Francisco, CA Schimmels, J. M., and Peshkin, M. A. (1994) Force Assembly with Friction, IEEE Transactions on Robotics and Automation, 10: 465 479. Vossoughi, G., Donath, M. (1995) Dynamic Feedback Linearization for Electrohydraulically Actuated Control Systems, ASME Journal of Dynamic Systems, Measurement, and Control, 117 : 468 477. Whitney, D. (1987) Historical Perspective and State of the Art in Robot Force Control, International Journal of Robotics Research 6 : 3 14. Zheng, K., Shen, T., and Yao, Y. (2011) Stationary Set Analysis for PD Controlled Mechanical Systems, IEEE Transactions on Control Systems Technology 1 9 : 1 236 1244. PAGE 288 288 BIOGRAPHIC AL SKETCH Allen Nease completed his BSME at UF in 1971 after serving in the United States Marine Corps in Vietnam as a fighter pilot and a forward air controller. He then worked in industry for fifteen years, primarily in the areas of heat transfer, HVAC design, and controls. While working full time, he completed his Masters Degree at UCF and then was accepted as a graduate research fellow at UF in 1983 in the robotics program at the Center for Intelligent Machines and Robotics (CIMAR) He completed his Ph.D. course requirements and residency requirements before going to work for the United States Air Force in 1989. He is currently the Strategic Research Manager for AFRL/Air Base Technologies Division at Tyndall AFB, FL. He is a registered professional engineer in the state of Florida. He is married with three children, one deceased, and has three grandchildren. 