Programmer 7500

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

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

Wrap

Text File | 1987-03-02 | 50.7 KB | 2,237 lines

.ND .EQ delim $$ .EN .nr H1 11 .NH Teaching .PP The .I teach mode is activated by a call to the .B teach function : .br .cs R 23 .DS L teach(trans, pos) TRSF_PTR trans; POS_PTR pos; .DE .cs R The .B teach function gives control to the user on the manipulator motions. When the teach mode begins, the following message appears on the terminal : .br .cs R 23 .DS L teach mode V1.0, transform TRANS, position POS ? .DE .cs R a simple command line language allows the user to move the manipulator around. When the desired position is obtained, the transform `TRANS' can be solved for the position equation `POS' for the current value of $T6$. This is obtained on user's command by a call to .B update. Once the position is recorded, the manipulator can be moved elsewhere. Upon exit of the .I teach mode, if no position have been recorded the user is prompted as : .br .cs R 23 .DS L nothing taught, exit ? (y/n) .DE .cs R If `n' is answered, the .I teach mode is not exited, if `y' is answered the .I teach mode exits and the .B teach function return the value `NO'. When a transform has been recorded, upon exit the function .B teach returns the value `YES'. Even if a transform has been recorded, .B teach can be forced to return `NO' by typing a `q!'. Applications of this have been shown in section 7. If successive records are made, only the last one is taken into account. .PP When a data base file has been specified, the .I teach mode behaves differently. The transform to be taught is searched in the data base under its name, if found, the function .B teach directly uses the value and immediately exits returning `YES', .B update is then not called. If the transform cannot be found in the data base, .B teach enter the regular manual mode. The user can record the transform value and save it on the data base. If no data base has been specified the user is informed of that fact. A data base editor (see below) can be used for off-line maintenance. .PP The interactive commands are displayed when a `?' mark is typed. By convention, the lower case `x', `y', `z' characters stand for translations or forces, and the upper case `X', `Y', `Z' stand for rotations of moments : .br .cs R 23 .DS L These commands are executed one per line: <return> interrupt arm motion q quit q! quit, ignore not recorded r record transform p display current settings s save transform on data base l toggle force monitor v <t r> set velocities m <m> set mass of object ? this message These commands cumulate: o open hand c close hand w <x/y/z/X/Y/Z> <k> move world coordinates t <x/y/z/X/Y/Z> <k> move tool coordinates e <x/y/z/X/Y/Z> <k> change tool transform f <x/y/z/X/Y/Z> <k> set force limits .DE .cs R .PP Messages from the system can be : .br .cs R 23 .DS L no data base specified nothing recorded stopped on force >> stopped next to limit(s) not so fast .DE .cs R .PP A teach session can be obtained by running the program : .br .cs R 23 .DS L #include "rccl.h" #include "umac.h" pumatask() { TRSF_PTR z, e , b0; POS_PTR p0; z = gentr_trsl("Z", 0., 0., 864.); e = gentr_trsl("E" , 0. , 0. , 170.); b0 = gentr_rot("B0", 600. , -200., 800., yunit, 180.); b0->fn = varb; p0 = makeposition("P0" , z, t6, e, EQ, b0, TL, e); setmod('c'); setvel(300, 100); move(p0); while (teach(b0, p0)) ; setvel(300, 100); move(park); } .DE .cs R .PP The session can look like : .EQ delim off .EN .br .cs R 23 .DS L $ a.out -Ddata data does'nt exit, create ? (y/n) y gettr : B0 not found teach mode V1.0, transform B0, position P0 ?p T6: -1.000 -0.000 -0.000 600.001 -0.000 1.000 0.000 -200.000 0.000 0.000 -1.000 106.000 E: 1.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 1.000 170.000 veloc t:100 r:10 ____1_______ _______2____ _________3__ ____4_______ _________5__ ____6_______ no force limit mass of object : 0.000000 kg ?v 30 7 ?wx200 wz -300 wY10 ? >> stopped not so fast ?wz200 ?l ?p T6: -0.985 0.000 -0.171 826.436 -0.000 1.000 0.000 -200.000 0.171 0.000 -0.985 6.044 E: 1.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 1.000 170.000 veloc t:30 r:7 ____1_______ ________2___ ______3_____ ___4________ __________5_ ____6_______ force limits :fx 0.00 fy 0.00 fz 0.00 fX 0.00 fY 0.00 fZ 0.00 mass of object : 0.000000 kg ?fx20 fY5 ?wz-30 stopped on force ?p T6: -0.985 0.000 -0.171 826.436 -0.000 1.000 0.000 -200.000 0.171 0.000 -0.985 16.013 E: 1.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 1.000 170.000 veloc t:30 r:7 ____1_______ ________2___ ______3_____ ___4________ __________5_ ____6_______ force limits :fx 20.00 fy 0.00 fz 0.00 fX 0.00 fY 5.00 fZ 0.00 mass of object : 0.000000 kg .DE .cs R .EQ delim $$ .EN .PP The .I teach mode uses its own position equation to move the arm around. The tool transform is preset to a 170 mm translation in the Z direction, but can be changed. The messages "not so fast" or "next to limit(s)" do not appear when the condition occurs, but when the next command is typed. The `p' command prints the current values of $T6$, $E$, velocities, the relative position of the joints in their range, the current force limits when toggled on, and the current mass of object. .bp .NH Summary .NH 2 Error Messages .PP An RCCL internal error, causes a message to be printed and an exit of the process for the planning version. When run in real time mode, the process does not exit but the arm power is turned off and the process is put to sleep, this is to allow the user to `break' the program and take advantage of the automatic home position return [6]. If the error occur at the level of the real time interface, we refer the reader to [6] for a determination of the error. If the error is a RCCL error condition, the messages can be : .IP "position "POS" : transform not initialized - makeposition" : one of the transform pointer is `NULL'. .IP "position "POS" : missing t6 or tool - makeposition" : bad position equations structure. .IP "position "POS" : missing rhs - makeposition" : bad position equation structure. .IP "position "POS", transform "TRANS" : pos functionally defined - makeposition" : the $POS$ part of the canonized equation cannot be a moving frame. .IP "position "POS" : pos cannot seriously be t6 - makeposition" : the $POS$ part is equal to $T6$ due to a bad choice of the $TOOL$ part. .IP "giveup" : The function .B giveup has been called, a message follows. .IP "bad spec. - limit" : wrong directions specifications. .IP "bad force spec. - comply" : wrong directions specifications. .IP "bad force spec. - lock" : ditto. .IP "bad distance spec. - distance" : ditto. .IP "conf must change in joint mode" : the current motion mode is not correct for a configuration change. .IP "invalid update transform type" : the involved transform must be of type .I varb. .IP "could'nt find updatable transform" : a transform has been required to be updated but does not belong to the specified equation. .IP "alloc err" : motion queue saturated. .IP "mem. alloc error" : no more dynamic memory allocation space. .IP "limit `time'" : a joint limit occurred at time `time', the program does not exit and tries to recover by stopping and getting a new motion from the queue. (planning version only). .IP "joint(s) limit" : an unrecoverable joint limit occurred. (planning version only). .IP "glitch `time'" : a velocity discontinuity occurred at time `time' (planning version only). .IP "jam" : unexpected behavior of the queue management, should never occur. .IP "cannot unit vector" : the .B unit function has been required to unit a zero magnitude vector. .IP "Write io error", "write io error", "close io error" : an i/o error occurred while writing data (planning version only). .IP "*** could'nt queue at `time'", this message may occasionally appear, but it never did so far. .SH Note .PP The user can use the function .B giveup to cause a task cancellation : .br .cs R 23 .DS L giveup(message, level); .DE .cs R .PP The first argument is a string, the second argument tells if the error condition occur in a background function (level 0) or in the user process (level 1), for example : .br .cs R 23 .DS L pumatask() { ... evalfn(monitor); move(p); while (goalpos == p) { ... if (big_mess) { giveup("cannot do that", 1); } } } monitor() { ... if (not_good) { giveup("wrong data", 0); } } .DE .cs R .PP The error message would be : .br .cs R 23 .DS L cannot do that giveup or wrong data giveup .DE .cs R .NH 2 Functions, Global Variables, and Macros .PP Follows a brief description of the RCCL function library : .SH Dictionary of the terms .IP "ax : " x element of `a' vector of a transform (real). .IP "ay : " y element of `a' vector of a transform (real). .IP "az : " z element of `a' vector of a transform (real). .IP "bool : " an integer expression evaluating to 0 or non zero. .IP "code : " an integer expression (OK LIMIT ONF OND predefined). .IP "conf : " a string at most one of the `l' `r' `u' `d' `f' `n' characters and ` '. .IP "diff : " DIFF_PTR, a pointer to a DIFF structure. .IP "dirs : " a string of the form "fx ty", "tz", ..., for force and distance specs. .IP "eve : " an event count. .IP "force : " FORCE_PTR, a pointer to a FORCE structure. .IP "fp : " a UNIX file pointer *FILE (stdout, stderr ...). .IP "func : " pointer to a function. .IP "level : " an integer expression evaluating to 0 (interrupt) or 1 (user). .IP "list : " a list of transform pointers (TRSF_PTR) separated by commas. .IP "msg : " a string. .IP "mode : " the character `j' or `c'. .IP "name : " a string. .IP "ox : " x element of `o' vector of a transform (real). .IP "oy : " y element of `o' vector of a transform (real). .IP "oz : " z element of `o' vector of a transform (real). .IP "period : " an integer expression in milliseconds. .IP "phi : " an angle in degrees (real). .IP "pos : " a pointer to a position structure (POS_PTR). .IP "pphi : " a pointer to a angle in degrees (*real). .IP "ppsi : " a pointer to a angle in degrees (*real). .IP "psi : " an angle in degrees (real). .IP "pthe : " a pointer to a angle in degrees (*real). .IP "px : " x element of `p' vector of a transform in millimeters (real). .IP "py : " y element of `p' vector of a transform in millimeters (real). .IP "pz : " z element of `p' vector of a transform in millimeters (real). .IP "rotvel : " a rotational velocity in degrees per second (int). .IP "tacc : " an acceleration time in milliseconds (int). .IP "the : " an angle in degrees (real). .IP "time : " a time in milliseconds (int). .IP "trans : " a pointer to a transform structure (TRSF_PTR). .IP "transvel : " a translational velocity in millimeters per second (int). .IP "values : " a list of specifications in millimeters, degrees, Newtons, or Newton-meters. .IP "vect : " pointer to a vector structure (VECT_PTR). .SH Description of Functions, Variables, and Macros .PP Note : if p is pointer, *p is what is pointed to. functions names are marked `f', variables names `v', macros names `m'. .IP f .B assigndiff(diff1, diff2) : .R copy *diff2 into *diff1, return diff1. .IP f .B assignforce(force1, force2) : .R copy *force2 into *force1, return force1. .IP f .B assigntr(trans1, trans2) : .R copy *trans2 into *trans1, return trans1. .IP f .B assignvect(vect1, vect2) : .R copy *vect2 into *vect1, return vect1. .IP v .B completed : .R signaled when motion queue goes empty and the arm is evaluating last position (event). .IP f .B comply(dirs, values) : .R specify compliance for subsequent requests. .IP f .B const() : .R does nothing but typifies a transform as constant (TRFN). .IP f .B cross(vect1, vect2, vect3) : .R compute in *vect1 cross product of *vect2 and *vect3, return vect1. .IP f .B df_to_tr(trans, diff) : .R builds differential transform *trans out of differential motion *diff, return trans. .IP v .B dgtord_m : .R read only (real), convert from degrees to radians what is multiplied by. .IP f .B difftr(diff1, diff2, trans) : .R transforms differential motion *diff2 into differential motion *diff1, with a frame differential relationship *trans, return diff1. .IP f .B distance(dirs, values) : .R internally changes the position expressed in .I tool frame. .IP f .B dot(vect1, vect2) : .R return (real) the dot product of *vect1 and *vect2. .IP f .B eul(trans, phi, theta, psi) : .R set the rotational part of *trans from Euler angles, return trans. .IP f .B eulm(trans, phi, the, psi) : .R multiplies *trans, by a rotation expressed with Euler angles, returns trans. .IP f .B evalfn(func) : .R causes the function *func to be evaluated for next motion request. .IP v .B force_ctl : .R turns on/off force control features (bool). .IP f .B forcetr(force1, force2, trans) : .R transform generalized forces *force2 into generalized forces *force1, with a frame differential relationship *trans, return force1. .IP v .B fpi : .R information file pointer (*FILE). .IP f .B freepos(pos) : .R returns to the memory pool the storage allocated for building a positions equation ring structure. .IP f .B gensym() : .R return a pointer to an always different string (_TEMP1, _TEMP2, ...). .IP f .B gentr_eul(name, px, py, pz, phi, theta, psi) : .R make a constant transforms out of a `p' vector and Euler angles, return a trans. .IP f .B gentr_pao(name, px, py, pz, ax, ay, az, ox, oy, oz) : .R make a constant transforms out of a `p' vector and `a', `o' vectors, return a trans. .IP f .B gentr_rot(name, px, py, pz, vect, theta) : .R make a constant transform out of a `p' vector and a rotation of theta degrees around *vect, return a trans. .IP f .B gentr_rpy(name, px, py, pz, phi, theta, psi) : .R make a constant transforms out of a `p' vector and roll, pitch, yaw angles. return a trans. .IP f .B gentr_trsl(name, px, py, pz) : .R make a constant transforms out of a `p' vector and a unit rotation, return a trans. .IP f .B giveup(msg, level) : .R cancel a task, and print msg when broken. .IP v .B goalpos : .R a read only (POS_PTR), equal to the position pointer of the equation currently evaluated. .IP v .B hdpos : .R a write only (short), hand position information. .IP v .B here : .R a read only (TRSF_PTR), equal to $T6$ at segment termination. .IP f .B hold() : .R does nothing but typifies a transform as to be held (TRFN). .IP f .B invert(trans1, trans2) : .R store in *trans1 the inverse of *trans2, trans1 and trans2 different, return trans1. .IP f .B invertinp(trans) : .R stores in *trans the inverse of *trans, return trans. .IP v .B j6 : .R a read only (JNTS_PTR), the current desired joint setpoint in range coordinates. .IP v .B jd : .R a read only (JNTS_PTR), the desired differential joint setpoint. .IP v .B lastpos : .R a read only (POS_PTR), equal to the position pointer of the last evaluated equation. .IP f .B limit(dirs, values) : .R trigger force or differential motion monitoring for the next motion request. .IP f .B lock(dirs) : .R bring back the arm in position servo mode for the specified directions. .IP f .B makeposition(name, list, EQ, list, TL, trans) : .R build a position equation ring structure, returns a pos. .IP f .B move(pos) : .R enter a motion request toward a position described by pos in the motion queue. .IP m .B movecart(pos, tacc, time) : .R do setmod('c'); setime(tacc, time); move(p). .IP m .B moveconf(pos, tacc, time, conf) : .R do setconf(conf); setmod('j'); setime(tacc, time); move(p). .IP m .B movejnts(pos, tacc, time) : .R do setmod('j'); setime(tacc, time); move(p). .IP f .B newtrans(name, func) : .R allocate storage for a *trans, attach it to function *func, return a trans. .IP v .B nextmove : .R a write only code, when set, causes the current motion interruption and the value returned in the corresponding position structure field `code'. .IP f .B noatoeul(pphi, pthe, ppsi, trans) : .R derive the Euler angles from *trans. .IP f .B noatorpy(pphi, pthe, ppsi, trans) : .R derive the roll pitch yaw angles from *trans. .IP f .B optimize(pos) : .R optimize a position equation ring structure. .IP v .B park : .R a read only (POS_PTR), the park position. .IP v .B pi_m : .R a read only approximation of the number pi (real). .IP v .B pib2_m : .R a read only approximation of the number pi/2 (real). .IP v .B pit2_m : .R a read only an approximation of the number pi*2 (real). .IP f .B printd(diff, fp) : .R print *diff on file *fp. .IP f .B printe(trans, fp) : .R print *trans on file *fp (Euler angles). .IP f .B printm(force, fp) : .R print *force on file *fp. .IP f .B printr(trans, fp) : .R print *trans on file *fp (n o a p). .IP f .B printrn(trans, fp) : .R printf *trans on file *fp (name, n o a p, Euler, rpy). .IP v .B prints_out : .R causes prints when set (bool). .IP f .B printy(trans, fp) : .R print *trans on file *fp (Euler angles). .IP v .B rdtodg_m : .R a read only (real), convert from radians to degrees what is multiplied by. .IP f .B release(msg) : .R closes real time channel. .IP f .B requestnb : .R read only (int), the number of not served motion requests. .IP v .B rest : .R a read only (TRSF_PTR), T6 at the park position. .IP f .B rot(trans, vect, theta) : .R set the rotation part of *trans from a rotation around *vect, of angle theta, returns trans. .IP f .B rotm(trans, vect, theta) : .R multiplies *trans by a rotations made out of a rotation around *vect, of angle theta, return trans. .IP f .B rpy(trans, phi, the, psi) : .R set the rotation part of *trans from a rotations of roll pitch an yaw angles, return trans. .IP f .B rpym(trans, phi, the, psi) : .R multiplies *trans by a rotation of roll pitch and yaw angles, return trans. .IP v .B rtime : .R an (int), the time spend since the last reset, in milliseconds. .IP f .B sample(period) : .R change the sample rate, next motion request. .IP f .B setconf(conf) : .R change the arm configuration next motion request. .IP f .B setime(tacc, time) : .R set the acceleration and segment time next motion request. .IP f .B setmod(mode) : .R set the motion mode, next motion request. .IP f .B setvel(transvel, rotvel) : .R set the translational an rotational velocities, next motion request. .IP f .B startup() : .R start real time channel. .IP f .B stop(time) : .R repeat last motion request, during time. .IP f .B strsave(string) : .R copies string in allocated storage and return pointer to it. .IP f .B suspendfg() : .R put foreground process to sleep for 1/10 of a second. .IP f .B takerot(trans1, trans2) : .R copy `n' `o' `a' vectors of *trans2 into *trans1, return trans1. .IP f .B taketrsl(trans1, trans2) : .R copy `p' vector of *trans2 into *trans1, return trans1. .IP v .B t6 : .R read only (TRSF_PTR), the current desired value of T6. .IP f .B teach(trans, pos) : .R enters manual teach mode, may update *trans, using pos, return user's exit style. .IP v .B there : .R a (POS_PTR) such as move(there) stops the arm. .IP v .B timeincrement : .R a read only (int), the current sample time. .IP f .B tr_to_df(diff, trans) : .R make *diff out a differential transform *trans, returns diff. .IP f .B trmult(trans1, trans2, trans3) : .R multiply *trans2 by *trans3, and store the result in distinct *trans1, return trans1. .IP f .B trmultinp(trans1, trans2) : .R multiply *trans1 by *trans2, and store the result in *trans1, return trans1. .IP f .B trmultinv(trans1, trans2) : .R multiply *trans1 by inverse of *trans2, and store the result in *trans1, return trans1. .IP f .B trsl(trans, px, py, pz) : .R sets the translation part of *trans from p vector, return trans. .IP f .B trslm(trans, px, py, pz) : .R multiply *trans by a translation from p vector, return trans. .IP f .B unit(vect1, vect2) : .R store in *vect1, the unit magnitude vector, collinear with vect2, return vect1. .IP v .B unitr : .R a read only (TRSF_PTR), the unit transform. .IP f .B update(trans, pos) : .R solve *trans in equation *pos, for the value of T6 at the end of the execution of the subsequent motion request. .IP f .B vao(trans, ax, ay, az, ox, oy, oz) : .R set rotation part of *trans from elements of non necessarily orthogonal vectors, return trans. .IP f .B vaom(trans, ax, ay, az, ox, oy, oz) : .R multiply *trans by a rotation from elements of non necessarily orthogonal vectors, return trans. .IP f .B varb() : .R does nothing but typifies a transform as to be variable (TRFN). .IP m .B waitas(bool) : .R evaluates bool every 1/10 of a second and proceed if exp is not 0. .IP m .B waitfor(eve) : .R increment eve, test eve every 1/10 of a second, proceed if eve drops to 0. .IP v .B xunit : .R a read only (VECT_PTR), the X unit vector. .IP v .B yunit : .R a read only (VECT_PTR), the Y unit vector. .IP v .B zunit : .R a read only (VECT_PTR), the Z unit vector. .NH 2 Undocumeted Library Entry Points .PP The following list is a set of undocumented entry points of the basic RCCL library that may cause link conflicts. The labels always end with a recognizable suffix. The user must keep in mind that the entry points of the .I real time control library are still available, but should normally be used only for reading analog to digital conversions, for example. .br .cs R 23 .DS L Functions : assignjs_n checkstate_n dequeue_n diffjnts_n drivefn_n enqueue_n focpyc_n fojnts_n fopar_n getobsj_n getobst_n gravload_n jacobD_n jacobI_n jacobT_n jns_to_tr_n jnsend_n newposition_n newterm_n polycpyc_n polyjnts_n polypar_n select_n setpar_n setpoint_n shifttr_n solveconf_n solved_n solvedo_n solvei_n solveio_n t2jnts_n t2par_n tr_to_jns_n Variables : armk_c iobf_n motionreq_n mqueue_n opsw_n sncs_d .DE .cs R .NH 2 Include Files .SH rccl.h .PP This file includes all the necessary ingredients for writing programs that will link with the RCCL library : .br .cs R 23 .DS L .B " rccl.h" #include <stdio.h> /* included here for safety */ #include <math.h> /* .................................... */ #define YES 1 #define NO 0 #define UNDEF 2 #define OK -1 /* normal path segment termination code */ #define LIMIT -2 /* ran into a limit, arm stopped */ #define ONF -3 /* terminated on force */ #define OND -4 /* terminated on differential motion */ #define PIB2 1.57079632679489660 /* pi / 2 */ #define PI 3.14159265358979320 /* pi */ #define PIT2 6.28318530717958650 /* pi * 2 */ #define RADTODEG 57.29577951308232100 /* 180 / pi */ #define DEGTORAD 0.01745329251994330 /* pi / 180 */ #define SMALL (1.e-5) /* considered as small */ #define EQ 1 /* lhs = rhs */ #define TL 2 /* tool = */ #define malloc malloc_l /* .................................... */ #define free free_l #define realloc realloc_l /* replace dynamic allocation entries */ #define calloc calloc_l #define cfree cfree_l /* .................................... */ .DE .cs R .br .cs R 23 .DS L .B " rccl.h" /* * RCCL typedefs */ typedef int bool; typedef float real; typedef int event; typedef struct vector { real x, y, z; } VECT, *VECT_PTR; typedef int(* TRFN)(); typedef struct transform { char *name; TRFN fn; VECT n, o, a, p; int timeval; } TRSF, *TRSF_PTR; typedef struct jns { char *conf; real th1, th2, th3, th4, th5, th6; } JNTS, *JNTS_PTR; typedef struct posit { char *name; int code; real scal; event end; } POS, *POS_PTR; typedef struct force { VECT f, m; } FORCE, *FORCE_PTR; typedef struct diff { VECT t, r; } DIFF, *DIFF_PTR; .DE .cs R .br .cs R 23 .DS L .B " rccl.h" /* * RCCL functions */ extern POS_PTR makeposition(); extern TRSF_PTR newtrans(), gentr_rot(), gentr_eul(), gentr_rpy(), gentr_pao(), gentr_trsl(), assigntr(), taketrsl(), takerot(), trmult(), trmultinp(), trmultinv(), invert(), invertinp(), trsl(), vao(), rot(), eul(), rpy(), trslm(), vaom(), rotm(), eulm(), rpym(), df_to_tr(); extern DIFF_PTR assigndiff(), tr_to_df(), difftr(); extern FORCE_PTR assignforce(), forcetr(); extern VECT_PTR assignvect(), cross(), unit(); extern real dot(); .DE .cs R .br .cs R 23 .DS L .B " rccl.h" extern int const(), hold(), varb(), optimize(), printd(), printm(), freepos(), startup(), suspendfg(), giveup(), release(), setmod(), setime(), setvel(), evalfn(), setconf(), update(), sample(), massis(), limit(), comply(), lock(), distance(), move(), stop(), noatoeul(), noatorpy(), printr(), printrn(), printe(), printy(), teach(); .DE .cs R .br .cs R 23 .DS L .B " rccl.h" /* * variables */ extern JNTS_PTR j6, /* current joint range values */ jd; /* current joint increments */ extern TRSF_PTR t6, /* current T6 */ here, /* equals T6 each end of segment*/ rest, /* T6 park position */ unitr; /* unit transform */ extern VECT_PTR xunit, /* X unit vector */ yunit, /* Y unit vector */ zunit; /* Z unit vector */ extern POS_PTR lastpos, /* last evaluated position */ goalpos, /* current evaluated position */ there, /* such as t6 = here */ park; /* such as t6 = rest */ extern event completed; /* queue empty */ extern FILE *fpi; /* info file pointer */ extern bool prints_out, /* info prints switch */ force_ctl; /* force control switch */ extern int fddb; /* data base file descriptor */ extern int rtime, /* current time since reset */ timeincrement, /* current sample period */ requestnb, /* number of requests in queue */ nextmove, /* motion interruption flag */ terminate; /* in rtc */ extern real pi_m, /* math constants */ pib2_m, pit2_m, dgtord_m, rdtodg_m; extern short hdpos; /* hand control information */ #define waitas(predicate) {while(!(predicate)) suspendfg();} #define waitfor(event) {++(event);\\\\ while(event > 0) suspendfg();} .DE .cs R .br .cs R 23 .DS L .B " rccl.h" #define Assigntr (void)assigntr #define Taketrsl (void)taketrsl #define Takerot (void)takerot #define Trmult (void)trmult #define Trmultinp (void)trmultinp #define Trmultinv (void)trmultinv #define Invert (void)invert #define Invertinp (void)invertinp #define Trsl (void)trsl #define Vao (void)vao #define Rot (void)rot #define Eul (void)eul #define Rpy (void)rpy #define Trslm (void)trslm #define Vaom (void)vaom #define Rotm (void)rotm #define Eulm (void)eulm #define Rpym (void)rpym #define Assigndiff (void)assigndiff #define Df_to_tr (void)df_to_tr #define Tr_to_df (void)tr_to_df #define Assignforce (void)assignforce #define Forcetr (void)forcetr #define Difftr (void)difftr #define Assignvect (void)assignvect #define Cross (void)cross #define Unit (void)unit #define movecart(p, ta, ts) {setmod('c'); setime(ta, ts); move(p);} #define movejnts(p, ta, ts) {setmod('j'); setime(ta, ts); move(p);} #define moveconf(p, ta, ts, cf) {setconf(cf); setmod('j'); setime(ta, ts); move #define freetrans(t) {free((char *)t); t = NULL;} #define freeposition(p) {freepos(p); p = NULL;} .DE .cs R .SH kine.h .PP This file describes the items related to the kinematics of the considered manipulator. That is why, if you are using the Puma 600, the name 'PUMA' must be #defined somehow. The macros updates the jacobian coefficients, they can be ignored and are listed here for completeness only. The external entries may be of some importance. .br .cs R 23 .DS L .B " kine.h" #ifdef PUMA #define ELBOW_DEG 01 /* elbow degeneracy */ #define ALIGN_DEG 02 /* T6 in X Z Jt 1 plan */ #define WRIST_DEG 03 /* wrist degeneracy */ typedef struct kindyn { real a2, a3, d3, d4, d32, e432, aa3d4, e4aa4ad; real cp21, cp31, cp32, cp50; } KINDYN, *KINDYN_PTR; typedef struct sincos { real c1, s1, c2, s2, c23, s23, c3, s3, c4, s4, c5, s5, c6, s6; real d1x, d1y, d1z, r1x, r1z, d2x, d2y, d2z, d3x, d3y, d3z; real h; TRSF u5; } SNCS, *SNCS_PTR; .DE .cs R .br .cs R 23 .DS L .B " kine.h" /* * Macro updates coef of Jacob from the sin cos */ #define GETH\\\\ {\\\\ sncs_d.h = sncs_d.c2 * armk_c.a2 +\\\\ sncs_d.s23 * armk_c.d4 +\\\\ sncs_d.c23 * armk_c.a3;\\\\ } #define UPDJ\\\\ {\\\\ sncs_d.d1x = sncs_d.h * sncs_d.s4 -\\\\ armk_c.d3 * sncs_d.c23 * sncs_d.c4;\\\\ sncs_d.d1y = sncs_d.s23 * armk_c.d3;\\\\ sncs_d.d1z = sncs_d.h * sncs_d.c4 + armk_c.d3 * sncs_d.c23 * sncs_d.s4;\ sncs_d.r1x = -sncs_d.s23 * sncs_d.c4;\\\\ sncs_d.r1z = sncs_d.s23 * sncs_d.s4;\\\\ sncs_d.d2x = armk_c.a2 * sncs_d.s3 * sncs_d.c4;\\\\ sncs_d.d2y = armk_c.a2 * sncs_d.c3;\\\\ sncs_d.d2z = -armk_c.a2 * sncs_d.s3 * sncs_d.s4;\\\\ sncs_d.d3x = sncs_d.c4 * armk_c.d4;\\\\ sncs_d.d3y = armk_c.a3;\\\\ sncs_d.d3z = -sncs_d.s4 * armk_c.d4;\\\\ } #define GETU5\\\\ {\\\\ sncs_d.u5.n.x = sncs_d.c5 * sncs_d.c6;\\\\ sncs_d.u5.n.y = sncs_d.s5 * sncs_d.c6;\\\\ sncs_d.u5.n.z = sncs_d.s6;\\\\ sncs_d.u5.o.x= -sncs_d.c5 * sncs_d.s6;\\\\ sncs_d.u5.o.y= -sncs_d.s5 * sncs_d.s6;\\\\ sncs_d.u5.o.z = sncs_d.c6;\\\\ sncs_d.u5.a.x = sncs_d.s5;\\\\ sncs_d.u5.a.y= -sncs_d.c5;\\\\ sncs_d.u5.a.z = 0.;\\\\ } #endif #ifdef STAN typedef struct kindyn { real d2, d22; } KINDYN, *KINDYN_PTR; typedef struct sincos { real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6; real d1x, d1y, d1z, r1x, r1y, r1z, d2x, d2y, d2z, r2x, r2y, r2z, d3x, d3y, d3z, r4x, r4y; } SNCS, *SNCS_PTR; .DE .cs R .br .cs R 23 .DS L .B " kine.h" #define UPDJ\\\\ {\\\\ real\\\\ k1 = sncs_d.c4 * sncs_d.c5,\\\\ k2 = sncs_d.s4 * sncs_d.c5,\\\\ k3 = sncs_d.c4 * sncs_d.s5,\\\\ k4 = sncs_d.s2 * sncs_d.d3,\\\\ k5 = k1 * sncs_d.c6,\\\\ k6 = sncs_d.s4 * sncs_d.c6,\\\\ k7 = k5 - sncs_d.s4 * sncs_d.s6,\\\\ k8 = k2 * sncs_d.c6 + sncs_d.c4 * sncs_d.s6,\\\\ k9 = k1 * sncs_d.s6 + k6,\\\\ k10= - k2 * sncs_d.s6 + sncs_d.c4 * sncs_d.c6,\\\\ k11= k5 + k6,\\\\ k12= sncs_d.s4 * sncs_d.s5,\\\\ k13= sncs_d.s5 * sncs_d.c6,\\\\ k14= sncs_d.s5 * sncs_d.s6;\\\\ sncs_d.d1x = (-armk_c.d2 * (sncs_d.c2 * k7 - sncs_d.s2 * k13) +\\\\ k4 * k8);\\\\ sncs_d.d2x = sncs_d.d3 * k7;\\\\ sncs_d.d3x = -k13;\\\\ sncs_d.d1y = (-armk_c.d2 * (- sncs_d.c2 * k9 + sncs_d.s2 * k14) +\\\\ k4 * k10);\\\\ sncs_d.d2y = -sncs_d.d3 * k11;\\\\ sncs_d.d3y = k14;\\\\ sncs_d.d1z = (-armk_c.d2 * (sncs_d.c2 * k3 + sncs_d.s2 * sncs_d.c5) +\\\ k4 * k12);\\\\ sncs_d.d2z = sncs_d.d3 * k3;\\\\ sncs_d.d3z = sncs_d.c5;\\\\ sncs_d.r1x = (-sncs_d.s2 * k7 - sncs_d.c2 * k13);\\\\ sncs_d.r2x = k8;\\\\ sncs_d.r4x = -k13;\\\\ sncs_d.r1y = (sncs_d.s2 * k9 + sncs_d.c2 * k14);\\\\ sncs_d.r2y = k10;\\\\ sncs_d.r4y = k14;\\\\ sncs_d.r1z = (-sncs_d.s2 * k3 + sncs_d.c2 * sncs_d.c5);\\\\ sncs_d.r2z = k12;\\\\ } #endif .DE .cs R .br .cs R 23 .DS L .B " kine.h" extern KINDYN armk_c; /* arm kinematic and dynamic */ /* constants */ extern SNCS sncs_d; /* current sin cos, jacob coeff */ /* and U5 matrix */ extern JNTS jcal_c; /* rest position joint range */ extern JNTS jmin_c; /* angles range offset values */ extern JNTS jrng_c; /* maximum joint range values */ extern JNTS jmxv_c; /* max joint velocities */ .DE .cs R .PP The variable .B armk_c contains all the arm constants : link parameters, and gravity joint loads. The variable .B sncs_d contains a set of variable kinematic parameters updated at sample time intervals : joint angles sines and cosines, the terms of 3 by 3 upper left Jacobian submatrix, computed in link 4, and the matrix $U5$. The variable .B jcal_c is the joint angle values at the `park' position, in radians. The variable .B jmin_c is the set of angle offsets used to map joint angles expressed in solution coordinate frame [-$pi$ ,+$pi$] onto joint angles expressed in range coordinates [0, range]. The variable .B jrng_c is the set of joint ranges in radians. The variable .B jmxv_c is the set of admissible velocities in radians per second. .SH which.h .PP Including this file is equivalent to #define PUMA for now. .br .cs R 23 .DS L .B " which.h" #define PUMA /* current system setting */ #ifdef PUMA #define ARMTYPE 1 /* for the interface */ #define NJOINTS 6 #define VALII /* for the hardware clock */ #else #ifdef STAN #define ARMTYPE 2 #define NJOINTS 6 #else not rich enough #endif #endif .DE .cs R .SH hand.h .PP Macros to operate the pneumatic gripper. .br .cs R 23 .DS L .B " hand.h" #define CLOSE hdpos = 'o'; /* close pneumatic gripper */ #define OPEN hdpos = 'c'; /* open pneumatic gripper */ .DE .cs R .SH umac.h .PP This file defines some useful macros that are self explanatory. The dangerous side effects of macros must be kept in mind, for example : .br .cs R 23 .DS L FABS(dot(vect)) .DE .cs R will call .B dot twice ! .br .cs R 23 .DS L .B " umac.h" #define SINCOS(s, c, a) {s = sin(a); c = cos(a);} #define FABS(a) (((a) < 0.) ? -(a) : (a)) #define ABS(a) (((a) < 0) ? -(a) : (a)) #define ROUND(a) ((a - (double)(int)a >= .5) ? (int)a + 1 : (int)a) #define TERMIO(z) do {errno = 0; z; pause();} while (errno == EINTR); #define GETCHAR(c) while ((c = getchar()) == ' ' \\\\ || c == '\\\\t' || c == '\\\\n') ; #define QUERY(c) printf(" (y/n) "); \\\\ do { \\\\ GETCHAR(c); \\\\ } while (c != 'y' && c != 'n'); \\\\ {int v; \\\\ if ((v = getchar()) != '\\\\n') \\\\ (void) ungetc(v, stdin);} .DE .cs R .SH exiod.h .PP This file describes the bit definition of the 'exio' field of the .B how structure of the real time interface [6]. .br .cs R 23 .DS L .B " exiod.h" #define EXTERN0 01 /* external input/output */ #define EXTERN1 02 /* bit definitions */ #define EXTERN2 04 /* */ #define EXTERN3 010 /* */ #define EXTERN4 020 /* */ #define EXTERN5 040 /* */ #define EXTERN6 0100 /* */ #define EXTERN7 0200 /* */ #define ARMPWR 0400 /* high power on/off bit (high/low) */ #define OFFL 01000 /* external low signal to stop the arm */ #define RUN 02000 /* front panel switch - run bit low */ #define RESTART 04000 /* front panel switch - restart bit low */ #define HNDOH 01000 /* close pneumatic hand/release (high/low) */ #define HNDCH 02000 /* open pneumatic hand/release (high/low) */ #define EXTRA4 04000 /* spare output bit (not wired) */ #define EXTRA0 010000 /* spare I/O bit (not wired) */ #define EXTRA1 020000 /* spare I/O bit (not wired) */ #define EXTRA2 040000 /* spare I/O bit (not wired) */ #define EXTRA3 0100000 /* spare I/O bit (not wired) */ .DE .cs R .bp .NH Transform Data Base .PP A very simple data base system is implemented. Transforms are stored under their names as set in the `name' field of the `TRSF' structure. From the programming point of view the following functions can be called : .br .cs R 23 .DS L maketdb(name) char *name; savetr(trans, fd) TRSF_PTR trans; int fd; gettr(trans, fd) TRSF_PTR trans; int fd; remtr(name, fd) char *name; int fd; dumpdb(fd, v) int fd; bool v; compact(name) char *n; .DE .cs R .PP The function .B maketdb creates an empty transform data base and returns the corresponding file descriptor. This function .B cannot be called, when the real time channel is opened, this is the purpose of the option `-D'. The function .B savetr stores a transform under its name in the data base. If the transform already exits the user is prompt : .br .cs R 23 .DS L change ? (y/n) .DE .cs R if `y' is answered, the value `1' is returned otherwise `0' is returned. The function .B gettr retrieves a transform and sets its value. The value `0' is returned, when the transform is found, `-2' if not. Both functions print an informative message on `stderr' at the time the action is performed. The function .B remtr removes a transform from the data base. The value `0' is returned, when the transform is found, `-2' if not. The function .B dumpdb dumps the contents of the data base described by the first argument on the `stdout' file. The second argument, when non zero, specifies a `verbose' dump. The function .B compact compacts the data base, and permits to save some file space if the data base as been extensively used. This function should not be called from manipulator programs. .PP In manipulator programs, use the file descriptor .B fddb as argument for the data base functions. All these functions return `-1' if something goes wrong. The messages are : .br .cs R 23 .DS L Informative messages : savetr : NAME created : DATE savetr : NAME changed at DATE savetr : NAME added at DATE gettr : NAME last change : DATE gettr : NAME not found\n remtr : NAME removed remtr : NAME not found dump : NUMBER entries Errors messages are : read error on data base file write error on data base file seek error on data base file can't duplicate data base file could'nt unlink bad magic number could'nt creat transform data base file open error on data base file search error data base file saturated .DE .cs R .PP A data base editor called .I edb allows the user to maintain transforms files. The user can modify an active transform with patches or multiplications The active transform can also be read from the data base, renamed, or reset to the unity transform. Transforms can be added to, changed in, or removed from the data base. All combinations are thus allowed. When a `break' is typed at the terminal the following message is printed : .br .cs R 23 .DS L These commands are executed one per line: q quit and save file q! quit and do not save d[v] dump data base [verbose] u <name> use transform 'name' s save active transform n <name> rename active transform : show active transform r <name> remove transform 'name' from file i invert active transform pt x y z patch a translation x y z p <X/Y/Z> a patch a rotation a around X, Y, or Z axis pa x y z x y z patch a rotation defined by a and o vectors pe phi the psi patch a rotation from Euler angles pr phi the psi patch a rotation from roll pitch and yaw angles These commands are cumulative: mt x y z multiply by translation x y z m <X/Y/Z> a multiply by rotation a around X, Y, or Z axis ma x y z x y z multiply by rotation defined by a and o vectors me phi the psi multiply by rotation from Euler angles mr phi the psi multiply by rotation from roll pitch and yaw angles .DE .cs R .EQ delim off .EN .bp .NH Details .NH 2 Compile .PP Nothing special about compilations, use UNIX's .I cc command. In order to be able to include the declaration files independently from the directory they may have been be stored in, a possibility is to define a shell variable, `rccl' say, in your .login or .profile files as .br .cs R 23 .DS L rccl="-I/b/rccl/h" for sh users set rccl=(-I/b/rccl/h) for csh users .DE .cs R .NH 2 Link .PP Your code must be linked with four libraries : .br .cs R 23 .DS L rccl.a The real time version basic library dbot.a The data base library rtc.a The real time channel libnm.a system new math library .DE .cs R .PP One may conveniently expand the `rccl' shell variable : .br .cs R 23 .DS L rccl="-I/b/rccl/h /b/rccl/l/rccl.a /b/rccl/l/dbot.a /b/rccl/l/rtc.a -lnm" set rccl=(-I/b/rccl/h /b/rccl/l/rccl.a /b/rccl/l/dbot.a /b/rccl/l/rtc.a -lnm) .DE .cs R Such that you can type : .br .cs R 23 .DS L $ cc myprog.c $rccl .DE .cs R .PP In order to get the planning version, just set a shell variable, `plan' say : .br .cs R 23 .DS L plan="-I/b/rccl/h /b/rccl/l/rccl.plan /b/rccl/l/dbot.a /b/rccl/l/rtc.a -lnm" set plan=(-I/b/rccl/h /b/rccl/l/rccl.plan /b/rccl/l/dbot.a /b/rccl/l/rtc.a -lnm) .DE .cs R and type : .br .cs R 23 .DS L $ cc myprog.c $plan .DE .cs R .NH 2 Lint .PP Linting programs proves to be very useful, set a shell variable, `rlint' say : .br .cs R 23 .DS L rlint="-I/b/rccl/h -v /b/rccl/l/llib-rccl /b/rccl/l/llib-dbot /b/rccl/l/llib-rtc set rlint=(-I/b/rccl/h -v /b/rccl/l/llib-rccl /b/rccl/l/llib-dbot /b/rccl/l/llib .DE .cs R and type : .br .cs R 23 .DS L $ lint myprog.c $rlint .DE .cs R The llib-rccl, llib-dbot, llib-rtc files contain the descriptions of the functions compiled and stored in the corresponding libraries. .NH 2 Run .PP Type : .br .cs R 23 .DS L $ a.out [-options] .DE .cs R once the channel has been set up and the arm calibrated. The options can be cumulated after `-' (except the `D' option) : .br .cs R 23 .DS L $ a.out -b -v -e -d -g -k -Ddata .DE .cs R is equivalent to .br .cs R 23 .DS L $ a.out -bvedgk -Ddata .DE .cs R .PP You will get the programs .I calib, mkenc, play, dl ,edb, and dsp .R if the `path' of your shell leads to the right directory : .br .cs R 23 .DS L PATH=$PATH:/b/rccl/s (for sh users) export PATH set path=($path /b/rccl/s) (for csh users) .DE .cs R .NH The display program. .PP The .I dsp program uses the terminal in pseudo graphic mode like a page editor. The user's terminal must possess screen addressing capabilities (see termcap(5)). The user's session environment shell variable TERM must be set to the corresponding terminal (adm3a, adm5, vt100, etc..). By default, the .I dsp program reads files of the form : .br .cs R 23 .DS L ../g/file.out .DE .cs R The display of this file is obtained by typing : .br .cs R 23 .DS L $ dsp file .DE .cs R If no argument is given, the user is prompted. .PP The program displays files that are a sequence of numbers of type .B double. The program also looks for a file of the form : .br .cs R 23 .DS L ../g/t.out .DE .cs R that must be a sequence of same length of numbers of type .B int. If the file `t.out' has the proper length, these numbers will appear in the left column of the display. The program also looks for a file of the form : .br .cs R 23 .DS L ../g/c.out .DE .cs R that must be a sequence of characters. These characters will be used for the display on the basis of a one to one correspondence. If the character file is not found, .I dsp uses a `#'. The pseudo graphic display is tilted of 90 degrees to provide a maximum resolution. (low on the left, hight on the right, instead of the usual bottom/top). If you do not like the idea of the "../g" directory place in your shell's environment : .br .cs R 23 .DS L GRAPHDIR="the directory you like" .DE .cs R but the planning library assume that the "../g" directory exists. The program is interactive and the help message is : .br .cs R 23 .DS L !*/s/r/u/d/g/h/b/f/a/v/p/q/+-n/? ? his message +-n [+,-]digits <space> : direct addressing q quit p position display v velocity display a acceleration display f forward one page b backward one page h half page forward g half page backward d down one line u up one line r redraw s scale @ back to prompt !* ! <space> file <space> : show another file Type any character to continue .DE .cs R .bp .NH References .IP [1] Kernighan ,B. K., "The C Programming Language", Prentice-Hall, 1978. .IP [2] Paul, R. P., "Robot Manipulators: Mathematics, Programming, and Control", MIT Press 1981. .IP [3] Hayward V., "Introduction to RCCL : A Robot Control "C" Library", TR-EE 83-43, October 1983. .IP [4] "High Speed QBUS-UNIBUS Interface", Engineering Drawings, School of EE, Purdue University, Nov. 1963. .IP [5] Fisher, W. D., "The Modification of a Robotic Manipulator and Digital Controller to Incorporate Both Force and Possition Control", MSEE Thesis, Purdue University, May 1981. .IP [6] Hayward V., "Robot Real Time Control User's Manual", TR-EE 83-42, October 1983. .IP [7] Paul, R. P., Shimano, B. E., Mayer , E. G., "Kinematic Control Equations for Simple Manipulator", IEEE Transactions on Systems, Man, and Cybernetics, Vol SMC-11, No 6, June 1981. .IP [8] Zhang, H., Paul, R. P., "Determination of Simplified Dynamics of Puma Manipulator", Purdue University. .IP [9] Paul, R. P., Rong Ma, Zhang H., "The Dynamics of the Puma Manipulator", The International Journal of Robotic Research, (to be published).