DPRG
DPRG List  



[DPRG] Robot Code

Subject: [DPRG] Robot Code
From: David P. Anderson dpa at io.isem.smu.edu
Date: Wed Mar 14 16:50:55 CDT 2007

Howdy,

It seems from this thread that a brief discussion of the elements
of subsumption might be useful, both for those unfamiliar with
the techniques and for those with multiple, even competing, definitions
of the methodology.

I thought a good starting place might be to look at several different
arbitration schemes, both from published literature and practical
examples.  This might be a bit ambitious for a list posting, but
might be useful in moving the discussion from theory to practice.
If this works out, we might want to similarly look at other basic
elements of the technique and their variations.

So here goes.  

(Note: I've used a bunch of ascii art here which will probably only 
make sense when viewed with a monospaced font.)

A good starting point might be to read Chapter 9, "Robot Programming,"
of Flynn & Jones' "Mobile Robots" first edition, and Chapter 4, 
"Arbitration," of Jones' "Robot Programming: A Practical Guide."  
I believe the DPRG library has copies of both.

I.  Basic primitive tasks

Most automation basic primitive tasks take on something like the
following generic form:

void task()
{
    while (1) {

        read_sensor();
        calculate_output();
        output_control();

        delay(some_millisecs);
    end 
}

This is an endless loop that executes a certain number of times per second,
as determined by the "some_millisecs" parameter.  (Most of my robots have
a cycle time of 20 Hz, so some_millisecs = 50).

For most tasks, the execution time of the read/calculate/output is
insignificant, measured in microseconds or less, compared to the time
spent in the delay(), measured in milliseconds or greater, as the following
ascii cartoon attempts to illustrate:


 r/c/s             r/c/s             r/c/s             r/c/s
  __                __                __                __
  ||                ||                ||                ||
  ||                ||                ||                ||
  ||      delay     ||     delay      ||    delay       ||    delay
__||________________||________________||________________||________________etc
 
(not to scale)

The delay is, of course, where the other tasks run.  This can be implemented
with a simple cooperative multitasking kernel, or as a series of finite state
machines augmented with timers (AFSMs), the discussion of which is beyond the
scope of this thread (and I hope we don't get bogged down in it!)

Of course some tasks take lots of CPU cycles, like the Kalman filter for a
two-wheel balancer or IMU, image processing, sonar arrays, etc.  My experience
is that these sorts sensors and their associated filters are best offloaded to
separate processors, depending on the horsepower of your particular robot
controller. 


II.  Subsumption and behaviors

For robot tasks used in a subsumption architecture, there is an additional
requirement: an active/inactive flag and its associated threshold:

void subsumption_task()
{
    while (1) {
        read_sensor();
        calculate_output();
        output_control();

        if (output > threshold) flag = TRUE;
        else flag = FALSE;
        
         delay(some_millisecs);
    end
}

(The word "threshold" is used loosely here, as the active/inactive test
may be more complex than a simple greater-than.)

The flag signals whether the task wishes to control the robot for this
cycle (50 millisecs on my robots) or is happy with things the way they
are.  Every task must have a flag that is sometimes true and sometimes
false except the default task, which we'll get to later, which is always
true (hence "default").

As the delay() described in section I allows other tasks to execute, the
flag described here allows those other executing tasks to control the robot,
through the mechanism of "subsumption."

III.  Arbitration

Subsumption tasks are arranged by priority, from lowest to highest.  An
arbitrator() selects the output of the highest priority task whose flag
is true to pass on to the physical subsystem (motors or whatever) for
this pass through the loop, this 20th of a second.

So low priority tasks can only control the robot when all higher priority
tasks' flags are false.  When any higher priority task sets its flag to true,
that tasks' output controls the robot for the next 50 milliseconds, and the
lower priority tasks are all subsumed.

    A.  Mobile Robots arbitration example

Jones and Flynn use the cooperative multi-tasker available in IC for their
example.  They define a group of subsumtion tasks and start them up at
initialization like this:

void main()
{
    start_process(motor_driver());
    start_process(cruise());
    start_process(follow());
    start_process(avoid());
    start_process(escape());
    start_process(arbitrate());
}

These 6 tasks run asychronously and signal the arbitrate process with
their output flags. See the book for details of the particular tasks.

Here is Flynn and Jones' arbitration code from "Mobile Robots" p264 for a scheme
with 4 tasks/layers/behaviors (pick your own terminology...):

void arbitrate()
{
    while (1) {
        if (cruise_output_flag == 1)
            {motor_input = cruise_output; }
        if (follow_output_flag == 1)
            {motor_input = follow_output; }
        if (avoid_output_flag == 1)
            {motor_input = avoid_output; }
        if (escape_output_flag == 1)
            {motor_input = escape_output; }
        sleep(tick);
    }
}

In this case the tasks are arranged from lowest priority, cruise, to highest
priority, escape.   The lowest priority, cruise, is the default behavior and
its flag is always true, so this loop always begins with the motor_input set
to cruise_output.  If no higher priority tasks are asserting their flags, then
that is the output delivered to the motors when the motor task runs during the
sleep(tick) period of delay.

Note that this technique for arbitration might not work in an interrupt driven
environment when the motor updates are happening asynchronously from the
arbitrate() loop.

    B.  Robot Programming arbitration example.

Here is Jones' example from "Robot Programming" fig 4.5 pp82-83.  Jones seems
to prefer schematics over code, as in the following example:


                            +--------------+
    sensor --+------------->|              |
             |              |  Behavior A  |------------+
             |       +----->|  ID = B01    |            |
             |       |      +--------------+            |
             |       |                                  |
             |       |      +--------------+            |
    sensor -----+---------->|              |            |
             |  |    |      |  Behavior B  |----------+ |
             |  |    +----->|  ID = B02    |          | |
             |  |    |      +--------------+          | |
             |  |    |                                | |
             |  |    |      +--------------+          | |
             |  +---------->|              |          | |
             |       |      |  Behavior C  |--------+ | |
             |       |      |  ID = B03    |        | | |
             |       |      +--------------+        | | |
             |       |                              | | |
             |       |      +--------------+        | | |
             +------------->|              |        | | |
                     |      |  Behavior D  |------+ | | |
             +------------->|  ID = B04    |      | | | |
             |       |      +--------------+      | | | |
             |       |                            | | | | 
             |       |      +--------------+      | | | |
    sensor --+------------->|              |      | | | |
                     |      |  Behavior E  |----+ | | | |
                     +----->|  ID = B05    |    | | | | |
                     |      +--------------+    | | | | |
                     |                          V V V V V
                     |     Winner = B02       +----------- +     +-----------+
                     +------------------------| Arbitrater |---->| Actuators |
                                              +----------- +     +-----------+

Note that Jones' illustrates multiple behaviors making use of the same
sensors.  He also includes a feedback line from the arbitrater to the tasks.
This is essentially a global variable that can be used by the individual tasks
to determine which task "won" this round of arbitration.  

                            
    C.  SR04 arbitration example

Here is the arbitration code from my SR04 robot, he of can-collecting fame.
The tasks on SR04 each generate three outputs: a flag, a command, and an
argument.  For the platform motion control, the command is the requested
velocity of the center of the robot, and the argument is the rotational
velocity about the center of the robot, i.e., speed and steering.  These
are implemented as an array of C structures where the tasks write their
outputs, and which the arbitrater reads.

These are defined in a header file as follows:

/* --------------------------------------------------------------- */
/* flags.h */

typedef struct layer LAYER;

struct layer {
        int cmd;        /* assertion command */
        int arg;        /* assertion argument */
        int state;      /* layer state, used by ASFM tasks */
};

/* bit definitions for srat_{flag,enable,suppress,avoid} words */

#define BUMP    1
#define MOTION  2
#define IR      4
#define BOUNDARY 8
#define SONAR 0x10
#define PHOTO 0x20
#define XLATE 0x40
#define PROWL 0x80
#define DEFAULT 0x100

/* --------------------------------------------------------------- */

The arbitration flags on SR04 are implemented as bit positions in a
16 bit flag word (hence only 16 possible layers), as defined above.
These bits definition apply to a set of global masks:

/* srat subsumption global masks */

int srat_flags;                 /* bit = layer asserting */
int srat_avoid;                 /* bit = invert response */
int srat_enable;                /* bit = enable this layer */
int srat_suppress;              /* bit = ignore this layer */

/* --------------------------------------------------------------- */
/* subsumption layers */

LAYER bump,motion,ir,boundary,sonar,photo,xlate,prowl,stop;

LAYER *layers[LAYERS] =
        {&bump,&motion,&ir,&boundary,&sonar,&photo,&xlate,&prowl,&stop};

/* --------------------------------------------------------------- */

(Those familiar with SR04 might notice that there is no "can collecting"
layer.  SR04's can collecting is an emergent behavior formed from a
particular combination of the these tasks and their modes, and a particular
physical environment).

The arbitrater itself looks like this:

int arbitrate()
{
        int i,j;
        i = 0;
        if (srat_system & ARBITRATE) {

                j = ~srat_suppress & srat_flags;  /* suppress */

                while ((j & 1) == 0) {            /* subsume */
                        j /= 2;
                        i++;
                        if (i == LAYERS-1) break; /* default */
                }

                motor_cmd(layers[i]);             /* execute */
        }
}

Note that this is not an endless loop, but rather is called within the robot's
sensor loop, which runs at 20 hz.  The motor_cmd() takes a pointer to the
winning layer and calculates motor commands for the speed control subsystem:

int top_speed;                  /* top allowed speed (user) */
int bot_speed;                  /* current requested robot speed */

int motor_cmd(LAYER *l)
{
        int extern Lvel, Rvel;  /* Left and Right requested motor velocity */

        bot_speed = l->cmd;

        Lvel = bot_speed + l->arg;	/* left motor = velocity + rotation */
        if (Lvel > 100) Lvel = 100;
        else if (Lvel < -100) Lvel = -100;

        Rvel = bot_speed - l->arg;	/* right motor = velocity - rotation*/
        if (Rvel > 100) Rvel = 100;
        else if (Rvel < -100) Rvel = -100;
}


By handing in the actual velocity and rotation arguments, the tasks are able
to generate velocity profiles such that the behaviors blend one into another
for smooth and graceful maneuvering.  But that is a subject for another post.


    D.  jBot's arbitration

jBot arbitration differs from the above only in that there is an indirect
mapping between tasks and their priorities, so that different over-all
priority schemes can be used to solve different problems, as Jones outlines
in Fig 8.5, pp223-225 of "Robot Programming."

The basic jBot structure is like this:

/* --------------------------------------------------------------- */

extern LAYER xlate, user, radio, prowl, escape, obstacle, perimeter;

int arbitrate;                  /* global flag to enable subsumption */
int halt;                       /* global flag to halt robot */

LAYER stop;                     /* the default layer */
LAYER *this_layer = &stop;      /* output, layer chosen by arbitrator() */

LAYER **job;                    /* pointer to job priority list */
int job_size;                   /* number of tasks in priority list */

int arbitrator()
{

        int i;
        i = 0;
        if (arbitrate) {

                for (i = 0; i < job_size-1; i++) {

                        if (job[i]->flag) break;        /* subsume */
                }

                this_layer = job[i];    /* output */

                motorcmd(this_layer->cmd,this_layer->arg);
        }
}


/* ------------------------------------------------------------ */
/* SUBSUMPTION PRIORITY

   The following array determines the subsumption priority
   for job1, from highest (user) to lowest (stop)
*/

#define JOB1_SIZE 8     /* number of layers in job1 */

LAYER *job1[JOB1_SIZE] =
{&user,&radio,&escape,&xlate,&obstacle,&perimeter,&prowl,&stop};

/* make job1 the active job */
int job1_init()
{
        job = &job1[0];         /* global job priority list pointer */
        job_size = JOB1_SIZE;   /* number of tasks in job1 list */
        return 0;
}

/* ------------------------------------------------------------ */
/* EOF */


    IV.  A word about Ballistic Behaviors

Jones uses the term "ballistic" to define a class of behaviors that,
once initiated, use internal timers rather than external sensors to
generate outputs.  The most familiar of these for most robot builders
is the classic bumper behavior, when a bumper press causes the robot
to back up for a while, rotate away from the collision for a while,
and then go forward for a while, before setting its flag back to false.

These are called ballistic behaviors in order to make the distinction
that, like a cannon ball, once begun, they continue to completion
"on their own," as it were.  These behaviors in a certain sense break
the subsumption paradigm, because they cannot be subsumed.  They are
however, in my experience, the exception rather than the rule in terms
of how often they actually control the robot.

Two attributes of ballistic behaviors that may not be immediately apparent:

1.  Ballistic behaviors cannot be subsumed: the must run to completion.

2.  Ballistic behaviors can , however, be aborted.  

In fact, abort is really the only reasonable response of a ballistic task
when subsumed.  For the SR04 robot, the only thing that can subsume a
ballistic bumper behavior, which is the highest priority behavior, is
another bumper press.  That is, if, in the course of a ballistic bumper
behavior, the robot has another collision, the current behavior is aborted,
and a new behavior is initiated from the beginning.

For robots with more than one ballistic behaviors at different priority
levels, like jBot, the "arbitration winner" global defined in II.B.
above, can be used by each task to see if it has been subsumed, and to
abort itself in that case.

best regards,
dpa

More information about the DPRG mailing list

Copyright © 1984 - 2006 Dallas Personal Robotics Group. All rights reserved.
Website Design by NCC

For the latest robot news visit robots.net