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⎇⎇␈←
16⎇⎇␈→60⎇0⎇⎇1.0  INTRODUCTION␈←
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␈←
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␈←
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
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␈←
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
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
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␈←
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
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⎇⎇␈←
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.
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␈←
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
251⎇⎇For each Face we now associate its vertices and edges
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␈←
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␈←
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␈←
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.
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
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␈←
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
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
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␈←
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.
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␈←
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;
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␈←
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⎇⎇␈←
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
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␈←
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
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␈←
565⎇⎇␈→60⎇0⎇⎇R0␈-2⎇␈π_␈π_ = | Ti| * | Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.4]␈←
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]␈←
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 =
591⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
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.
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␈←
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.
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␈←
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␈←
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
699⎇⎇␈→60⎇0⎇⎇dR0␈-2⎇␈π_␈π_/dt = | Vi | * | Ri |␈←␈→60⎇-1000⎇⎇[Eq. 3.24]␈←
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]␈←
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␈←
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␈←
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⎇.
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␈←
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]␈←
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
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⎇⎇␈←
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.
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]␈←
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␈←
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.
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
867⎇⎇␈→60⎇0⎇⎇RAi␈-3⎇␈π_␈π_␈π_' ← | Tf | * | Ti |␈↑-1␈↓ * | RAi |␈←␈→60⎇-1000⎇⎇[Eq. 4.3]␈←
868⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.2  MOVE␈←
870⎇⎇the shift S, between approach ranges may then be calculated
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␈←
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
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⎇⎇␈←
937⎇⎇␈→60⎇0⎇⎇5.0  TRAJECTORIES␈←
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␈←
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␈←
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
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
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]␈←
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␈←
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␈←
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␈←
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
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
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␈←
1140⎇⎇from which we can obtain the coefficients of t.
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
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
1162⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.3  EXTREMA␈←
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).
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␈←
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␈←
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).
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␈←
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⎇⎇␈←
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.
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
1323⎇⎇By writing the loop equation we can obtain the system
1325⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1  FEEDBACK␈←
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␈←
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␈←
1378⎇⎇␈→60⎇0⎇⎇Fi =    ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓k=1␈↑ Cik qk␈-2⎇␈↑..␈↓ + Ci␈←␈→60⎇-1000⎇⎇[Eq. 6.9]␈←
1380⎇⎇␈→60⎇0⎇⎇  Cik=  ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 6.10]␈←
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]␈←
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␈←
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␈←
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
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␈←
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)
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␈←
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.
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␈←
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⎇⎇␈←
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
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␈←
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
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␈←
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␈←
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␈←
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.
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
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␈←
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.
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␈←
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
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.
1827⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4  PROGRAMS␈←
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
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␈←
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␈←
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
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
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␈←
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
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␈←
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
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
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
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⎇⎇␈←
1991⎇⎇␈→60⎇0⎇⎇8.0  MOTION ANALYSIS␈←
1993⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
1995⎇⎇␈→60⎇0⎇⎇9.0  CONCLUSIONS␈←
1997⎇⎇July 20, 1972 ␈→60⎇-1000⎇⎇␈←
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
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␈←
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
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␈←
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]␈←
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.
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␈←
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␈←
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.␈←
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␈←
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
2136⎇⎇It is important to note the difference between the dot
2137⎇⎇product (Equation 10.12) and the inner product (Equation
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␈←
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.