home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl014
< prev
next >
Wrap
Text File
|
1987-03-02
|
25KB
|
784 lines
.ND
.EQ
delim $$
.EN
.nr H1 7
.NH 1
Structuring Programs
.PP
We shall attempt in this section to show how higher level primitives
can be written in term of RCCL functions.
We shall make use of the macro processing facilities to
define in a few lines some manipulator language statements
often encountered.
A primitive
.B insert
based on a bare bone version of the insertion task explained earlier
is described.
This
.B insert
primitive, newly defined is used in a repetitive task.
Each loop the manipulator moves to a `get' position where
a feeder conveys pegs to be inserted on an assembly.
The holes locations are stored on file and may have been taught in a
previous operation or obtained from a CAD/CAM system.
The loop synchronizes with the feeder's actions via an external
variable :
.br
.cs R 23
.DS L
1 #include "rccl.h"
2 #include "umac.h"
3 #include "hand.h"
4
5 #define AWAYZ(p, l) {distance("dz", - (l)); move(p);}
6 #define OVERSHOOTZ(p, l) {distance("dz", (l)); move(p);}
7 #define FAST setvel(300, 300.);
8 #define SLOW setvel(50, 50.);
9 #define CAUTIOUS setvel(7, 7);
10
11 /*
12 * do one insertion
13 */
14
15 insert(z, grip, peg, hole, depth, ang)
16 TRSF_PTR z, grip, peg, hole;
17 real depth, ang;
18 {
19 TRSF_PTR bottom, angle, roty;
20 POS_PTR align, in, touch;
21
22 bottom = gentr_trsl("BOTTOM", 0., 0., -depth);
23 angle = gentr_rot("ANGLE", 0., 0., 0., yunit, ang);
24 roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
25
26 align = makeposition(
27 "ALIGN", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
28
29 touch = makeposition(
30 "TOUCH", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
31
32 in = makeposition(
33 "IN", z, t6, grip, peg, EQ, hole, bottom, roty, TL, peg);
34
.DE
.cs R
.br
.cs R 23
.DS L
35 setmod('c');
36 FAST
37 AWAYZ(touch, 10.)
38 CAUTIOUS
39 AWAYZ(touch, 4.)
40 limit("fz", 25.);
41 OVERSHOOTZ(touch, 5.)
42 comply("fz", 15.);
43 move(align);
44 lock("fz");
45 comply("fx fy", 0., 0.);
46 update(hole, in);
47 limit("fz", 20.);
48 OVERSHOOTZ(in, 10.);
49 lock("fx fy");
50
51 SLOW
52 AWAYZ(align, 50.)
53 waitfor(in->end)
54 OPEN
55 move(there);
56 waitfor(completed);
57 freepos(align);
58 freepos(in);
59 freepos(touch);
60 freetrans(bottom);
61 freetrans(angle);
62 freetrans(roty);
63 return;
64 }
65
.DE
.cs R
.br
.cs R 23
.DS L
66 /*
67 * monitors feeder
68 */
69
70 #define PARTS 1
71 #define EMPTY 2
72
73 monfeeder()
74 {
75 if (feedersensor == PARTS) {
76 nextmove = YES;
77 CLOSE
78 }
79 if (feedersensor == EMPTY) {
80 parts = 0;
81 }
82 }
83
84 /*
85 * Do insertions
86 */
87
88 int parts = YES;
89
90 pumatask()
91 {
92 TRSF_PTR z, e, assy, h, feeder, grasp, pegs;
93 POS_PTR get;
94
95 z = gentr(); /* base frame */
96 e = gentr(); /* end effector */
97 assy = gentr(); /* assembly */
98 grasp = gentr(); /* gasp pos */
99 feeder = gentr(); /* feeder */
100 pegs = gentr(); /* peg rel. e */
101 h = newtrans("H", hold); /* h rel. to assy */
102
103 get = makeposition("GET", z, t6, e, EQ, feeder, grasp, TL, e);
104
105 while(parts) {
106 move(get);
107 evalfn(monfeeder);
108 setime(200, 10000);
109 move(get);
110 gettr(h, file);
111 insert(z, e, pegs, h, 20., 15.);
112 }
113 }
.DE
.cs R
.bp
.PP
Conveyors are expensive, and rugged objects could be thrown from place
to place.
We shall see here how a `throw' primitive (seldom found in regular
robot programming languages) can be easily written.
In order to obtain a maximum acceleration, we shall program a sequence of
motions that only uses the transition part.
This example is only given as an illustration because the
dynamic qualities of the Puma manipulator proved to be not quite sufficient.
.br
.cs R 23
.DS L
1 #include "rccl.h"
2 #include "hand.h"
3 #include "umac.h"
4
5 real when; /* to open the gripper */
6
7 #define MAXACC .015 /* mm/ms2 */
8
9 throw(v0)
10 VECT_PTR v0;
11 {
12 int openat();
13
14 real Tx = (12. * v0->x) / MAXACC; /* compute */
15 real Ty = (12. * v0->y) / MAXACC; /* the acceleration */
16 real Tz = (12. * v0->z) / MAXACC; /* times */
17 int T = ((FABS(Tx) > (Ty)) /* and pick up */
18 ? ((FABS(Tx) > FABS(Tz))
19 ? Tx : Tz) /* the longest one */
20 : ((FABS(Ty) > FABS(Tz))
21 ? Ty : Tz));
22
23 real dx, dy, dz;
24
25 stop(0);
26 setmod('c');
27 dx = Tx * v0->x / 2.;
28 dy = Ty * v0->y / 2.;
29 dz = Tz * v0->z / 2.;
30
31 distance("dx dy dz", -dx, -dy, -dz);
32 setime(T / 2, T);
33 move(there); /* back up */
34
35 when = .90; /* open the gripper just */
36 evalfn(openat); /* before the end */
37 distance("dx dy dz", 2. * dx, 2. * dy, 2. * dz);
38 setime(T / 2, T);
39 move(there); /* move as fast as possible */
40
41 setime(T / 2, T); /* come back */
42 move(there);
43 stop(0);
44 return;
45 }
46
47
48 openat() /* opens the gripper at a given moment */
49 {
50 if (goalpos->scal >= when) {
51 OPEN
52 }
53 }
.DE
.cs R
.br
.cs R 23
.DS L
54
55 pumatask()
56 {
57 TRSF_PTR b0, grip;
58 POS_PTR p0;
59 VECT vel;
60 int q;
61
62 grip = gentr_trsl("GRIP", 0., 0., 170.);
63 b0 = gentr_rot("B0", 400., 150., 700., yunit, 45.);
64
65 p0 = makeposition("P0", t6, grip, EQ, b0, TL ,grip);
66
67 QUERY(q)
68 CLOSE
69 setconf("d"); /* elbow down, like the Great Di Maggio */
70 setime(100, 3000);
71 move(p0); /* move above the shoulder */
72 vel.x = .0;
73 vel.y = .0;
74 vel.z = .6; /* m/s at 45 degrees, see B0 */
75
76 throw(&vel);
77
78 setmod('j'); /* back to park */
79 setconf("u"); /* elbow up */
80 setime(100, 3000);
81 move(park);
82 }
.DE
.cs R
The acceleration times , lines 14 to 21, and the magnitudes, lines 27 to 29,
are derived from the coefficients of
the quartic polynomial
functions used to generate the transitions [2].
The segment times are exactly twice the accelerations times.
.bp
.NH
Limitations
.NH 2
Force Control
.PP
In the case of the Puma manipulator,
the implementation of force control suffers a number a limitations
due to the simplicity of the implemented method.
Force measurements are obtained by monitoring the motor currents.
Coulomb friction terms, at the joint level,
have been experimently measured [8].
When the velocity of a joint is small or null, these terms are
irrelevant and cannot be used to improve the accuracy of the control.
When the arm if to stop on force, this is of little importance since
the joints likely to provide the guarded motion are moving.
Nevertheless, this fact has to be kept in mind.
Gravity loadings have also been experimently measured.
Experiments have shown that although the mass of an object carried by
the manipulator could be measured, the accuracy is not sufficient
and is likely to cause offset errors for the gravity loadings.
The function
.B massis
has been implemented to get around this.
.PP
Force specifications possess an estimated accuracy
of approximately 10 Newton in most of the work space.
This is pretty close to the load capabilities of the manipulator,
therefore extreme prudence is recommanded.
Despite this lack of accuracy, the tasks using force control
explained in this document all run with a good reliability.
.PP
When the manipulator transitions from
.I comply
servo mode to
.I position
servo mode, a glitch often occurs and is as noticeable as the velocity is high
and the load important.
It is usually harmless, and correspond to the position
servo correcting the first setpoint.
.PP
Compliance in a given direction is obtained by selecting the
joints most suitable to provide the desired effects [2].
The joint selection method is simplified.
It does not take into account the translation part the
.I tool
frame.
This means that in
.I comply
servo mode, force specifications will always
match the inner joints (1, 2, 3) and torques specifications the wrist
joints (4, 5, 6).
Although the method is reliable and simple, it suffers
the drawback that no remote center of compliance can be specified.
Time constraints have prevented further work on this points, and
any contributions are welcome.
.NH 2
Machine Errors.
.PP
When the robot task is running in real time, the process
is locked into core memory and the interrupt function
of the system as well as the user's background functions
are run at very high priority in kernel mode.
Any system call (machine trap), will crash the system (beware of the prints).
The same problem occurs for any machine error like a bad memory reference
of a floating point exception in any part of the process.
Some debugging tools are provided as explained later.
.NH 2
Process Size
.PP
When the real time process is run, it is locked into core memory
and the virtual memory system desactivated.
Therefore, the process cannot grow it's allocated region.
Dynamic allocation is performed within a preallocated memory area.
The system calls like `malloc' are replaced by alternative
functions [6].
A set a macros :
.br
.cs R 23
.DS L
#define malloc malloc_l
#define free free_l
#define realloc realloc_l
#define calloc calloc_l
#define cfree cfree_l
.DE
.cs R
allows the user to safely write :
.br
.cs R 23
.DS L
p = malloc(20);
.DE
.cs R
.PP
This causes a more annoying problem when it comes to opening files.
Files can be opened only when the real time channel is closed.
However, the user can always code :
.br
.cs R 23
.DS L
...
move(p);
stop(200);
waitfor(completed);
release("opening files");
fd1 = creat(...
fd2 = open(...
startup();
move(...);
.DE
.cs R
The process is temporally put back in normal mode by the function
.B release
[6], and file `opens' can
take place.
The function
.B startup
will resume real time execution by the depressing the ARM POWER
button when requested by the system.
Failing to follow the procedure will also cause a system crash.
.NH 2
Sample
.PP
The sample period is normally 28 ms.
It can be set to 14 via the function
.B sample
and when not needed, the sample period can be reset to 28 ms.
Changing the sample period can cause a slight glitch.
If the velocity if the manipulator is small, the glitch is negligible.
For example the for loop of the example section 7.3
can be coded :
.br
.cs R 23
.DS L
32 for (; ; ) {
33 setvel(400, 300);
34 move(get);
35 move(p1);
stop(0);
36 setvel(100, 100);
37 sample(15);
38 move(p2);
stop(0);
39 sample(30);
40 printf("more ?"); QUERY(q); if (q == 'n') break;
41 }
.DE
.cs R
.PP
If the user's background functions take to much time to execute, the
behavior of the real time interface no always easy to predict.
In the best cases, it causes a crash of the superviser program running
in the LSI-11.
The arm power is immediately turned off, and nothing annoying happens.
The superviser is restarted and everything comes back to
normal.
It seems that when the user's functions processing time is slightly too long,
the VAX still accepts interrupts, but stacks them and this quickly
causes a system crash.
Finally, if the interrupt code is very long (an infinite loop, for example),
the system is totally blocked and a manual boot is necessary.
.NH 2
Large Rotations.
.PP
For a reason that has not been yet determined, some motion transitions
involving large rotations do not behave quite correctly.
.NH
The Planner and Play Program
.PP
In order to write and debug the first draft of manipulator
programs, a special library is provided.
This library has exactly the same entry points as the regular
library, but replaces the interrupt code with a loop.
Exactly the same programs can by run and tested.
The synchronization features are simulated so that everything happens
in the same order as in the real time version.
The user is advised to run the programs in this mode
before actual execution.
The resulting sequence of points can be dumped onto file
for execution by the
.I play
program [6].
Trajectories can be also stored and displayed on the terminal
by a special program
called
.I dsp.
.PP
When programs with guarded motions are run in this fashion,
the conditions will never be met, unless
special simulation monitoring functions are written.
When programs include comply statements, the comply mode is
simulated as follow :
the compliant joints are selected according to the geometry of the task
and are artificially frozen as if the resulting forces would keep
them immobile.
The accommodation motions compensation feature being still activated,
it may produce
funny but meaningful trajectories.
Tracking with external information can produce various results
according to the situation at hand.
Nevertheless it is very useful to test ahead manipulator programs.
All branches must be tested because manipulator control is
essentially programming with side effects.
It is always useful to `play' the resulting trajectories in free
space to get an idea of what is going to happen.
.bp
.NH
Program Options
.PP
Programs can be run with a number of options :
.IP v
This option allows the user to specify the printing of information.
A file called `@@.out' is created in the current directory.
It contains informations about what the system understood of the
calls to
.B makeposition.
A record will be printed for each move request.
For the planning version only, a record will be printed by
the trajectory generator at the time the request is executed, for example
the beginning of the file `@@.out' for the camera guided task
Section 7.1. is :
.br
.cs R 23
.DS L
makeposition, pos "LOOK" Z T6 E = EXPECT
optim, initial equation : T6 = -Z EXPECT -E
optim, final equation : T6 = _TEMP1 -E
"COORD": "TOOL": -E "POS": _TEMP1
makeposition, pos "GET" T6 E = COORD CAM O
optim, initial equation : T6 = COORD(h) CAM O(h) -E
optim, final equation : T6 = COORD(h) CAM O(h) -E
"COORD": COORD(h) CAM "TOOL": -E "POS": O(h)
makeposition, pos "PUT" Z T6 E = DROP
optim, initial equation : T6 = -Z DROP -E
optim, final equation : T6 = _TEMP2 -E
"COORD": "TOOL": -E "POS": _TEMP2
.DE
.cs R
.br
.cs R 23
.DS L
request LOOK mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
PARK -1 28 j 84 84 280 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
LOOK -1 336 j 56 56 2660 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
.DE
.cs R
.br
.cs R 23
.DS L
request GET mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
dist dz : -30
request GET mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
request STOP mode j acct 0 sgt 28 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
request GET mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
dist dz : -30
GET -1 3024 j 56 56 1568 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 04 0 0 -30 0 0 0
GET -1 4592 j 56 56 280 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
GET -1 4872 j 56 56 140 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
.DE
.cs R
.br
.cs R 23
.DS L
request PUT mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
GET -1 5012 j 56 56 280 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 04 0 0 -30 0 0 0
PUT -1 5292 j 56 56 2492 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
PUT -1 7784 j 56 56 112 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 00 0 0 0 0 0 0
request LOOK mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
Etc ...
.DE
.cs R
The equations are printed, then their canonized form before and
after optimization.
Transforms are marked according to their type : varb (v), hold (h),
functional (s).
The optimization premultiplications generate the `_TEMPx' names.
For each request all the parameters are printed, for example :
.br
.cs R 23
.DS L
request LOOK mode j acct 56 sgt 0 velt 200 velr 100
conf upd : smpl 0 mass 0.000000
.DE
.cs R
means : position `LOOK', mode `joint', acceleration time 56 ms,
segment time is 0 that is : will be computed at execution time,
current velocities are 200 mm/s and 100 deg/s, no configuration change
required, no transform to update, no sample time change, current mass of
object is 0. kg.
.IP
The trajectory generator prints in a compact format the specification
at the beginning of each motion (planning version only) :
.br
.cs R 23
.DS L
GET -1 5012 j 56 56 280 28
force 00 0 0 0 0 0 0
cply 00 0 0 0 0 0 0
dst 00 0 0 0 0 0 0
exd 04 0 0 -30 0 0 0
.DE
.cs R
means : previous motion terminated `OK' (-1), time is 5012 ms,
mode is `joint', accelerations times first transition is 56, second 56,
segment time is 280, time increment is 28.
No force limit, no comply, no differential motion stop, distance is
-30 mm in Z direction.
For the records `force', `cpy', `dst', and `exd', the first number is
an octal code (00 for no specification, translation or forces :
01 for X, 02 for Y, 04 for Z, rotations or torques : 10 for X, 20 for Y,
40 for Z, and the combinations : 01, 03, 05, 06, etc ...);
.IP
If the the option `-vv' as very verbose is given, the values
of the transforms created by the `gentr...' style function is also
printed.
.IP
This option corresponds to the global flag
.B prints_out.
This flag can be turned on or off the text of the programs
themselves :
.br
.cs R 23
.DS L
...
prints_out = YES;
p0 = makeposition(...);
p1 = makeposition(...);
move(p0);
move(p1);
prints_out = NO;
.DE
.cs R
The information is printed to the
.B fpi
file pointer :
.br
.cs R 23
.DS L
FILE *fpi;
.DE
.cs R
This file pointer is initialized to the `stderr' file pointer.
When the flag
.B prints_out
is set to a non zero value, the
.B makeposition
and
.B move
messages go to the terminal.
When the option `-v' is specified, the file `@@.out' is created and
.B fpi
points to it, and the messages are stored on the file.
One can use this feature for any purposes, for example :
.br
.cs R 23
.DS L
pumatask()
{
TRSF_PTR ...
POS_PTR ...
...
prints_out = NO;
p = makeposition(...);
...
move(p);
...
fprintf(fpi, "bla bla");
...
}
.DE
.cs R
If the task is run without option `-v', "bla bla" goes on `stderr' file,
if the task is run with option `-v', "bla bla" goes into `@@.out'.
.IP g
This is the `graphic' option (planing version only).
The setpoints are stored in the
files `../g/f1.out', `../g/f2.out', one for each joint.
When displayed with the program
.I dsp
a character `J' stands for joint mode straight part, `T' for joint mode
transition, `E' for first point of joint mode transition, `C' for
a Cartesian mode straight part, `H' for Cartesian transition, and `V'
for first point of Cartesian transition.
In order to use this option, the user is required to have a `graphic'
directory `../g' at the same level in the file tree
hierarchy as the current directory.
This will avoid having the current directory constantly clustered with
junk files.
.IP d
This is the `data' option (planning version only),
when specified, the system creates
the file `@.out' in the current directory that will contain one line
per setpoint according to the following format :
.br
.cs R 23
.DS L
POS M time tseg j1 j2 j3 j4 j5 j6 sel
.DE
.cs R
Where `POS' is the name of the goal position, `M' is the mode
(J, T, E, C ,V ,H)
as described above, `j1'...`j6' are the joint angles in range coordinate [6],
and `sel' an octal value showing which joint are complying in comply mode
(0 no joint, 01 for joint 1 , 02 for joint 2, 04 for joint 3, 10 for joint 4,
20 for joint 5, 40 for joint 6, 3 for joint 1 and 2, etc. ).
.IP a
This option when set, causes the joint angles to be output in solution
coordinates [6].
It serves for option `d' and `g'.
.IP k
This option when set, causes the values of $T6$ to be printed in lieu
of the joint angles.
For the option `g' twelve files (f1.out ... f12.out) are created,
the values of the vectors `p', `n', `o', and `a';
For the option `d' the format becomes :
.br
.cs R 23
.DS L
POS M time tseg px py pz nx ny nz ox oy oz ax ay az
.DE
.cs R
It serves for option `d' and `g'.
.IP e
This option causes the file `@@@.out' to be created in the
current directory (planing version only).
The file contains the sequence of encoder values suitable to be
used by the
.I play
program [6].
.IP "Dname "
This option specifies the file `name' as a data base of transforms.
Can be used in association with the teach mode (see below).
.IP
This option corresponds to the global file descriptor
.B fddb
initialized to `-1'.
When the option `-Dname' is specified,
.B fddb
describes the file `name'.
If the file `name' does not exit the user is prompt as :
.br
.cs R 23
.DS L
name does'nt exit, create ? (y/n)
.DE
.cs R
Answer as appropriate.
.IP b
This option turns off the force control features (brute option).
In the case of the planning version, no simulated joint accommodation
will occur.
In the case of the real time version, it allows us to test the manipulator
programs free of obstacles.
.IP
This option corresponds to the global flag
.B force_ctl
which is turn off by the `-b' option.
The flag can be turned on or off in the text of the programs.