Double pole balacing
/* ------------------------------------------------------------------ */
/* Double pole balacing */
/* ------------------------------------------------------------------ */
//Perform evolution on double pole balacing, for gens generations
//If velocity is false, then velocity information will be withheld from the
//network population (non-Markov)
Population *pole2_test(int gens,int velocity) {
Population *pop;
Genome *start_genome;
char curword[20];
int id;
ostringstream *fnamebuf;
int gen;
CartPole *thecart;
//Stat collection variables
int highscore;
int record[NEAT::num_runs][1000];
double recordave[1000];
int genesrec[NEAT::num_runs][1000];
double genesave[1000];
int nodesrec[NEAT::num_runs][1000];
double nodesave[1000];
int winnergens[NEAT::num_runs];
int initcount;
int champg, champn, winnernum; //Record number of genes and nodes in champ
int run;
int curtotal; //For averaging
int samples; //For averaging
ofstream oFile("statout",ios::out);
champg=0;
champn=0;
//Initialize the stat recording arrays
for (initcount=0;initcount<200;initcount++) {
recordave[initcount]=0;
genesave[initcount]=0;
nodesave[initcount]=0;
for (run=0;run<NEAT::num_runs;++run) {
record[run][initcount]=0;
genesrec[run][initcount]=0;
nodesrec[run][initcount]=0;
}
}
char *non_markov_starter="pole2startgenes2";
char *markov_starter="pole2startgenes1";
char *startstring;
if (velocity==0) startstring=non_markov_starter;
else if (velocity==1) startstring=markov_starter;
ifstream iFile(startstring,ios::in);
//ifstream iFile("pole2startgenes",ios::in);
cout<<"START DOUBLE POLE BALANCING EVOLUTION"<<endl;
if (!velocity)
cout<<"NO VELOCITY INPUT"<<endl;
cout<<"Reading in the start genome"<<endl;
//Read in the start Genome
iFile>>curword;
iFile>>id;
cout<<"Reading in Genome id "<<id<<endl;
start_genome=new Genome(id,iFile);
iFile.close();
cout<<"Start Genome: "<<start_genome<<endl;
for (run=0;run<NEAT::num_runs;run++) {
cout<<"RUN #"<<run<<endl;
//Spawn the Population from starter gene
cout<<"Spawning Population off Genome"<<endl;
pop=new Population(start_genome,NEAT::pop_size);
//Alternative way to start off of randomly connected genomes
//pop=new Population(pop_size,7,1,10,false,0.3);
cout<<"Verifying Spawned Pop"<<endl;
pop->verify();
//Create the Cart
thecart=new CartPole(true,velocity);
for (gen=1;gen<=gens;gen++) {
cout<<"Epoch "<<gen<<endl;
fnamebuf=new ostringstream();
(*fnamebuf)<<"gen_"<<gen<<ends; //needs end marker
#ifndef NO_SCREEN_OUT
cout<<"name of fname: "<<fnamebuf->str()<<endl;
#endif
char temp[50];
sprintf (temp, "gen_%d", gen);
highscore=pole2_epoch(pop,gen,temp,velocity, thecart,champg,champn,winnernum,oFile);
//highscore=pole2_epoch(pop,gen,fnamebuf->str(),velocity, thecart,champg,champn,winnernum,oFile);
//cout<<"GOT HIGHSCORE FOR GEN "<<gen<<": "<<highscore-1<<endl;
record[run][gen-1]=highscore-1;
genesrec[run][gen-1]=champg;
nodesrec[run][gen-1]=champn;
fnamebuf->clear();
delete fnamebuf;
//Stop right at the winnergen
if (((pop->winnergen)!=0)&&(gen==(pop->winnergen))) {
winnergens[run]=NEAT::pop_size*(gen-1)+winnernum;
gen=gens+1;
}
//In non-MARKOV, stop right at winning (could go beyond if desired)
if ((!(thecart->MARKOV))&&((pop->winnergen)!=0))
gen=gens+1;
#ifndef NO_SCREEN_OUT
cout<<"gen = "<<gen<<" gens = "<<gens<<endl;
#endif
if (gen==(gens-1)) oFile<<"FAIL: Last gen on run "<<run<<endl;
}
if (run<NEAT::num_runs-1) delete pop;
delete thecart;
}
cout<<"Generation highs: "<<endl;
oFile<<"Generation highs: "<<endl;
for(gen=0;gen<=gens-1;gen++) {
curtotal=0;
for (run=0;run<NEAT::num_runs;++run) {
if (record[run][gen]>0) {
cout<<setw(8)<<record[run][gen]<<" ";
oFile<<setw(8)<<record[run][gen]<<" ";
curtotal+=record[run][gen];
}
else {
cout<<" ";
oFile<<" ";
curtotal+=100000;
}
recordave[gen]=(double) curtotal/NEAT::num_runs;
}
cout<<endl;
oFile<<endl;
}
cout<<"Generation genes in champ: "<<endl;
for(gen=0;gen<=gens-1;gen++) {
curtotal=0;
samples=0;
for (run=0;run<NEAT::num_runs;++run) {
if (genesrec[run][gen]>0) {
cout<<setw(4)<<genesrec[run][gen]<<" ";
oFile<<setw(4)<<genesrec[run][gen]<<" ";
curtotal+=genesrec[run][gen];
samples++;
}
else {
cout<<setw(4)<<" ";
oFile<<setw(4)<<" ";
}
}
genesave[gen]=(double) curtotal/samples;
cout<<endl;
oFile<<endl;
}
cout<<"Generation nodes in champ: "<<endl;
oFile<<"Generation nodes in champ: "<<endl;
for(gen=0;gen<=gens-1;gen++) {
curtotal=0;
samples=0;
for (run=0;run<NEAT::num_runs;++run) {
if (nodesrec[run][gen]>0) {
cout<<setw(4)<<nodesrec[run][gen]<<" ";
oFile<<setw(4)<<nodesrec[run][gen]<<" ";
curtotal+=nodesrec[run][gen];
samples++;
}
else {
cout<<setw(4)<<" ";
oFile<<setw(4)<<" ";
}
}
nodesave[gen]=(double) curtotal/samples;
cout<<endl;
oFile<<endl;
}
cout<<"Generational record fitness averages: "<<endl;
oFile<<"Generational record fitness averages: "<<endl;
for(gen=0;gen<gens-1;gen++) {
cout<<recordave[gen]<<endl;
oFile<<recordave[gen]<<endl;
}
cout<<"Generational number of genes in champ averages: "<<endl;
oFile<<"Generational number of genes in champ averages: "<<endl;
for(gen=0;gen<gens-1;gen++) {
cout<<genesave[gen]<<endl;
oFile<<genesave[gen]<<endl;
}
cout<<"Generational number of nodes in champ averages: "<<endl;
oFile<<"Generational number of nodes in champ averages: "<<endl;
for(gen=0;gen<gens-1;gen++) {
cout<<nodesave[gen]<<endl;
oFile<<nodesave[gen]<<endl;
}
cout<<"Winner evals: "<<endl;
oFile<<"Winner evals: "<<endl;
curtotal=0;
samples=0;
for (run=0;run<NEAT::num_runs;++run) {
cout<<winnergens[run]<<endl;
oFile<<winnergens[run]<<endl;
curtotal+=winnergens[run];
samples++;
}
cout<<"Average # evals: "<<((double) curtotal/samples)<<endl;
oFile<<"Average # evals: "<<((double) curtotal/samples)<<endl;
oFile.close();
return pop;
}
//This is used for list sorting of Species by fitness of best organism
//highest fitness first
//Used to choose which organism to test
//bool order_new_species(Species *x, Species *y) {
//
// return (x->compute_max_fitness() >
// y->compute_max_fitness());
//}
int pole2_epoch(Population *pop,int generation,char *filename,bool velocity,
CartPole *thecart,int &champgenes,int &champnodes,
int &winnernum, ofstream &oFile) {
//char cfilename[100];
//strncpy( cfilename, filename.c_str(), 100 );
//ofstream cfilename(filename.c_str());
vector<Organism*>::iterator curorg;
vector<Species*>::iterator curspecies;
vector<Species*> sorted_species; //Species sorted by max fit org in Species
int pause;
bool win=false;
double champ_fitness;
Organism *champ;
//double statevals[5]={-0.9,-0.5,0.0,0.5,0.9};
double statevals[5]={0.05, 0.25, 0.5, 0.75, 0.95};
int s0c,s1c,s2c,s3c;
int score;
thecart->nmarkov_long=false;
thecart->generalization_test=false;
//Evaluate each organism on a test
for(curorg=(pop->organisms).begin();curorg!=(pop->organisms).end();++curorg) {
//shouldn't happen
if (((*curorg)->gnome)==0) {
cout<<"ERROR EMPTY GEMOME!"<<endl;
cin>>pause;
}
if (pole2_evaluate((*curorg),velocity,thecart)) win=true;
}
//Average and max their fitnesses for dumping to file and snapshot
for(curspecies=(pop->species).begin();curspecies!=(pop->species).end();++curspecies) {
//This experiment control routine issues commands to collect ave
//and max fitness, as opposed to having the snapshot do it,
//because this allows flexibility in terms of what time
//to observe fitnesses at
(*curspecies)->compute_average_fitness();
(*curspecies)->compute_max_fitness();
}
//Take a snapshot of the population, so that it can be
//visualized later on
//if ((generation%1)==0)
// pop->snapshot();
//Find the champion in the markov case simply for stat collection purposes
if (thecart->MARKOV) {
champ_fitness=0.0;
for(curorg=(pop->organisms).begin();curorg!=(pop->organisms).end();++curorg) {
if (((*curorg)->fitness)>champ_fitness) {
champ=(*curorg);
champ_fitness=champ->fitness;
champgenes=champ->gnome->genes.size();
champnodes=champ->gnome->nodes.size();
winnernum=champ->gnome->genome_id;
}
}
}
//Check for winner in Non-Markov case
if (!(thecart->MARKOV)) {
cout<<"Non-markov case"<<endl;
//Sort the species
for(curspecies=(pop->species).begin();curspecies!=(pop->species).end();++curspecies) {
sorted_species.push_back(*curspecies);
}
//sorted_species.sort(order_new_species);
std::sort(sorted_species.begin(), sorted_species.end(), NEAT::order_new_species);
//std::sort(sorted_species.begin(), sorted_species.end(), order_species);
cout<<"Number of species sorted: "<<sorted_species.size()<<endl;
//First update what is checked and unchecked
for(curspecies=sorted_species.begin();curspecies!=sorted_species.end();++curspecies) {
if (((*curspecies)->compute_max_fitness())>((*curspecies)->max_fitness_ever))
(*curspecies)->checked=false;
}
//Now find a species that is unchecked
curspecies=sorted_species.begin();
cout<<"Is the first species checked? "<<(*curspecies)->checked<<endl;
while((curspecies!=(sorted_species.end()))&&
((*curspecies)->checked))
{
cout<<"Species #"<<(*curspecies)->id<<" is checked"<<endl;
++curspecies;
}
if (curspecies==(sorted_species.end())) curspecies=sorted_species.begin();
//Remember it was checked
(*curspecies)->checked=true;
cout<<"Is the species now checked? "<<(*curspecies)->checked<<endl;
//Extract the champ
cout<<"Champ chosen from Species "<<(*curspecies)->id<<endl;
champ=(*curspecies)->get_champ();
champ_fitness=champ->fitness;
cout<<"Champ is organism #"<<champ->gnome->genome_id<<endl;
cout<<"Champ fitness: "<<champ_fitness<<endl;
winnernum=champ->gnome->genome_id;
cout<<champ->gnome<<endl;
//Now check to make sure the champ can do 100,000
thecart->nmarkov_long=true;
thecart->generalization_test=false;
//The champ needs tp be flushed here because it may have
//leftover activation from its last test run that could affect
//its recurrent memory
(champ->net)->flush();
//champ->gnome->print_to_filename("tested");
if (pole2_evaluate(champ,velocity,thecart)) {
cout<<"The champ passed the 100,000 test!"<<endl;
thecart->nmarkov_long=false;
//Given that the champ passed, now run it on generalization tests
score=0;
for (s0c=0;s0c<=4;++s0c)
for (s1c=0;s1c<=4;++s1c)
for (s2c=0;s2c<=4;++s2c)
for (s3c=0;s3c<=4;++s3c) {
thecart->state[0] = statevals[s0c] * 4.32 - 2.16;
thecart->state[1] = statevals[s1c] * 2.70 - 1.35;
thecart->state[2] = statevals[s2c] * 0.12566304 - 0.06283152;
/* 0.06283152 = 3.6 degrees */
thecart->state[3] = statevals[s3c] * 0.30019504 - 0.15009752;
/* 00.15009752 = 8.6 degrees */
thecart->state[4]=0.0;
thecart->state[5]=0.0;
cout<<"On combo "<<thecart->state[0]<<" "<<thecart->state[1]<<" "<<thecart->state[2]<<" "<<thecart->state[3]<<endl;
thecart->generalization_test=true;
(champ->net)->flush(); //Reset the champ for each eval
if (pole2_evaluate(champ,velocity,thecart)) {
cout<<"----------------------------The champ passed its "<<score<<"th test"<<endl;
score++;
}
}
if (score>=200) {
cout<<"The champ wins!!! (generalization = "<<score<<" )"<<endl;
oFile<<"(generalization = "<<score<<" )"<<endl;
oFile<<"generation= "<<generation<<endl;
(champ->gnome)->print_to_file(oFile);
champ_fitness=champ->fitness;
champgenes=champ->gnome->genes.size();
champnodes=champ->gnome->nodes.size();
winnernum=champ->gnome->genome_id;
win=true;
}
else {
cout<<"The champ couldn't generalize"<<endl;
champ->fitness=champ_fitness; //Restore the champ's fitness
}
}
else {
cout<<"The champ failed the 100,000 test :("<<endl;
cout<<"made score "<<champ->fitness<<endl;
champ->fitness=champ_fitness; //Restore the champ's fitness
}
}
//Only print to file every print_every generations
if (win||
((generation%(NEAT::print_every))==0)) {
cout<<"printing file: "<<filename<<endl;
pop->print_to_file_by_species(filename);
}
if ((win)&&((pop->winnergen)==0)) pop->winnergen=generation;
//Prints a champion out on each generation
//IMPORTANT: This causes generational file output!
print_Genome_tofile(champ->gnome,"champ");
//Create the next generation
pop->epoch(generation);
return (int) champ_fitness;
}
bool pole2_evaluate(Organism *org,bool velocity, CartPole *thecart) {
Network *net;
int thresh; /* How many visits will be allowed before giving up
(for loop detection) NOW OBSOLETE */
int pause;
net=org->net;
thresh=100; //this is obsolete
//DEBUG : Check flushedness of org
//org->net->flush_check();
//Try to balance a pole now
org->fitness = thecart->evalNet(net,thresh);
#ifndef NO_SCREEN_OUT
if (org->pop_champ_child)
cout<<" <<DUPLICATE OF CHAMPION>> ";
//Output to screen
cout<<"Org "<<(org->gnome)->genome_id<<" fitness: "<<org->fitness;
cout<<" ("<<(org->gnome)->genes.size();
cout<<" / "<<(org->gnome)->nodes.size()<<")";
cout<<" ";
if (org->mut_struct_baby) cout<<" [struct]";
if (org->mate_baby) cout<<" [mate]";
cout<<endl;
#endif
if ((!(thecart->generalization_test))&&(!(thecart->nmarkov_long)))
if (org->pop_champ_child) {
cout<<org->gnome<<endl;
//DEBUG CHECK
if (org->high_fit>org->fitness) {
cout<<"ALERT: ORGANISM DAMAGED"<<endl;
print_Genome_tofile(org->gnome,"failure_champ_genome");
cin>>pause;
}
}
//Decide if its a winner, in Markov Case
if (thecart->MARKOV) {
if (org->fitness>=(thecart->maxFitness-1)) {
org->winner=true;
return true;
}
else {
org->winner=false;
return false;
}
}
//if doing the long test non-markov
else if (thecart->nmarkov_long) {
if (org->fitness>=99999) {
//if (org->fitness>=9000) {
org->winner=true;
return true;
}
else {
org->winner=false;
return false;
}
}
else if (thecart->generalization_test) {
if (org->fitness>=999) {
org->winner=true;
return true;
}
else {
org->winner=false;
return false;
}
}
else {
org->winner=false;
return false; //Winners not decided here in non-Markov
}
}
CartPole::CartPole(bool randomize,bool velocity)
{
maxFitness = 100000;
MARKOV=velocity;
MIN_INC = 0.001;
POLE_INC = 0.05;
MASS_INC = 0.01;
LENGTH_2 = 0.05;
MASSPOLE_2 = 0.01;
// CartPole::reset() which is called here
}
//Faustino Gomez wrote this physics code using the differential equations from
//Alexis Weiland's paper and added the Runge-Kutta himself.
double CartPole::evalNet(Network *net,int thresh)
{
int steps=0;
double input[NUM_INPUTS];
double output;
int nmarkovmax;
double nmarkov_fitness;
double jiggletotal; //total jiggle in last 100
int count; //step counter
//init(randomize); // restart at some point
if (nmarkov_long) nmarkovmax=100000;
else if (generalization_test) nmarkovmax=1000;
else nmarkovmax=1000;
init(0);
if (MARKOV) {
while (steps++ < maxFitness) {
input[0] = state[0] / 4.8;
input[1] = state[1] /2;
input[2] = state[2] / 0.52;
input[3] = state[3] /2;
input[4] = state[4] / 0.52;
input[5] = state[5] /2;
input[6] = .5;
net->load_sensors(input);
//Activate the net
//If it loops, exit returning only fitness of 1 step
if (!(net->activate())) return 1.0;
output=(*(net->outputs.begin()))->activation;
performAction(output,steps);
if (outsideBounds()) // if failure
break; // stop it now
}
return (double) steps;
}
else { //NON MARKOV CASE
while (steps++ < nmarkovmax) {
//Do special parameter summing on last hundred
//if ((steps==900)&&(!nmarkov_long)) last_hundred=true;
/*
input[0] = state[0] / 4.8;
input[1] = 0.0;
input[2] = state[2] / 0.52;
input[3] = 0.0;
input[4] = state[4] / 0.52;
input[5] = 0.0;
input[6] = .5;
*/
//cout<<"nmarkov_long: "<<nmarkov_long<<endl;
//if (nmarkov_long)
//cout<<"step: "<<steps<<endl;
input[0] = state[0] / 4.8;
input[1] = state[2] / 0.52;
input[2] = state[4] / 0.52;
input[3] = .5;
net->load_sensors(input);
//cout<<"inputs: "<<input[0]<<" "<<input[1]<<" "<<input[2]<<" "<<input[3]<<endl;
//Activate the net
//If it loops, exit returning only fitness of 1 step
if (!(net->activate())) return 0.0001;
output=(*(net->outputs.begin()))->activation;
//cout<<"output: "<<output<<endl;
performAction(output,steps);
if (outsideBounds()) // if failure
break; // stop it now
if (nmarkov_long&&(outsideBounds())) // if failure
break; // stop it now
}
//If we are generalizing we just need to balance it a while
if (generalization_test)
return (double) balanced_sum;
//Sum last 100
if ((steps>100)&&(!nmarkov_long)) {
jiggletotal=0;
cout<<"step "<<steps-99-2<<" to step "<<steps-2<<endl;
//Adjust for array bounds and count
for (count=steps-99-2;count<=steps-2;count++)
jiggletotal+=jigglestep[count];
}
if (!nmarkov_long) {
if (balanced_sum>100)
nmarkov_fitness=((0.1*(((double) balanced_sum)/1000.0))+
(0.9*(0.75/(jiggletotal))));
else nmarkov_fitness=(0.1*(((double) balanced_sum)/1000.0));
#ifndef NO_SCREEN_OUTR
cout<<"Balanced: "<<balanced_sum<<" jiggle: "<<jiggletotal<<" ***"<<endl;
#endif
return nmarkov_fitness;
}
else return (double) steps;
}
}
void CartPole::init(bool randomize)
{
static int first_time = 1;
if (!MARKOV) {
//Clear all fitness records
cartpos_sum=0.0;
cartv_sum=0.0;
polepos_sum=0.0;
polev_sum=0.0;
}
balanced_sum=0; //Always count # balanced
last_hundred=false;
/*if (randomize) {
state[0] = (lrand48()%4800)/1000.0 - 2.4;
state[1] = (lrand48()%2000)/1000.0 - 1;
state[2] = (lrand48()%400)/1000.0 - 0.2;
state[3] = (lrand48()%400)/1000.0 - 0.2;
state[4] = (lrand48()%3000)/1000.0 - 1.5;
state[5] = (lrand48()%3000)/1000.0 - 1.5;
}
else {*/
if (!generalization_test) {
state[0] = state[1] = state[3] = state[4] = state[5] = 0;
state[2] = 0.07; // one_degree;
}
else {
state[4] = state[5] = 0;
}
//}
if(first_time){
cout<<"Initial Long pole angle = %f\n"<<state[2]<<endl;;
cout<<"Initial Short pole length = %f\n"<<LENGTH_2<<endl;
first_time = 0;
}
}
void CartPole::performAction(double output, int stepnum)
{
int i;
double dydx[6];
const bool RK4=true; //Set to Runge-Kutta 4th order integration method
const double EULER_TAU= TAU/4;
/*random start state for long pole*/
/*state[2]= drand48(); */
/*--- Apply action to the simulated cart-pole ---*/
if(RK4){
for(i=0;i<2;++i){
dydx[0] = state[1];
dydx[2] = state[3];
dydx[4] = state[5];
step(output,state,dydx);
rk4(output,state,dydx,state);
}
}
else{
for(i=0;i<8;++i){
step(output,state,dydx);
state[0] += EULER_TAU * dydx[0];
state[1] += EULER_TAU * dydx[1];
state[2] += EULER_TAU * dydx[2];
state[3] += EULER_TAU * dydx[3];
state[4] += EULER_TAU * dydx[4];
state[5] += EULER_TAU * dydx[5];
}
}
//Record this state
cartpos_sum+=fabs(state[0]);
cartv_sum+=fabs(state[1]);
polepos_sum+=fabs(state[2]);
polev_sum+=fabs(state[3]);
if (stepnum<=1000)
jigglestep[stepnum-1]=fabs(state[0])+fabs(state[1])+fabs(state[2])+fabs(state[3]);
if (false) {
//cout<<"[ x: "<<state[0]<<" xv: "<<state[1]<<" t1: "<<state[2]<<" t1v: "<<state[3]<<" t2:"<<state[4]<<" t2v: "<<state[5]<<" ] "<<
//cartpos_sum+cartv_sum+polepos_sum+polepos_sum+polev_sum<<endl;
if (!(outsideBounds())) {
if (balanced_sum<1000) {
cout<<".";
++balanced_sum;
}
}
else {
if (balanced_sum==1000)
balanced_sum=1000;
else balanced_sum=0;
}
}
else if (!(outsideBounds()))
++balanced_sum;
}
void CartPole::step(double action, double *st, double *derivs)
{
double force,costheta_1,costheta_2,sintheta_1,sintheta_2,
gsintheta_1,gsintheta_2,temp_1,temp_2,ml_1,ml_2,fi_1,fi_2,mi_1,mi_2;
force = (action - 0.5) * FORCE_MAG * 2;
costheta_1 = cos(st[2]);
sintheta_1 = sin(st[2]);
gsintheta_1 = GRAVITY * sintheta_1;
costheta_2 = cos(st[4]);
sintheta_2 = sin(st[4]);
gsintheta_2 = GRAVITY * sintheta_2;
ml_1 = LENGTH_1 * MASSPOLE_1;
ml_2 = LENGTH_2 * MASSPOLE_2;
temp_1 = MUP * st[3] / ml_1;
temp_2 = MUP * st[5] / ml_2;
fi_1 = (ml_1 * st[3] * st[3] * sintheta_1) +
(0.75 * MASSPOLE_1 * costheta_1 * (temp_1 + gsintheta_1));
fi_2 = (ml_2 * st[5] * st[5] * sintheta_2) +
(0.75 * MASSPOLE_2 * costheta_2 * (temp_2 + gsintheta_2));
mi_1 = MASSPOLE_1 * (1 - (0.75 * costheta_1 * costheta_1));
mi_2 = MASSPOLE_2 * (1 - (0.75 * costheta_2 * costheta_2));
derivs[1] = (force + fi_1 + fi_2)
/ (mi_1 + mi_2 + MASSCART);
derivs[3] = -0.75 * (derivs[1] * costheta_1 + gsintheta_1 + temp_1)
/ LENGTH_1;
derivs[5] = -0.75 * (derivs[1] * costheta_2 + gsintheta_2 + temp_2)
/ LENGTH_2;
}
void CartPole::rk4(double f, double y[], double dydx[], double yout[])
{
int i;
double hh,h6,dym[6],dyt[6],yt[6];
hh=TAU*0.5;
h6=TAU/6.0;
for (i=0;i<=5;i++) yt[i]=y[i]+hh*dydx[i];
step(f,yt,dyt);
dyt[0] = yt[1];
dyt[2] = yt[3];
dyt[4] = yt[5];
for (i=0;i<=5;i++) yt[i]=y[i]+hh*dyt[i];
step(f,yt,dym);
dym[0] = yt[1];
dym[2] = yt[3];
dym[4] = yt[5];
for (i=0;i<=5;i++) {
yt[i]=y[i]+TAU*dym[i];
dym[i] += dyt[i];
}
step(f,yt,dyt);
dyt[0] = yt[1];
dyt[2] = yt[3];
dyt[4] = yt[5];
for (i=0;i<=5;i++)
yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);
}
bool CartPole::outsideBounds()
{
const double failureAngle = thirty_six_degrees;
return
state[0] < -2.4 ||
state[0] > 2.4 ||
state[2] < -failureAngle ||
state[2] > failureAngle ||
state[4] < -failureAngle ||
state[4] > failureAngle;
}
void CartPole::nextTask()
{
LENGTH_2 += POLE_INC; /* LENGTH_2 * INCREASE; */
MASSPOLE_2 += MASS_INC; /* MASSPOLE_2 * INCREASE; */
// ++new_task;
cout<<"#Pole Length %2.4f\n"<<LENGTH_2<<endl;
}
void CartPole::simplifyTask()
{
if(POLE_INC > MIN_INC) {
POLE_INC = POLE_INC/2;
MASS_INC = MASS_INC/2;
LENGTH_2 -= POLE_INC;
MASSPOLE_2 -= MASS_INC;
cout<<"#SIMPLIFY\n"<<endl;
cout<<"#Pole Length %2.4f\n"<<LENGTH_2;
}
else
{
cout<<"#NO TASK CHANGE\n"<<endl;
}
}