Om verwarring te voorkomen: De twee getoonde simulaties zijn zeer verschillend, ze zijn door verschillende mensen geschreven en de gebruikte methoden zijn potentiëel verschillend. Niettegenstaande zijn ze beide gebaseerd op de Dual Dynamics design van Herbert Jaeger. Hetgeen ik er nog over te zeggen heb vindt u hieronder (ik hoop dat Sven zijn uitleg ook binnen brengt)...
We zijn oorspronkelkijk van start gegaan met het zo dicht mogelijk benaderen van Herberts schema. De source zag er dan ongeveer als volgt uit:
#include <assert.h> #include <math.h> #include "smb2pdl1.h" #include "smb2io.h" #include "smb2dlog.h" #include <stdio.h> #include <stdlib.h> quantity nest_l, nest_r; quantity motor_l, motor_r;
Omdat Herbert overal met vectoren
werkt gebruiken wij ook arrays van actuatoren. We werken zeer
specifiek niet met quantities in de array omdat quantities te veel
beperkingen hebben. Het zetten van een value in een quantity is
bijvoorbeeld een beetje een ramp.
De z-vector is de z gedefinieerd
in eq. 12 (p.9); met andere woorden z bevat de huidige toestand van
de actuatoren. Deze wordt bij elke cyclus weer geupdated met de
values uit de motoren.
#define AANTAL_ACTUATOREN 2
#define MOTOR_LINKS 0
#define MOTOR_RECHTS 1
float z[AANTAL_ACTUATOREN];
int cycle=0;
float xabs(float value)
{
if (value<0) return -value;
return value;
}
Het berekenen van de productterm moet steeds gedaan worden op het einde van een elementair behavior. De resulteerde waarde is een value die opgeteld moet worden bij de motoren, vandaar de action procedure. De product procedure neemt de argumenten zoals opgegeven in vergelijking 12 (p.9), de uitvoer komt in de vector u te zitten.
void Action(float a[])
{
add_value(motor_l,a[MOTOR_LINKS]);
if (!cycle) printf("%f|",a[MOTOR_RECHTS]);
add_value(motor_r,a[MOTOR_RECHTS]);
}
void Product(float k, float alfa, float g[], float z[], float u[])
{
int i;
for(i=0;i<AANTAL_ACTUATOREN;i++)
u[i]=k*alfa*(g[i]-z[i]);
}
Ook zoals opgegeven in Herberts
design scheme gebruikten we hier een motor_decay die door elk
elementair behavior opgehoogd/verlaagd wordt. Op het einde van elke
cyclus wordt deze motor_decay term afgetrokken van de motor values.
(dit komt overeen met vergelijking 31 (p.20)). Dat dit niet zo een
goede oplossing heeft Sven reeds uitgelegd...
Verder is er nog
steeds maar een complex behavior dat constant op volle toeren draait.
float motor_decay;
float mu_trace = 1.0F;
float xsigmoid(float x, float beta, float alfa)
{
return 1/(1+exp(-4*beta*(x-alfa)));
}
De procedures die met een T_... beginnen zijn de T_ termen uit vergelijking 6 (p.7).
float T_trl_R(float alfa)
{
float diff;
diff=(value(nest_r)-value(nest_l));
diff/=255.0F;
return xsigmoid(diff,1,0)*(xsigmoid(diff,1,0)-1-alfa);
}
float T_trl_L(float alfa)
{
float diff;
diff=(value(nest_l)-value(nest_r));
diff/=255.0F;
return xsigmoid(diff,1,0)*(xsigmoid(diff,1,0)-1-alfa);
}
float T_trl_F(float alfa)
{
float diff;
diff=(value(nest_l)-value(nest_r));
diff/=255.0F;
return (1-xabs(xsigmoid(diff,1,0)))*
((1-xabs(xsigmoid(diff,1,0)))-1-alfa);
}
De elementaire behaviors zichzelve. Er zijn er 3 gedefinieerd. TurnRight, TurnLeft en Forward. De TurnRight & Turnleft zijn exact overgenomen van p.7 eq. 7. Aan de forward hebben we zelf wat zitten baggeren.
void TurnRight(void)
{
static float alfa=0.0F;
float g[AANTAL_ACTUATOREN],u[AANTAL_ACTUATOREN];
/* alfa */
alfa += mu_trace * T_trl_R(alfa)
-alfa*(1-mu_trace);
assert(alfa>=-1.0F && alfa <=1.0F);
/* toevoegen aan motor_decay */
motor_decay*=(1.0F-xabs(alfa));
/* g */
g[MOTOR_LINKS]=z[MOTOR_LINKS]+50.0F;
g[MOTOR_RECHTS]=z[MOTOR_RECHTS]-50.0F;
/* u */
Product(1.0F,alfa,g,z,u);
/* doit */
Action(u);
}
void TurnLeft(void)
{
static float alfa=0.0F;
float g[AANTAL_ACTUATOREN],u[AANTAL_ACTUATOREN];
/* alfa */
alfa += mu_trace * T_trl_L(alfa)
-alfa*(1-mu_trace);
assert(alfa>=-1.0F && alfa <=1.0F);
/* toevoegen aan motor_decay */
motor_decay*=(1.0F-xabs(alfa));
/* g */
g[MOTOR_LINKS]=z[MOTOR_LINKS]-50.0F;
g[MOTOR_RECHTS]=z[MOTOR_RECHTS]+50.0F;
/* u */
Product(1.0F,alfa,g,z,u);
/* doit */
Action(u);
if (!cycle) printf("a=%f|",alfa);
}
void Forward(void)
{
static float alfa=0.0F;
float g[AANTAL_ACTUATOREN],u[AANTAL_ACTUATOREN];
/* alfa */
alfa += mu_trace * T_trl_F(alfa)
-alfa*(1-mu_trace);
assert(alfa>=-1.0F && alfa <=1.0F);
/* toevoegen aan motor_decay */
motor_decay*=(1.0F-xabs(alfa));
/* g */
g[MOTOR_LINKS]=z[MOTOR_LINKS]-5.0F;
g[MOTOR_RECHTS]=z[MOTOR_RECHTS]-5.0F;
/* u */
Product(1.0F,alfa,g,z,u);
/* doit */
Action(u);
if (!cycle) printf("a=%f|",alfa);
}
De routine main_process initialiseerd de motor_decay op 1. Later worden hier andere factoren mee vermenigvuldigt. De z-vector wordt ingesteld, dan worden al de elementaire behaviors aangeroepen en uiteindelijk wordt de decayterm in leven geroepen.
void main_process(void)
{
/* om te beginnen moeten de motor_decay op 1 worden gezet
* en moetde actuator-vector gezet worden */
motor_decay=1.0F;
cycle++;
if (cycle==40) cycle=0;
z[MOTOR_LINKS]=value(motor_l);
z[MOTOR_RECHTS]=value(motor_r);
/* al de complexe behaviors berekenen (alle motivaties) */
/* al de elementaire behaviors berekenen */
TurnLeft();
TurnRight();
Forward();
/* De motor-signalen 'decayen' */
motor_decay=-xabs(motor_decay);
if (!cycle) printf("mr=%f,D=%f\r\n",value(motor_r),motor_decay);
add_value(motor_l,value(motor_l)*motor_decay);
add_value(motor_r,value(motor_r)*motor_decay);
}
int main(void)
{
printf("test4.c v00.00000314\r\n");
init_pdl();
nest_l = add_quantity("Nest l", 255.0,0.0,0.0);
nest_r = add_quantity("Nest r", 255.0,0.0,0.0);
motor_l = add_quantity("Motor l", 100.0,-100.0,0.0);
motor_r = add_quantity("Motor r", 100.0,-100.0,0.0);
connect_sensor(SID_AN2, nest_l);
connect_sensor(SID_AN3, nest_r);
connect_actuator(AID_MOTOR1, motor_l);
connect_actuator(AID_MOTOR2, motor_r);
log_quantity(nest_l,0);
log_quantity(nest_r,0);
log_quantity(motor_l,0);
log_quantity(motor_r,0);
add_process("Main Process",main_process);
run_pdl(-1L);
return(0);
}
Bovenstaande source werkt, maar niet goed. Het gedrag is als volgt. De robot traced zeer goed naar het licht maar als hij dicht bij de lamp komt begint hij zeer sterk te oscileren. In het algemeen zijn we van deze methode moeten afstappen omdat ze te moeilijk te verstaan is. Ik (en Sven zeker) hebben ons suf gepiekerd over het gedrag van de robot bij bepaalde vergelijkingen. Ik veronderstel dat ik te weinig geschoold ben in differentiaalvergelijkingen en hun algemene vorm om vallabele aanpassingen aan te brengen aan de activation-dynamics zodat de robot doet wat ons gevraagd wordt.
Een ander praktisch nadeel was dat de gekregen bal (de lamp dus) uiterst zwak was. Uiteindelijk heb ik dan maar zelf een voetbal gemaakt met een rol plakband , wc-papier, voeding en een 5A-autokoplamp. Cruciaal hierbij was dat de hoogte van de lamp een zeer sterke invloed blijkt te hebben op de sensoren.
Een paar dagen voor de dead-line hadden we nogaltijd niets deftig. Daarom heb ik me tijdens het weekend bezig gehouden met het herwerken van het algemene design. Hierbij heb ik volgende aanpassingen gemaakt.
De kanten van de tekening zijn 1 of -1 (dat wijst zichzelf uit denk ik zo).
De bovenstaande ideeen worden in de volgende code gedemonstreerd
#include <stdlib.h> #include <assert.h> #include <math.h> #include "smb2pdl1.h" #include "smb2io.h" #include "smb2dlog.h" #include <stdio.h> #include <stdlib.h> quantity nest_l, nest_r; quantity motor_l, motor_r; #define AANTAL_ACTUATOREN 2 #define MOTOR_LINKS 0 #define MOTOR_RECHTS 1
Som van de lightsensoren
en verschil tussen de lichtsensoren
beiden tussen -1 en 1
float sum; float diff;
z is de vector die de huidige waarde van de motoren bevat
u is de vector die de totale gevraagde aanpassing aan de actuatoren bevat. Deze zal nadien
genormailzeerd worden met behulp van de totale activatie. Vandaar ook
dat er een sum_activations nodig is.
float z[AANTAL_ACTUATOREN]; /* current state actuators */
float u[AANTAL_ACTUATOREN]={0.0,0.0}; /* uit te voeren staat actuators */
float sum_activations=0.0;
int cycle=0;
float xabs(float value)
{if (value<0.0) return -value;
return value;}
De powerfunction die de achilles hiel van dit programma vormt.
float powerfunc(float a, float x)
{
if (x>1.0F || x<-1.0)
{
printf("assertion !(x>=1.0F || x<=-1.0) x=%g; a=%g\n",x,a);
exit(1);
}
if (x<0) return -powerfunc(a,-x);
if (a<0) return 1.0-powerfunc(-a,1.0-x);
return log(x*(a-1.0)+1.0)/log(a);
}
float xtrapowerfunc(float a, float x)
{
if (x>1.0) return 1.0F;
if (x<-1.0) return -1.0F;
return powerfunc(a,x);
}
float xsigmoid(float x, float beta, float alfa)
{
return 1.0/(1.0+exp(-4.0*beta*(x-alfa)));
}
Het normalizeren en het uitvoeren naar de motoren
void Action()
{
/* al de activations zijn opgeteld en al de changes zijn opgeteld */
/* dan nu eens het gemiddelde maken... */
int i;
for(i=0;i<AANTAL_ACTUATOREN;i++) u[i]/=sum_activations;
add_value(motor_l,u[MOTOR_LINKS]);
add_value(motor_r,u[MOTOR_RECHTS]);
u[0]=0.0;
u[1]=0.0;
sum_activations=0.0;
}
Hier ziet men hoe de product-term functioneert. De alfa bepaalt hoe sterk de voorgestelde aanpassing doorgevoerdt wordt. 'g' bevat de goal-dynamics. 'k' is een algemene constante die alfa verder aanpast, zoals later uitgelegt.
void Product(float k, float alfa, float g[])
{
int i;
assert(alfa>=-1.0F && alfa<=1.0F);
for(i=0;i<AANTAL_ACTUATOREN;i++)
u[i]+=k*alfa*(g[i]-z[i]);
sum_activations+=xabs(k*alfa);
}
float mu_trace = 1.0F;
float mu_goal = 0.0F;
Weer de elementaire behaviors.
De draaibewegingen moeten trager reageren als er veel licht is (anders oscilleert hij zeer sterk) en
moeten natuurlijk reageren op een diff.
float T_trl_L()
{
float answer;
answer=powerfunc(32.0,-diff);
return answer;
}
float T_trl_R()
{
float answer;
answer=powerfunc(32.0,diff);
return answer;
}
float T_goal_L()
{
float answer;
answer=powerfunc(32.0,-diff);
return answer;
}
float T_goal_R()
{
float answer;
answer=powerfunc(32.0,diff);
return answer;
}
Trager voorwaards rijden naarmate er meer light aanwezig is...
float T_trl_F()
{
float answer;
answer=1.0;
return answer;
}
float T_goal_F()
{
float answer;
answer=0.0;
return answer;
}
De elementaire behaviors zichzelve. Merk op dat deze nu veel intuitiever gedefinieerd worden.
void TurnRight(void)
{
float alfa;
float g[AANTAL_ACTUATOREN];
alfa = mu_trace * T_trl_R()
+ mu_goal * T_goal_R();
g[MOTOR_LINKS]=50.0F;
g[MOTOR_RECHTS]=-50.0F;
Product(1.0F,alfa,g);
}
void TurnLeft(void)
{
float alfa;
float g[AANTAL_ACTUATOREN];
alfa = mu_trace * T_trl_L()
+ mu_goal * T_goal_L();
g[MOTOR_LINKS]=-50.0F;
g[MOTOR_RECHTS]=50.0F;
Product(1.0F,alfa,g);
}
void Forward(void)
{
float alfa;
float g[AANTAL_ACTUATOREN];
alfa = mu_trace * T_trl_F()
+ mu_goal * T_goal_F();
g[MOTOR_LINKS]=50.0F;
g[MOTOR_RECHTS]=50.0F;
Product(2.0F,alfa,g);
}
Peter Stuers oplossing om de motoren vanzelf stil te laten vallen. In het algemeen bepaalt dit elementair behavior ook hoe schichtig de robot zich gedraagt. Met andere woorden: als de activatie van deze term zeer groot is zal de robot zich uitermate lijzig voortbewegen en traag reageren...
void DecayMotors(void)
{
float alfa;
float g[AANTAL_ACTUATOREN];
alfa = mu_trace*0.15F+mu_goal*1.0F;
g[MOTOR_LINKS]=0;
g[MOTOR_RECHTS]=0;
Product(2.0F,alfa,g);
}
void MuTrace()
{
mu_trace=1-xtrapowerfunc(-90.0,sum*2);
mu_trace=1.0F;
}
void MuGoal()
{
mu_goal=1.0-mu_trace;
}
Merk op dat de totale actie nu in 1 keer wordt meegedeeld aan PDL i.p.v beetje bij beetje zoals in het eerste frame-work.
void main_process(void)
{
cycle++;
if (cycle==40) cycle=0;
z[MOTOR_LINKS]=value(motor_l);
z[MOTOR_RECHTS]=value(motor_r);
sum=(value(nest_l)+value(nest_r))/512.0F;
diff=(value(nest_r)-value(nest_l))/258.0F;
MuTrace();
MuGoal();
TurnLeft();
TurnRight();
Forward();
DecayMotors();
Action();
}
int main(void)
{
init_pdl();
nest_l = add_quantity("Nest l", 255.0,0.0,0.0);
nest_r = add_quantity("Nest r", 255.0,0.0,0.0);
motor_l = add_quantity("Motor l", 100.0,-100.0,0.0);
motor_r = add_quantity("Motor r", 100.0,-100.0,0.0);
connect_sensor(SID_AN2, nest_l);
connect_sensor(SID_AN3, nest_r);
connect_actuator(AID_MOTOR1, motor_l);
connect_actuator(AID_MOTOR2, motor_r);
log_quantity(nest_l,0);
log_quantity(nest_r,0);
log_quantity(motor_l,0);
log_quantity(motor_r,0);
add_process("Main Process",main_process);
run_pdl(-1L);
return(0);
}
Omdat het moeilijk was in het weekend testen te doen op de robot en omdat dergelijke testen heel erg tijdrovend zijn en moeilijk te debuggen heb ik een simulatie ontwikkeld die de waarde van de sensoren aanpast naar believen. (in de responsefunctie zal zo dadelijk duidelijk worden). De resulteerde waarde voor de actuatoren zijn dan onmiddelijk te zien op het scherm. Het verbazende aan deze simulatie is dat ze (bij benadering) perfect de werkelijkheid nabootst. Ik heb er een paar programmas die fouten vertoonden ingedraaid en de simulatie gaf hetzelfde effect als de robot.
Om bovenstaande programma (de trace_light) te simuleren hebben we een kleine library geschrven die de harware kan vervangen. Het enige wat extra toegevoegd moet worden is natuurlijk een responsefunctie, die beschrijft hoe de sensoren en actuatoren met elkaar in connectie staan.
De header files aanpassen
#ifdef sim #include <i86.h> #include "sim7.c" #endif #ifndef sim #include "smb2pdl1.h" #include "smb2io.h" #include "smb2dlog.h" #endif
De body van het programma, inclusief de main blijven hetzelfde. Enkel een response-functie moet toegevoegd worden.
void responsefunc(void)
{
static float rs=255;
static float ls=255;
static float ri=128;
static float li=128;
rs-=0.25;
ls-=0.5;
li-=2;
ri-=1;
if (rs<0) rs=255;
if (ls<0) ls=255;
if (ri<0) ri=255;
if (li<0) li=255;
set_value(box_l,li);
set_value(box_r,ri);
set_value(nest_l,ls);
set_value(nest_r,rs);
if (kbhit())
{int c;
c=getch();
if (c=='1') set_value(bump_fl,1.0F);
if (c=='2') set_value(bump_fr,1.0F);
if (c=='3') set_value(bump_bl,1.0F);
if (c=='4') set_value(bump_br,1.0F);
if (c=='5') exit(1);}
else
{set_value(bump_fl,0.0F);
set_value(bump_fr,0.0F);
set_value(bump_bl,0.0F);
set_value(bump_br,0.0F);}
delay(5);
}
#include <conio.h>
/* een definitie van een quantity */
char*Screen=(char*)0xA0000;
void DrawDot(int x, int y, char color)
{
if (x<0 || x>319) return;
if (y<0 || y>199) return;
Screen[x+(199-y)*320]=color;
}
extern void BiosScreenMode(short mode);
#pragma aux BiosScreenMode = \
"int 10h" \
parm [ax];
extern void outportw(short, short);
#pragma aux outportw = \
"out dx,ax" \
parm [dx] [ax] \
modify exact [];
#define bitplane(nr) outportw(0x3C4,((1<<((nr)+8))+2))
#define R320x200x256 0x13
#define Go320x200x256() BiosScreenMode(R320x200x256)
#define Go80x25() BiosScreenMode(0x3)
#define SID_AN2 0
#define SID_AN3 1
#define AID_MOTOR1 2
#define AID_MOTOR2 3
#define SID_AN0 4
#define SID_AN1 5
#define SID_BIN1 6
#define SID_BIN2 7
#define SID_BIN3 8
#define SID_BIN4 9
#define NESTL 0
#define NESTR 1
#define MOTORL 2
#define MOTORR 3
#define BOXL 4
#define BOXR 5
struct squantity
{
int id;
short curpos;
int showat;
float minval;
float maxval;
float oldval;
float newval;
short dots[320];
int cycle;
};
typedef struct squantity *quantity;
quantity viertal[10];
void add_value(quantity q, float r)
{
q->newval+=r;
}
#define value(name) ((name)->oldval)
#define set_value(name,nov) ((name)->oldval=(name)->newval=(nov))
void (*mainproc)(void)=NULL;
quantity add_quantity(char* name,float maxval,float minval,float curval)
{
quantity q;
q=(quantity)malloc(sizeof(struct squantity));
assert(q);
q->minval=minval;
q->maxval=maxval;
q->showat=400;
q->oldval=curval;
q->newval=0;
q->id=10000;
q->curpos=0;
q->cycle=0;
return q;
}
void log_quantity(quantity q ,int showat)
{q->showat=showat;}
void connect_actuator(int id, quantity q)
{assert(q);
q->id=id;
viertal[id]=q;}
void connect_sensor(int id, quantity q)
{assert(q);
q->id=id;
viertal[id]=q;}
void init_pdl(void)
{
Go320x200x256();
}
void add_process(char* name, void (*func)(void))
{assert(func);
mainproc=func;}
void ShowAll(void)
{
int i;
int p;
quantity q;
for(i=0;i<10;i++)
{
q=viertal[i];
if (q->cycle==q->showat)
{
p=q->curpos;
DrawDot(p,q->dots[p],0);
DrawDot(p,
q->dots[p]=(q->oldval-q->minval)*199.0/(q->maxval-q->minval)
,q->id+1);
q->curpos++;
if (p>=320) q->curpos=0;
q->cycle=0;
} else q->cycle++;
}
}
void run_pdl(long ignore)
{
int i;
quantity q;
assert(mainproc);
while (1)
{
mainproc();
for(i=0;i<10;i++)
{
q=viertal[i];
q->oldval=q->newval;
if (q->oldval<q->minval) q->oldval=q->minval;
if (q->oldval>q->maxval) q->oldval=q->maxval;
}
responsefunc();
ShowAll();
}
};
int random(int max)
{
return ((int)rand())*max/RAND_MAX;
}
De uitvoer van het programma is als volgt... (het is ne rauwen bitmap rechtstreeks van het scherm geplukt)
De schuin naar beneden lopende diagonale lijnen zijn de lichtsensoren. Het 'ruisen' van deze lijnen is express gedaan om te kijken of mijn methode niet te heftig reageert op plostklapse veranderingen.
De motoren zijn de twee gebogen lijnen. De schaal is als volgt: De horizontale is het nulpunt voor de motoren, positief naar boven, negatief naar beneden. De lichtsensoren hebben hun maximumwaarde helemaal bovenaan de tekening (255) en hebben hun minimumwaarde volledig beneden.
Het is duidelijk dat op het ogenblik dat er veel licht is de motor niet sterk voorwaarts wil, maar naarmate het donkerder wordt, wil de motor sterker naar voor lopen.
Het toffe aan deze methode is dat ze gemakkelijk te calibreren is. Op 10 minuten stel ik een robot volledig in om naar wit licht te lopen. De omzetting naar modulated light was gedaan op een half uurtje. Het botsbehavior er bij insteken was een kwestie van plug & play.
Onderstaand worden de activaties voor de verschilende gedragingen gelist.
|
|
Forward |
Backward |
TurnLeft |
TurnRight |
Decay |
|
|
0.625 |
0 |
f(32,-diffWL) |
f(32,diffWL) |
0.25 |
|
|
1 |
0 |
f(640,-diffBL)/2 |
f(640,diffBL)/2 |
0.25 |
|
|
0 |
1 |
0 |
2 |
0 |
|
|
0 |
1 |
2 |
0 |
0 |
|
|
1 |
0 |
0 |
2 |
0 |
|
|
1 |
0 |
2 |
0 |
0 |
Het bereken van de µ § wordt gedaan op volgende wijze. Gewenst is dat de activatie daalt op het ogenblik dat de robot tegen het balletje duwt. Deze laatste detecteren was een beetje experimenteren met intensiteiten en de posities van het balletje. Uiteindelijk is er volgend resultaat uitgekomen:
Als de robot tussen de twee lichtsensoren stond daalde de totale lichtintensiteit. De center-term is hier ene correctie voor.
center=powerfunc(-18.0F,xabs(diffWL))*0.08F;De xtrapowerfunc is een functie die 4 parameters neemt. Noem ze a,b,c en d. Het interval [a,b] bepaalt waartussen de functie van waarde moet veranderen. Voor a is de functie 0, na b is de functie altijd 1. Ertussen grijpt een overgang van 0 naar 1 plaats met sterkte c. De laatste parameter, d, is de waarde waarin men het functieresultaat wil kennen.
In dit geval daalt de activatie van µ § op het ogenblik dat sumWL voldoende groot wordt.
mu_trace=1-xtrapowerfunc(0.16+center,0.28+center,90.0,sumWL);
De complexe botsbehaviors µ §,µ §,µ § en µ § worden op 1 gezet op het ogenblik dat de juiste sensor geactiveerd is en vallen langzaam terug op 0. Zolang de complexe botsbehaviors aan staan zal er helemaal niet getraced of gegoald worden.
