|
[DPRG] Robot Code
Subject: [DPRG] Robot Code
From: Chris Jang
cjang at ix.netcom.com
Date: Thu Mar 15 08:36:08 CDT 2007
This is one of the most directly useful posts for my own project work.
Thank you very much, David. Robotic control is not my primary focus.
Computer vision is. But neither is very useful in a robot without the
other. I think everyone soon discovers that all robotic aspects tend
towards high coupling with one another. Mechanical design affects
control systems which affects environment sensing and vice versa.
I've had thoughts of hidden Markov chains and other high concepts...
But my best judgement is that I should build on something proven to
work as well as leverage the experience of those who have already
made it work. I will study your email and read some more.
You posted something earlier about progress as a result of unconventional
approaches to solving problems. I think this is true. But it is also a
balance. Progress happens when working within the limitations of current
technology. In history there are people who are so far ahead of their
time that they make almost no meaningful contribution to the state of the
art. The useful stuff comes from being right on that edge of possibility.
So I believe it is a balance between futuristic concepts and practical
engineering.
Also, at last Tuesday's Garland RBNO, I found that Stella (hacked 1/12
scale RC toy of BMW X5 SUV) can drift. If she backs up at speed and
then turns, the nose tends to rotate around. I think she is capable of
180 degree turns (although I am afraid of tearing up the tires). This
would be a ballistic type behavior.
Chris
-----Original Message-----
>From: "David P. Anderson" <dpa at io.isem.smu.edu>
>Sent: Mar 14, 2007 4:50 PM
>To: dprglist at dprg.org
>Subject: [DPRG] Robot Code
>
>
>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
>
>_______________________________________________
>DPRGlist mailing list
>DPRGlist at dprg.org
>http://list.dprg.org/mailman/listinfo/dprglist
More information about the DPRG mailing list
|
