perm filename PUI2S.PUI[2,TES] blob sn#009887 filedate 1972-07-20 generic text, type T, neo UTF8

14⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 15⎇⎇␈→60⎇0⎇⎇16414⎇␈←␈→60⎇-1000⎇⎇␈← 16⎇⎇␈→60⎇0⎇⎇1.0 INTRODUCTION␈← 17⎇⎇1.1 SYSTEM DESCRIPTION 18⎇⎇We are concerened here with a computer controlled arm and 19⎇⎇hand. This arm and hand function together as a general 20⎇⎇purpose manipulator which forms part of a robot as an 21⎇⎇effector subsystem. 22⎇⎇The computer plans and executes sequences of arm and hand 23⎇⎇motions to accomplish tasks. Arm motions are along smooth, 24⎇⎇collision free, space trajectories; all known forces are 25⎇⎇predicted during planning and compensated for during 26⎇⎇execution. Hand motions which consist of opening and closing 27⎇⎇the hand are controlled by a touch sense. The hand can also 28⎇⎇exert a force while following an externally defined motion. 29⎇⎇Any robot must contain both a sensory input mechanism, 30⎇⎇whereby it can gain information about the environment, and 31⎇⎇an effector subsystem by which it can change the 32⎇⎇environment. In addition to being the main effector, the 33⎇⎇hand also has primitive touch sensors and force detection 34⎇⎇ability, and may be considered a sensory subsystem. 35⎇⎇The environment in which the robot works must be one which 36⎇⎇it can represent internally. It has certain information a 37⎇⎇priori, and it is able to gain more information by 38⎇⎇interacting with its environment. 39⎇⎇In our case it knows, a priori, that objects are plane- 40⎇⎇bounded, solid and may be colored. It knows that all objects 41⎇⎇are supported by a plane, or by each other and that objects 42⎇⎇may be moved, but not through each other. It is given 43⎇⎇prototypes of all the possible objects that can exist, and 44⎇⎇learns of the existence and position of instances of these 45⎇⎇prototypes. Work is in progress to increase the scope of the 46⎇⎇robot's environment; curved objects are currently being 47⎇⎇added [Agin]. 48⎇⎇The type of interaction between robot subsystems is 49⎇⎇important. Currently all subsystems perform under the 50⎇⎇direction of the strategy subsystem, with little interaction 51⎇⎇between other subsystems. For instance the arm does not call 52⎇⎇for vision to locate an object that it has dropped, nor does 53⎇⎇vision call the arm to move an obscuring object in order 54⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈← 55⎇⎇␈→60⎇0⎇⎇16417⎇␈←␈→60⎇-1000⎇⎇␈← 56⎇⎇that it may "see" better. One important exception is the 57⎇⎇case of visual feedback used to position the hand on an 58⎇⎇object, here the vision subsystem interacts directly with 59⎇⎇the arm [Gill]. 60⎇⎇To illustrate the system we will describe the interaction to 61⎇⎇solve the Instant Insanity puzzle [Feldman]. Here it is 62⎇⎇first required that four colored cubes be found, and that 63⎇⎇the color of the faces be determined. The cubes are then 64⎇⎇turned and stacked so that each side of the stack has four 65⎇⎇different colored faces visible. 66⎇⎇The interaction between the STRATEGY subsystem, VISION, 67⎇⎇RECOGNIZE, COLOR and the arm is on the following level. The 68⎇⎇STRATEGY subsystem asks the VISION subsystem to find an 69⎇⎇outline and then RECOGNIZE to identify the outline as a 70⎇⎇cube, this process is repeated until four cubes have been 71⎇⎇found. COLOR is then told to find the colors at the center 72⎇⎇of each of the faces. The ARM is told to turn over each cube 73⎇⎇and VISION called to refind the outline; RECOGNIZE is then 74⎇⎇called to reidentify the outline as a cube. When all four 75⎇⎇cubes have been turned over and re-found, COLOR is told to 76⎇⎇find the colors on all the now visible back faces. The 77⎇⎇STRATEGY subsystem now determines the necessary turns and 78⎇⎇stack positions of each of the cubes to obtain a solution. 79⎇⎇The ARM is then told to move each cube accordingly. There 80⎇⎇are two levels of error recovery, within the subsystem, and 81⎇⎇by the stategy subsystem when a subsystem reports faiure. 82⎇⎇The programming environment in which the robot operates is 83⎇⎇known as the "Hand Eye" system [Feldman]. Here individual 84⎇⎇subsystems are represented by time sharing jobs, which may 85⎇⎇interact together. This is done by two means: 1) a global 86⎇⎇data structure which represents information about the 87⎇⎇environment available to all subsystems; 2) The message 88⎇⎇procedure construct whereby one job may execute a procedure 89⎇⎇in another job. For example, cubes are moved by a message 90⎇⎇procedure "MOVE␈π_INSTANCE" which in the Instant Insanity case 91⎇⎇was used both to turn the cubes over and to stack them. 92⎇⎇The arm subsystem provides a series of functions which are 93⎇⎇of general utility, such as to move the arm to a given 94⎇⎇position "MOVE", or to open the hand "OPEN". In order to 95⎇⎇perform these functions the arm subsystem maintains a data 96⎇⎇base in the form of constants and procedures, which describe 97⎇⎇the arm and hand in detail. If the arm is required to 98⎇⎇perform some task then an attempt is made to describe the 99⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈← 100⎇⎇␈→60⎇0⎇⎇16418⎇␈←␈→60⎇-1000⎇⎇␈← 101⎇⎇task in terms of existing arm functions, if this can be done 102⎇⎇then the problem is solved. If a task is proposed that 103⎇⎇cannot be solved in terms of existing functions, then either 104⎇⎇a function is modified or a new function is added. 105⎇⎇Other subsystems are not expected to use the arm's data 106⎇⎇base,which is local to the arm subsystem. Problem solving 107⎇⎇requiring use of this data base would be considered a 108⎇⎇function of the arm subsystem. Global variables are 109⎇⎇maintained which describe the current state of the arm for 110⎇⎇the other subsystems, in particular "ARM␈π_MOTION" which is 111⎇⎇true when the arm is moving, serves as a warning to other 112⎇⎇subsystems, notably vision, that the environment is 113⎇⎇changing. 114⎇⎇To build a stack of blocks would not be an arm function, as 115⎇⎇it could be accomplished in terms of simpler, existing 116⎇⎇functions. To turn a cube over would be an arm function as 117⎇⎇this requires that the range throughout which the hand can 118⎇⎇grasp the cube be considered. To move the arm through a 119⎇⎇cluttered space of objects is an arm task as this requires 120⎇⎇that the arm be considered in relation to the other objects 121⎇⎇when planning the trajectory that the arm will follow. 122⎇⎇Information is given to the arm subsystem in the function 123⎇⎇call and in the form of a three dimensional description of 124⎇⎇the space. If this space is completely described and the 125⎇⎇arm fails to accomplish a task then the strategy subsystem 126⎇⎇knows that the task must be specified differently. For 127⎇⎇instance, if a block must be moved but some obstacle is in 128⎇⎇the way, then the strategy program must first ask the arm to 129⎇⎇move the obstruction and then ask that the original move be 130⎇⎇accomplished. The arm subsystem would not move other objects 131⎇⎇and thus change the state of the environment without being 132⎇⎇told to do so by the strategy subsystem. If the the space 133⎇⎇model is incomplete and the arm cannot accomplish a task 134⎇⎇based on the available information, then the arm will not 135⎇⎇call the vision subsystem to have this space investigated 136⎇⎇but will report failure. 137⎇⎇There are two main parts to the arm subsystem: the planning 138⎇⎇program and the arm execution program. In order to move an 139⎇⎇object a plan must be made, and the object must be 140⎇⎇considered in relation to the hand to determine how it may 141⎇⎇be grasped. Other objects are considered in relation to the 142⎇⎇hand as it is moved in order to prevent collisions. This 143⎇⎇plan is represented in terms of a coordinated time dependent 144⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈← 145⎇⎇␈→60⎇0⎇⎇16419⎇␈←␈→60⎇-1000⎇⎇␈← 146⎇⎇trajectory for each joint of the arm. As the computer has 147⎇⎇planned a trajectory the program knows before the arm is 148⎇⎇moved the configuration, velocity and acceleration of all 149⎇⎇the links of the arm and can compute the effective inertia 150⎇⎇and the gravity torque of each link. These terms together 151⎇⎇with the trajectory are given to the arm program, where the 152⎇⎇inertia and gravity terms are used to improve the execution 153⎇⎇of the trajectory. As the gravity and inertia terms may be 154⎇⎇computed during planning, they need not be computed in real 155⎇⎇time. 156⎇⎇The arm servo program executes the trajectory by moving the 157⎇⎇arm, it also performs such actions as opening and closing 158⎇⎇the hand. Trajectories together with hand actions are 159⎇⎇written out in a file, and may be repeatedly executed by the 160⎇⎇arm program if required. The arm program is small and is 161⎇⎇suitable for execution in a mini-computer directly related 162⎇⎇to the arm. The planning program can be run under time 163⎇⎇sharing and can make plans for many such arms. 164⎇⎇The servo is a conventional sampled data servo executed by 165⎇⎇the computer with the following modification: certain 166⎇⎇control constants, the loop gain, predicted gravity and 167⎇⎇external torques are varied with arm configuration. 168⎇⎇In addition to the needs of the current vision and strategy 169⎇⎇subsystems, the arm has been programmed to perform other 170⎇⎇tasks such as turning cranks, screwing in screws and pushing 171⎇⎇and pulling. The touch sense is used in some of these tasks. 172⎇⎇Arm programs may be witten where the course of execution of 173⎇⎇the program by the arm may be modified depending on 174⎇⎇activation of the touch sensors or other conditions. Such 175⎇⎇programs may be written in a form of assembly language but 176⎇⎇are identical to the message procedure calls of a strategy 177⎇⎇program. 178⎇⎇In subsequent sections of this thesis we first describe the 179⎇⎇model of the environment and the hand's interaction with it 180⎇⎇(Section 432⎇). We then describe the model of the arm and 181⎇⎇derive all the relations that we will use (Section 762⎇). 182⎇⎇Section 79⎇ describes MOVE␈π_INSTANCE the highest level 183⎇⎇strategy function of the arm subsystem, primarily provided 184⎇⎇for strategy subsystems performing operations with blocks. 185⎇⎇Is Section 175⎇ we describe the requirements and solution of 186⎇⎇the smooth trajectories used by the arm. The servo loop is 187⎇⎇described and we then deal with control(Section 548⎇), 188⎇⎇giving a list of the arm functions or primitives. A final 189⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈← 190⎇⎇␈→60⎇0⎇⎇16420⎇␈←␈→60⎇-1000⎇⎇␈← 191⎇⎇section deals with an investigation of the possible motion 192⎇⎇of an unknown object. Here the arm tries to move the object 193⎇⎇in various ways and reports the resultant motion. Analysis 194⎇⎇of this reported data allows the resulting motion to be 195⎇⎇classified. 196⎇⎇The arm is described fully in [Scheinman], and we give a 197⎇⎇brief description in Appendix 946⎇. Two other appendices are 198⎇⎇given Appendix 353⎇ describes SAIL a form of ALGOL with LEAP 199⎇⎇added, which is the language that the programs are written 200⎇⎇in, and which we will use to illustrate some of the 201⎇⎇algorithmns. Appendix 684⎇ briefly covers homogeneous 202⎇⎇coordinate vectors and transformations, more fully covered 203⎇⎇in [ ]. 204⎇⎇1.2 HISTORY 205⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 206⎇⎇␈→60⎇0⎇⎇16422⎇␈←␈→60⎇-1000⎇⎇␈← 207⎇⎇␈→60⎇0⎇⎇2.0 WORLD MODEL␈← 208⎇⎇This section describes the model of the arm's environment 209⎇⎇which consists of solid plane-faced objects. These are the 210⎇⎇objects that the vision system can identify [Falk] and it is 211⎇⎇necessary to maintain a representation of them. This is done 212⎇⎇both for the Arm program and for the Vision program, which 213⎇⎇share a common data base [Paul]. 214⎇⎇We describe the prototype representation and the manner of 215⎇⎇specifying instances of these prototypes. The hand 216⎇⎇coordinates are then defined and the problem of grasping 217⎇⎇this class of objects is reduced to finding a set of 218⎇⎇orientation vectors. 219⎇⎇2.1 PROTOTYPE DESCRIPTION 220⎇⎇We will start our description of an object by first 221⎇⎇describing its prototype. The prototype is located with its 222⎇⎇center of mass at the origin and its principal inertial axes 223⎇⎇aligned with the coordinate axes. Each vertex is represented 224⎇⎇by a vector giving its distance from the origin and each 225⎇⎇face is represented by a row matrix giving its position and 226⎇⎇outward pointing normal. 227⎇⎇Items are created for each part of a prototype (face, 228⎇⎇vertex, edge). Those not familiar with "items" should 229⎇⎇consult Appendix 353⎇. The spacial information associated 230⎇⎇with vertices and faces is stored as array datums of these 231⎇⎇items. Each of these items is associated with the 232⎇⎇prototype, which is itself an item. Different attributes 233⎇⎇are used to indicate which part value is being associated. 234⎇⎇For example in the case of the cube shown in Figure 2.1. 235⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F1␈←␈→60⎇-1000⎇⎇[Eq. 2.1]␈← 236⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F2␈← 237⎇⎇␈→60⎇0⎇⎇" " "␈← 238⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F6␈← 239⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.2]␈← 240⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V2␈← 241⎇⎇␈→60⎇0⎇⎇" " "␈← 242⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V8␈← 243⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E1␈←␈→60⎇-1000⎇⎇[Eq. 2.3]␈← 244⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈← 245⎇⎇␈→60⎇0⎇⎇16426⎇␈←␈→60⎇-1000⎇⎇␈← 246⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E2␈← 247⎇⎇␈→60⎇0⎇⎇" " "␈← 248⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E12␈← 249⎇⎇In the case of edges the length of the edge is kept as its 250⎇⎇datum. 251⎇⎇For each Face we now associate its vertices and edges 252⎇⎇together 253⎇⎇Consider for example face F1: 254⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E1␈←␈→60⎇-1000⎇⎇[Eq. 2.4]␈← 255⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E2␈← 256⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E3␈← 257⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E4␈← 258⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.5]␈← 259⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V2␈← 260⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V3␈← 261⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V4␈← 262⎇⎇And for edges we associate the edge with its endpoints: 263⎇⎇␈→60⎇0⎇⎇ENDPT ⊗ E1 ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.6]␈← 264⎇⎇␈→60⎇0⎇⎇ENDPT ⊗ E1 ≡ V2␈← 265⎇⎇etc. 266⎇⎇This completes the topological description of a prototype. 267⎇⎇Prototypes are kept for the objects shown in Figure 2.2, 268⎇⎇this data is kept in the global data store and is available 269⎇⎇to all programs. 270⎇⎇Based on such a description it is possible to find many 271⎇⎇relationships, for example: 272⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈← 273⎇⎇␈→60⎇0⎇⎇16428⎇␈←␈→60⎇-1000⎇⎇␈← 274⎇⎇ Given any face F1 find its neighbors which share a 275⎇⎇ common vertex V1. 276⎇⎇ We can find the faces which share the common vertex 277⎇⎇ V1 by finding all the faces which satisfy 278⎇⎇ ␈→60⎇8⎇⎇FOREACH F | VERTEX ⊗ F ≡ V1␈← 279⎇⎇ Here F represents an item variable satisfied by the 280⎇⎇ association. The set of these F's are all the faces 281⎇⎇ which share this common vertex. However they are 282⎇⎇ not all neighbors of F1. To ascertain that they are 283⎇⎇ neighbors we must require that they share a common 284⎇⎇ edge and that they are not F1. 285⎇⎇ FOREACH F | VERTEX ⊗ F ≡ V1 286⎇⎇ ∧ EDGE ⊗ F ≡ E 287⎇⎇ ∧ EDGE ⊗ F1 ≡ E 288⎇⎇ ∧ F ≠ F1 289⎇⎇ This specifies the set of faces { F } which are the 290⎇⎇ neighbors of F1 and share the common vertex V1. 291⎇⎇Another example: it is required to go around the vertices of 292⎇⎇a face (F1) in order, starting with V1 293⎇⎇ T ← V1; 294⎇⎇ FOREACH E,H | EDGE ⊗ F1 ≡ E 295⎇⎇ ∧ END ⊗ E ≡ T 296⎇⎇ ∧ END ⊗ E ≡ H 297⎇⎇ ∧ H ≠ T DO BEGIN 298⎇⎇ IF H = V1 THEN DONE; 299⎇⎇ <statement>; 300⎇⎇ T ← H END; 301⎇⎇When an instance is located a new item is created which is 302⎇⎇associated with the prototype as follows 303⎇⎇␈→60⎇0⎇⎇INSTANCE ⊗ CUBE ≡ INST1␈←␈→60⎇-1000⎇⎇[Eq. 2.7]␈← 304⎇⎇In this "prototype - instance" scheme all information common 305⎇⎇to the instances is kept only once, with the prototype. The 306⎇⎇individual information, the instance transform, is kept with 307⎇⎇each instance. The position information relates to the 308⎇⎇prototype, which is located at the origin. 309⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈← 310⎇⎇␈→60⎇0⎇⎇16429⎇␈←␈→60⎇-1000⎇⎇␈← 311⎇⎇Most calculations can be performed by transforming the 312⎇⎇instance back to the prototype rather than transforming the 313⎇⎇prototype out to the instance. 314⎇⎇Consider for example the problem of finding the support face 315⎇⎇of a body by finding through which face the weight vector 316⎇⎇passes. We could transform each face out to the instance 317⎇⎇and calculate if the weight vector was in the general 318⎇⎇direction of the outward pointing normal and then see if the 319⎇⎇weight vector actually passed through the face. This would 320⎇⎇require that we transform each vertex of the face to the 321⎇⎇instance. It is more efficient to transform the weight 322⎇⎇vector back to the prototype by using the inverse transform 323⎇⎇and check through which face it passed, avoiding all the 324⎇⎇other transformations, which take of the order of 0.5 m sec. 325⎇⎇each. This completes our description of the "prototype - 326⎇⎇instance" scheme, and in the next section we will discuss 327⎇⎇the use of this world model by the arm system. 328⎇⎇2.2 ORIENTATION VECTORS 329⎇⎇The prototype discription is used when it is required to 330⎇⎇move an instance of some prototype. Apart from the 331⎇⎇positional information which is obtainable directly from the 332⎇⎇instance transform, the prototype description is used to 333⎇⎇calculate how the instance may be picked up. Knowing how a 334⎇⎇body is oriented and where it is located does not specify a 335⎇⎇hand position which may be used in order to pick the 336⎇⎇instance up. 337⎇⎇The position and orientation of the hand are described the 338⎇⎇same as for any other object, with the exception of the 339⎇⎇location of the origin of coordinates. The origin of the 340⎇⎇coordinate system in the hand is not at the center of mass 341⎇⎇as in the case of a body, but is located centrally between 342⎇⎇the finger tips with the z axis pointing in the direction of 343⎇⎇the fingers. The y axis is directed across, between the 344⎇⎇fingers. 345⎇⎇To specify a hand position we must give the cartesian 346⎇⎇coordinates of a point centrally located between the finger 347⎇⎇tips P␈-1⎇␈π_. We then give the orientation vector which points 348⎇⎇from one finger tip to the other O␈-1⎇␈π_. Finally, we generate a 349⎇⎇reference approach vector RA␈-2⎇␈π_␈π_ formed by crossing a vertical 350⎇⎇vector with the orientation vector, and then specify the 351⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈← 352⎇⎇␈→60⎇0⎇⎇16431⎇␈←␈→60⎇-1000⎇⎇␈← 353⎇⎇approach angle between the fingers and this reference 354⎇⎇direction measured about the orientation vector. This gives 355⎇⎇us the approach vector A␈-1⎇␈π_. See Figure 2.3. 356⎇⎇This information can be used to construct the 4 x 4 357⎇⎇transformation which locates the hand as: 358⎇⎇␈→60⎇0⎇⎇| (O x A)[x] O[x] A[x] P[x]|␈← 359⎇⎇␈→60⎇0⎇⎇| |␈← 360⎇⎇␈→60⎇0⎇⎇| (O x A)[y] O[y] A[y] P[y]|␈← 361⎇⎇␈→60⎇0⎇⎇| |␈← 362⎇⎇␈→60⎇0⎇⎇| (O x A)[z] O[z] A[z] P[z]|␈← 363⎇⎇␈→60⎇0⎇⎇| |␈← 364⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈← 365⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 2.8]␈← 366⎇⎇Having described how a hand position is specified we must 367⎇⎇now make some specifications about picking up objects. For 368⎇⎇planar or convex surfaces an object may be picked up by two 369⎇⎇parallel faces on an axis containing the center of mass; 370⎇⎇this will prevent the object from rotating. One but not both 371⎇⎇surfaces may be replaced by an apex of the body. Both 372⎇⎇surfaces may be replaced by edges if a normal from the edge 373⎇⎇intersects the center of mass. These considerations define 374⎇⎇the set of orientation vectors. if the hand is positioned 375⎇⎇at the center of mass with one of these orientation vectors, 376⎇⎇it will be in a position to grasp the object. See Figure 377⎇⎇2.4. 378⎇⎇To find systematically all the possible orientation vectors 379⎇⎇we first find all the contact vectors from the center of 380⎇⎇mass of the object that intersect and are 1) normal to any 381⎇⎇edge, 2) normal to any plane, 3) pass through any apex. We 382⎇⎇then search this list for anti-parallel vectors, being 383⎇⎇careful not to take both vectors from the third class. this 384⎇⎇is done in the following manner: 385⎇⎇For faces we simply check that the perpendicular from the 386⎇⎇plane to the center of mass lies inside the face and thus 387⎇⎇the surface is perpendicular at the contact point 388⎇⎇ FOREACH F | FACE⊗PROTOTYPE≡F DO 389⎇⎇ BEGIN 390⎇⎇ D←DATUM(F)[4]; 391⎇⎇ IF D>0.0 THEN GO TO NFACE; 392⎇⎇ if we use this plane the center of mass will 393⎇⎇ not lie between the finger tips 394⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈← 395⎇⎇␈→60⎇0⎇⎇16434⎇␈←␈→60⎇-1000⎇⎇␈← 396⎇⎇ IF CONTAINED(DATUM(F),F) THEN FOUND ONE; 397⎇⎇ is the point of intersection of the normal 398⎇⎇ and the face within the boundary? 399⎇⎇NFACE: END; 400⎇⎇The procedure CONTAINED counts the number of region boundary 401⎇⎇crossings of a ray from the point to infinity. If the number 402⎇⎇is odd then the point is inside the region, if even then it 403⎇⎇is outside. 404⎇⎇Let us now look at edges. It is necessary that a normal 405⎇⎇vector from the edge pass through the center of mass. 406⎇⎇ FOREACH E,A,B|EDGE⊗PROTOTYPE≡E 407⎇⎇ ∧ END⊗E≡A 408⎇⎇ ∧ END⊗E≡B 409⎇⎇ ∧ A≠B DO BEGIN 410⎇⎇ T←A␈-1⎇␈π_ . (A␈-1⎇␈π_ - B␈-1⎇␈π_)/(A␈-1⎇␈π_ - B␈-1⎇␈π_) . (A␈-1⎇␈π_ - B␈-1⎇␈π_); 411⎇⎇ T is the directed distance from end point A 412⎇⎇ to the normal, divided by the directed 413⎇⎇ distance of A from B. 414⎇⎇ IF T≤0.0 ∨ T≥1.0 THEN GO TO NEDGE; 415⎇⎇ The normal intersects on the edge. 416⎇⎇ C␈-1⎇␈π_ ← A␈-1⎇␈π_ - ((A␈-1⎇␈π_ - B␈-1⎇␈π_) * T); 417⎇⎇ C is a vector from the center of mass and 418⎇⎇ perpendicular to the edge at the point of contact. 419⎇⎇ See Figure 2.5 420⎇⎇ We now check that this is an outside edge: 421⎇⎇ FOREACH N1,N2|BOUNDARY⊗N1≡E 422⎇⎇ ∧ BOUNDARY⊗N2≡E 423⎇⎇ ∧ N1≠N1 DO; 424⎇⎇ N1␈-2⎇␈π_␈π_ ← DATUM(N1); 425⎇⎇ N2␈-2⎇␈π_␈π_ ← DATUM(N2); 426⎇⎇ N␈-1⎇␈π_ ← N1␈-2⎇␈π_␈π_ X N2␈-2⎇␈π_␈π_; 427⎇⎇ N␈-1⎇␈π_ is a refrence vector such that we move 428⎇⎇ outside as we rotate about N␈-1⎇␈π_ from N1␈-2⎇␈π_␈π_ to N2␈-2⎇␈π_␈π_. 429⎇⎇ V␈-1⎇␈π_ ← N1␈-2⎇␈π_␈π_ X C␈-1⎇␈π_; 430⎇⎇ IF N␈-1⎇␈π_ . V␈-1⎇␈π_ ≤ 0 431⎇⎇ C␈-1⎇␈π_ points to the outside of the vertex. 432⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈← 433⎇⎇␈→60⎇0⎇⎇16436⎇␈←␈→60⎇-1000⎇⎇␈← 434⎇⎇ THEN FOUND ONE; 435⎇⎇ END; 436⎇⎇ END; 437⎇⎇In the caes of vertices it is only the outside corners of 438⎇⎇the object that we can use. See Figure 2.6 That is for all 439⎇⎇edges at this vertex the angle O␈-1⎇- must be less than 90 440⎇⎇degrees. See Figure 2.6. 441⎇⎇ FOREACH V | VERTEX ⊗ BDY ≡ V DO BEGIN 442⎇⎇ C2←V␈-1⎇␈π_ . V␈-1⎇␈π_; 443⎇⎇ FOREACH E,A | END ⊗ E ≡ V 444⎇⎇ ∧ END ⊗ E ≡ A 445⎇⎇ ∧ A ≠ V DO 446⎇⎇ IF A␈-1⎇␈π_ . V␈-1⎇␈π_ > C2 THEN GO TO NOGOOD; 447⎇⎇ FOUND ONE; 448⎇⎇NOGOOD: END; 449⎇⎇We then search this list of contact points looking for anti- 450⎇⎇parallel vectors, being careful not to take both vectors 451⎇⎇from the class of vertices. These then are the orientation 452⎇⎇vectors and are stored with the prototype, together with the 453⎇⎇contact information: 454⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O1␈←␈→60⎇-1000⎇⎇[Eq. 2.9]␈← 455⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O2␈← 456⎇⎇␈→60⎇0⎇⎇" " "␈← 457⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O9␈← 458⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O1 ≡ F1␈←␈→60⎇-1000⎇⎇[Eq. 2.10]␈← 459⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O1 ≡ F3␈← 460⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O2 ≡ F2␈← 461⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O2 ≡ F4␈← 462⎇⎇␈→60⎇0⎇⎇" " "␈← 463⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈← 464⎇⎇␈→60⎇0⎇⎇16438⎇␈←␈→60⎇-1000⎇⎇␈← 465⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O9 ≡ E6␈← 466⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O9 ≡ E2␈← 467⎇⎇The contact information is used in determining which 468⎇⎇orientation vectors can be used for a given instance. The 469⎇⎇datum of the orientation vector is a 5 element matrix with 470⎇⎇the following elements: If ␈_C1␈≡ and ␈_C2␈≡ are two anti-parallel 471⎇⎇contact vectors then the datum of the orientation vector is: 472⎇⎇␈→60⎇0⎇⎇O[1] = C1[1] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈← 473⎇⎇␈→60⎇0⎇⎇O[2] = C1[2] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈← 474⎇⎇␈→60⎇0⎇⎇O[3] = C1[3] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈← 475⎇⎇␈→60⎇0⎇⎇O[4] = | C1␈-2⎇␈π_␈π_ |␈← 476⎇⎇␈→60⎇0⎇⎇O[5] = | C2␈-2⎇␈π_␈π_ |␈← 477⎇⎇␈→60⎇0⎇⎇O[6] = | O[4] | + | O[5] |␈← 478⎇⎇All arm operations with bodies can be reduced to 479⎇⎇manipulations of these vectors. With such orientation 480⎇⎇vectors associated with each prototype the problem of 481⎇⎇picking up an instance can be reduced to the following: Each 482⎇⎇orientation vector is transformed out to the instance and 483⎇⎇the orientation vectors which contact the support face, 484⎇⎇based on the contact associations, discarded. The instance 485⎇⎇can then be picked up by any one of the remaining 486⎇⎇orientation vectors. When an approach angle is specified the 487⎇⎇hand transform may be specified. The problem of selecting 488⎇⎇the corect orientation vector and approach angle is 489⎇⎇discussed fully in Subsection 310⎇. 490⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 491⎇⎇␈→60⎇0⎇⎇16439⎇␈←␈→60⎇-1000⎇⎇␈← 492⎇⎇␈→60⎇0⎇⎇3.0 ARM MODEL␈← 493⎇⎇In this section we will consider the arm and develop a model 494⎇⎇for it [Piper]. We will describe the solution which, given 495⎇⎇a hand position, returns a set of joint angles. Then, based 496⎇⎇on the model of the arm, we will develop a solution for 497⎇⎇differential motion. We will then derive the relation 498⎇⎇between acceleration and force for the arm [Kahn], to obtain 499⎇⎇the effective link inertia and gravity loading. Finally we 500⎇⎇derive the relationship between a force and moment acting at 501⎇⎇hand and the six joint reaction torques. The results of 502⎇⎇this section are used in later sections of the work but as 503⎇⎇they all relate to the arm model they are derived together 504⎇⎇here. 505⎇⎇3.1 KINEMATICS 506⎇⎇The arm shown in Figure 3.1 is a six degree of freedom 507⎇⎇device allowing the hand to be positioned anywhere and with 508⎇⎇any orientation. The position and orientation of any link 509⎇⎇is described by a four by four transformation in the same 510⎇⎇manner as an instance. 511⎇⎇The prototype description for the arm's links is not given 512⎇⎇as we have no need for it, although each link could have a 513⎇⎇prototype associated with it. 514⎇⎇Each link is connected to the next link by a joint. There 515⎇⎇are two kinds of joints, prismatic, or sliding, and 516⎇⎇revolute. In order to describe the link transformation in 517⎇⎇terms of the joint constraint and the joint variable we will 518⎇⎇introduce a coordinate system in which the joint constraint 519⎇⎇is implicit. 520⎇⎇We will develop the "A" matrices which relate between link 521⎇⎇coordinate systems and the "T" matrices, the link 522⎇⎇transformations which specify the position and orientation 523⎇⎇of each link in space. In the case of links the origin of 524⎇⎇coordinates is not the center of mass but is specified at 525⎇⎇the joint axis. 526⎇⎇Associated with each link is an orthogonal coordinate system 527⎇⎇fixed in the link (see Figure 3.2). 528⎇⎇For link i the Zi axis is directed along the axis of the 529⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.1 KINEMATICS␈← 530⎇⎇␈→60⎇0⎇⎇16444⎇␈←␈→60⎇-1000⎇⎇␈← 531⎇⎇joint between link i and i+1. The xi axis is along the 532⎇⎇common normal between the two joint axes of the link in the 533⎇⎇direction from x(i-1) to xi. The y axis completes the right 534⎇⎇handed set. 535⎇⎇We can relate between coordinate systems i+1 and i by 536⎇⎇performing a rotation, followed by two translations, 537⎇⎇followed by a rotation in the following manner: 538⎇⎇ 1). A rotation about zi of O␈-1⎇-i to align xi with 539⎇⎇ x(i+1) the common normal. 540⎇⎇ 2). A translation si along zi to locate the origin 541⎇⎇ on the common normal. 542⎇⎇ 3). A translation of ai along x(i+1) to bring the 543⎇⎇ origins into coincidence. 544⎇⎇ 4). A rotation about x(i+1) of αi to bring the z 545⎇⎇ axes together. 546⎇⎇If this represents a revolute joint then O␈-1⎇- is the joint 547⎇⎇variable. If it is a prismatic joint then s is the joint 548⎇⎇variable. 549⎇⎇If we express points in link i by a vector Ri␈-2⎇␈π_␈π_ then the 550⎇⎇relationship between coordinate systems Ri␈-2⎇␈π_␈π_ and R(i-1)␈-6⎇␈π_␈π_␈π_␈π_␈π_␈π_ may be 551⎇⎇expressed by 552⎇⎇␈→60⎇0⎇⎇R(i-1)␈-6⎇␈π_␈π_␈π_␈π_␈π_␈π_ = | Ai| * | Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.1]␈← 553⎇⎇where | Ai | is given by: 554⎇⎇␈→60⎇0⎇⎇| cos O␈-1⎇- -cos α sin O␈-1⎇- sin α cos O␈-1⎇- a cos O␈-1⎇- |␈← 555⎇⎇␈→60⎇0⎇⎇| sin O␈-1⎇- cos α cos O␈-1⎇- -sin α cos O␈-1⎇- a sin O␈-1⎇- |␈← 556⎇⎇␈→60⎇0⎇⎇| 0 sin α cos α s |␈← 557⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈← 558⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.2]␈← 559⎇⎇if we let link0 be the table coordinate system then we may 560⎇⎇relate from any link coordinates to R0␈-2⎇␈π_␈π_ by 561⎇⎇␈→60⎇0⎇⎇R0␈-2⎇␈π_␈π_ =|A1|*|A2|*|A3| ... |Ai|*|Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.3]␈← 562⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.1 KINEMATICS␈← 563⎇⎇␈→60⎇0⎇⎇16445⎇␈←␈→60⎇-1000⎇⎇␈← 564⎇⎇or 565⎇⎇␈→60⎇0⎇⎇R0␈-2⎇␈π_␈π_ = | Ti| * | Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.4]␈← 566⎇⎇where 567⎇⎇␈→60⎇0⎇⎇| Ti| = |A1|*|A2|*|A3| ... |Ai|␈←␈→60⎇-1000⎇⎇[Eq. 3.5]␈← 568⎇⎇| Ti | is the transform of the ith link of the arm and is 569⎇⎇the same as an instance transform. 570⎇⎇Based on these matrices we can obtain the differential 571⎇⎇change dR0␈-2⎇␈π_␈π_ with respect to a change of joint variable dq: 572⎇⎇␈→60⎇0⎇⎇dR0␈-2⎇␈π_␈π_ = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑i␈↓␈-2⎇␈↓j=1␈↑ |Uij| * dqj * |Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.6]␈← 573⎇⎇where 574⎇⎇␈→60⎇0⎇⎇|Uij| = ∂|Ti|/∂qj␈←␈→60⎇-1000⎇⎇[Eq. 3.7]␈← 575⎇⎇␈→60⎇0⎇⎇= |A1|*|A2| ... |Aj-1|*|Qj|*|Aj|*|Aj+1| ... *|Ai|␈← 576⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.8]␈← 577⎇⎇and depending on whether the joint is rotary or prismatic 578⎇⎇ | 0-1 0 0 | 579⎇⎇ Q(O␈-1⎇-) = | 1 0 0 0 |␈→60⎇-1000⎇⎇[Eq. 3.9]␈← 580⎇⎇ | 0 0 0 0 | 581⎇⎇ | 0 0 0 0 | 582⎇⎇ | 0 0 0 0 | 583⎇⎇ Q(s) = | 0 0 0 0 |␈→60⎇-1000⎇⎇[Eq. 3.10]␈← 584⎇⎇ | 0 0 0 1 | 585⎇⎇ | 0 0 0 0 | 586⎇⎇In the case of the arm that we are using, we pick the origin 587⎇⎇of coordinates at the shoulder. We have two revolute joints 588⎇⎇followed by a prismatic joint followed by three intersecting 589⎇⎇revolute joints. The only offsets are S2 = 6.0in and S6 = 590⎇⎇10.25in. 591⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 592⎇⎇␈→60⎇0⎇⎇16446⎇␈←␈→60⎇-1000⎇⎇␈← 593⎇⎇3.2 ARM SOLUTION 594⎇⎇The arm solution is that procedure which, given |T6|, the 595⎇⎇transformation of the hand, returns the six joint angles 596⎇⎇which will position the arm such that the hand has the 597⎇⎇required transformation. 598⎇⎇Because the last three joints intersect and there is only 599⎇⎇one offset, S6, we can obtain the position of the end of 600⎇⎇link 3 by subtracting a vector A␈-1⎇␈π_ the length of S6 and in the 601⎇⎇direction of the approach vector from the position vector P␈-1⎇␈π_. 602⎇⎇See Figure 3.3 to obtain W␈-1⎇␈π_. 603⎇⎇␈→60⎇0⎇⎇W␈-1⎇␈π_ = P␈-1⎇␈π_ - A␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.11]␈← 604⎇⎇We can now solve for S3 the prismatic joint variable as: 605⎇⎇␈→60⎇0⎇⎇S3 = ( W␈-1⎇␈π_ . W␈-1⎇␈π_ - S2↑2 )↑(1/2)␈←␈→60⎇-1000⎇⎇[Eq. 3.12]␈← 606⎇⎇Having solved for S3, O␈-1⎇-1 is given by: 607⎇⎇␈→60⎇0⎇⎇O␈-1⎇-1 = O␈-1⎇- + O␈-1⎇|␈←␈→60⎇-1000⎇⎇[Eq. 3.13]␈← 608⎇⎇Where O␈-1⎇- and O␈-1⎇| are given by: 609⎇⎇␈→60⎇0⎇⎇Tan O␈-1⎇- = W[2] / W[1]␈←␈→60⎇-1000⎇⎇[Eq. 3.14]␈← 610⎇⎇␈→60⎇0⎇⎇Sin O␈-1⎇| = S2 / ( W[1]↑2 + W[2]↑2 )↑(1/2)␈←␈→60⎇-1000⎇⎇[Eq. 3.15]␈← 611⎇⎇See Figure 3.3 612⎇⎇O␈-1⎇-2 is then given by: 613⎇⎇␈→60⎇0⎇⎇Cos O␈-1⎇-2 = W[3] / S3␈←␈→60⎇-1000⎇⎇[Eq. 3.16]␈← 614⎇⎇See Figure 3.3. 615⎇⎇The unit vectors y3 and z3 are now calculated for link 3 and 616⎇⎇a vector: 617⎇⎇␈→60⎇0⎇⎇RR␈-2⎇␈π_␈π_ = z3␈-2⎇␈π_␈π_ x z6␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.17]␈← 618⎇⎇is calculated. 619⎇⎇Then: 620⎇⎇␈→60⎇0⎇⎇O␈-1⎇-4 angle between RR␈-2⎇␈π_␈π_ and y3␈-2⎇␈π_␈π_ about z3␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.18]␈← 621⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.2 SOLUTION␈← 622⎇⎇␈→60⎇0⎇⎇16449⎇␈←␈→60⎇-1000⎇⎇␈← 623⎇⎇␈→60⎇0⎇⎇O␈-1⎇-5 angle between z6␈-2⎇␈π_␈π_ and z3␈-2⎇␈π_␈π_ about RR␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.19]␈← 624⎇⎇␈→60⎇0⎇⎇O␈-1⎇-6 angle between y6␈-2⎇␈π_␈π_ and RR␈-2⎇␈π_␈π_ about z6␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.20]␈← 625⎇⎇See Figure 3.4 626⎇⎇All joints with the exception of joint 6 have only a partial 627⎇⎇range of motion. As the solution for each joint is obtained, 628⎇⎇it must be checked to see that it is within the range of 629⎇⎇motion of that joint. 630⎇⎇3.3 DIFFERENTIAL SOLUTION 631⎇⎇Given an arm solution it is often necessary to compute the 632⎇⎇differential change in joint angles in order to make a small 633⎇⎇change in position while maintaining the present orientation 634⎇⎇of the hand. 635⎇⎇We already have expressions for the differential change of 636⎇⎇| Ti| Equation 3.7 in terms of dqj the differential change 637⎇⎇in the j joint angles. 638⎇⎇If we evaluate the six | U6j| matrices (Equation 3.7) we 639⎇⎇will have the differential change in the 16 elements of the 640⎇⎇hand matrix | T6 | for each of the six joints. This 641⎇⎇corresponds to 16 equations in 6 unknowns. We must pick 6 642⎇⎇of the 16 equations and then solve for the required change 643⎇⎇in the qj. 644⎇⎇From | U6j | we will pick the first three elements of the 645⎇⎇right hand column as these correspond to dx, dy, dz. We then 646⎇⎇pick the two smallest elements of column 3, the approach 647⎇⎇vector, to constrain its direction. Finally we pick one 648⎇⎇additional element from column 2, the orientation vector, in 649⎇⎇order to constrain rotation about the approach vector. 650⎇⎇The six equations: 651⎇⎇| dx | | U61 U62 U63 U64 U65 U66 | | dq | 652⎇⎇| | | 14 14 14 14 14 14 | | 1| 653⎇⎇| | | | | | 654⎇⎇| dy | | U61 U62 U63 U64 U65 U66 | | dq | 655⎇⎇| | | 24 24 24 24 24 24 | | 2| 656⎇⎇| | | | | | 657⎇⎇| dz | = | U61 U62 U63 U64 U65 U66 | * | dq | 658⎇⎇| | | 34 34 34 34 34 34 | | 3| 659⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.3 DIFFERENTIAL␈← 660⎇⎇␈→60⎇0⎇⎇16452⎇␈←␈→60⎇-1000⎇⎇␈← 661⎇⎇| | | | | | 662⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq | 663⎇⎇| | | u3 u3 u3 u3 u3 u3 | | 3| 664⎇⎇| | | | | | 665⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq | 666⎇⎇| | | v3 v3 v3 v3 v3 v3 | | 4| 667⎇⎇| | | | | | 668⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq | 669⎇⎇| | | w2 w2 w2 w2 w2 w2 | | 6| 670⎇⎇␈→60⎇-1000⎇⎇[Eq. 3.21]␈← 671⎇⎇are then solved to give the six dqj, the differential change 672⎇⎇in joint angle. 673⎇⎇3.4 DYNAMICS 674⎇⎇From the kinematic arm model we can also develop the dynamic 675⎇⎇model [Uicker]. We will derive the Lagrangian [Simon] for 676⎇⎇the arm in a gravitational force field, and obtain the 677⎇⎇equations relating acceleration to joint torque, including 678⎇⎇the static torques necessary to overcome the effects of 679⎇⎇gravity. These results will be used in the section which 680⎇⎇relates to servoing the arm. 681⎇⎇If the Lagrangian L is defined as 682⎇⎇␈→60⎇0⎇⎇L = K - P␈←␈→60⎇-1000⎇⎇[Eq. 3.22]␈← 683⎇⎇ where: 684⎇⎇ K is the Kinetic Energy of the system in terms of 685⎇⎇ joint variables 686⎇⎇ P is the potential energy in terms of joint 687⎇⎇ variables 688⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈← 689⎇⎇␈→60⎇0⎇⎇16454⎇␈←␈→60⎇-1000⎇⎇␈← 690⎇⎇ The joint variables "q" are either "O␈-1⎇-" or "s" 691⎇⎇ depending upon whether the joint is revolute or 692⎇⎇ prismatic. 693⎇⎇The equations of motion are given by: 694⎇⎇␈→60⎇0⎇⎇d/dt ( ∂L/∂q␈-1⎇␈↑.␈↓j ) - ∂L/∂qj = Fj␈←␈→60⎇-1000⎇⎇[Eq. 3.23]␈← 695⎇⎇␈→60⎇0⎇⎇for j = 1,2, ... n␈← 696⎇⎇where Fj is the force on joint j. 697⎇⎇From Equation 3.6 we can obtain the velocity of any point 698⎇⎇as: 699⎇⎇␈→60⎇0⎇⎇dR0␈-2⎇␈π_␈π_/dt = | Vi | * | Ri |␈←␈→60⎇-1000⎇⎇[Eq. 3.24]␈← 700⎇⎇where: 701⎇⎇␈→60⎇0⎇⎇| Vi | = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑i␈↓␈-2⎇␈↓j=1␈↑( | Uij | * dqj/dt )␈←␈→60⎇-1000⎇⎇[Eq. 3.25]␈← 702⎇⎇We may now express the kinetic energy of a link as follows. 703⎇⎇Consider a particle of mass dm on link i at Ri␈-2⎇␈π_␈π_ then the 704⎇⎇kinetic energy is: 705⎇⎇␈→60⎇0⎇⎇dHi = 1/2 ( R␈-1⎇␈↑.␈↓i␈-2⎇␈π_␈π_ . R␈-1⎇␈↑.␈↓i␈-2⎇␈π_␈π_ ) dm␈←␈→60⎇-1000⎇⎇[Eq. 3.26]␈← 706⎇⎇or: 707⎇⎇␈→60⎇0⎇⎇dHi = 1/2 Trace(| Vi|*| Ri|*| Ri|␈↑T␈↓*| Vi|␈↑T␈↓)*dm␈← 708⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.27]␈← 709⎇⎇The total kinetic energy for the link can be found by 710⎇⎇integrating over the mass of the link. 711⎇⎇␈→60⎇0⎇⎇dHi = 1/2 Trace[|Vi|*(␈π~␈-3⎇␈↓link␈↑|Ri|*|Ri|␈↑T␈↓*dm)*|Vi|␈↑T␈↓]␈← 712⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.28]␈← 713⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈← 714⎇⎇␈→60⎇0⎇⎇16455⎇␈←␈→60⎇-1000⎇⎇␈← 715⎇⎇to obtain: 716⎇⎇Hi = mi* 717⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈← 718⎇⎇␈→60⎇0⎇⎇|1/2(-k +k +k ) k k x |␈← 719⎇⎇␈→60⎇0⎇⎇| i11 i22 i33 i12 i13 i|␈← 720⎇⎇␈→60⎇0⎇⎇| |␈← 721⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈← 722⎇⎇␈→60⎇0⎇⎇| k k -k +k k y |␈← 723⎇⎇␈→60⎇0⎇⎇| i12 i11 i22 i33 i23 i|␈← 724⎇⎇␈→60⎇0⎇⎇| |␈← 725⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈← 726⎇⎇␈→60⎇0⎇⎇| k k k +k -k z |␈← 727⎇⎇␈→60⎇0⎇⎇| i13 i23 i11 i22 i33 i|␈← 728⎇⎇␈→60⎇0⎇⎇| |␈← 729⎇⎇␈→60⎇0⎇⎇| ␈π_ ␈π_ ␈π_ |␈← 730⎇⎇␈→60⎇0⎇⎇| x y z 1 |␈← 731⎇⎇␈→60⎇0⎇⎇| i i i |␈← 732⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.29]␈← 733⎇⎇ where:k␈↓ijk␈↑ is the radius of gyration of link i about 734⎇⎇ the j,k axes. x␈-1⎇␈↑_␈↓i, y␈-1⎇␈↑_␈↓i, z␈-1⎇␈↑_␈↓i is the center of 735⎇⎇ mass of link i. mi is the mass of link i. 736⎇⎇The kinetic energy of the entire system is 737⎇⎇␈→60⎇0⎇⎇K = 1/2 ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓i=1␈↑ Trace(| Vi |*| Hi |*| Vi |␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 3.30]␈← 738⎇⎇The potential energy of the system due to gravity in the 739⎇⎇negative z direction, is expressed by: 740⎇⎇␈→60⎇0⎇⎇P = - ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓i=1␈↑ mi * | G | * | Ti | * | R␈-1⎇␈↑_␈↓i |␈←␈→60⎇-1000⎇⎇[Eq. 3.31]␈← 741⎇⎇where | G | = | 0 0 g 0 |␈→60⎇-1000⎇⎇[Eq. 3.32]␈← 742⎇⎇and g is the acceleration due to gravity. 743⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈← 744⎇⎇␈→60⎇0⎇⎇16456⎇␈←␈→60⎇-1000⎇⎇␈← 745⎇⎇Substituting for K from Equation 3.30 and for P from 746⎇⎇Equation 3.31 into Equation 3.22 and then differentiating 747⎇⎇according to Equation 3.23 we obtain 748⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓j=i␈↑[~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓*qk␈-2⎇␈↑..␈↓) 749⎇⎇ +~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓l=1␈↑Trace(| Ujkl|*| Hj|*| Uji|␈↑T␈↓*q␈-1⎇␈↑.␈↓j*q␈-1⎇␈↑.␈↓k) 750⎇⎇ -mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq. 3.33]␈← 751⎇⎇the equation relating acceleration to force, from which we 752⎇⎇will infer the effective link inertia and gravity loading 753⎇⎇torque Tg in Subsection 330⎇. 754⎇⎇3.5 FORCE AND MOMENTS 755⎇⎇Given a force F␈-1⎇␈π_ that acts through the origin of the hand 756⎇⎇coordinate system and a moment M, we wish to find the joint 757⎇⎇reaction torques. 758⎇⎇We represent a force in the n'th coordinate system as: 759⎇⎇␈→60⎇0⎇⎇Fn = | Fn[x] Fn[y] Fn[z] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 3.34]␈← 760⎇⎇and a moment similarly: 761⎇⎇␈→60⎇0⎇⎇Mn = | Mn[x] Mn[y] Mn[z] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 3.35]␈← 762⎇⎇We first transform the force F and the moment M into the 763⎇⎇hand coordinate system by: 764⎇⎇␈→60⎇0⎇⎇| F6 | = | T6 |␈↑-1␈↓*| F |␈←␈→60⎇-1000⎇⎇[Eq. 3.36]␈← 765⎇⎇␈→60⎇0⎇⎇| M6 | = | T6 |␈↑-1␈↓*| M |␈←␈→60⎇-1000⎇⎇[Eq. 3.37]␈← 766⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.5 FORCE␈← 767⎇⎇␈→60⎇0⎇⎇16458⎇␈←␈→60⎇-1000⎇⎇␈← 768⎇⎇We then proceed to transform the force and moment back 769⎇⎇through the links as follows. (see Figure 3.5) 770⎇⎇␈→60⎇0⎇⎇| F(n-1) | = | An | * | Fn |␈←␈→60⎇-1000⎇⎇[Eq. 3.38]␈← 771⎇⎇and the moment: 772⎇⎇␈→60⎇0⎇⎇| M(n-1) | = | An | * | Mn |␈←␈→60⎇-1000⎇⎇[Eq. 3.39]␈← 773⎇⎇However the force F(n-1) does not act through the origin of 774⎇⎇the (n-1)th coordinate system (see Figure 3.5) and we have 775⎇⎇an additional moment given by: 776⎇⎇␈→60⎇0⎇⎇P␈-1⎇␈π_ x F␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.40]␈← 777⎇⎇where: 778⎇⎇␈→60⎇0⎇⎇␈_F(n-1)␈≡ is | F(n-1)[x] F(n-1)[y] F(n-1)[z] 1 |␈← 779⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.41]␈← 780⎇⎇and P␈-1⎇␈π_ is the right hand column of | An | 781⎇⎇␈→60⎇0⎇⎇P␈-1⎇␈π_ = | An[1,4] An[2,4] An[3,4] 1 |␈←␈→60⎇-1000⎇⎇[Eq. 3.42]␈← 782⎇⎇the total moment is then: 783⎇⎇␈→60⎇0⎇⎇| M(n-1) | = | An | * | Mn | + P␈-1⎇␈π_ x ␈_F(n-1)␈≡␈←␈→60⎇-1000⎇⎇[Eq. 3.43]␈← 784⎇⎇If the (n-1)th joint is revolute then the reaction torque is 785⎇⎇M(n-1)␈↓z␈↑ if it is prismatic then the force is F(n-1)␈↓z␈↑. 786⎇⎇The six reaction torques are known as the equivalent arm 787⎇⎇torque. 788⎇⎇When the hand is to exert a force the equivalent arm torque 789⎇⎇is calculated and added to the gravity torque for each 790⎇⎇joint. If each joint is run at these torques then the hand 791⎇⎇will exert the required force. 792⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 793⎇⎇␈→60⎇0⎇⎇16460⎇␈←␈→60⎇-1000⎇⎇␈← 794⎇⎇␈→60⎇0⎇⎇4.0 MOVE INSTANCE␈← 795⎇⎇The procedure MOVE␈π_INSTANCE is called when it is desired to 796⎇⎇have the arm move an object. MOVE␈π_INSTANCE is required to 797⎇⎇find two suitable hand positions such that if the arm 798⎇⎇assumes the first position it will be in a position to grasp 799⎇⎇the object located by | Ti |, and then if it moves to the 800⎇⎇second arm position and releases the object the required 801⎇⎇move will have been accomplished, and the object will be at 802⎇⎇| Tf |. The suffixes "i" and "f" refer to the initial and 803⎇⎇final positions. 804⎇⎇4.1 RANGE OF SOLUTION 805⎇⎇In manipulating objects we need to find not only a single 806⎇⎇solution but a range of solutions. Given a position and an 807⎇⎇orientation vector, the program should compute the range of 808⎇⎇approach angle throughout which the arm can reach the object 809⎇⎇to perform the desired manipulation. 810⎇⎇The approach angle is the angle measured between the 811⎇⎇approach vector A␈-1⎇␈π_ and the reference approach vector RA␈-2⎇␈π_␈π_ which 812⎇⎇is given by: 813⎇⎇␈→60⎇0⎇⎇RA␈-2⎇␈π_␈π_ ← z␈-1⎇␈π_ x O␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 4.1]␈← 814⎇⎇where: 815⎇⎇␈→60⎇0⎇⎇z␈-1⎇␈π_ = | 0 0 1 1 |␈←␈→60⎇-1000⎇⎇[Eq. 4.2]␈← 816⎇⎇It is doubtful that this problem can be solved analytically. 817⎇⎇Heuristics are used to find an approach angle at which an 818⎇⎇arm solution will exist, if one exists at all, and a further 819⎇⎇test is made to determine if solutions exist for all 820⎇⎇approach angles. If a solution exists, but not for the full 821⎇⎇range, a binary search is conducted, using the arm solution 822⎇⎇procedure as a test, to define the limits of the range. This 823⎇⎇is quite fast as the limits of the range are only needed to 824⎇⎇within ␈π-5 degrees, and the arm solution is very simple. 825⎇⎇The range of approach angle is that through which the arm 826⎇⎇can reach the object for a given position and orientation 827⎇⎇vector see (Figure 4.1). 828⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.1 RANGE␈← 829⎇⎇␈→60⎇0⎇⎇16464⎇␈←␈→60⎇-1000⎇⎇␈← 830⎇⎇We then consider the support plane and obtain a range of 831⎇⎇approach angle for the same position and orientation vector, 832⎇⎇limited by the support plane. This is solved for 833⎇⎇approximately, assuming the worst case condition of link 5 834⎇⎇See Figure 4.2. 835⎇⎇This range of approach angle is then intersected with the 836⎇⎇possible range of approach angle defined by the arm to give 837⎇⎇a range of approach angle throughout which the arm can reach 838⎇⎇the object considering both the arm and the support. 839⎇⎇This process is repeated for the support post of the arm. A 840⎇⎇final intersection is made to keep the hand from 841⎇⎇intersecting the post, and the limiting range of approach 842⎇⎇angle is obtained. 843⎇⎇4.2 MOVE INSTANCE 844⎇⎇The first action of MOVE␈π_INSTANCE is to select those 845⎇⎇orientation vectors of the prototype ( see section 2.2) 846⎇⎇which can be used to grasp the object. To do this we 847⎇⎇transform a gravity vector back to the prototype using the 848⎇⎇inverse transform and determine through which face it 849⎇⎇passes. This is then the support face. Any orientation 850⎇⎇vector, (Equation 2.9) which has a contact point (Equation 851⎇⎇2.10) on this face or on any edge or vertex of this face is 852⎇⎇then discarded; the remaining orientation vectors are marked 853⎇⎇as possible. This procedure is performed for the object in 854⎇⎇its initial position and again in its final position. If 855⎇⎇the action is to be accomplished in one move then the set of 856⎇⎇available orientation vectors that can be used is the 857⎇⎇intersection of the two sets of orientation vectors for the 858⎇⎇initial and final positions. These orientation vectors are 859⎇⎇then ordered by length; the shortest, representing face - 860⎇⎇face contacts, are considered first, as this is the most 861⎇⎇stable way that an object may be picked-up. 862⎇⎇For each of the orientation vectors so ordered the range of 863⎇⎇approach is calculated at both the initial and final 864⎇⎇positions. By transforming the reference approach vector 865⎇⎇(Equation 4.1) at the initial position to the final position 866⎇⎇by: 867⎇⎇␈→60⎇0⎇⎇RAi␈-3⎇␈π_␈π_␈π_' ← | Tf | * | Ti |␈↑-1␈↓ * | RAi |␈←␈→60⎇-1000⎇⎇[Eq. 4.3]␈← 868⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.2 MOVE␈← 869⎇⎇␈→60⎇0⎇⎇16467⎇␈←␈→60⎇-1000⎇⎇␈← 870⎇⎇the shift S, between approach ranges may then be calculated 871⎇⎇as: 872⎇⎇␈→60⎇0⎇⎇S ← the angle between RAi␈-3⎇␈π_␈π_␈π_' and RAf␈-3⎇␈π_␈π_␈π_ about Of␈-2⎇␈π_␈π_␈← 873⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 4.4]␈← 874⎇⎇After applying the shift S to the initial range the two 875⎇⎇ranges of approach are then intersected. If the 876⎇⎇intersection is not empty then the solution has been found. 877⎇⎇An approach vector A␈-1⎇␈π_ is picked within the range of 878⎇⎇intersection, vertically down if this is within the range, 879⎇⎇and two arm solutions are obtained, one at the initial 880⎇⎇position and one at the final position, such that the shift 881⎇⎇between approach vectors is maintained. 882⎇⎇If there is no intersection between the first two ranges, 883⎇⎇the next orientation vector in the intersection of the 884⎇⎇available set of orientation vectors is tried. 885⎇⎇When the set of orientation vectors is empty at either the 886⎇⎇initial or final positions, then there is no way that the 887⎇⎇move can be accomplished as the arm cannot reach the object. 888⎇⎇If the intersection of the set of orientation vectors was 889⎇⎇empty, or if after evaluating the ranges for all the 890⎇⎇orientation vectors the intersection of the ranges was empty 891⎇⎇then an intermediate position would be tried. 892⎇⎇An attempt is made to move the object from its initial 893⎇⎇position to an intermediate position and then from the 894⎇⎇intermediate position to the final position. The calling 895⎇⎇program specifies whether an intermediate position is to be 896⎇⎇tried and if so where. A clear space in front of the arm 897⎇⎇will give the greatest range of approach, however choosing 898⎇⎇the initial of final position as the intermediate position 899⎇⎇ensures that it is clear, and the strategy subsystem does 900⎇⎇not have to find a clear space. 901⎇⎇Even when the initial position is used as the intermediate 902⎇⎇position, the move is usually accomplished, as it is only 903⎇⎇the position and not the orientation that is specified. On 904⎇⎇most occasions when the arm is unable to carry out a task in 905⎇⎇one move, it is because the object must be turned over.It is 906⎇⎇then necessary for the arm to turn the object part-way over, 907⎇⎇put it down, and pick it up again to complete the move. 908⎇⎇Given the new intermediate position, a set of possible new 909⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.2 MOVE␈← 910⎇⎇␈→60⎇0⎇⎇16468⎇␈←␈→60⎇-1000⎇⎇␈← 911⎇⎇support faces is determined as the intersection of the set 912⎇⎇of faces which are neighbors to the original support face 913⎇⎇and to the final support face. A transformation is 914⎇⎇constructed for this intermediate position to bring the new 915⎇⎇support face parallel to the original support surface. The 916⎇⎇object is then turned about a normal axis to the support 917⎇⎇such that its principal axes are perpendicular to the 918⎇⎇shoulder of the arm, as this gives the maximum freedom of 919⎇⎇approach range. 920⎇⎇The height of the center of the object is adjusted if the 921⎇⎇distance from the center of mass to the original support 922⎇⎇face is different from the present distance. 923⎇⎇The process is now repeated to find a common approach. First 924⎇⎇a move solution to the intermediate position is computed and 925⎇⎇then, if successful, a move solution from the intermediate 926⎇⎇position to the final is attempted . Ranges are saved during 927⎇⎇the "initial - final" move attempt as they are needed in the 928⎇⎇"initial - intermediate", "intermediate - final" move 929⎇⎇evaluations. 930⎇⎇By this procedure it is possible to make any re-positionings 931⎇⎇and re-orientations, even when the goal must be accomplished 932⎇⎇in two moves. 933⎇⎇About 4 seconds are required to compute the arm positions 934⎇⎇when two moves must be used. 935⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 936⎇⎇␈→60⎇0⎇⎇16469⎇␈←␈→60⎇-1000⎇⎇␈← 937⎇⎇␈→60⎇0⎇⎇5.0 TRAJECTORIES␈← 938⎇⎇5.1 GENERAL CONSIDERATIONS 939⎇⎇In moving the arm we have two positions, the initial and 940⎇⎇final. The discussion until now has emphasized these 941⎇⎇positions and their determination (Subsection 4.2). In this 942⎇⎇section we will describe the move in detail. 943⎇⎇The simplest solution is to move the joints independently 944⎇⎇from their initial position to their final position, using a 945⎇⎇simple velocity-damped position servo. Consider the 946⎇⎇situation shown in figure Figure 5.1. The motion is mostly 947⎇⎇in joint 5; if all the joints were moved to their final 948⎇⎇positions then the hand would move through the support. 949⎇⎇What is needed is to lift the arm up and down as joint 5 is 950⎇⎇moved, in order to clear the support. Two points should be 951⎇⎇made: 1)some joints cannot move directly from their initial 952⎇⎇to their final position; 2) the motion of all joints should 953⎇⎇be coordinated. 954⎇⎇When the arm starts to move, it is normally working with 955⎇⎇respect to some surface, for instance, picking up a block 956⎇⎇from a table. As it starts to move the motion of the hand 957⎇⎇should be directly away from the surface. 958⎇⎇If we were to specify a position on a normal to the surface 959⎇⎇out from the initial position, and then to require that the 960⎇⎇hand pass through this position, we would achieve the 961⎇⎇correct departure motion. If we could further specify the 962⎇⎇time required to reach this position, we could control the 963⎇⎇speed at which the block was to be lifted. 964⎇⎇To estimate how far this position should be from the 965⎇⎇surface, consider Figure 5.2, as this represents the worst 966⎇⎇case. If the hand were lifted by the maximum surface 967⎇⎇penetration, approximately 25% the length of the last link, 968⎇⎇the collision would have been avoided. 969⎇⎇For such an initial move, the differential change of joint 970⎇⎇angles is calculated (Subsection 3.3) for a move in the 971⎇⎇direction of the outward pointing normal of 3 inches. A 972⎇⎇time based on a fairly low arm force is then calculated. The 973⎇⎇same set of requirements exists in the case of the final 974⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.1 GENERAL␈← 975⎇⎇␈→60⎇0⎇⎇16474⎇␈←␈→60⎇-1000⎇⎇␈← 976⎇⎇position. Here we wish once again to approach the surface in 977⎇⎇the direction of the normal. The time is increased to 978⎇⎇ensure a smooth setdown. 979⎇⎇We now have 4 positions: initial,liftoff,letdown, and final. 980⎇⎇At present the arm works in a clear space and the "collision 981⎇⎇avoider" has not been implemented. If it were we might have 982⎇⎇additional points through which we would require the arm to 983⎇⎇pass. If we were to servo the arm from one position to the 984⎇⎇next we would not collide with the support (see Figure 5.3). 985⎇⎇We would, however, like the arm to start and end its motion 986⎇⎇with zero velocity and acceleration, Further there is no 987⎇⎇need to stop the arm at all the intermediate positions. All 988⎇⎇that we require is that the joints of the arm pass through 989⎇⎇the trajectory points corresponding to these intermediate 990⎇⎇positions at the same time. 991⎇⎇By estimating the time for each joint to move through the 992⎇⎇segments of its motion and specifying the same maximum 993⎇⎇segment time for all joints, we can determine a polynomial 994⎇⎇for each joint, which passes through all the points and has 995⎇⎇zero initial and final velocity and acceleration. 996⎇⎇Although such trajectories satisfy our conditions, they 997⎇⎇often have extrema between the initial and final points. We 998⎇⎇must evaluate the joint variable at each extremum to check 999⎇⎇that it has not exceeded the working range of the joint. 1000⎇⎇As the extrema are difficult to evaluate for high order 1001⎇⎇polynomials, we use a different approach. We specify three 1002⎇⎇polynomials, one for the trajectory from the initial point 1003⎇⎇to the liftoff point. A second trajectory from the liftoff 1004⎇⎇to the setdown point, and a third from the setdown to the 1005⎇⎇final point. We specify that velocity and acceleration 1006⎇⎇should be zero at the initial and final points and that they 1007⎇⎇should be continuous at the intermediate points. This 1008⎇⎇sequence of polynomials satisfies our conditions for a 1009⎇⎇trajectory, and has extrema which are easily evaluated. 1010⎇⎇If a joint exceeds its working range at an extremum, then 1011⎇⎇the trajectory segment in which it occurs is split in two, 1012⎇⎇and a new intermediate point equal to the joint range limit 1013⎇⎇is specified at the break. (See Figure 5.4). 1014⎇⎇If an extremum occurs in the initial or final segments, 1015⎇⎇which corresponds to an overshoot of the initial or final 1016⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.1 GENERAL␈← 1017⎇⎇␈→60⎇0⎇⎇16478⎇␈←␈→60⎇-1000⎇⎇␈← 1018⎇⎇position (see Figure 5.5), then the liftoff point is added 1019⎇⎇as a new intermediate point through which the trajectory 1020⎇⎇should pass at a time after liftoff equal to the time 1021⎇⎇interval between the extremum and the liftoff (see Figure 1022⎇⎇5.5). 1023⎇⎇We have another type of trajectory that we wish to be able 1024⎇⎇to compute; that is to move the arm along a well defined 1025⎇⎇space curve. Here we obtain a sequence of joint angles at 1026⎇⎇equal time intervals and such that the velocity along the 1027⎇⎇space curve is controlled by relating distance along the 1028⎇⎇curve to time between points (See primitive DRAW Subsection 1029⎇⎇397⎇). Here we do not have any difficulties with extrema, 1030⎇⎇as was the case in the previous problem, as the space curve 1031⎇⎇is well defined. This type of curve leads to a trajectory 1032⎇⎇with many points, currently 8. If we were to use a single 1033⎇⎇polynomial it would need to be 10th order, for this reason 1034⎇⎇here also the sequence of low order polynomials is 1035⎇⎇preferred. 1036⎇⎇5.2 POLYNOMIALS 1037⎇⎇For each trajectory segment we have position, velocity and 1038⎇⎇acceleration constraints at each end. Except at the 1039⎇⎇beginning and end of the trajectory the velocity and 1040⎇⎇acceleration constraints are continuity constraints. There 1041⎇⎇are only four constraints on the intermediate trajectory 1042⎇⎇segments and five constraints at the ends. Thus for the 1043⎇⎇first and last trajectory segments a fourth order polynomial 1044⎇⎇will suffice and for the intermediate trajectory segments a 1045⎇⎇third order polynomial will be needed. 1046⎇⎇Consider a trajectory segment described by: 1047⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai4*t␈↑4␈↓ + Ai3*t␈↑3␈↓ + Ai2*t␈↑2␈↓ + Ai1*t +Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.1]␈← 1048⎇⎇where 1049⎇⎇␈→60⎇0⎇⎇t = t/T1␈←␈→60⎇-1000⎇⎇[Eq. 5.2]␈← 1050⎇⎇where Aij is the coefficient of the jth. power of the ith. 1051⎇⎇trajectory segment. and time is normalized to unity at the 1052⎇⎇end of the trajectory segment of duration Ti. 1053⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈← 1054⎇⎇␈→60⎇0⎇⎇16480⎇␈←␈→60⎇-1000⎇⎇␈← 1055⎇⎇For the first trajectory segment at time t=0: 1056⎇⎇␈→60⎇0⎇⎇O␈-1⎇-0 = A10␈←␈→60⎇-1000⎇⎇[Eq. 5.3]␈← 1057⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓0 = 0 =A11␈←␈→60⎇-1000⎇⎇[Eq. 5.4]␈← 1058⎇⎇␈→60⎇0⎇⎇O␈-1⎇-0␈-2⎇␈↑..␈↓ = 0 =A12␈←␈→60⎇-1000⎇⎇[Eq. 5.5]␈← 1059⎇⎇and at time t=1: 1060⎇⎇␈→60⎇0⎇⎇O␈-1⎇-1 - O␈-1⎇-0 = ~␈-1⎇VO␈-1⎇-1 = A14 + A13␈←␈→60⎇-1000⎇⎇[Eq. 5.6]␈← 1061⎇⎇␈→60⎇0⎇⎇T1*O␈-1⎇-␈-1⎇␈↑.␈↓1 = 4*A14 + 3*A13␈←␈→60⎇-1000⎇⎇[Eq. 5.7]␈← 1062⎇⎇␈→60⎇0⎇⎇T1␈↑2␈↓*O␈-1⎇-1␈-2⎇␈↑..␈↓ = 12*A14 + 6*A13␈←␈→60⎇-1000⎇⎇[Eq. 5.8]␈← 1063⎇⎇for the last trajectory segment we substitute: 1064⎇⎇␈→60⎇0⎇⎇t' = t - 1␈←␈→60⎇-1000⎇⎇[Eq. 5.9]␈← 1065⎇⎇and at time t'=0: 1066⎇⎇␈→60⎇0⎇⎇O␈-1⎇-N = AN0␈←␈→60⎇-1000⎇⎇[Eq. 5.10]␈← 1067⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓n = 0 =An1␈←␈→60⎇-1000⎇⎇[Eq. 5.11]␈← 1068⎇⎇␈→60⎇0⎇⎇O␈-1⎇-n␈-2⎇␈↑..␈↓ = 0 =An2␈←␈→60⎇-1000⎇⎇[Eq. 5.12]␈← 1069⎇⎇and at time t'=-1: 1070⎇⎇␈→60⎇0⎇⎇O␈-1⎇-n - O␈-1⎇-(n-1) = ~␈-1⎇VO␈-1⎇-n = -An4 + An3␈←␈→60⎇-1000⎇⎇[Eq. 5.13]␈← 1071⎇⎇␈→60⎇0⎇⎇Tn*O␈-1⎇-␈-1⎇␈↑.␈↓(n-1) = -4*An4 + 3*An3␈←␈→60⎇-1000⎇⎇[Eq. 5.14]␈← 1072⎇⎇␈→60⎇0⎇⎇Tn␈↑2␈↓*O␈-1⎇-(n-1)␈-6⎇␈↑.. ␈↓ = 12*An4 - 6*An3␈←␈→60⎇-1000⎇⎇[Eq. 5.15]␈← 1073⎇⎇For the general ith. trajectory segment we have 1074⎇⎇␈→60⎇0⎇⎇0 = Ai4␈←␈→60⎇-1000⎇⎇[Eq. 5.16]␈← 1075⎇⎇␈→60⎇0⎇⎇O␈-1⎇-(i-1) = Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.17]␈← 1076⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓(i-1) = Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.18]␈← 1077⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i␈-2⎇␈↑..␈↓(-1) = Ai2␈←␈→60⎇-1000⎇⎇[Eq. 5.19]␈← 1078⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i - O␈-1⎇-(i-1) = ~␈-1⎇VO␈-1⎇-i = Ai3 + Ai2 + Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.20]␈← 1079⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈← 1080⎇⎇␈→60⎇0⎇⎇16481⎇␈←␈→60⎇-1000⎇⎇␈← 1081⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓i = 3*Ai3 + 2*Ai2 +Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.21]␈← 1082⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i␈-2⎇␈↑..␈↓ = 6*Ai3 + 2*Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.22]␈← 1083⎇⎇Equations 5.3, 5.4, 5.5, 5.10, 5.11, 5.12, and Equations 1084⎇⎇5.17 specify coefficients directly. The remaining 1085⎇⎇coefficients may be solved in the form: 1086⎇⎇|COEFF| = 1087⎇⎇␈→60⎇0⎇⎇| |␈← 1088⎇⎇␈→60⎇0⎇⎇| 1 1 |␈← 1089⎇⎇␈→60⎇0⎇⎇| ␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_ |␈← 1090⎇⎇␈→60⎇0⎇⎇|3/T1 4/T1 |-1/T2 | |␈← 1091⎇⎇␈→60⎇0⎇⎇| 2 2| 2 | |␈← 1092⎇⎇␈→60⎇0⎇⎇|6/T1 12/T1 | 0 -2/T2 | |␈← 1093⎇⎇␈→60⎇0⎇⎇| | | |␈← 1094⎇⎇␈→60⎇0⎇⎇| | 1 1 1 | |␈← 1095⎇⎇␈→60⎇0⎇⎇| | |␈π_␈π_␈π_␈π_␈π_␈π_␈π_ |␈← 1096⎇⎇␈→60⎇0⎇⎇| | 1/T2 2/T2 3/T2 | | |␈← 1097⎇⎇␈→60⎇0⎇⎇| | 2 2| | |␈← 1098⎇⎇␈→60⎇0⎇⎇| | 2/T2 6/T2 | | |␈← 1099⎇⎇␈→60⎇0⎇⎇| |␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_| " " | |␈← 1100⎇⎇␈→60⎇0⎇⎇| | | |␈← 1101⎇⎇␈→60⎇0⎇⎇| | | |␈← 1102⎇⎇␈→60⎇0⎇⎇| | | -3/Tn 4/Tn |␈← 1103⎇⎇␈→60⎇0⎇⎇| | | 2 2|␈← 1104⎇⎇␈→60⎇0⎇⎇| |␈π_␈π_␈π_␈π_␈π_␈π_␈π_| 6/Tn -12/Tn |␈← 1105⎇⎇␈→60⎇0⎇⎇| |␈← 1106⎇⎇␈→60⎇0⎇⎇| 1 -1 |␈← 1107⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.23]␈← 1108⎇⎇ | ~␈-1⎇VO␈-1⎇-1 | | A13 | 1109⎇⎇ |␈π_␈π_0␈π_␈π_| |␈π_A14␈π_| 1110⎇⎇ | 0 | | A21 | 1111⎇⎇ | ~␈-1⎇VO␈-1⎇-2 | | A22 | 1112⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈← 1113⎇⎇␈→60⎇0⎇⎇16482⎇␈←␈→60⎇-1000⎇⎇␈← 1114⎇⎇ |␈π_␈π_0␈π_␈π_| = | COEFF | * |␈π_A23␈π_|␈→60⎇-1000⎇⎇[Eq. 5.24]␈← 1115⎇⎇ | | | | 1116⎇⎇ | " " | | " " | 1117⎇⎇ |␈π_␈π_␈π_␈π_␈π_| |␈π_␈π_␈π_␈π_␈π_| 1118⎇⎇ | 0 | | An3 | 1119⎇⎇ | ~␈-1⎇VO␈-1⎇-4 | | An4 | 1120⎇⎇where the blocks indicated in Equation 5.23 and Equation 1121⎇⎇5.24 may be repeated for each additional point that the 1122⎇⎇trajectory must pass through. 1123⎇⎇In the last trajectory segment we solved in terms of t'. As 1124⎇⎇we wish to evaluate all the trajectories similarly we 1125⎇⎇substitute: 1126⎇⎇␈→60⎇0⎇⎇t = t' + 1␈←␈→60⎇-1000⎇⎇[Eq. 5.25]␈← 1127⎇⎇we have: 1128⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = A4n*t'␈↑4␈↓ + A3n*t'␈↑3␈↓ + A2n*t'␈↑2␈↓ + A1n*t' + A0n␈← 1129⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.26]␈← 1130⎇⎇substituting for t' in Equation 5.26 from Equation 5.25 we 1131⎇⎇obtain: 1132⎇⎇ O␈-1⎇- = A4n*t␈↑4␈↓ 1133⎇⎇ +(-4*A4n + A3n)*t␈↑3␈↓ 1134⎇⎇ +(6*A4n -3*A3n +A2n)*t␈↑2␈↓ 1135⎇⎇ +(-4*A4n +3*A3n -2*A2n +A1n)*t 1136⎇⎇ +(A4n -A3n +A2n -A1n +A0n) 1137⎇⎇␈→60⎇-1000⎇⎇[Eq. 5.27]␈← 1138⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈← 1139⎇⎇␈→60⎇0⎇⎇16483⎇␈←␈→60⎇-1000⎇⎇␈← 1140⎇⎇from which we can obtain the coefficients of t. 1141⎇⎇5.3 TRAJECTORY EXTREMA 1142⎇⎇Intermediate trajectory segments are described by third 1143⎇⎇order polynomials, one such polynomial for each joint: 1144⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai3*t␈↑3␈↓ + Ai2*t␈↑2␈↓ + Ai1*t +Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.28]␈← 1145⎇⎇the derivative is: 1146⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓ = 3*Ai3*t␈↑2␈↓ + 2*Ai2*t + Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.29]␈← 1147⎇⎇The times of the extrema are given as the roots of Equation 1148⎇⎇5.29: 1149⎇⎇␈→60⎇0⎇⎇t = -1/3 Ai2/Ai3 ␈π+ {(1/3 Ai2/A13)␈↑2␈↓ - 1/3 Ai1/Ai3}␈↑1/2␈↓␈← 1150⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.30]␈← 1151⎇⎇If the discriminant is positive and 0 < t < 1 then an 1152⎇⎇extremum exists and it can be evaluated by Equation 5.28. 1153⎇⎇In the case of the initial and final trajectory segments the 1154⎇⎇trajectories are described by fourth order polynomials 1155⎇⎇(Equation 5.1) with the low order terms missing (see 1156⎇⎇Equation 5.4, 5.5, 5.11, and 5.12). 1157⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai4*t␈↑4␈↓ + Ai3*t␈↑3␈↓ + Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.31]␈← 1158⎇⎇and the derivative by: 1159⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓ = 4*Ai4*t␈↑3␈↓ + 3*Ai3*t␈↑2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 5.32]␈← 1160⎇⎇The time of the extremum is given as the roots of Equation 1161⎇⎇5.32: 1162⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.3 EXTREMA␈← 1163⎇⎇␈→60⎇0⎇⎇16485⎇␈←␈→60⎇-1000⎇⎇␈← 1164⎇⎇␈→60⎇0⎇⎇t = -3/4 Ai3/Ai4␈←␈→60⎇-1000⎇⎇[Eq. 5.33]␈← 1165⎇⎇if 0 < t < 1 then an extremum exists and the value of O␈-1⎇- is: 1166⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = -1/4 Ai3*t␈↑3␈↓␈←␈→60⎇-1000⎇⎇[Eq. 5.34]␈← 1167⎇⎇If the value of O␈-1⎇- at the extremum causes an appreciable 1168⎇⎇overshoot. then the trajectory is required to pass through 1169⎇⎇the liftoff/set-down point once more Figure 5.5). 1170⎇⎇5.4 CONTINUOUS MOTION 1171⎇⎇In Subsection 5.1 we mentioned trajectories along space 1172⎇⎇curves, where arm solutions are obtained at regular 1173⎇⎇intervals along the curve and then a trajectory is planned 1174⎇⎇through these positions. Such trajectories are not checked 1175⎇⎇for overshoot of physical limits as they are more 1176⎇⎇continuous. There is a special case of these trajectories 1177⎇⎇in which the first and last arm positions are the same. (see 1178⎇⎇Figure 5.6). Here we have a circular arm motion such as 1179⎇⎇turning a crank, the position of the hand is described by an 1180⎇⎇angle; the orientation remains the same during the motion. 1181⎇⎇Solutions are obtained every 60 degrees. In the case of the 1182⎇⎇beginning and end, two additional positions are specified, 1183⎇⎇one at 20 and one at 340 degrees respectively. The arm is 1184⎇⎇given the same period of time for each of the 8 segments, 1185⎇⎇and thus the arm will accelerate from rest, move around the 1186⎇⎇circle and come back to rest. The first and last small 1187⎇⎇segments ensure the smooth departure and arrival. 1188⎇⎇For continuous motion we provide an alternate path from 1189⎇⎇position 6 through position 0 to position 2 (see Figure 5.6) 1190⎇⎇requiring continuity of both velocity and acceleration at 1191⎇⎇these points. The arm is caused to rotate continuously 1192⎇⎇having first been started from point 0 by causing it to move 1193⎇⎇along the alternate trajectory at point 6. When it is 1194⎇⎇desired to stop it is not be diverted at the "switch" at 1195⎇⎇position 6 but allowed to stop by moving through position 7 1196⎇⎇to 8. 1197⎇⎇In the following matrix equation the x's represent non zero 1198⎇⎇elements of Equation 5.23 Equation 131⎇ is for the simple 1199⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.4 LOOPING␈← 1200⎇⎇␈→60⎇0⎇⎇16488⎇␈←␈→60⎇-1000⎇⎇␈← 1201⎇⎇case and Equation 369⎇ is in the case of looping providing 1202⎇⎇for the solution of two additional fourth order trajectory 1203⎇⎇segments 9 and 10. 1204⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-1 | | XX | | A13 |␈← 1205⎇⎇␈→60⎇0⎇⎇| 0 | | XXX | | A14 |␈← 1206⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A21 |␈← 1207⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-2 | | XXX | | A22 |␈← 1208⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A23 |␈← 1209⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A31 |␈← 1210⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-3 | | XXX | | A32 |␈← 1211⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A33 |␈← 1212⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A41 |␈← 1213⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-4 | = | XXX | * | A42 |␈← 1214⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A43 |␈← 1215⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A51 |␈← 1216⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-5 | | XXX | | A52 |␈← 1217⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A53 |␈← 1218⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A61 |␈← 1219⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-6 | | XXX | | A62 |␈← 1220⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A63 |␈← 1221⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A71 |␈← 1222⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-7 | | XXX | | A72 |␈← 1223⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A73 |␈← 1224⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A83 |␈← 1225⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-8 | | XX | | A84 |␈← 1226⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.35]␈← 1227⎇⎇and in the case of looping, the blocks of elements marked as 1228⎇⎇Y's and Z's are the same. 1229⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-1 | | XX | | A13 |␈← 1230⎇⎇␈→60⎇0⎇⎇| 0 | | XXX | | A14 |␈← 1231⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A21 |␈← 1232⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-2 | | XXX | | A22 |␈← 1233⎇⎇␈→60⎇0⎇⎇| 0 | | XXXY | | A23 |␈← 1234⎇⎇␈→60⎇0⎇⎇| 0 | | XX Y | | A31 |␈← 1235⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-3 | | XXX | | A32 |␈← 1236⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A33 |␈← 1237⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A41 |␈← 1238⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-4 | | XXX | | A42 |␈← 1239⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A43 |␈← 1240⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A51 |␈← 1241⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-5 | = | XXX | * | A52 |␈← 1242⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A53 |␈← 1243⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A61 |␈← 1244⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.4 LOOPING␈← 1245⎇⎇␈→60⎇0⎇⎇16489⎇␈←␈→60⎇-1000⎇⎇␈← 1246⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-6 | | XXX | | A62 |␈← 1247⎇⎇␈→60⎇0⎇⎇| 0 | | ZZZX | | A63 |␈← 1248⎇⎇␈→60⎇0⎇⎇| 0 | | ZZ X | | A71 |␈← 1249⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-7 | | XXX | | A72 |␈← 1250⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A73 |␈← 1251⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A83 |␈← 1252⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-8 | | XX | | A84 |␈← 1253⎇⎇␈→60⎇0⎇⎇| 0 | | ZZZ X | | A91 |␈← 1254⎇⎇␈→60⎇0⎇⎇| 0 | | ZZ X | | A92 |␈← 1255⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-9 | | XXXX | | A93 |␈← 1256⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A94 |␈← 1257⎇⎇␈→60⎇0⎇⎇| 0 | | XXX X | | A101|␈← 1258⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-10| | XXXX | | A102|␈← 1259⎇⎇␈→60⎇0⎇⎇| 0 | | Y XXXX | | A103|␈← 1260⎇⎇␈→60⎇0⎇⎇| 0 | | Y XXX | | A104|␈← 1261⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.36]␈← 1262⎇⎇Equation 5.36 is the same as Equation 5.35 except for the 1263⎇⎇addition of the last eight equations for segments 9 and 10. 1264⎇⎇These are fourth order polynomials due to the extra end 1265⎇⎇continuity constraints. The groups of non zero elements in 1266⎇⎇the lower triangle of Equation 5.36 relate to the continuity 1267⎇⎇requirements at points 2 and 6 (see Figure 5.6). 1268⎇⎇5.5 DIFFERENTIAL MOTION 1269⎇⎇The arm can also make differential motions where all six 1270⎇⎇joints are required to change a certain amount in a given 1271⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.5 DIFFERENTIAL␈← 1272⎇⎇␈→60⎇0⎇⎇16491⎇␈←␈→60⎇-1000⎇⎇␈← 1273⎇⎇time. In this case all the joints are driven together to 1274⎇⎇make the change in the specified time. The changes of joint 1275⎇⎇angles are determined as in Subsection 3.3. 1276⎇⎇Joint angle O␈-1⎇- as a function g(t) of normalized time t, for a 1277⎇⎇change of ~␈-1⎇VO␈-1⎇- is: 1278⎇⎇␈→60⎇0⎇⎇t = t/Tm␈←␈→60⎇-1000⎇⎇[Eq. 5.37]␈← 1279⎇⎇␈→60⎇0⎇⎇O␈-1⎇-=g(t)=~␈-1⎇VO␈-1⎇-*t␈↑3␈↓( 6t␈↑2␈↓ - 15t + 10)␈←␈→60⎇-1000⎇⎇[Eq. 5.38]␈← 1280⎇⎇This gives zero initial and final acceleration and velocity. 1281⎇⎇This type of motion has all the undesirable properties 1282⎇⎇described in Subsection 5.1 and is only suitable when small 1283⎇⎇changes are to be made, as in correcting the hand position 1284⎇⎇during visual servoing. Such motions are of course much 1285⎇⎇simpler to plan than regular trajectory controlled motion. 1286⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 1287⎇⎇␈→60⎇0⎇⎇16492⎇␈←␈→60⎇-1000⎇⎇␈← 1288⎇⎇␈→60⎇0⎇⎇6.0 SERVO␈← 1289⎇⎇In this section we relate position error to joint torque in 1290⎇⎇the arm servo. We then discuss the model of the joint drive 1291⎇⎇in order to convert joint torque into motor drive. In the 1292⎇⎇final section provision for degrees of freedom of the arm as 1293⎇⎇it is servoed is considered. 1294⎇⎇6.1 FEEDBACK LOOP 1295⎇⎇In this section we will describe the servo response. We 1296⎇⎇will treat the system as continuous, ignoring the effects of 1297⎇⎇sampling, based on the assumption that the sampling period 1298⎇⎇is much less than the response time of the arm. At the end 1299⎇⎇of the section we will ckeck that this assumption is valid. 1300⎇⎇Time is normalized to the sampling period which has the 1301⎇⎇effect of scaling the link inertia up by f↑2 where f is the 1302⎇⎇sampling frequency. The Laplace transform is used 1303⎇⎇throughout represented by F(s). 1304⎇⎇The set point for each joint of the arm is obtained by 1305⎇⎇evaluating the appropriate part trajectory polynomial for 1306⎇⎇the required time. The velocity and acceleration could also 1307⎇⎇be evaluated as the first and second derivatives of the 1308⎇⎇polynomial but instead they are approximated by the first 1309⎇⎇and second differences as this saves time. 1310⎇⎇The position error is the observed position O␈-1⎇- less the 1311⎇⎇required value O␈-1⎇-s. Likewise the velocity error is the 1312⎇⎇observed velocity less the required velocity. Position 1313⎇⎇feedback is applied to decrease position error and velocity 1314⎇⎇feedback is used to provide damping. 1315⎇⎇The feedback loop is shown in Figure 6.1 The arm is 1316⎇⎇represented by 1/s␈↑2␈↓J, where J is the effective link inertia 1317⎇⎇a function of arm configuration, and T(s) is an external 1318⎇⎇disturbing torque. The set point R(s) is subtracted from 1319⎇⎇the current position to obtain the position error E(s) and 1320⎇⎇is multiplied by s to obtain the error velocity. There are 1321⎇⎇two feedback gains ke and kv, position and velocity 1322⎇⎇respectively. 1323⎇⎇By writing the loop equation we can obtain the system 1324⎇⎇response: 1325⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1326⎇⎇␈→60⎇0⎇⎇16496⎇␈←␈→60⎇-1000⎇⎇␈← 1327⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓J)/(s␈↑2␈↓J + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*T(s)␈← 1328⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.1]␈← 1329⎇⎇and the condition for critical damping is: 1330⎇⎇␈→60⎇0⎇⎇kv = 2(J*ke)␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 6.2]␈← 1331⎇⎇It can be seen that the system response is dependent on J as 1332⎇⎇would be expected. Because the effective link inertia J can 1333⎇⎇vary by 10:1 as the arm configuration changes, we are unable 1334⎇⎇to maintain a given response (see Equation 6.2) independent 1335⎇⎇of arm configuration. If however we add a gain of -J as 1336⎇⎇shown in Figure 6.2 then we obtain: 1337⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓)/(s␈↑2␈↓ + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*T(s)/J␈← 1338⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.3]␈← 1339⎇⎇and the condition for critical damping is: 1340⎇⎇␈→60⎇0⎇⎇kv = 2*(ke)␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 6.4]␈← 1341⎇⎇It can be seen that the servo response is now independent of 1342⎇⎇arm configuration. 1343⎇⎇The principal disturbing torque is that due to gravity, 1344⎇⎇causing a large position error, especially in the case of 1345⎇⎇joint 2. If we were able to add a term equal to the negative 1346⎇⎇of the gravity loading Tg (see Figure 6.3) then we would 1347⎇⎇obtain the same system response as in Equation 6.3 except 1348⎇⎇that T is now Te, the external disturbing torque, less the 1349⎇⎇gravity dependent torque, and reducing the position error. 1350⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓)/(s␈↑2␈↓ + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*Te(s)/J␈← 1351⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.5]␈← 1352⎇⎇We can compensate for the effect of acceleration of the set 1353⎇⎇point R(s), the first term in Equation 6.5, if we add a term 1354⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1355⎇⎇␈→60⎇0⎇⎇16500⎇␈←␈→60⎇-1000⎇⎇␈← 1356⎇⎇s␈↑2␈↓R(s) (seeFigure 6.4) and obtain a system response: 1357⎇⎇␈→60⎇0⎇⎇E(s)= 1/(s␈↑2␈↓ + skv + ke)*T(s)/J␈←␈→60⎇-1000⎇⎇[Eq. 6.6]␈← 1358⎇⎇The gain of -J and the torque Tg are obtained from Equation 1359⎇⎇3.33 which we restate here: 1360⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓j=i␈↑[~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓*qk␈-2⎇␈↑..␈↓) 1361⎇⎇ +~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓l=1␈↑Trace(| Ujkl|*| Hj|*| Uji|␈↑T␈↓*q␈-1⎇␈↑.␈↓j*q␈-1⎇␈↑.␈↓k) 1362⎇⎇ -mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq 3.33]␈← 1363⎇⎇What is desired is to obtain an expression of the form: 1364⎇⎇␈→60⎇0⎇⎇ Fi = Ji * qi␈-2⎇␈↑..␈↓ + Tg␈←␈→60⎇-1000⎇⎇[Eq. 6.7]␈← 1365⎇⎇given an arm configuration qi. 1366⎇⎇The velocity dependent terms of Equation 3.33 are only 1367⎇⎇significant at high speed and small for the arm we are 1368⎇⎇using. We will ignore the second term of Equation 3.33, 1369⎇⎇although it could be included with the third term as the 1370⎇⎇values of q␈-1⎇␈↑.␈↓j and q␈-1⎇␈↑.␈↓k are known from the trajectory. 1371⎇⎇We may interchange the order of summation of Equation 3.33 1372⎇⎇to obtain: 1373⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Ujk|*|Hj|*|Uji|␈↑T␈↓)*qk␈-2⎇␈↑..␈↓ 1374⎇⎇ -~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq. 6.8]␈← 1375⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1376⎇⎇␈→60⎇0⎇⎇16501⎇␈←␈→60⎇-1000⎇⎇␈← 1377⎇⎇or 1378⎇⎇␈→60⎇0⎇⎇Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓k=1␈↑ Cik qk␈-2⎇␈↑..␈↓ + Ci␈←␈→60⎇-1000⎇⎇[Eq. 6.9]␈← 1379⎇⎇where 1380⎇⎇␈→60⎇0⎇⎇ Cik= ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 6.10]␈← 1381⎇⎇and 1382⎇⎇␈→60⎇0⎇⎇ Ci = -~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈←␈→60⎇-1000⎇⎇[Eq. 6.11]␈← 1383⎇⎇One further simplification is that we may disregard the 1384⎇⎇terms of Equation 6.9 in which i≠k as they are very small, 1385⎇⎇to obtain: 1386⎇⎇␈→60⎇0⎇⎇ Fi = Ji * qi␈-2⎇␈↑..␈↓ + Tg␈←␈→60⎇-1000⎇⎇[Eq. 6.12]␈← 1387⎇⎇where 1388⎇⎇␈→60⎇0⎇⎇ Ji = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Uji|*| Hj|*| Uji|␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 6.13]␈← 1389⎇⎇and Tg as Ci in Equation 6.11 1390⎇⎇Ji is the effective joint inertia and Tg is the constant 1391⎇⎇term due to gravity. 1392⎇⎇The servo is compensated for system response, under varying 1393⎇⎇arm configuration, for gravity loading and for the 1394⎇⎇acceleration of the set point r. 1395⎇⎇Three plots of position error v. time for joint 2 are shown: 1396⎇⎇Figure 6.5, Figure 6.6, and Figure 6.7. 1397⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1398⎇⎇␈→60⎇0⎇⎇16505⎇␈←␈→60⎇-1000⎇⎇␈← 1399⎇⎇In Figure 6.5 there is no gravity or feedback compensation. 1400⎇⎇In Figure 6.6 the gravity term is added, and finally in 1401⎇⎇Figure 6.7 the acceleration compensation is included. 1402⎇⎇The steady state error for a given error torque Te is: 1403⎇⎇␈→60⎇0⎇⎇Ess = Te/( J * ke )␈←␈→60⎇-1000⎇⎇[Eq. 6.14]␈← 1404⎇⎇We would like the arm to be fairly stiff when it is servoing 1405⎇⎇and a value of 50 oz/in is the minimum acceptable. if a 1406⎇⎇lesser value is used the arm moves very sloppily. For this 1407⎇⎇value of stiffness the values of ke can be calculated by 1408⎇⎇means of Equation 6.14. Representative values of kv based on 1409⎇⎇critical damping may then be calculated (Equation 6.4) and 1410⎇⎇this will give us the lower limits on ke. and kv. 1411⎇⎇There is, however, some noise in position measurement and if 1412⎇⎇we are to obtain velocity by differencing observed position 1413⎇⎇readings then the same noise is in the velocity 1414⎇⎇determination. The primary source of noise is the 1415⎇⎇quantizing noise of the analog to digital converter used to 1416⎇⎇determine position. The A/D converter is a 12 bit device and 1417⎇⎇thus has a relative error of 1/4096. The equivalent noise 1418⎇⎇torque at the joint is given by: 1419⎇⎇␈→60⎇0⎇⎇Tn = k * J * e␈←␈→60⎇-1000⎇⎇[Eq. 6.15]␈← 1420⎇⎇where k is either ke if the velocity is determined 1421⎇⎇separately or ke+kv if the velocity is determined by 1422⎇⎇differencing, and e is the quantizing noise. A noise torque 1423⎇⎇of approximately F0/2 (see Subsection 202⎇) seems to be 1424⎇⎇acceptable and this gives an upper bound on k. 1425⎇⎇Based on this information we can evaluate Equations 6.14, 1426⎇⎇6.4 and 6.15 for each joint: 1427⎇⎇␈→60⎇0⎇⎇JOINT ke␈-2⎇␈π_␈π_ kv␈-2⎇␈π_␈π_ k␈-1⎇␈↑_␈↓␈← 1428⎇⎇␈→60⎇0⎇⎇ 1 0.038 0.39 0.18␈← 1429⎇⎇␈→60⎇0⎇⎇ 2 0.032 0.36 0.33␈← 1430⎇⎇␈→60⎇0⎇⎇ 3 0.005 0.14 0.75␈← 1431⎇⎇␈→60⎇0⎇⎇ 4 0.06 0.49 0.49␈← 1432⎇⎇␈→60⎇0⎇⎇ 5 0.06 0.49 1.6␈← 1433⎇⎇␈→60⎇0⎇⎇ 6 0.06 0.49 50.0␈← 1434⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1435⎇⎇␈→60⎇0⎇⎇16506⎇␈←␈→60⎇-1000⎇⎇␈← 1436⎇⎇The second column is based on Equation 6.14 and the third on 1437⎇⎇Equation 6.4 These are the minimum acceptable gains. The 1438⎇⎇fourth column is from Equation 6.15 and represents the 1439⎇⎇maximum allowable gain if the information is to be obtained 1440⎇⎇by means of position measurement using the available A/D 1441⎇⎇converter. In the case of Joints 1 and 2 it can be seen 1442⎇⎇that an alternative means must be used to obtain velocity 1443⎇⎇information although the position information is acceptable 1444⎇⎇using the A/D converter. For these two joints a tachometer 1445⎇⎇generator is used to measure the velocity, which has much 1446⎇⎇lower noise than velocity obtained by differencing position 1447⎇⎇measurements. 1448⎇⎇We made an assumption at the beginning of this section that 1449⎇⎇the sampling frequency was much higher than the frequency 1450⎇⎇response of the arm. We can obtain the frequency response 1451⎇⎇from Equation 6.6 by finding the inverse transform. This is 1452⎇⎇found to be: 1453⎇⎇␈→60⎇0⎇⎇frequency = kv * f / 2␈←␈→60⎇-1000⎇⎇[Eq. 6.16]␈← 1454⎇⎇As maximum kv that we require is 0.49 this give us a 1455⎇⎇response of 1/4 the sampling frequency. 1456⎇⎇At the beginning and end of each trajectory and at the end 1457⎇⎇of each part trajectory, the values of Ji and Tg are 1458⎇⎇evaluated (Equation 6.12). They are then given to the servo 1459⎇⎇along with the part trajectory polynomial coefficients. As 1460⎇⎇the trajectory is executed the values of Ji and Tg are 1461⎇⎇linearly interpolated. 1462⎇⎇Although this gives an acceptable response from the point of 1463⎇⎇view of stiffness, the gain is too low to maintain the high 1464⎇⎇positional tolerance of ␈π-0.05 in, which we are just able to 1465⎇⎇measure using the 12 bit A/D converter. In order to achieve 1466⎇⎇this error tolerance the position error is integrated when 1467⎇⎇the arm has reached the end of its trajectory. When the 1468⎇⎇position error of a joint is within tolerance the brake for 1469⎇⎇that joint is applied and the joint is no longer servoed. 1470⎇⎇When all the joints are within the error tolerance the 1471⎇⎇trajectory has been executed. 1472⎇⎇If the arm is to move a heavy load, its predicted effects 1473⎇⎇may be taken into account by increasing the effective mass 1474⎇⎇and inertia of the last link of the arm before evaluating 1475⎇⎇Equation 6.12. Similarly if the hand is to exert a given 1476⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈← 1477⎇⎇␈→60⎇0⎇⎇16507⎇␈←␈→60⎇-1000⎇⎇␈← 1478⎇⎇force or moment then the equivalent arm torque (see 1479⎇⎇Subsection 3.5) is added to Tg. 1480⎇⎇6.2 MOTOR DRIVE 1481⎇⎇The output of the servo equation is a torque to be applied 1482⎇⎇at the joint. Each joint has an electric motor drive and a 1483⎇⎇harmonic drive gear reduction. The motors are driven by a 1484⎇⎇pulse-width modulated voltage signal. The output of the 1485⎇⎇computer is this pulse␈π_width and the polarity. The drive 1486⎇⎇module relates torque to drive voltage pulse-width. 1487⎇⎇The motors are driven by a 360 Hertz pulse-width modulated 1488⎇⎇voltage source. The program output "h" is the relative "on" 1489⎇⎇time of this signal. If we plot an experimental curve of "h" 1490⎇⎇v. joint torque we obtain two discontinuous curves depending 1491⎇⎇on the joint velocity. see Figure 6.8 1492⎇⎇This curve can be explained in terms of friction effects; 1493⎇⎇load dependent, causing the two curves to diverge, and load 1494⎇⎇independent, causing the separation at the two curves at the 1495⎇⎇origin. The electrical motor time constant also affects the 1496⎇⎇shape of the curve near the origin. Curves determined 1497⎇⎇experimentally are supplied to the servo program in the 1498⎇⎇following piecewise linear form, (see Figure 6.9) 1499⎇⎇where: 1500⎇⎇ V0 is the motor drive at which the joint will move 1501⎇⎇ at constant velocity exerting zero force in the 1502⎇⎇ direction of motion; 1503⎇⎇ F0 is the force that the joint will exert at drive 1504⎇⎇ level V0 but with a negative velocity; 1505⎇⎇ the slopes and slope differences are obtained from 1506⎇⎇ the experimental curves. 1507⎇⎇When the velocity is very low the direction of intended 1508⎇⎇motion is substituted for the velocity. 1509⎇⎇One other factor taken into account is the back emf of the 1510⎇⎇motor. The value of "h" is the ratio of required voltage to 1511⎇⎇supply voltage. The supply voltage is simply augmented by 1512⎇⎇the computed back emf before "h" is calculated. 1513⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.2 DRIVE␈← 1514⎇⎇␈→60⎇0⎇⎇16511⎇␈←␈→60⎇-1000⎇⎇␈← 1515⎇⎇When the velocity is non zero the output torque is 1516⎇⎇predictable but at zero velocity and with zero intended 1517⎇⎇motion the error in the output torque is half the horizontal 1518⎇⎇displacement between the two curves at the origin. this is 1519⎇⎇F0/2. The values for this error torque at a typical arm 1520⎇⎇configuration in terms of force at the hand are: 1521⎇⎇␈→60⎇0⎇⎇JOINT F0 oz.FORCE oz. ␈← 1522⎇⎇␈→60⎇0⎇⎇ 1 400 ␈π-10 ␈← 1523⎇⎇␈→60⎇0⎇⎇ 2 700 ␈π-20 ␈← 1524⎇⎇␈→60⎇0⎇⎇ 3 70 ␈π-35 ␈← 1525⎇⎇␈→60⎇0⎇⎇ 4 100 ␈π-5 ␈← 1526⎇⎇␈→60⎇0⎇⎇ 5 160 ␈π-8 ␈← 1527⎇⎇␈→60⎇0⎇⎇ 6 100 ␈← 1528⎇⎇It can be seen that the arm can exert forces with a typical 1529⎇⎇tolerance of ␈π-10 oz. 1530⎇⎇6.3 PARTIALLY CONSTRAINED MOTION 1531⎇⎇If we require the arm to exhibit a degree of freedom along a 1532⎇⎇given direction or about a given axis at the hand then we 1533⎇⎇select a joint to be "free" on the following basis. 1534⎇⎇We calculate the equivalent arm torque (Subsection 3.5) for 1535⎇⎇a unit force in the given direction or moment about the 1536⎇⎇given axis, and then normalize the torques by dividing 1537⎇⎇through by their respective F0 (Subsection 6.2). We then 1538⎇⎇select that joint which has the largest normalized torque to 1539⎇⎇be "free". This is the joint which is most sensitive to 1540⎇⎇motion in the required direction and it would be the first 1541⎇⎇joint to move if the force were slowly increased from zero 1542⎇⎇in the free direction. 1543⎇⎇If we require more degrees of freedom we repeat the process 1544⎇⎇being careful not to select the same joint twice. 1545⎇⎇To free the joint during motion, the feedback gains ke and 1546⎇⎇kv are set to zero (see Subsection 6.1). This means that the 1547⎇⎇free joint still has acceleration compensation (Equation 1548⎇⎇6.6) and gravity compensation, (Equation 6.5) If the hand is 1549⎇⎇required to exert an external force then this is also added 1550⎇⎇so that the joint is compensated for all known forces and 1551⎇⎇has no position directed feedback. 1552⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.3 PARTIAL␈← 1553⎇⎇␈→60⎇0⎇⎇16513⎇␈←␈→60⎇-1000⎇⎇␈← 1554⎇⎇The free joint servo response is then from (Equation 6.6) 1555⎇⎇␈→60⎇0⎇⎇E(s) = Te/( J * s␈↑2␈↓ )␈←␈→60⎇-1000⎇⎇[Eq. 6.17]␈← 1556⎇⎇In such actions as pushing a peg into a hole the hand must 1557⎇⎇have two degrees of freedom as the peg and hole represent 1558⎇⎇two constraints. 1559⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 1560⎇⎇␈→60⎇0⎇⎇16514⎇␈←␈→60⎇-1000⎇⎇␈← 1561⎇⎇␈→60⎇0⎇⎇7.0 CONTROL␈← 1562⎇⎇In addition to moving, the arm can perform such functions as 1563⎇⎇opening and closing its hand. These functions and motions 1564⎇⎇are called "primitives" and can be put together to make an 1565⎇⎇"arm program." An arm program is assembled to make a 1566⎇⎇trajectory file specifying the primitives together with 1567⎇⎇supporting data, trajectories, effective inertia constants 1568⎇⎇and gravity loading terms. 1569⎇⎇Two programs exist, one for assembling "arm programs" and 1570⎇⎇the other for executing the resulting trajectory files. 1571⎇⎇7.1 ARM STATE 1572⎇⎇The state of the arm is described by the following global 1573⎇⎇variables, which are located in the upper segment and are 1574⎇⎇available to all other programs sharing this segment, such 1575⎇⎇as a strategy program. At the termination of execution of a 1576⎇⎇program the state variables which describe the arm are 1577⎇⎇updated. 1578⎇⎇ ARM␈π_MOTION The name of the program currently being 1579⎇⎇ executed. This is a warning flag to other 1580⎇⎇ programs that the arm is in motion. 1581⎇⎇ ARM␈π_WAIT The name of the program, execution of which 1582⎇⎇ is temporarily suspended. 1583⎇⎇ ARM␈π_STATUS The error state of the arm at the end of 1584⎇⎇ execution. 1585⎇⎇ ARM␈π_LINK The 4x4 transform which describes the 1586⎇⎇ position and orientation of the hand at the 1587⎇⎇ end of execution. 1588⎇⎇ GRASP The separation between the finger tips. 1589⎇⎇ ARM␈π_PC The address of the next primitive to be 1590⎇⎇ executed in the current ARM␈π_SEGMENT. 1591⎇⎇ ARM␈π_SEGMENT An integer variable incremented at the 1592⎇⎇ beginning and end of each MOVE primitive. 1593⎇⎇Programs can be executed by the following two procedures. 1594⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.1 STATE␈← 1595⎇⎇␈→60⎇0⎇⎇16517⎇␈←␈→60⎇-1000⎇⎇␈← 1596⎇⎇ DO␈π_IT (NAME) causes the program NAME to be executed. 1597⎇⎇ DO␈π_PROCEED causes continued execution of the program 1598⎇⎇ whose execution was interrupted. 1599⎇⎇During execution various errors can occur; they are known as 1600⎇⎇errors because they cause the arm to stop and to apply all 1601⎇⎇the brakes, although some errors may be desired states. 1602⎇⎇There are various touch sensors on the arm and if, when 1603⎇⎇selected, one of them touchs anything, an "error" occurs. 1604⎇⎇The following is a list of the error messages: 1605⎇⎇ x1 Excessive force at joint x. 1606⎇⎇ 2 Hand closed beyond minimum specified opening 1607⎇⎇ x6 Touch sensor x was touched. 1608⎇⎇ 22 Excessive force at hand 1609⎇⎇ 23 Arm failed to stop on specified force. 1610⎇⎇There are other messages which refer to system errors, such 1611⎇⎇as file not found. 1612⎇⎇If an error occurs, execution of the program is suspended 1613⎇⎇and the state variables are updated, the contents of 1614⎇⎇ARM␈π_MOTION and ARM␈π_WAIT are exchanged and the error code is 1615⎇⎇set in ARM␈π_STATUS. Execution can proceed with the next 1616⎇⎇primitive, after erorr recovery, by calling procedure 1617⎇⎇DO␈π_PROCEED. 1618⎇⎇7.2 PRIMITIVES 1619⎇⎇This section lists the arm primitives, which have meaning at 1620⎇⎇two times: once at assembly when the trajectory file is 1621⎇⎇being created, when feasibility must be checked, 1622⎇⎇trajectories planned etc., and once at execution time when 1623⎇⎇the primitives are executed in the same way that 1624⎇⎇instructions are executed in a computer. 1625⎇⎇ OPEN (DIST) Open or close the hand such that the gap 1626⎇⎇ between the finger tips is DIST. 1627⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈← 1628⎇⎇␈→60⎇0⎇⎇16519⎇␈←␈→60⎇-1000⎇⎇␈← 1629⎇⎇ CLOSE (MINIMUM) Close the hand until it stops 1630⎇⎇ closing and then check that the gap between 1631⎇⎇ the finger tips is greater than MINIMUM. If 1632⎇⎇ it is less then give error 2. 1633⎇⎇ CHANGE (DX␈π_DY␈π_DZ, VELOCITY) Move the arm 1634⎇⎇ differentially (Subsection 5.5) to achieve a 1635⎇⎇ change of hand position of vector DX␈π_DY␈π_DZ 1636⎇⎇ at a maximum speed of VELOCITY. 1637⎇⎇ SWEEP (DIST, VELOCITY) Move the hand differentially 1638⎇⎇ (Subsection 5.5) in the direction of the 1639⎇⎇ hand's orientation vector a distance DIST at 1640⎇⎇ a maximum velocity VELOCITY. 1641⎇⎇ LIFT (DIST, VELOCITY) This is the same as SWEEP 1642⎇⎇ except that it is in the direction of O␈-1⎇␈π_ x A␈-1⎇␈π_ 1643⎇⎇ where O and A are the orientation and 1644⎇⎇ approach vectors respectively. 1645⎇⎇ REACH (DIST, VELOCITY) Once again this is the same 1646⎇⎇ as SWEEP except in this case it is in the 1647⎇⎇ direction of the Approach vector. 1648⎇⎇ TILT (ANGLE) Rotate the hand differentially 1649⎇⎇ Subsection 5.5 about the orientation vector 1650⎇⎇ ANGLE degrees. 1651⎇⎇ TURN (ANGLE) This is a similar rotation about O␈-1⎇␈π_ x A␈-1⎇␈π_. 1652⎇⎇ TWIST (ANGLE) Here the rotation is about the 1653⎇⎇ approach vector. 1654⎇⎇ PLACE Move the hand vertically down until the hand 1655⎇⎇ meets some resistance, that is, the minimum 1656⎇⎇ resistance that the arm can reliably detect. 1657⎇⎇ MOVE ( T ) At assembly time check that the position 1658⎇⎇ specified by the hand transformation T is 1659⎇⎇ clear. Move the hand along a trajectory from 1660⎇⎇ its present position to | T |. The hand is 1661⎇⎇ moved up through a point ␈_LIFTOFF␈≡ given by 1662⎇⎇ ␈_␈_LIFTOFF␈≡ = ␈_INITIAL␈π_POSITION␈≡ + ␈_DEPART␈≡. where 1663⎇⎇ ␈_␈_DEPART␈≡ is a global vector initialized to z = 1664⎇⎇ 3 inches. Similarly on arrival the hand is 1665⎇⎇ moved down through a point ␈_SET␈π_DOWN␈≡ given 1666⎇⎇ by: ␈_SET␈π_DOWN␈≡ = ␈_FINAL␈π_POSITION␈≡ + ␈_ARRIVE␈≡. 1667⎇⎇ ␈_␈_ARRIVE␈≡ is also set to z = 3 inches. 1668⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈← 1669⎇⎇␈→60⎇0⎇⎇16520⎇␈←␈→60⎇-1000⎇⎇␈← 1670⎇⎇ PARK Plan a move as in MOVE but to the "park" 1671⎇⎇ position. 1672⎇⎇ DRAW (␈_DX␈π_DY␈π_DZ␈≡, ␈_ROT␈π_AXIS␈≡, O␈-1⎇-1, ␈_CRANK␈≡, ␈_CRANK␈π_AXIS␈≡, O␈-1⎇-2, 1673⎇⎇ TIME, LOOPS, FORCE, NUMBER␈π_FREE, 1674⎇⎇ ␈_FREE␈π_VECTOR␈≡) This is a trajectory motion of 1675⎇⎇ the hand. The hand is at the end of a vector 1676⎇⎇ (␈_CRANK␈≡) which is rotated O␈-1⎇-2 degrees around 1677⎇⎇ an axis (␈_CRANK␈π_AXIS␈≡) as its origin is 1678⎇⎇ translated (␈_DX␈π_DY␈π_DZ␈≡). At the same time the 1679⎇⎇ hand is re-orientated about another axis 1680⎇⎇ ␈_(␈_ROT␈π_AXIS␈≡) O␈-1⎇-1 degrees. See Figure 7.1. If 1681⎇⎇ the end point is the same as the initial 1682⎇⎇ then looping may be specified (see 1683⎇⎇ Subsection 5.2) finally a number of degrees 1684⎇⎇ of freedom may be specified and an excess 1685⎇⎇ force to be applied during the program. With 1686⎇⎇ this primitive we can do almost anything! 1687⎇⎇There are also control primitives which specify how the 1688⎇⎇other primitives are to be carried out. 1689⎇⎇ STOP (FORCE, MOMENT) During the next arm motion stop 1690⎇⎇ the arm when the feedback force is greater 1691⎇⎇ than the equivalent joint force (Subsection 1692⎇⎇ 3.5) if the arm fails to stop for this 1693⎇⎇ reason before the end of the motion, 1694⎇⎇ generate error 23. 1695⎇⎇ SKIPE (ERROR) If error ERROR occured during the 1696⎇⎇ previous primitive then skip the next 1697⎇⎇ primitive. 1698⎇⎇ SKIPN (ERROR) if error ERROR occured during the 1699⎇⎇ previous primitive execute the next 1700⎇⎇ primitive otherwise skip the next primitive. 1701⎇⎇ JUMP (N) Jump ahead N primitives where N may also be 1702⎇⎇ negative. 1703⎇⎇ WAIT Stop execution and update the state variables 1704⎇⎇ and wait for a proceed command. 1705⎇⎇ TOUCH (MASK) Activate the touch sensors specified by 1706⎇⎇ mask for the next primitive. 1707⎇⎇ SAVE Save the differential deviation from the 1708⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈← 1709⎇⎇␈→60⎇0⎇⎇16522⎇␈←␈→60⎇-1000⎇⎇␈← 1710⎇⎇ trajectory set point. This can be caused by 1711⎇⎇ CHANGE type primitives. 1712⎇⎇ RESTORE Cause the arm to deviate from the trajectory 1713⎇⎇ set point at the end of the next motion by 1714⎇⎇ the deviation last saved. 1715⎇⎇7.3 ASSEMBLY PROGRAM 1716⎇⎇The assembly program has two modes of input, message 1717⎇⎇procedure or source file. We will describe the source file 1718⎇⎇input here although it should be kept in mind that a 1719⎇⎇strategy program would be able to execute the same 1720⎇⎇functions. 1721⎇⎇Planning normally begins from the present position and 1722⎇⎇proceeds from there. The planned state of the arm is kept 1723⎇⎇in an 4x4 transformation STATE. Thus when a series of moves 1724⎇⎇is planned it is always from STATE to the specified 1725⎇⎇transform T that moves are made. At the end of each 1726⎇⎇successful move STATE is updated. In order to start an 1727⎇⎇program a BEGIN pseudo-op must be assembled. This causes a 1728⎇⎇trajectory file to be named and specifies the initial state 1729⎇⎇of STATE. Similarly at the end of an program the files must 1730⎇⎇be closed. We will list the pseudo-ops: 1731⎇⎇ BEGIN (FILE, T) Open file FILE as the trajectory 1732⎇⎇ file and initialize STATE to transformation 1733⎇⎇ T. 1734⎇⎇ MERGE Merge the last assembled primitive with the 1735⎇⎇ preceeding motion primitive. 1736⎇⎇ END Close the trajectory file. 1737⎇⎇ MACRO (FILE) Causes input to the planning program to 1738⎇⎇ be switched to FILE until the end of FILE. 1739⎇⎇ This gives us macros without parameters The 1740⎇⎇ MACROs may be nested. 1741⎇⎇In the case of most primitives their parameters are either 1742⎇⎇vectors or transformations, in the case of another program 1743⎇⎇using the assembly program these vectors and transformations 1744⎇⎇would be data structures of that program. In the case of 1745⎇⎇source input we need to be able to define such data types 1746⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.3 ASSEMBLY␈← 1747⎇⎇␈→60⎇0⎇⎇16524⎇␈←␈→60⎇-1000⎇⎇␈← 1748⎇⎇and associate them with names, which will be given in the 1749⎇⎇primitive call. All names must be defined before an 1750⎇⎇primitive can be assembled. 1751⎇⎇Data types: 1752⎇⎇ TRANS (NAME, R, X, Y, Z, Ox, Oy, Oz) Set up a 4x4 1753⎇⎇ transformation NAME such that it has 1754⎇⎇ position x,y,z and orientation vector 1755⎇⎇ Ox,Oy,Oz and that the approach vector is 1756⎇⎇ rotated R degrees from the reference 1757⎇⎇ approach vector about the orientation vector 1758⎇⎇ Equation 4.1. 1759⎇⎇ VECT (NAME, x␈↓1␈↑, x␈↓2␈↑, x␈↓3␈↑, x␈↓4␈↑) Define a vector NAME of 1760⎇⎇ value x␈↓1␈↑,x␈↓2␈↑,x␈↓3␈↑,x␈↓4␈↑. 1761⎇⎇ MOVE␈π_INSTANCE (TI, TF, IP) This is partly a data 1762⎇⎇ type primitive as it sets up a series of 1763⎇⎇ transformations to move an object with 1764⎇⎇ transformation TI such that it has 1765⎇⎇ transformation TF using intermediate 1766⎇⎇ position IP if necessary. However it 1767⎇⎇ assembles all the move and hand primitives 1768⎇⎇ to accomplish the move. 1769⎇⎇ PROTOTYPE (OBJECT) this sets up the prototype of the 1770⎇⎇ body to be moved by MOVE␈π_INSTANCE. 1771⎇⎇The planning program is 30K and shares a 14K segment which 1772⎇⎇contains runtime routines and global data. Typical running 1773⎇⎇times are 1 sec to plan a move. and 4 sec for MOVE␈π_INSTANCE. 1774⎇⎇7.4 PROGRAMMING EXAMPLES 1775⎇⎇We will give some examples of hand programs to clarify the 1776⎇⎇use of primitives. The first example is to move the hand to 1777⎇⎇a position 20,30,1 and to pick-up an object, then to move it 1778⎇⎇to 40,20,2 and to place it on the table. 1779⎇⎇ BEGIN TRANSFER 0 1780⎇⎇ TRANS T 1781⎇⎇ 90 20 30 1 1 0 0 Set up a transform to 1782⎇⎇ position the hand. 1783⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈← 1784⎇⎇␈→60⎇0⎇⎇16526⎇␈←␈→60⎇-1000⎇⎇␈← 1785⎇⎇ MOVE T 1786⎇⎇ CLOSE 0.5 If there is nothing here 1787⎇⎇ then an error will occur. 1788⎇⎇ TRANS T 1789⎇⎇ 90 40 20 2 1 0 0 Change the transform to 1790⎇⎇ the new position. 1791⎇⎇ MOVE T 1792⎇⎇ PLACE 1793⎇⎇ PARK 1794⎇⎇ OPEN 3 1795⎇⎇ MERGE The hand will open as it 1796⎇⎇ starts to move. 1797⎇⎇ END 1798⎇⎇The next example is to grasp an object without moving it in 1799⎇⎇case there is some error in its position. The hand is closed 1800⎇⎇with the touch sensors activated until a finger touches the 1801⎇⎇object. The hand is then "swept" and closed in 0.1 inch 1802⎇⎇steps until the other finger touches. The hand is then 1803⎇⎇closed. 1804⎇⎇ TOUCH 1 1805⎇⎇ OPEN -1 This will cause the hand to 1806⎇⎇ close with touch "on". 1807⎇⎇L2: SKIPE 6 Did the left finger touch? 1808⎇⎇ JUMP L1 1809⎇⎇ SWEEP -0.1 1 Yes move right. 1810⎇⎇ TOUCH 1 1811⎇⎇ OPEN -1 And close the hand again. 1812⎇⎇ SKIPE 16 Did the other finger touch? 1813⎇⎇ JUMP L2 No move right again. 1814⎇⎇ JUMP L3 Yes all done. 1815⎇⎇L1: SKIPE 16 Check that the right finger 1816⎇⎇ touched. 1817⎇⎇ JUMP L8 No then some error. 1818⎇⎇ SWEEP 0.1 1 Move left. 1819⎇⎇ TOUCH 1 1820⎇⎇ OPEN -1 And close the hand again. 1821⎇⎇ SKIPE 6 Did the left finger touch? 1822⎇⎇ JUMP L1 No move again 1823⎇⎇L3: CLOSE 0.5 Yes close the hand. 1824⎇⎇ JUMP L10 And finish. 1825⎇⎇L8: WAIT The error state. 1826⎇⎇L10: 1827⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈← 1828⎇⎇␈→60⎇0⎇⎇16527⎇␈←␈→60⎇-1000⎇⎇␈← 1829⎇⎇The last example, whose file name is CATCH, has no begin and 1830⎇⎇end as we are going to use it as a macro. We will use CATCH 1831⎇⎇first to position the hand on the object, then to turn it 90 1832⎇⎇degrees and close the hand again, so that the object will be 1833⎇⎇picked up centrally. 1834⎇⎇ MACRO CATCH Here we assume that the 1835⎇⎇ hand has been positioned 1836⎇⎇ over the object. The 1837⎇⎇ macro call will substitute 1838⎇⎇ the text from our previous 1839⎇⎇ example. 1840⎇⎇ OPEN 3 1841⎇⎇ TWIST 90 Now open the hand and turn 1842⎇⎇ it around 90 degrees. 1843⎇⎇ MACRO CATCH And close it again. 1844⎇⎇This we will also use as a macro called PICK. If the 1845⎇⎇primitive MACRO PICK were to replace the CLOSE 0.5 primitive 1846⎇⎇in the first example then we would have accomplished the 1847⎇⎇move but without having disturbed the object in its initial 1848⎇⎇position. We would also have located the object centrally. 1849⎇⎇We will give one final example, that is to put a nut, which 1850⎇⎇we will assume the hand to be holding, on a vertical bolt 1851⎇⎇located at 20,30,1 We will stop turning when the torque is 1852⎇⎇200 oz.in. 1853⎇⎇ BEGIN SCREW 0 1854⎇⎇ TRANS T 1855⎇⎇ 90 20 30 2 1 0 0 1856⎇⎇ MOVE T 1857⎇⎇ PLACE Move the nut to the bolt 1858⎇⎇ and place it. 1859⎇⎇ VECT ROTATION The bolt axis 1860⎇⎇ 0 0 -1 1861⎇⎇ VECT NULL 1862⎇⎇ 0 0 0 1863⎇⎇ VECT MOMENT The stopping torque. 1864⎇⎇ STOP NULL MOMENT Stop the arm during the 1865⎇⎇ motion when the torque 1866⎇⎇ is 200 oz. in. 1867⎇⎇ VECT FORCE 1868⎇⎇ 0 0 -20 We will want to push 1869⎇⎇ down on the bolt as we 1870⎇⎇ turn. 1871⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈← 1872⎇⎇␈→60⎇0⎇⎇16528⎇␈←␈→60⎇-1000⎇⎇␈← 1873⎇⎇ VECT FREEX 1874⎇⎇ 1 0 0 The hand must be free in 1875⎇⎇ the x,y and z directions 1876⎇⎇ as we screw. 1877⎇⎇ VECT FREEY 1878⎇⎇ 0 1 0 1879⎇⎇ VECT FREEZ 1880⎇⎇ 0 0 -1 1881⎇⎇ DRAW 1882⎇⎇ NULL ROTATION 360 No change of position 1883⎇⎇ NULL NULL 0 No crank axis 1884⎇⎇ 60 3 FORCE 1 Second per turn, plan 1885⎇⎇ for a maximum of three 1886⎇⎇ turns. Exert the force. 1887⎇⎇ 3 Three drgrees of freedom. 1888⎇⎇ FREEX 1889⎇⎇ FREEY 1890⎇⎇ FREEZ 1891⎇⎇ END 1892⎇⎇This will screw the nut on the bolt. 1893⎇⎇7.5 EXECUTE 1894⎇⎇With the exception of MOVE and DRAW, which require 1895⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.5 EXECUTE␈← 1896⎇⎇␈→60⎇0⎇⎇16530⎇␈←␈→60⎇-1000⎇⎇␈← 1897⎇⎇trajectory files, most functions can be executed directly by 1898⎇⎇prefixing the primitive name by "DO." The assembly program 1899⎇⎇plans the action and sends it to the arm program to be 1900⎇⎇executed. This does not change the state of the arm program 1901⎇⎇if it is in a "wait" state and execution can continue after 1902⎇⎇any number of executed primitives. This method is used by 1903⎇⎇the interactive programs, which will plan a move to bring 1904⎇⎇the hand close to the required place and then plan a "wait." 1905⎇⎇When executed the hand position will be modified during the 1906⎇⎇wait phase by the interacting program executing a series of 1907⎇⎇"DO" commands. Execution of the preplanned trajectory can 1908⎇⎇then continue by executing a "DO␈π_PROCEED" command. 1909⎇⎇7.6 ARM PROGRAM 1910⎇⎇A simplified flow chart for the execution program is shown 1911⎇⎇in Figure 7.2. The loop is executed 60 times a second. If 1912⎇⎇the arm is not in motion then RUN is false and the touch 1913⎇⎇sensors are checked before performing any function. At the 1914⎇⎇completion of a motion or, if the arm is not moving at the 1915⎇⎇completion of a function, the program counter is incremented 1916⎇⎇and the next primitive executed. A zero primitive terminates 1917⎇⎇execution. 1918⎇⎇The block ANGLES measures all the joint angles and performs 1919⎇⎇a piecewise non-linear conversion on them. The velocities 1920⎇⎇are also determined, either by reading the tachometer 1921⎇⎇generator outputs (joints 1,2) or by differencing the 1922⎇⎇position information. 1923⎇⎇The block SERVO corresponds to Subsection 6.1; here the 1924⎇⎇errors are computed and the drive torques calculated, If a 1925⎇⎇stop arm primitive is in effect then the error torques are 1926⎇⎇checked against the equivalent arm torques to determine if 1927⎇⎇the arm should be stopped. At the end of the trajectory the 1928⎇⎇position errors are nulled. When each individual joint is 1929⎇⎇within the error tolerance the brake is applied, and when 1930⎇⎇all joints are stopped the next primitive is executed. 1931⎇⎇Section DRIVE takes as input the joint torques and computes 1932⎇⎇the motor drive. It checks for excessive force and stops the 1933⎇⎇arm. 1934⎇⎇The THETA section computes the next values of the joint 1935⎇⎇angles. interpolates the Ji and Tg values (see Equation 1936⎇⎇6.12) and controls the switching as required for looping. 1937⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.6 ARM␈← 1938⎇⎇␈→60⎇0⎇⎇16533⎇␈←␈→60⎇-1000⎇⎇␈← 1939⎇⎇The Touch sensors are then checked and, if any, have been 1940⎇⎇touched the arm is stopped. Finally the function if any is 1941⎇⎇performed. Functions are not normally performed while 1942⎇⎇running with the exception of hand opening. 1943⎇⎇The execution time for the loop is approximately 9msec on 1944⎇⎇the PDP-6 processor, using floating point hardware. The 1945⎇⎇program length is 3K words including trajectory buffers. 1946⎇⎇While the arm is running the trajectory set point O␈-1⎇-t is 1947⎇⎇given by: 1948⎇⎇␈→60⎇0⎇⎇O␈-1⎇-t = f(t)␈←␈→60⎇-1000⎇⎇[Eq. 7.1]␈← 1949⎇⎇where f(t) is the appropriate part trajectory polynomial 1950⎇⎇Subsection 5.2 The arm set point is as follows: 1951⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + dO␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.2]␈← 1952⎇⎇where O␈-1⎇-t is a constant offset between the set point and the 1953⎇⎇trajectory point. Between liftoff and set-down for a period 1954⎇⎇of time Tm: 1955⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + dO␈-1⎇- +d2O␈-1⎇-*g(t)␈←␈→60⎇-1000⎇⎇[Eq. 7.3]␈← 1956⎇⎇where g(t) is given in Equation 5.38. When t = Tm dO␈-1⎇- is 1957⎇⎇changed as follows: 1958⎇⎇␈→60⎇0⎇⎇dO␈-1⎇- ← dO␈-1⎇- + d2O␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.4]␈← 1959⎇⎇␈→60⎇0⎇⎇d2O␈-1⎇- ← 0␈←␈→60⎇-1000⎇⎇[Eq. 7.5]␈← 1960⎇⎇At the beginning of each trajectory motion we set: 1961⎇⎇␈→60⎇0⎇⎇dO␈-1⎇- ← O␈-1⎇- - O␈-1⎇-t␈←␈→60⎇-1000⎇⎇[Eq. 7.6]␈← 1962⎇⎇␈→60⎇0⎇⎇d2O␈-1⎇- ← -dO␈-1⎇- + d2O␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.7]␈← 1963⎇⎇where O␈-1⎇- is the observed value. Thus at the beginning of a 1964⎇⎇trajectory: 1965⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇- = O␈-1⎇-t + dO␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.8]␈← 1966⎇⎇and at the end of let-down: 1967⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + d2O␈-1⎇-i␈←␈→60⎇-1000⎇⎇[Eq. 7.9]␈← 1968⎇⎇where d2O␈-1⎇-i is the value of d2O␈-1⎇- at the beginning of the 1969⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.6 ARM␈← 1970⎇⎇␈→60⎇0⎇⎇16534⎇␈←␈→60⎇-1000⎇⎇␈← 1971⎇⎇trajectory Equation 7.7. If d2O␈-1⎇- was set to zero at the end 1972⎇⎇of the previous trajectory by Equation 7.5 then Equation 7.9 1973⎇⎇becomes: 1974⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t␈←␈→60⎇-1000⎇⎇[Eq. 7.10]␈← 1975⎇⎇This means that if the arm is displaced from the point from 1976⎇⎇which the trajectory was planned, the arm is gradually 1977⎇⎇brought back to the trajectory during the mid-part of the 1978⎇⎇motion. 1979⎇⎇All the differential motions are accomplished by loading d2O␈-1⎇- 1980⎇⎇setting Tm and seting RUN, then at the end of Tm the arm has 1981⎇⎇moved d2O␈-1⎇- and the value of dO␈-1⎇- has also been incremented by 1982⎇⎇d2O␈-1⎇-. 1983⎇⎇The save command causes dO␈-1⎇- to be saved and the restore 1984⎇⎇primitive loads d2O␈-1⎇- with the previously stored dO␈-1⎇-. By this 1985⎇⎇means we can find some location by differential motion, save 1986⎇⎇the dO␈-1⎇-, and then if we wish to return to this differentially 1987⎇⎇modified position we restore dO␈-1⎇- into d2O␈-1⎇- before returning to 1988⎇⎇the unmodified position. 1989⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 1990⎇⎇␈→60⎇0⎇⎇16535⎇␈←␈→60⎇-1000⎇⎇␈← 1991⎇⎇␈→60⎇0⎇⎇8.0 MOTION ANALYSIS␈← 1992⎇⎇8.1 FORCE AND RESULTANT MOTION ANALYSIS 1993⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈← 1994⎇⎇␈→60⎇0⎇⎇16538⎇␈←␈→60⎇-1000⎇⎇␈← 1995⎇⎇␈→60⎇0⎇⎇9.0 CONCLUSIONS␈← 1996⎇⎇9.1 CONCLUSIONS AND FUTURE WORK 1997⎇⎇July 20, 1972 ␈→60⎇-1000⎇⎇␈← 1998⎇⎇␈→60⎇0⎇⎇16545⎇␈←␈→60⎇-1000⎇⎇␈← 1999⎇⎇␈→60⎇0⎇⎇APPENDIX␈← 2000⎇⎇A.1 HARDWARE DESCRIPTION 2001⎇⎇The arm is shown in Figure 3.1, we will give its esential 2002⎇⎇features here. The hand, a simple parallel jaw hand of 10 2003⎇⎇cm opening, can be positioned anywhere in a work space of 1 2004⎇⎇meter radius about the shoulder and with any orientation. It 2005⎇⎇is provided with elementary "switch" type touch sensors. 2006⎇⎇The arm, designed to work with 5kg loads, is powered by 2007⎇⎇electric motors; harmonic drive gear reductions are employed 2008⎇⎇on most joints. These reductions are efficient which means 2009⎇⎇that the arm can react to external forces (see Subsection 2010⎇⎇6.3). and thus the hand can follow an externally 2011⎇⎇constrained motion. Brakes are provided to hold the arm in 2012⎇⎇position when it is stopped, so that the arm does not have 2013⎇⎇to be continuously servoed. 2014⎇⎇The power to weight ratio is high, resulting in a very high 2015⎇⎇performance arm. It is possible to move the arm 180 degrees 2016⎇⎇at the shoulder in little over a second including stopping 2017⎇⎇and starting time. The absolute accuracy of the arm is of 2018⎇⎇the order of ␈π-0.1 inches and its repeatability ␈π-0.03 inches. 2019⎇⎇Point to point servoing usually takes in the order of 1 to 2 2020⎇⎇seconds. 2021⎇⎇Joint angle is measured by integral potentiometers and is 2022⎇⎇read into the computer by a 12 bit A/D converter. In the 2023⎇⎇case of joint 6 where continuous motion is possible two 2024⎇⎇wipers are provided on a common element. Whenever the 2025⎇⎇current wiper is within 1/8th of the end of scale the other 2026⎇⎇wiper is read. As wipers are interchanged an offset is added 2027⎇⎇when appropriate. This continuous motion is used in such 2028⎇⎇tasks as screwing in screws. 2029⎇⎇Control of the arm is by means of a voltage pulse width 2030⎇⎇modulated signal, the polarity and duration are set by the 2031⎇⎇computer. If the arm is not addressed by the computer once 2032⎇⎇every 20 msec. a hardware interlock automatically stops the 2033⎇⎇motors and puts on the brakes. This must be "unlocked" by 2034⎇⎇the computer before the arm can be run again. 2035⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.2 SAIL␈← 2036⎇⎇␈→60⎇0⎇⎇16549⎇␈←␈→60⎇-1000⎇⎇␈← 2037⎇⎇A.2 SAIL 2038⎇⎇SAIL is an extended form of ALGOL with LEAP added to it. 2039⎇⎇LEAP provides the ITEM and DATUM constructs which we use to 2040⎇⎇represent the models. Bodies and parts of them (faces, 2041⎇⎇vertices, edges) are represented by items, a data type which 2042⎇⎇is treated simply as a name. 2043⎇⎇For example in the expression: 2044⎇⎇␈→60⎇0⎇⎇I←I+5;␈←␈→60⎇-1000⎇⎇[Eq. 10.1]␈← 2045⎇⎇we are referring to the value of I, In the case of ITEMS we 2046⎇⎇refer to the name. 2047⎇⎇The main use of items is that they may be associated 2048⎇⎇together in the following manner: 2049⎇⎇␈→60⎇0⎇⎇Attribute of Object is Value ␈←␈→60⎇-1000⎇⎇[Eq. 10.2]␈← 2050⎇⎇Where Attribute, Object and Value are items. 2051⎇⎇To associate three items the "MAKE" construct is used. 2052⎇⎇␈→60⎇0⎇⎇MAKE A⊗O≡V ␈←␈→60⎇-1000⎇⎇[Eq. 10.3]␈← 2053⎇⎇Where the "⊗" stands for "of" and the "≡" stands for "is". 2054⎇⎇To delete such an association the "ERASE" construct is used. 2055⎇⎇␈→60⎇0⎇⎇ERASE A⊗O≡V ␈←␈→60⎇-1000⎇⎇[Eq. 10.4]␈← 2056⎇⎇There exists a mechanism for searching the store of 2057⎇⎇associations in an efficient manner, the "FOREACH" 2058⎇⎇construct. Assume that we had MADE the following 2059⎇⎇associations: 2060⎇⎇␈→60⎇0⎇⎇FACE⊗CUBE≡FACE1␈← 2061⎇⎇␈→60⎇0⎇⎇FACE⊗CUBE≡FACE2␈← 2062⎇⎇Then the following FOREACH statement: 2063⎇⎇␈→60⎇0⎇⎇FOREACH F| FACE⊗CUBE≡F DO <statement> ␈←␈→60⎇-1000⎇⎇[Eq. 10.5]␈← 2064⎇⎇where <statement> is an ALGOL statement and F is an item 2065⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.2 SAIL␈← 2066⎇⎇␈→60⎇0⎇⎇16550⎇␈←␈→60⎇-1000⎇⎇␈← 2067⎇⎇variable and will be progressively assigned to FACE1 and 2068⎇⎇then to FACE2, the statement being executed each time. 2069⎇⎇One additional piece of data can be associated with an item 2070⎇⎇known as its DATUM. This is usually of algebraic type, for 2071⎇⎇example an array. In order to refer to this array by name 2072⎇⎇the DATUM construct is used, and to refer to an element of 2073⎇⎇the array the subscript list is added. 2074⎇⎇␈→60⎇0⎇⎇DATUM(A) ␈←␈→60⎇-1000⎇⎇[Eq. 10.6]␈← 2075⎇⎇␈→60⎇0⎇⎇DATUM(A)[1,3]␈← 2076⎇⎇Thus if we represent a vertex of a body by an item e.g. 2077⎇⎇VERTEX1 then we may give as its datum the vector 2078⎇⎇representing the position of the vertex, where 2079⎇⎇DATUM(VERTEX1)[1] would be for instance the "x" coordinate. 2080⎇⎇A.3 VECTORS AND TRANSFORMATIONS 2081⎇⎇Vectors representing points in space are denoted by an 2082⎇⎇under-bar "V␈-1⎇␈π_" and are described by four components: 2083⎇⎇␈→60⎇0⎇⎇| V[1] V[2] V[3] V[4]|␈←␈→60⎇-1000⎇⎇[Eq. 10.7]␈← 2084⎇⎇Such that the components of the vector V␈-1⎇␈π_ along the x,y,z 2085⎇⎇axes is given by: 2086⎇⎇␈→60⎇0⎇⎇X = V[1]/V[4]␈←␈→60⎇-1000⎇⎇[Eq. 10.8]␈← 2087⎇⎇␈→60⎇0⎇⎇Y = V[2]/V[4]␈← 2088⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈← 2089⎇⎇␈→60⎇0⎇⎇16552⎇␈←␈→60⎇-1000⎇⎇␈← 2090⎇⎇␈→60⎇0⎇⎇Z = V[3]/V[4]␈← 2091⎇⎇With this scheme both the null vector and vectors at 2092⎇⎇infinity are representable: 2093⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.9]␈← 2094⎇⎇␈→60⎇0⎇⎇| V[1] V[2] V[3] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 10.10]␈← 2095⎇⎇All usual vector operations are defined. Addition and 2096⎇⎇subtraction where: 2097⎇⎇␈→60⎇0⎇⎇R␈-1⎇␈π_ ← ( A␈-1⎇␈π_ ␈π- B␈-1⎇␈π_ )␈←␈→60⎇-1000⎇⎇[Eq. 10.11]␈← 2098⎇⎇␈→60⎇0⎇⎇R[1] = A[1]/A[4] ␈π- B[1]/B[4]␈← 2099⎇⎇␈→60⎇0⎇⎇R[2] = A[2]/A[4] ␈π- B[2]/B[4]␈← 2100⎇⎇␈→60⎇0⎇⎇R[3] = A[3]/A[4] ␈π- B[3]/B[4]␈← 2101⎇⎇␈→60⎇0⎇⎇R[4] = 1.0␈← 2102⎇⎇The dot product where: 2103⎇⎇␈→60⎇0⎇⎇( A␈-1⎇␈π_ . B␈-1⎇␈π_ ) = ( A[1]B[1] + A[2]B[2] + A[3]B[3] )/A[4]B[4]␈← 2104⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 10.12]␈← 2105⎇⎇The cross product where: R␈-1⎇␈π_ ← (A␈-1⎇␈π_ X B␈-1⎇␈π_ )␈→60⎇-1000⎇⎇[Eq. 10.13]␈← 2106⎇⎇␈→60⎇0⎇⎇R[1] = A[2]*B[3] - B[2]*A[3]␈← 2107⎇⎇␈→60⎇0⎇⎇R[2] = A[3]*B[1] - B[3]*A[1]␈← 2108⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈← 2109⎇⎇␈→60⎇0⎇⎇16553⎇␈←␈→60⎇-1000⎇⎇␈← 2110⎇⎇␈→60⎇0⎇⎇R[3] = A[1]*B[2] - B[1]*A[2]␈← 2111⎇⎇␈→60⎇0⎇⎇R[4] = A[4]*B[4]␈← 2112⎇⎇Magnitude ␈→60⎇10⎇⎇| A␈-1⎇␈π_ | = 1/A[4](A[1]␈↑2␈↓ + A[2]␈↑2␈↓ +A[3]␈↑2␈↓ )␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq.␈← 2113⎇⎇␈→60⎇-1000⎇⎇10.14]␈← 2114⎇⎇Scale ␈→60⎇6⎇⎇sA␈-1⎇␈π_ = | A[1] A[2] A[3] A[4]/s |␈←␈→60⎇-1000⎇⎇[Eq. 10.15]␈← 2115⎇⎇Planes are also represented by four components 2116⎇⎇␈→60⎇0⎇⎇| F[1] F[2] F[3] F[4] |␈←␈→60⎇-1000⎇⎇[Eq. 10.16]␈← 2117⎇⎇but in this case the first three components represent the 2118⎇⎇outward pointing normal of the plane normalized to unity, 2119⎇⎇and the fourth component represents the negative directed 2120⎇⎇distance to the plane in the direction of the normal from 2121⎇⎇the origin. See Figure 10.1 2122⎇⎇␈→60⎇0⎇⎇F[1] = N[1]␈←␈→60⎇-1000⎇⎇[Eq. 10.17]␈← 2123⎇⎇␈→60⎇0⎇⎇F[2] = N[2]␈← 2124⎇⎇␈→60⎇0⎇⎇F[3] = N[3]␈← 2125⎇⎇␈→60⎇0⎇⎇F[4] = -D␈← 2126⎇⎇If a plane is so represented, a simple test exists to 2127⎇⎇determine if a point V␈-1⎇␈π_ lies inside or outside the plane. We 2128⎇⎇form the inner product. 2129⎇⎇␈→60⎇0⎇⎇|F| * |V| = | V[1]F[1] + V[2]F[2] + V[3]F[3] + V[4]F[4] |␈← 2130⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 10.18]␈← 2131⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈← 2132⎇⎇␈→60⎇0⎇⎇16555⎇␈←␈→60⎇-1000⎇⎇␈← 2133⎇⎇and depending on the sign the point lies inside or outside 2134⎇⎇the plane, If the product is zero then the point lies in the 2135⎇⎇plane. 2136⎇⎇It is important to note the difference between the dot 2137⎇⎇product (Equation 10.12) and the inner product (Equation 2138⎇⎇10.18). 2139⎇⎇Having represented points and planes it remains to be able 2140⎇⎇to rotate and translate them. We do this by pre-multiplying 2141⎇⎇by a transformation matrix. In the case of points, a 2142⎇⎇rotation is represented by a four by four matrix: 2143⎇⎇ | xx' yx' zx' 0 | 2144⎇⎇ | T | = | xy' yy' zy' 0 |␈→60⎇-1000⎇⎇[Eq. 10.19]␈← 2145⎇⎇ | xz' yz' zz' 0 | 2146⎇⎇ | 0 0 0 1 | 2147⎇⎇Here the first three columns represent the components of the 2148⎇⎇unit vectors in the reference system. i.e. a unit vector 2149⎇⎇along the x axis 2150⎇⎇␈→60⎇0⎇⎇x␈-1⎇␈π_ = | 1 0 0 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.20]␈← 2151⎇⎇if then transformed by 2152⎇⎇␈→60⎇0⎇⎇| x'| = | T | * | x |␈←␈→60⎇-1000⎇⎇[Eq. 10.21]␈← 2153⎇⎇has components 2154⎇⎇␈→60⎇0⎇⎇x␈-1⎇␈π_' = | xx' xy' xz' 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.22]␈← 2155⎇⎇in the rotated system. (see Figure 10.2) 2156⎇⎇In the case of a translation we have the matrix: 2157⎇⎇ | 1 0 0 xt | 2158⎇⎇ | T | = | 0 1 0 yt |␈→60⎇-1000⎇⎇[Eq. 10.23]␈← 2159⎇⎇ | 0 0 1 zt | 2160⎇⎇ | 0 0 0 1 | 2161⎇⎇from which it can be seen that: 2162⎇⎇␈→60⎇0⎇⎇| T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.24]␈← 2163⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈← 2164⎇⎇␈→60⎇0⎇⎇16557⎇␈←␈→60⎇-1000⎇⎇␈← 2165⎇⎇has components: 2166⎇⎇␈→60⎇0⎇⎇| V[1]+xt V[2]+yt V[3]+zt V[4] |␈←␈→60⎇-1000⎇⎇[Eq. 10.25]␈← 2167⎇⎇of the translated vector. 2168⎇⎇To be able to rotate and translate we multiply the 2169⎇⎇transformation matrices together: 2170⎇⎇␈→60⎇0⎇⎇| T | = | Tt| * | Tr|␈←␈→60⎇-1000⎇⎇[Eq. 10.26]␈← 2171⎇⎇ | xx' yx' zx' xt | 2172⎇⎇ | T | = | xy' yy' zy' yt |␈→60⎇-1000⎇⎇[Eq. 10.27]␈← 2173⎇⎇ | xz' yz' zz' zt | 2174⎇⎇ | 0 0 0 1 | 2175⎇⎇These are the only transformations that we will be 2176⎇⎇performing although such a system is capable of many more 2177⎇⎇transformations such as scaling, stretching and perspective. 2178⎇⎇In the case of planes we require Equation 10.18 to hold 2179⎇⎇under these transformations such that if: 2180⎇⎇␈→60⎇0⎇⎇| F | * | V | = | F'| * | V'| ␈←␈→60⎇-1000⎇⎇[Eq. 10.28]␈← 2181⎇⎇␈→60⎇0⎇⎇| V'| = | T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.29]␈← 2182⎇⎇Then if we substitute for | V | in Equation 10.28 we obtain 2183⎇⎇␈→60⎇0⎇⎇| F | * | V | = | F'| * | T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.30]␈← 2184⎇⎇and thus: 2185⎇⎇␈→60⎇0⎇⎇| F'| = | F | * | T |␈↑-1␈↓␈←␈→60⎇-1000⎇⎇[Eq. 10.31]␈← 2186⎇⎇␈→60⎇0⎇⎇| F'| = | T |␈↑-1␈↑T␈↓␈↓* | F |␈←␈→60⎇-1000⎇⎇[Eq. 10.32]␈← 2187⎇⎇Planes are thus transformed by pre-multiplying by the 2188⎇⎇inverse transform transposed.