Programmer 7500

home *** CD-ROM | disk | FTP | other *** search

/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL016 < prev next >

Wrap

Text File | 1987-03-02 | 52.2 KB | 1,825 lines

.ND .EQ delim $$ .EN .nr H1 4 .NH 1 Task Description .PP Describing a task consist of specifying positions to be reached in space and motions to these positions. RCCL implements structured positions descriptions, and asynchronous motion requests. .NH 2 Position Equations .PP Position equations do not necessitate the use of absolute reference coordinates. Position equations are one representation of the more general concept of transformation graphs. The position relationships of the frames $F sub {i~i=1,n}$ can be expressed in terms of transformations products. Let a transformation $T sub i$ describe the position of the frame $F sub i+1$ relative to the frame $F sub i$ with $T sub n$ describing the transformation from frame $F sub n$ to $F sub 1$, we have : .EQ T sub 1~T sub 2~...~T sub n~=~Idendity .EN A closed path of transformations from frame $F sub 1$ to frame $F sub 1$, via the frames $F sub {i~i=2,n}$ describes the position of $F sub 1$ with respect to itself : the identity transform. The situation is depicted by a directed closed graph : .br .cs R 23 .DS L T1 T2 ---> F1 ---> F2 ---> F3 ....... Fn | | ---------------------------------- Tn .DE .cs R where the vertices are frames and the arcs transforms. .PP Given a set of frames, containing two frames $A$ and $B$, we can certainly find more than one path connecting $A$ to $B$. Let the frames on one path be called $F sub {i~i=1,n}$, and the the frames on the other path be called $G sub {i~i=1,m}$, we obtain : .br .cs R 23 .DS L T0 T1 T2 Tn ---> F1 ---> F2 ---> F3 ....... Fn ---> A B ---> G1 ---> G2 ---> G3 ....... Gm ---> R0 R1 R2 Rm .DE .cs R The corresponding transformation equation is : .EQ T sub 0~T sub 1~T sub 2~...~T sub n~=~R sub 0~R sub 2~...~R sub m .EN Closed transformation graphs can be expressed in terms of a set of transformation equations. Transformation graphs can be generalized, but we will restrict them to the form above. .PP RCCL uses transformations equations in order to describe the positions the manipulator has to reach. We will first introduce the dedicated transform $T6$. We are dealing with manipulators having six links and six joints, labeled from 1 to 6. The base of the manipulator is labeled link 0. Each of the manipulator links is assigned a frame $A sub i$ describing its position with respect to the previous one as a function of the joint variable. The position of link 1 is described with respect to the base. The transformation product : .EQ ~T6~=~A sub 1~...~A sub 6 .EN describes the position of the last link with respect to the base. Note that for the manipulators we are dealing with, it is convenient to assign the last three frames at the intersection of the three last joint axes. Therefore, $T6$ does not take into account the end effector description. .KS By convention the following transform decompositions are given : .EQ ~T1~=~A1 .EN .EQ ~T2~=~A1~A2 .EN .EQ ~T3~=~A1~A2~A3 .EN .EQ ~T4~=~A1~A2~A3~A4 .EN .EQ ~T5~=~A1~A2~A3~A4~A5 .EN .EQ ~T6~=~A1~A2~A3~A4~A5~A6 .EN .EQ ~U6~=~A6 .EN .EQ ~U5~=~A5~A6 .EN .EQ ~U4~=~A4~A5~A6 .EN .EQ ~U3~=~A3~A4~A5~A6 .EN .EQ ~U2~=~A2~A3~A4~A5~A6 .EN .EQ ~U1~=~A1~A2~A3~A4~A5~A6 .EN .EQ ~T6~=~T5~U6~=~T4~U5~=~T3~U4~=~T2~U3~=T1~U2 .EN .KE .PP Let us set up a position equation that structurely describes the situation when the manipulator is to grasp an object lying on a table. We first need to assign frames to each of the elements involved : .KS .IP - A frame is assigned to the shoulder of the manipulator : $S$. .IP - A frame is located at the last link of the manipulator : $M$. .IP - A tool is attached to the link 6, the frame $T$ is assigned to the working end of tool. .IP - A frame $W$ describes the position of the working table. .IP - The position of an object lying on the table is described by $O$. .IP - A grasp position is described by the frame $G$. .KE .PP Suppose that the manipulator is moving such as to grasp the object, the corresponding graph is : .br .cs R 23 .DS L ------ M ------ T ------ | | S G | | ------ W ------ O ------ .DE .cs R In order to turn this graph into a transform equation, we first need to orient the arcs and label them with transforms. The choice is arbitrary but a convenient possibility is : .br .cs R 23 .DS L T6 TOOL DRIVE -----> M -----> T <----- | | S G | | <----- W -----> O -----> BASE OBJ GRASP .DE .cs R where $TOOL$, $BASE$, $GRASP$, $OBJ$ are predetermined transforms. The transform $T6$ is to be changed such that the transform $DRIVE$ comes to identity for the manipulator to reach the desired position, or in other words, such as the frames $T$ and $G$ become identical. The way the $DRIVE$ transform changes (and therefore $T6$) as a function of time determines the way the frame $M$ and $T$ move with respect to $S$, $W$, $O$, or $G$ (Note that no absolute coordinate system is involved and we could say that $S$, $W$, $O$, and $G$ move with respect to $M$ and $T$). .PP The equivalent equation of the position can be written : .EQ BASE~T6~TOOL~=~OBJ~GRASP~DRIVE .EN The equation of the final desired position can be written : .EQ BASE~T6~TOOL~=~OBJ~GRASP .EN Transformations equations can be rewritten, solved for any of the terms, or replaced by equivalent ones. For example, we have : .EQ BASE~T6~=~OBJ~GRASP~TOOL sup -1 .EN .EQ T6~=~BASE sup -1~OBJ~GRASP~TOOL sup -1 .EN .EQ T6~=~COORD~POS~TOOL sup -1~~~~if~ COORD~=~BASE sup -1~,POS~=~OBJ~GRASP .EN Etc.... .PP The RCCL function .B makeposition permits the user to set up such a position equation. The .I setpoint process will automatically compute the terms of the $DRIVE$ transform such that the resulting motion possess certain properties. The .B makeposition function expects a variable number of arguments. They represent the left hand side of the equation, the right hand side, and a transform that will tell which frame is to be considered as the .B tool frame, the frame $T$ in the example above. Assume that the transforms, $BASE$, $TOOL$, $OBJ$, and $GRASP$ have been created via RCCL calls, and that we have the respective transforms pointer available : $base$, $tool$, $obj$, and $grasp$. The `C' definition of the function .B makeposition is : .br .cs R 23 .DS L POS_PTR makeposition(n, lhs [, lhs] ..., EQ, rhs, [, rhs] ..., TL, tl) char *n; TRSF_PTR lhs ..., rhs ..., tl; .DE .cs R and the call corresponding to the above example is : .br .cs R 23 .DS L POS_PTR p; /* a position pointer */ p = makeposition("P", base, t6, tool, EQ, obj, grasp, TL, tool); .DE .cs R The names `EQ' and `TL' are predefined constants. The function .B makeposition returns a pointer to a ring data structure implementing the transform graph. The first argument is a string, the name of the position. The transform pointer .B t6, is built in RCCL, and predeclared in .B rccl.h. As the position data structure is built, .B makeposition calls the function .B optimize in order to premultiply all possible pairs of constant transformation (declared as .B const ), in order to decrease the run time computing load. The function .B optimize will internally replace the user specified position equation by an equivalent canonical form : .EQ T6~=~COORD~POS~TOOL .EN The terms $COORD$ or $TOOL$ of this canonical form can be missing. The calls : .br .cs R 23 .DS L makeposition("P1", t6, EQ, h, TL, t6); makeposition("P2", t6, t, EQ, h, TL, t); makeposition("P3", t6, t, EQ, h, g, TL, t6); .DE .cs R lead to the following canonical equations : .KS .EQ L T6~=~POS~~~~COORD~=~None~,~~POS~=~H~,~~TOOL~=~None .EN .EQ L T6~=~POS~TOOL~~~~COORD~=~None,~~POS~=~H~,~~TOOL~=~T sup -1 .EN .EQ L T6~=~COORD~POS~~~~COORD~=~H~G~,~~POS~=~T~sup -1~,~TOOL~=~None .EN .KE .PP There is an arbitrary number of argument transform pointers for .B makeposition. The only restriction is that the left hand side of the equation must contain the predeclared pointer .B t6 and the right hand side must contain at least one transform. The transforms can arbitrary belong to one of the following categories : .IP "const : " A transform of this type will be considered as constant through out the life of the corresponding position equation. Its value must not be changed, as the system can decided to premultiply it with another transform such as it may not appear in the internal equation used for the trajectory calculation. This is the default type of the functions of the style gentr_... and it is the type that one should use when possible. .IP "varb : " A transform of this type will not be premultiplied by the optimization function and its value will be used directly during the trajectory calculation. One sometimes need to change the value of a transform after the equation has been set up. If the change occurs while the equation is evaluated, the change will instantaneously be reflected in the manipulator's trajectory. This can cause jerky motions if the change is large and it should be carefully used. The function .B update described later on, knows when to change the value of the transform when it is safe. .IP "hold : " A transform of this type is not directly used in the position equations, but a copy of it. We will see that .B move requests are asynchronously issued and that a number of them can ve specified ahead of time. A .B hold transform belongs to the subsequent motion request and its value is taken into account only when the corresponding motion is actually performed. .IP The last category is the class of the functionally defined transforms. These transforms are attached to a function belonging to the user's manipulator program. The function is expected to compute the values of the transform as the corresponding motion is performed. The function is executed at interrupt level and therefore, is expected to have a reasonable execution time. As described in [6], these functions .B cannot perform .B any type of system calls, (prints, reads, etc...). If the function computes the values of the transform as a function of external data, one can obtain tracking. If the values are computed as a function of time, one obtains functionally defined trajectories. We shall call such a function a background function. .PP The type of a transform is indicated in the `fn' field of the transform structure, a few examples : .KS .br .cs R 23 .DS L int myfunction(); t1 = gentr_trsl("T1", 0., 0., 0.); t2 = newtrans("T2", const); t3 = newtrans("T3", varb); t4 = gentr_trsl("T4", 0., 0., 0., ); t4->fn = hold; t5 = gentr_rot("T5", 0., 0., 0., zunit, 90.); t5->fn = myfunction; t6 = newtrans("T6", myfunction); Rot(t6, zunit, 90.); .DE .cs R .IP t1 is a regular .B const transform initialized to the unit value. .IP t2 is a regular .B const transform initialized to the unit value. .IP t3 is a transform initialized as a unit transform of type .B varb. .IP t4 is first created as .B const but is turned into a .B hold transform. .IP t5 is first created as .B const but is turned into a functionally defined transform attached to the function `myfunction' and is initialized as a rotation of 90 degrees around the Z axis. .IP t6 is created as functionally defined, and then initialized as a rotation of 90 degrees around the Z axis. .KE .PP The .B makeposition function implements a restricted case of transformations graphs. This limitation may be removed one day. When multibranch transform graphs are required, the user must implement it in terms of the basic graph described above and a combination of other RCCL function like .B Rot, Trslm, Trmult, Invert, .R and so on. The .B varb and .B hold features are then very important. .PP It is now time to introduce the position structure as described in the file .B rccl.h. .br .cs R 23 .DS L typedef struct posit { char *name; int code; real scal; event end; } POS, *POS_PTR; .DE .cs R The first entry in the structure is the name of the position. The same remarks can be made as for the transforms structures. The name is not directly relevant from the robot control point of view, but may help in debugging. The second entry `code' is a termination code for the corresponding motion. Internal code values known by RCCL are currently : .br .cs R 23 .DS L #define OK -1 #define LIMIT -2 #define ONF -3 #define OND -4 .DE .cs R After the position has been reached, the code is set by the system to the value `OK' if the motion has not be interrupted by some condition. The value `LIMIT' means that the motion caused some joints to dangerously approach their physical stops. RCCL automatically issue a stop to the current position. It is then possible to recover from this error condition as explained later. The code `ONF' means that a prespecified force condition occurred, and the code `OND' that prespecified differential motion condition occurred. The next entry is a floating point number `scal'. The value of the field `scal' varies from 0 to 1 as the motion is performed, and is useful for generating functionally defined motions or to trigger some action at a given point of a trajectory. The entry `end' is classified as an .B event. It allows the user to synchronize the program flow with the execution of trajectories. The use and the function of the fields `code', `scal', and `end' will be explained in more detail as we go on. .PP When a position is no longer needed, memory space can be retuned to the memory pool by : .br .cs R 23 .DS L freepos(p) POS_PTR p; .DE .cs R Care must be taken so that the corresponding data in no longer in use. Transforms involved in the corresponding equation are not freed, an must be individually freed using .B freetrans. .NH 2 Motion Description .PP RCCL implements two basic types of motions known as .I joint mode and .I Cartesian mode. The first one consist of solving for $T6$ the position equation of the goal position at the beginning of the motion and obtaining for it the corresponding set of joint values. The trajectory is then generated by linear interpolation of the joint values from the current position to the goal position. This type of motion should be used for large motions as it requires the minimum joint motions and less computations. High velocities can be obtained, however trajectories are not always easily predictable. The .I Cartesian mode .R makes use the $DRIVE$ transform to produce straight line trajectories for the .I tool frame. The transform equation is evaluated for $T6$ each sample time interval and the set of joint values obtained. This type of motion permits us to obtain well predictable trajectories. If the position equation contains functionally defined transforms, the associated functions are also evaluated at sample time intervals. The values resulting from these evaluations will directly influence the arm trajectory. In that case the structure of the position equation must be carefully considered. .NH 3 The Basic Move Statement .PP The basic function definition is : .br .cs R 23 .DS L move(p) POS_PTR p; .DE .cs R The call : .br .cs R 23 .DS L move(pos); .DE .cs R where `pos' is a pointer to a position equation returned by .B makeposition. instructs the system to move the arm toward the described position such as the equation becomes verified. When the arm is moving from position to position, transitions occur between each path segment. It is important to smooth out the velocity discontinuities that would be caused by an abrupt change of direction and velocity from one path segment to another. There are a number of options and parameters that can be set globally or for each path segment. .NH 3 Setting Options and Parameters .PP The first group of parameters remains set until set to another value. The following calls cause a setting of the parameter starting at the next .B move request : .br .cs R 23 .DS L setvel(tv, rv) int tv, rv; setmod(m) int m; setconf(c) char *c; sample(s) int s; .DE .cs R .PP The .B setvel call takes two integer arguments. The first is the desired translational velocity of the .I tool frame in millimeters per second, the second one is the rotational velocity in degrees per second. The system will calculate the path segment durations to obtain the desired velocities. Since rotational and translational velocities lead to different durations, the system will pick which ever is the longest. One can give the priority to one or another by specifying very different values. For example, suppose that a motion involves a 30 millimeters translation and a 30 degrees rotation, the call .br .cs R 23 .DS L setvel(30, 300); .DE .cs R will result in a 1 second motion due to the translation, and not an unreasonable 1/10 of a second motion to perform the rotation. The function .B setmod defines the mode, .I Cartesian or .I joint, for the subsequent motions. The argument must be the character `c' for .I Cartesian mode, and `j' for .I joint mode. For example : .br .cs R 23 .DS L POS_PTR p, p1, p2; int i, m; p1 = makeposition(...); p2 = makeposition(...); for (move(p2), i = 0; i < 10; ++i) { if (i % 2 != 0) { m = 'c'; p = p1; } else { m = 'j'; p = p2; } setmod(m); move(p); } .DE .cs R will cause the arm to move from p2 to p1 (i odd) in .I Cartesian mode and from p1 to p2 (i even) in .I Joint mode. For C experts a more concise version could be : .br .cs R 23 .DS L for (move(p2), i = 10; i--;) { setmod((m = i % 2) ? 'c' : 'j'); move(m ? p1 : p2); } .DE .cs R In .I joint mode the segment durations are computed based on the distances between the frame $T6$ at each end of the segments, since this type of motion is joint oriented. The function .B setconf permits us to obtain an arm configuration change during the subsequent motion. This motion .B has to be performed in .I joint mode, since a configuration change always involves a degenerate arm configuration unreachable in .I Cartesian mode. Once the configuration change is obtained, the motions can again be performed in .I Cartesian mode. For the PUMA arm, the configurations can be : shoulder righty - lefty (r/l); elbow down - up (d/u); wrist flip - noflip (f/n). The argument is a string specifying the configuration change. For example, if the arm is lefty, up, noflip (``lun''), in order to obtain a wrist configuration change to flip, the arm remaining lefty and up (``luf''), we code : .br .cs R 23 .DS L /* the arm is currently "lun" */ setmod('j'); /* go in joint mode if it was'nt */ setconf("f"); /* specify flip */ move(new); /* go "luf" */ .DE .cs R Note that several letters can be specified in the string argument. A program with many configuration changes is safely terminated by : .br .cs R 23 .DS L setconf("lun"); move(park); .DE .cs R .PP The function .B sample allows us to change the sampling period of the whole system. Currently valid sample rates are: 14, 28, 56, and 112 milliseconds. However the function rounds down the specified value as : sample(15) leads to 14, sample(30) leads to 28, etc. The default value is 28 milliseconds which is a good compromise for most applications. The 14 millisecond rate is helpful for tracking applications, but it is good practice to reset the rate to 28 or 56 when not needed. .PP The next group of functions cause the parameter to be taken into account for the subsequent .I move request only. .br .cs R 23 .DS L setime(ta, ts) int ta, ts; evalfn(fn) int (* fn)(); distance(s, v[, v] ...) char *s, real v; .DE .cs R A call to .B setime allows us to force the motion to be performed within a given period of time. The first argument specifies the duration of the transition time at the end of the segment, and the second argument the duration of the segment itself. Times are specifies in milliseconds. Besides the cases when motion duration is the primary factor, this call serves two purposes. At the present time no call has been implemented to force a rate at the joint level. The consequence is that the system is unable to compute the correct segment duration to perform a configuration change, since the same position can sometimes be reached in different configurations. A duration calculation based on distances is in this case meaningless. Therefore, the user must explicitly specify the motion duration. For this reason a macro has been included in .B rccl.h : .br .cs R 23 .DS L #define moveconf(p, ta, ts, cf) \\\\ {setconf(cf); setmod('j'); setime(ta, ts); move(p);} .DE .cs R The example above can be more conveniently coded as : .br .cs R 23 .DS L moveconf(new, 100, 700, "f"); .DE .cs R The configuration change will be performed in 7/10 of a seconds with a 1/10 of a second transition time. The function .B setime is also very useful for functionally defined motions. When a position equation includes functionally defined transforms, there are situations when the system cannot compute the correct segment duration based on the distance of the goal position because it can depend on arbitrary factors. Likewise a macro has been added : .br .cs R 23 .DS L #define movecart(p, ta, ts) {setmod('c'); setime(ta, ts); move(p);} .DE .cs R The code would be : .br .cs R 23 .DS L movecart(spiral, 300, 2000); .DE .cs R perform a spiraling motion during 2 seconds with a 3/10 of a second escape transition. In the cases when the segment duration calculation is left to the system but the acceleration time still needs to be explicitly specified, the call : .br .cs R 23 .DS L setime(200, 0); .DE .cs R forces the acceleration time to be 2/10 of a seconds but the segment duration, being left unspecified, will be computed by the system. On the other hand, it is sometimes necessary to specify a zero acceleration time, meaning that no transition is desired. This is useful for some slow motions terminated on condition and when the reaction time is of primary importance. The acceleration time can be specified as zero : .br .cs R 23 .DS L setime(0, 1000); .DE .cs R .PP The function .B evalfn cause the function argument to be evaluated during the execution of the corresponding motion. One thus can code any needed synchronous processing. The first application is to perform some monitoring of external condition in order to interrupt a motion. For example, a flag called .B nextmove , causes at any moment the current path segment to terminate and the manipulator to transition to the next. Other applications can be to trigger some action at a precise point of a trajectory. For this the field `scal' of a position structure can be used. The user's function given as argument is executed at sample time and therefore bears the same restrictions as the background functions of functionally defined transforms : short processing, no read, prints and so on. This type of function will also be called a background function. .PP The function .B distance specifies a distance modifier for the goal positions. Modifications are expressed in the .I tool frame. The first argument is a string specifying the directions. Each direction is expressed with two letters. The first letter can be either `d' or `r', standing for `distance' and `rotation'. The second letter can be either `x', `y', or `z', standing for the principal axes of the tool frame. A valid directions specification is : .br .cs R 23 .DS L "dx rz" : translation along X, rotation around X .DE .cs R The remaining arguments are the magnitudes of the modifications. For example : .br .cs R 23 .DS L distance("dz", -30.); move(p); move(p); .DE .cs R implements a `approach' style motion in the `Z' direction of the tool. Modifications are obtained by internally multiplying the $POS$ part of the canonical transformation equation by a modification transform. Any combination of directions can be specified, however magnitudes should remain small for rotations. The function distance is also very usefull for motions terminated on condition to purposely specify an overshooting motion. .PP When a stop is needed the call : .br .cs R 23 .DS L stop(n); .DE .cs R where `n' is a duration in milliseconds repeats the last move statement with all it's attributes, except the time attributes. For example : .br .cs R 23 .DS L evalfn(myfunction); distance("dx ry", 10., 3.); move(p); evalfn(myfunction); distance("dx ry", 10., 3.); setime(200, 2000); move(p); .DE .cs R is more conveniently written as : .br .cs R 23 .DS L evalfn(myfunction); distance("dx ry", 10., 3.); move(p); stop(2000); .DE .cs R .NH 2 Synchronization .PP A more delicate programming of the time aspect is the price paid for the gain in flexibility obtained by the motion queue feature. Synchronization is basically achieved by `suspending' the execution of the user's program while motion requests are performed. This is can be done in two ways or by a combinations of both. The program execution is suspended when spending time performing computations and input output. Suppose a program that interacts with a user or with some long response sensor, we obtain the following pattern : .br .cs R 23 .DS L TRSF_PTR z, e , b; POS_PTR p; double iz; z = gentr_trsl("Z", 0., 0., 864.); e = gentr_trsl("E" , 0. , 0. , 170.); b = gentr_rot("B", 600. , 128., 800., yunit, 180.); b->fn = hold; p = makeposition("P" , z, t6, e, EQ, b, TL, e); for (; ; ) { printf("enter Z increment "); scanf("%f", &iz); b->p.z += iz; move(p); } .DE .cs R Each time the user enters data via `scanf', the value of the $B$ transform is changed, since its type is .I hold the new value is entered in the motion queue as well as the motion request itself and the next loop immediately prompts the user for a new data entry. If the user enters data quicker than the manipulator can move to the goal positions, she or he will be able to enter several requests ahead. If the user stops entering data, the requests will eventually be served, the manipulator brought to rest and the program execution suspended at the `scanf' instruction. If the data is provided by some external device, say another computer, a file ,or a sensing device the program will look like : .br .cs R 23 .DS L for (; ; ) { gettr(b, device); move(p); } .DE .cs R where `gettr' returns a new transform value. The data is obtained asynchronously with respect to the motions, consequently two situations can occur. Either the external device is faster and the queue will fill up, either the arm is faster and it will wait for new data. In both cases we obtain an optimal utilization of resources. The only problem is to prevent the queue from becoming saturated. The external variable .B requestnb is maintained by RCCL as the number of non served motion requests. We can now introduce the primitive .B waitas (a macro) that takes as argument a predicate : .br .cs R 23 .DS L waitas(pred) .DE .cs R The macro .B waitas suspends the programs execution until the predicate becomes true. The final version of the loop is : .br .cs R 23 .DS L for (; ; ) { gettr(b, device); waitas(nbrequest < MAX) move(p); } .DE .cs R .PP The primitive .B waitfor (a macro) suspends program execution until occurrence of an .I event. We have seen that the `end' .B event is associated with each position record. One application is to obtain a gripper opening and closing at given moments. The pattern of code : .br .cs R 23 .DS L distance("dz", -30.); move(p); move(p); distance("dz", -30.); move(p); .DE .cs R implements a possible position `approach, reach, and depart' sequence. To obtain a synchronized gripper opening and closing, the pattern is : .br .cs R 23 .DS L distance("dz", -30.); move(p); move(p); distance("dz", -30.); move(p); waitfor(p->end) OPEN; waitfor(p->end) CLOSE; .DE .cs R The program is first suspended until, the `approach' position is reached, opens the gripper, waits for the position to be reached, and closes the gripper. One other application of .B waitfor is to obtain a suspension of the program until all the requests are served. For example suppose that a function allocates positions and transforms that have to be freed of upon termination of the function, we must make sure that all the requests are executed before doing so : .br .cs R 23 .DS L dothat() { TRSF_PTR t1 = gentr_ ... TRSF_PTR t2 = gentr_ ... POS_PTR p1 = makeposition( ... POS_PTR p2 = makeposition( ... move(p1); move(p2); ... move(there); waitfor(completed); freetrans(t1); freetrans(t2); ... freepos(p1); freepos(p2); ... return; } .DE .cs R The following program makes use of the .B there position that always reflects the position that the manipulator is occupying at the end of the previous path segment. Thus, when the `waitfor' statement is issued, we are sure that all the previous requests are served and the corresponding positions are no longer in use. Note that the statement : .br .cs R 23 .DS L waitas(requestnb == 0) .DE .cs R would do equally well. .PP Another way to obtain synchronous actions is to trigger them from a motion associated background function. For example a gripper opening can be specified at a given point of a trajectory. We will make use of the external variable .B goalpos, which is an ordinary position pointer updated by the system such as to point at the position equation currently being evaluated. It can be used in the main program to decide which position equation is currently being evaluated. The background functions can also use the pointer .B goalpos to access the fields of a position structure, and the use of several global position pointers can be avoided. These function can then be written independently of a given motion statement. .br .cs R 23 .DS L pumatask() { int openfn(); ... evalfn(openfn); move(p); ... } openfn() { if (goalpos->scal > .5) { OPEN; } } .DE .cs R or using an event : .br .cs R 23 .DS L event openit = 0; /* global variable */ pumatask() { int openfn(); ... evalfn(openfn); move(p); waitfor(openit) OPEN; ... } openfn() { if (goalpos->scal > .5 && openit > 0) { --openit; } } .DE .cs R Yet another possibility would be : .br .cs R 23 .DS L pumatask() { ... move(p); waitas(goalpos == p && p->scal > .5) OPEN; ... } .DE .cs R .PP According to the situation, different combinations of these techniques can be used. Libraries of customized functions or macros could be written to best suit the requirements. .PP The `wait' style primitives have the property of suspending the program execution until occurrence of an event or condition. One must be aware that the following code pattern : .br .cs R 23 .DS L move(p0); waitfor(p0->end); move(p1); waitfor(p1->end); or move(p0); waitfor(completed); move(p1); waitfor(completed); .DE .cs R causes each position to be evaluated twice, since a new request is entered into the queue only when the previous is completed. At that time, the system finds the queue empty and reissues a move to the last position. .PP In more detail we show the body of the macros .B waitas and .B waitfor : .br .cs R 23 .DS L #define waitas(predicate) {while(!(predicate)) suspendfg();} #define waitfor(event) {++(event); while(event > 0) suspendfg();} .DE .cs R The function .B suspendfg merely suspends the foreground program or user's process during a short period of time (currently .1 second). The `setpoint' process, running in background at high priority maintains the .I events associated with positions and the .I event .B completed. The code in the .I setpoint process looks like : .br .cs R 23 .DS L newm = dequeue(&mqueue); /* try to get a new request */ if (newm == NULL) { /* then none */ --completed; /* signal queue is empty */ } .DE .cs R .PP Consider the following situation : .br .cs R 23 .DS L move(p0); move(p1); /* do a lot of computations and/or io */ ... waitfor(p0->end); .DE .cs R If the sequence of code between the move requests and the wait statement takes more time to execute than the motion to be performed, the task will not hang at the level of the wait statement. .PP One additonal point has to be considered, suppose we have the following situation : .br .cs R 23 .DS L move(p0); distance(...); move(p0); distance(...); move(p0); move(p0); waitfor(p0->end) /* do something */ waitfor(p0->end) /* do another thing */ .DE .cs R In this sequence of code the `p0->end' .I event will occur four times, but will be waited for only two times. If the sequence of code is in a loop, an unmatching number of moves and corresponding waits will shift the synchronization each loop by the difference. One way to get around that is to reset the event count prior to issuing the .I move requests : .br .cs R 23 .DS L for (...) { p0->end = 0; move(p0); distance(...); move(p0); distance(...); move(p0); move(p0); waitfor(p0->end) /* do something */ waitfor(p0->end) /* do another thing */ } .DE .cs R .NH 2 Functionally Defined Motions .PP One of the principal features of RCCL is the provision for functionally defined motions. They are approached in very general terms. Except the $POS$ transform of the canonical equation, any of the transforms of a position equation can be functionally defined. We will look in this section at transforms as functions of time. Let us examine again the typical transformation graph of a .I Cartesian motion : .br .cs R 23 .DS L T6 TOOL DRIVE -----> M -----> T <----- | | S G | | <----- W -----> O -----> BASE OBJ GRASP .DE .cs R We notice that the transforms $T6$ and $DRIVE$ are themselves function of time. The system internally computes the values of the $DRIVE$ transform such that the frame immediately on its left, $T$, moves along straight lines and rotates around fixed axes with respect to $S$, $W$, $O$, or $G$. One might notice that the combination $T~ TOOL$ can be considered as a single transform function of time such that : .EQ T6~TOOL~DRIVE sup -1 = CONSTANT .EN The $DRIVE$ transform can be broken down into a translational part and a rotational part : .EQ DRIVE~=~D sub t~D sub r .EN The $D sub t$ transform determines the path of the .I center of rotation, .R while $D sub r$ determines the rotation itself, which is a motion that one can easily visualize. The decomposition .EQ DRIVE~=~D sub r~D .EN is also possible but the second transformation cannot be a pure translation in the general case. It is also more difficult to visualize, because any change in the rotation part will also cause a change in the final translational position. Actually, this situation occurs for the transformation product $T6~TOOL$, which behaves symmetricly with respect to $DRIVE$. This effect can be observed when a manipulator equipped with a tool performs a pure rotation around the tip of the tool : the manipulator must perform translations and rotations whereas pure translations only require translations. This must be kept in mind when introducing functionally defined transforms in the position equations. It is important to carefully determine the placement of the center of rotation when laying out a functionally defined position equation. .PP For simplification we shall assume that the goal position has been reached so that the $DRIVE$ transform is reduced to unity. We shall also only keep two frames, the world frame $W$, and the tool frame $T$. Let $T(t)$ and $R(t)$ two transforms, pure translation and pure rotation function of time. The four graphs leading to pure translations and pure rotations in .I world or base coordinates and then in .I tool coordinates are : .IP Pure translation in .B world coordinates : .br .cs R 23 .DS L T6 TOOL -----> -----> | | W T | | <----- -----> BASE T(t) .DE .cs R .IP Pure translation in .B tool coordinates : .br .cs R 23 .DS L T6 TOOL -----> ------> | | W T | | <----- <----- BASE T(t) .DE .cs R .PP As mentioned before, pure translations lead to pure translations, no matter what frame we are working in. The difference between these two cases is that if $T(t)$ changes along a principal direction, the frame $T$ will also change along a principal direction in .I world coordinates in the first case, and in .I tool coordinates in the second case. .IP Pure rotation in .I world coordinates. .br .cs R 23 .DS L T6 TOOL -----> -----> | | W T | | <----- -----> BASE R(t) .DE .cs R .IP Pure rotation in .B tool coordinates : .br .cs R 23 .DS L T6 TOOL -----> ------> | | W T | | <----- <----- BASE R(t) .DE .cs R .PP The first case will cause the center of rotation to be fixed with respect to $W$ (move with respect to $T$). The second case will cause the center of rotation to move with respect to $W$ (be fixed with respect to $W$). We leave to reader the writing of the corresponding equations. Armed with this conceptual tool we can introduce actual examples. .bp .PP .B 1) .R The first example defines two locations that differ by position and orientation. The two positions are described with respect to a moving frame in .I world coordinates. A loop causes a motion back and forth from one position to the other. The final motion translates along the Y axis. .br .cs R 23 .DS L 1 #include "rccl.h" 2 3 pumatask() 4 { 5 TRSF_PTR z, e , b, pa1, pa2, conv; 6 POS_PTR p0, pt1, pt2; 7 int convfn(); 8 int i; 9 10 conv = newtrans("CONV",convfn); 11 z = gentr_trsl("Z", 0., 0., 864.); 12 e = gentr_trsl("E" , 0. , 0. , 170.); 13 b = gentr_rot("B", 600. , -500., 600., yunit, 180.); 14 pa1 = gentr_eul("PA1" , 30., 0., 50., 0., 20., 0.); 15 pa2 = gentr_eul("PA2" , -30., 0., 50., 0., -20., 0.); 16 17 p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e); 18 pt1 = makeposition("PT1", z, t6, e, EQ, conv, b, pa1, TL, e); 19 pt2 = makeposition("PT2", z, t6, e, EQ, conv, b, pa2, TL, e); 20 21 setvel(300, 50); 22 setmod('c'); 23 setime(300, 0); 24 move(p0); 25 for (i = 0; i < 4; ++i) { 26 movecart(pt1, 100, 1000); 27 movecart(pt2, 100, 1000); 28 } 29 setmod('j'); 30 move(park); 31 } 32 33 convfn(t) 34 TRSF_PTR t; 35 { 36 t->p.y += 3.; 37 } .DE .cs R Line 1 includes the necessary RCCL declarations. Line 3 deserves a comment : when using the puma manipulator, the RCCL library calls the function `pumatask' as the task to be executed. Before calling the `pumatask' function, the system perform some initializations. When the function returns, as you might expect, the system performs a `waitfor(completed)' before concluding and exiting. Line 5 and 6, allocates transform and position pointers as needed by the task. Line 7 declares the name `convfn' as a pointer to a function that describes the moving coordinate frame, and line 8 allocates a counter variable. Line 10, allocates a functionally defined transform attached to `convfn'. Lines 11 through 15, allocate and initialize transforms as described earlier. The $Z$ transform sets a frame at the base of the manipulator. The $E$ and $B$ transforms are the tool transform and a location with respect to the simulated conveyor. Note that the $B$ transform contains a 180 degree rotation around the Y axis such as the Z direction of frame described by $B$ points downward (relatively to $CONV$ and $Z$). The transforms $PA1$ and $PA2$ define two locations with respect to the frame described by $B$. .PP Lines 17, 18, and 19 set up the position equations as described earlier. .PP Line 21 sets the velocity to 300 millimeters per seconds and 50 degrees per second and the motion mode is set to .I Cartesian mode on line 22. The call to .B setime on line 23, containing a null segment time, and specifies a 3/10 of a second acceleration time when reaching P0 to allow for a sufficiently long transition time because the next motion occurs with respect to a moving frame (the system has no means to now how fast it is going to move). The `for' loop, lines 25 to 28, causes eight move requests to be entered in the queue. The eight motions are performed in 1 second each with a 1/10 of a second transition time as specified by the macro .B movecart. Line 29 sets the mode to .I joint because the arm is to perform a large motion and the path the .I tool frame is going to follow is of no concern. Line 30 is the last motion request to the `park' position. .PP The function `convfn', lines 33 to 37, starts being evaluated when the first motion to ``PT1'' begins and during the seven subsequent motions. The background function attached to the transform is called by the system with one argument pointer, a pointer to the transform it is attached to. This permits us to write functions independently from the actual transform they are attached to. Since .B newtrans created the transform $CONV$ as a unit transform, the value of the $p sub y$ element of the position vector increases from 0 to approximatively 286 millimeters( it is increased by 3 millimeters each 28 milliseconds for 8 seconds). At the time the manipulator reaches P0, the $CONV$ transform is equal to the unity transform. The first time the manipulator moves to PT1, the motion is the result of a combination of the .I Cartesian motion from P0 toward PT1 and the motion due to the moving coordinate frame. .PP This example introduce the first method for generating functionally defined motion by a periodic increment of a static variable (here a transform element). .bp .PP .B 2) .R In this second example we will suppose that the manipulator is mounted on a revolving base or that the manipulator is working with respect to a circular conveyor whose rotation axis is collinear with the first joint. We have introduced minor differences in order to point out some other aspects. .br .cs R 23 .DS L 1 #include "rccl.h" 2 3 static int t0; 4 5 pumatask() 6 { 7 TRSF_PTR z, e , b, pa1, pa2, pivot; 8 POS_PTR p0, pt1, pt2; 9 int pivotfn(); 10 int i; 11 12 pivot = newtrans("PIVOT", pivotfn); 13 z = gentr_trsl("Z", 0., 0., 864.); 14 e = gentr_trsl("E" , 0. , 0. , 170.); 15 b = gentr_rot("B1", 600. , -300., 700., yunit, 180.); 16 pa1 = gentr_eul("PA1" , 30., 0., 50., -10., 10., 0.); 17 pa2 = gentr_eul("PA2" , -30., 0., 50., 10., -10., 0.); 18 19 p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e); 20 pt1 = makeposition("PT1", pivot, z, t6, e, EQ, b, pa1, TL, e); 21 pt2 = makeposition("PT2", pivot, z, t6, e, EQ, b, pa2, TL, e); 22 23 setvel(300, 20); 24 setmod('c'); 25 setime(200, 0); 26 move(p0); 27 waitfor(completed); 28 t0 = rtime; 29 for (i = 0; i < 6; ++i) { 30 move(pt1); 31 move(pt2); 32 } 33 setvel(400, 100); 34 setmod('j'); 35 move(park); 36 } 37 38 pivotfn(t) 39 TRSF_PTR t; 40 { 41 Rot(t, zunit, (t0 - rtime) * .010); 42 } .DE .cs R .PP The lines 1 to 26 are basically the same ones and do not deserve further comments. .PP In this example, the moving coordinate frame will explicitly be written as a function of time. It makes use of the external variable .B rtime updated by the system each sample interval. The variable .B rtime reflects the time elapsed since the beginning of the task in milliseconds. Although this variable may be reset by the user to any value, we have chosen to record in `t0' the time when the functionally defined motion begins. Although the position of the moving coordinate frame is periodic, it is necessary to set the beginning of the motion at a given instant in order to keep the resulting task execution within the work range of the manipulator. The macro .B waitfor suspends execution until all the preceding motions are executed and the initial time is recorded at line 28. In actual implementations, the use of an .I event would permit us to synchronize the task execution with arbitrary motions of, say, the conveyor. The segment times in the `for' loop, lines 29 to 32, are left unspecified and will be computed as to obtain an angular velocity of 20 degrees per second (line 23). .PP It is important to notice the placement of the functionally defined transform in position equations so as to produce the desired effect. .PP The functionally defined frame is merely described as a negative rotation around the Z axis of 10 degrees per second. .bp .PP .B 3) .R In this third example the positions $PA1$ and $PA2$ are now described with respect to rotating table off the axis of the manipulator's first joint. This will cause the end effector to rotate such as to maintain a constant orientation with respect to the table. .br .cs R 23 .DS L 1 #include "rccl.h" 2 3 static int t0; 4 5 pumatask() 6 { 7 TRSF_PTR z, e , b, pa1, pa2, table; 8 POS_PTR p0, pt1, pt2; 9 int tablefn(); 10 int i; 11 12 table = newtrans("TABLE", tablefn); 13 z = gentr_trsl("Z", 0., 0., 864.); 14 e = gentr_trsl("E" , 0. , 0. , 170.); 15 b = gentr_rot("B", 600. , 300., 700., yunit, 180.); 16 pa1 = gentr_rpy("PA1" , 0., 0., 0., 0., 0., 10.); 17 pa2 = gentr_rpy("PA2" , 0., 0., 0., 0., 0., -10.); 18 19 p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e); 20 pt1 = makeposition("PT1", z, t6, e, EQ, b, table, pa1, TL, e); 21 pt2 = makeposition("PT2", z, t6, e, EQ, b, table, pa2, TL, e); 22 23 setvel(300, 20); 24 setmod('c'); 25 setime(200, 0); 26 move(p0); 27 waitfor(completed); 28 t0 = rtime; 29 for (i = 0; i < 6; ++i) { 30 move(pt1); 31 move(pt2); 32 } 33 setvel(400, 100); 34 setmod('j'); 35 move(park); 36 } 37 38 tablefn(t) 39 TRSF_PTR t; 40 { 41 real rps = .03; 42 real alpha = rps * (t0 - rtime) * .001; 43 44 t->p.x = 100. * cos(alpha * pit2_m); 45 t->p.y = 100. * sin(alpha * pit2_m); 46 Rot(t, zunit, alpha * 360.); 47 } .DE .cs R The positions equations set apart, this program is quite similar to the previous one. The main difference lies in the way those equations are set up in order to obtain the desired effect. The functionally described transformation is made up from a translation part and a rotation part. The variable `rps' describes a rotational velocity of 3/100 of a rotation per second. The variable .B pit2_m belong to the set of math constant entry points. .bp .PP .B 4) .R The last example describes a task that causes the manipulator end effector to follow a circular path while always being perpendicular to its trajectory. This achieved by setting up a position equation to obtain a remote center of rotation. .br .cs R 23 .DS L 1 #include "rccl.h" 2 3 4 pumatask() 5 { 6 TRSF_PTR z, e , b, perp0, perp, roty; 7 POS_PTR p0, pt; 8 int perpfn(); 9 10 z = gentr_trsl("Z", 0., 0., 864.); 11 e = gentr_trsl("E" , 0. , 0. , 170.); 12 b = gentr_trsl("B", 500. , 300., 600.); 13 roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.); 14 perp0 = gentr_rot("PERP0", 0., 0., 300., xunit, 0.); 15 perp = newtrans("PERP", perpfn); 16 17 p0 = makeposition("P0", z, t6, e, perp0, EQ, b, roty, TL, e); 18 pt = makeposition("PT", z, t6, e, perp0, EQ, b, perp, roty, TL, e); 19 20 setvel(400, 100); 21 setmod('c'); 22 setime(300, 0); 23 move(p0); 24 setime(200, 4000); 25 move(pt); 26 setmod('j'); 27 move(park); 28 } 29 30 perpfn(t) 31 TRSF_PTR t; 32 { 33 real rpm = .20; 34 35 Rot(t, xunit, rpm * goalpos->scal * 360.); 36 } .DE .cs R .PP In this example, the functional motion parameter is the `scal' position structure entry. When the background function is evaluated, the global .B goalpos pointer is equal to `pt'. The variable `rpm' stands for rotations per motion. .IP We have introduce some examples for coding functionally described trajectories. The lay out of the programs, especially the position equation specifications are certainly not unique, and a lot of room is left to imagination.