Skip to content

Instantly share code, notes, and snippets.

@EdgeCaseBerg
Created February 13, 2014 03:18
Embed
What would you like to do?
295 Evo Final. Code from several years ago
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
buggy with suspension.
this also shows you how to use geom groups.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include <time.h>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <string>
#include <sstream>
#include <fstream>
#include "glut.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define LENGTH 0.7 // chassis length
#define WIDTH 0.5 // chassis width
#define HEIGHT 0.2 // chassis height
#define RADIUS 0.36 // wheel radius
#define STARTZ 0.5 // starting height of chassis
#define CMASS 3 // chassis mass
#define WMASS 0.2 // wheel mass
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[11]; //one body 2 shoulders, 2 elbows, 2 legs, 4 sensors
static dGeomID geom[11];
static dJointGroupID contactgroup;
static dGeomID ground;
static dJointID joints[6]; // 6 for actual motor and joints, 4 to hold sensors on
static dJointID hooks[4]; //3 hooks to hold together the stairs
int IDs[11] = {0,1,2,3,4,5,6,7,8,9,10};
int touches[11]={0,0,0,1,1,1,1,0,0,0,0};
double weights[7][6];
static double bias = 0.000021;
double outputs[6];
double mutationProb = .6;
double prevOutputs[6]={bias,bias,bias,bias,bias,bias};
static int timerCount = 0;
bool drawingOn = true;
bool capturing = false;
bool outofevo = false;
bool recurrent = true;
const int globalgenerations = 100;
double bestFitness = -1000;
int globalcounter = 0;
//conversion python to c++ coding neccesities
double fitnessPlotInfoParent[globalgenerations];
double fitnessPlotInfoChild[globalgenerations];
double parent[7][6]; //parent matrix
double child[7][6];
double difficulty = 2;//0 is most difficult
// things that the user controls
static dReal speed=0,steer=0; // user commands
const char* weightPath = "C:\\Users\\Ethan\\Desktop\\weights.dat";
const char* fitsPath = "C:\\Users\\Ethan\\Desktop\\fit.dat";
const char* plot1 = "C:\\users\\Ethan\\Desktop\\plot1.dat";
const char* plot2 = "C:\\users\\Ethan\\Desktop\\plot2.dat";
const char* savedWeightsPath = "C:\\users\\Ethan\\Desktop\\BestWeights.dat";
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
void createParent(){
for(int i=0; i < 7; i++){
for(int j =0; j < 6; j++){
parent[i][j] = (rand() % 200 -100)/100.0;
}
}
}
void createChild(){
for(int i=0; i < 7; i++){
for(int j=0; j < 6; j++){
if((rand() % 200 - 100)/100.0 < mutationProb){
child[i][j] = parent[i][j] ;
}else{
child[i][j] = (rand() % 200 -100)/100.0;
}
}
}
}
void copyChildToParent(){
for(int i=0; i<7; i++){
for(int j=0; j<6; j++){
parent[i][j] = child[i][j];
}
}
}
void copyParentToWeights(){
for(int i =0; i < 7; i++){
for(int j=0; j<6; j++){
weights[i][j] = parent[i][j];
}
}
}
void copyChildToWeights(){
for(int i=0; i <7; i++){
for(int j=0; j <6; j++){
weights[i][j] = child[i][j];
}
}
}
void writeBestFitnessFiles(){
//writes the plotting stuff I need
std::ostringstream convert1 = std::ostringstream();
std::ostringstream convert2 = std::ostringstream();
std::ofstream p1(plot1);
std::ofstream p2(plot2);
//fitfile.open(fitsPath);
double temp,temp2;
if(p1.is_open() && p2.is_open()){
for(int i=0; i <globalgenerations; i++){
temp = fitnessPlotInfoParent[i];
temp2 = fitnessPlotInfoChild[i];
convert1 << temp;
convert2 << temp2;
convert1 << "\n";
convert2 << "\n";
}
p1 << convert1.str();
p2 << convert2.str();
p1.flush();
p2.flush();
p1.close();
p2.close();
}
}
void saveWeights()
{
std::ostringstream converter = std::ostringstream();
std::ofstream fitfile(savedWeightsPath);
//fitfile.open(fitsPath);
if(fitfile.is_open()){
for(int i=0; i < 7; i++){
for(int j=0; j < 6; j++){
converter << weights[i][j];
converter << "\n";
}
}
fitfile << converter.str();
fitfile.flush();
fitfile.close();
}
}
void CaptureFrame(int num)
{
int width = 352;
int height = 288;
fprintf(stderr,"capturing frame %04d\n",num);
char s[100];
if ( num<10 )
sprintf (s,"C:/Users/Ethan/Desktop/cs 295 final/hw10/frame/0000%d.ppm",num);
else if ( num<100 )
sprintf (s,"C:/Users/Ethan/Desktop/cs 295 final/hw10/frame/000%d.ppm",num);
else if ( num<1000 )
sprintf (s,"C:/Users/Ethan/Desktop/cs 295 final/hw10/frame/00%d.ppm",num);
else if ( num<10000 )
sprintf (s,"C:/Users/Ethan/Desktop/cs 295 final/hw10/frame/0%d.ppm",num);
else if ( num<100000 )
sprintf (s,"C:/Users/Ethan/Desktop/cs 295 final/hw10/frame/%d.ppm",num);
FILE *f = fopen (s,"wb");
fprintf (f,"P6\n%d %d\n255\n",width,height);
void *buf = malloc( width * height * 3 );
glReadPixels( 0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, buf );
for (int y=(height - 1); y>=0; y--) {
for (int x=0; x<width; x++) {
unsigned char *pixel = ((unsigned char *)buf)+((y*width+ x)*3);
unsigned char b[3];
b[0] = *pixel;
b[1] = *(pixel+1);
b[2] = *(pixel+2);
fwrite(b,3,1,f);
}
}
free(buf);
fclose(f);
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,n;
int *IDPointer ;
bool shouldReturn = false;
// only collide things with the ground
int g1 = (o1 == ground );
int g2 = (o2 == ground );
int geomid1 = -1;
int geomid2 = -1;
//if (!(g1 ^ g2)) shouldReturn = true;; //bitwise xor
if(g1 && g2) shouldReturn = true;;
//matching
if(!g1 && !g2){//if neither are ground check match set
bool match1 = false;
bool match2 = false;
for(int i = 0; ((i < 7) && (!match1)); i++){
if(o1 == geom[i]){
match1 = true;
geomid1 = i;
//std::cout << match1 << "," ;
}
}
for(int i = 0; ((i < 7) && (!match2)); i++){
if(o2 == geom[i]){
match2 = true;
geomid2 = i;
//std::cout << match2 << std::endl;
}
}
if(!(match1 ^ match2)){
shouldReturn = true;
}
}
if(shouldReturn) return;;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
for (i=0; i<n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.5;
contact[i].surface.soft_cfm = 0.3;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
//Now lets check which guy is the ground
if ((g1 && g2)) return;;
//
if(g1 == 1){//if g1 is the ground then o2 cant be (or else we would have returned)
if(g2 != 1){
IDPointer = (int*)dBodyGetData(dGeomGetBody(o2));
}
}else if(g2==1){
if(g1 != 1){
IDPointer = (int*)dBodyGetData(dGeomGetBody(o1));
}
}
if(!g1 && !g2){
if( geomid1 == -1){//if the first one is a stair
if(geomid2 == -1){//if the second one is stair
//do nothing
}else{
IDPointer = (int*)dBodyGetData(dGeomGetBody(o2));
}
}
if( geomid2 == -1){//check the second one for stair
if(geomid1 == -1){//check if the first one is a stair
//do nothing
}else{
IDPointer = (int*)dBodyGetData(dGeomGetBody(o1));
}
}
}
//end if statement
//printf("%d \n",*IDPointer);
touches[*IDPointer] = 1;
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.0f,-2.0f,2.0f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'1' to save the current state to 'state.dif'.\n");
}
// called when a key pressed
void createBox(int index, double x,double y, double z, double length, double width,double height){
dMass m; //create some mass for the object
body[index] = dBodyCreate(world); //create the body in the world
dBodySetPosition( body[index],x,y,z);//set it's position in the world
dMassSetBox(&m,1,length,width,height); //set the mass of the box using dmass and etc,
dMassAdjust(&m,CMASS); //CMASS = 1 for now
dBodySetMass(body[index],&m); //set the mass of the body,
dBodySetData(body[index],&IDs[index]);//set ID for collision detection
geom[index] = dCreateBox(space,length,width,height); ///create the geom
dGeomSetBody(geom[index],body[index]); //set the geom and body to be friends
}
void createBox(int index, double x,double y, double z, double length, double width,double height,double mass){
dMass m; //create some mass for the object
body[index] = dBodyCreate(world); //create the body in the world
dBodySetPosition( body[index],x,y,z);//set it's position in the world
dMassSetBox(&m,mass,length,width,height); //set the mass of the box using dmass and etc,
dMassAdjust(&m,mass); //CMASS = 1 for now
dBodySetMass(body[index],&m); //set the mass of the body,
dBodySetData(body[index],&IDs[index]);//set ID for collision detection
geom[index] = dCreateBox(space,length,width,height); ///create the geom
dGeomSetBody(geom[index],body[index]); //set the geom and body to be friends
}
void createCylinder(int index, double x,double y,double z,double length,int direction,double radius){
dMass m;// the m object we'll modify
body[index] = dBodyCreate(world); //the body object
dBodySetPosition(body[index],x,y,z);
dMassSetCylinder(&m,.25,direction,radius,length); // density is one
dMassAdjust(&m,CMASS);
dMatrix3 R;
dReal a,b,c,r;
if(direction==1){
a=0;b=1;c=0; r=3.14/2;
}else if(direction==2){
b=0; a=1; c=0; r = 3.14/2;
}else{
c=1; a=0;b=0; r =3.14/2;
}
dRFromAxisAndAngle(R,a,b,c,r);
dBodySetRotation(body[index],R);
dBodySetMass(body[index],&m);
dBodySetData(body[index],&IDs[index]);
geom[index] = dCreateCylinder(space,radius,length);
dGeomSetRotation(geom[index],R);
dGeomSetBody(geom[index],body[index]);
}
void deleteObject(int index){
//well, we need to remove all references to this here index...
dBodyDestroy(body[index]);
dGeomDestroy(geom[index]);
}
void drawBox(int index){
//will draw a box defined by body and geom at index
dsSetColor(0,1,1);//dunno what color this is but who cares!
dsSetTexture(DS_WOOD);//because i know no other constants
// I have to get the length,width, and height of the body
dVector3 s;
dGeomBoxGetLengths(geom[index],s);
dReal sides[3] = {s[0],s[1],s[2] };
dsDrawBox(dBodyGetPosition(body[index]),dBodyGetRotation(body[index]),sides);
//now place the geom with it
if(touches[index]==index){
dsSetColor(0,0,0);
}else{
dsSetColor(1,1,1);
}
dVector3 ss;
dGeomBoxGetLengths (geom[index],ss);
dsDrawBox(dGeomGetPosition(geom[index]),dGeomGetRotation(geom[index]),ss);
}
void drawCylinder(int index){
dsSetTexture(DS_WOOD);
dReal r,l;
dGeomCylinderGetParams(geom[index],&r,&l);
if(touches[index]==1){
dsSetColor(0,0,0);
}else{
dsSetColor(1,1,1);
}
dsDrawCylinder(dBodyGetPosition(body[index]),dBodyGetRotation(body[index]),l,r);
dsDrawCylinder(dGeomGetPosition(geom[index]),dGeomGetRotation(geom[index]),l,r);
}
// simulation loop
void createHinge(int index,int body1, int body2, double fulx,double fuly, double fulz,double ax,double ay, double az,double lowAngle,double highAngle){
joints[index] = dJointCreateHinge(world,0);
dJointAttach(joints[index],body[body1],body[body2]);
dJointSetHingeAnchor(joints[index],fulx,fuly,fulz);
dJointSetHingeAxis(joints[index],ax,ay,az);
dJointSetHingeParam(joints[index],dParamLoStop,lowAngle*(3.14159/180));
dJointSetHingeParam(joints[index],dParamHiStop,highAngle*(3.14159/180));
}
void destroyHinge(int index){
dJointDestroy(joints[index]);
}
void ActuateJoint(int jointIndex,double desiredAngle){
double force = 4;
double speed = 10;
dJointSetHingeParam(joints[jointIndex],dParamFMax,force);
double curAngle = dJointGetHingeAngle(joints[jointIndex]);
double diff = desiredAngle-curAngle;
dJointSetHingeParam(joints[jointIndex],dParamVel,speed*diff);
}
// start simulation - set viewpoint
void WeightsRead(){
std::ifstream ifile(weightPath);
std::string line;
bool isWeights = ifile;
while(!isWeights){//wait until weights exist
std::ifstream ifile(weightPath);
isWeights = ifile;
}
ifile.close();
std::ifstream weightf(weightPath);
if(weightf.is_open()){
for(int i =0; i < 7; i++){
for(int j=0; j < 6; j++){
getline(weightf,line);
std::stringstream s(line);
s >> weights[i][j];
}
}
}
weightf.close();
remove(weightPath);
}
void FitnessSave()
{
if(outofevo){
const dReal * x = dBodyGetPosition(body[0]);
std::cout << x[0] <<" 1 "<< x[1] <<" 2 "<< x[2] << std::endl;
}
if(!outofevo){
const dReal * x = dBodyGetPosition(body[0]);
dReal fitn;
fitn = (x[0]) + x[2] + (0-abs(x[1]));
//fitn = x[0];
if(x[0] > 9){
fitn = 0;
}
//std::cout << x[0] <<" 1 "<< x[1] <<" 2 "<< x[2] << std::endl;
fitnessPlotInfoChild[globalcounter] = fitn;
if(bestFitness < fitnessPlotInfoChild[globalcounter]){
bestFitness = fitnessPlotInfoChild[globalcounter];
copyChildToWeights();
copyChildToParent();
saveWeights();
}
fitnessPlotInfoParent[globalcounter] = bestFitness;
if(globalcounter == 0){
fitnessPlotInfoParent[0] = fitn;
}
std::cout << globalcounter <<" Best Fitness: " << bestFitness << " Child Fitness: " << fitn << std::endl;
}
}
void createRobot(){
double shoulderRad = .1;
double elbowRad = .1;
double shiftz = .2;
//lets make a robot shall we?
createBox(0, 0,0,.8+shiftz,.3,.6,1.0);
//direction 1=x 2=y 3=z
createCylinder(1,.5,-.3-shoulderRad,1.15+shiftz,1,1, shoulderRad);//index,pos123,length,direction,radius
createCylinder(2,.5,.3+shoulderRad,1.15+shiftz,1,1,shoulderRad);
createCylinder(3,1,-.3-shoulderRad,.65+shiftz,1,3,elbowRad);//elbows
createCylinder(4,1,.3+shoulderRad,.65+shiftz,1,3,elbowRad);
createCylinder(5,0,-.2,.2,.4+shiftz,3,0.1);//legs
createCylinder(6,0,.2,.2,.4+shiftz,3,0.1);
//making teh joints
createHinge(0,0,1,0,-.3,1.15+shiftz,0,0,1,-30,90);//body to left shoulder
createHinge(1,0,2,0,.3,1.15+shiftz,0,0,1,-90,30);//body to right shoulder
createHinge(2,1,3,1,-.3,1+shoulderRad+shiftz,0,1,0,-60,60);//shoulder to elbow (left)
createHinge(3,2,4,1,.3,1+shoulderRad+shiftz,0,1,0,-60,60);//shoulder to elbow (right)
createHinge(4,0,5,0,-.2,.4+shiftz,0,1,0,-45,45);//legs
createHinge(5,0,6,0,.2,.4+shiftz,0,1,0,-45,45);
if(drawingOn){
WeightsRead();
for(int i=0; i < 6; i++){
prevOutputs[i] = bias;}
}
}
void deleteRobot(){
deleteObject(0);
deleteObject(1);
deleteObject(2);
deleteObject(3);
deleteObject(4);
deleteObject(5);
deleteObject(6);
destroyHinge(0);
destroyHinge(1);
destroyHinge(2);
destroyHinge(3);
destroyHinge(4);
destroyHinge(5);
}
static void command (int cmd)
{
switch (cmd) {
case 'q': case 'Q':
ActuateJoint(0,-45*3.14/180);
break;
case 'w': case 'W':
ActuateJoint(0,45*3.14/180);
break;
case 'e':
ActuateJoint(2,-45*3.14/180);
break;
case 'r':
ActuateJoint(2,45*3.14/180);
break;
case ' ':
speed = 0;
steer = 0;
break;
case 'p':
timerCount = 0;
//Delete the robot! YAAAH
FitnessSave();
deleteRobot();
//create the robot YAAAH
createRobot();
break;
case '1': {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"");
fclose (f);
}
}
}
}
void createEnvironment(){
//0 is most difficult
createBox(7, 5-difficulty,0, .125, 1, 3,.25,20);
createBox(8, 6-difficulty,0, .25, 1, 3,.5,20);
createBox(9, 7-difficulty,0, .375, 1, 3,.75,20);
createBox(10,9-difficulty,0, .5, 3, 5, 1,20);
hooks[0] = dJointCreateHinge(world,0);
dJointAttach(hooks[0],body[7],body[8]);
dJointSetHingeAnchor(hooks[0],5.5-difficulty,0,0);
dJointSetHingeAxis(hooks[0],0,0,1);
dJointSetHingeParam(hooks[0],dParamLoStop,0);
dJointSetHingeParam(hooks[0],dParamHiStop,.1);
hooks[1] = dJointCreateHinge(world,0);
dJointAttach(hooks[1],body[8],body[9]);
dJointSetHingeAnchor(hooks[1],6.5-difficulty,0,0);
dJointSetHingeAxis(hooks[1],0,0,1);
dJointSetHingeParam(hooks[1],dParamLoStop,0);
dJointSetHingeParam(hooks[1],dParamHiStop,.1);
hooks[2] = dJointCreateHinge(world,0);
dJointAttach(hooks[2],body[9],body[10]);
dJointSetHingeAnchor(hooks[2],7.5-difficulty,0,0);
dJointSetHingeAxis(hooks[2],0,0,1);
dJointSetHingeParam(hooks[2],dParamLoStop,0);
dJointSetHingeParam(hooks[2],dParamHiStop,.1);
hooks[3] = dJointCreateHinge(world,0);
dJointAttach(hooks[3],body[9],body[10]);
dJointSetHingeAnchor(hooks[3],8.5-difficulty,0,0);
dJointSetHingeAxis(hooks[3],0,0,1);
dJointSetHingeParam(hooks[3],dParamLoStop,0);
dJointSetHingeParam(hooks[3],dParamHiStop,.1);
}
void drawEnvironment(){
drawBox(7);
drawBox(8);
drawBox(9);
drawBox(10);
}
void destroyEnvironment(){
deleteObject(7);
deleteObject(8);
deleteObject(9);
deleteObject(10);
dJointDestroy(hooks[0]);
dJointDestroy(hooks[1]);
dJointDestroy(hooks[2]);
dJointDestroy(hooks[3]);
}
static void simLoop (int pause)
{
//int i;
//if (!pause) {
// // motor
// dJointSetHinge2Param (joint[0],dParamVel2,-speed);
// dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
// // steering
// dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
// if (v > 0.1) v = 0.1;
// if (v < -0.1) v = -0.1;
// v *= 10.0;
// dJointSetHinge2Param (joint[0],dParamVel,v);
// dJointSetHinge2Param (joint[0],dParamFMax,0.2);
// dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
// dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
// dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
if(!pause){
timerCount++;
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.03);
dJointGroupEmpty (contactgroup);
double motorCommand=0;
for(int m =0; m < 6; m++){//
for(int s=0; s < 7; s++){//
motorCommand = motorCommand + touches[s]*weights[s][m];
}
motorCommand = tanh(motorCommand);
motorCommand = (motorCommand*60);
outputs[m] = motorCommand;
if(recurrent){
//bias = bias*-1;//alternate the bias so as not to be biased
//std::cout << bias << std::endl;
outputs[m] = /*outputs[m] + */outputs[m]*(tanh(prevOutputs[m])*60);//+bias;//add bias to avoid being stuck with 0
prevOutputs[m] = outputs[m];
motorCommand = outputs[m];
//std::cout << "r " << motorCommand << std::endl;
}
ActuateJoint(m,motorCommand*(3.14159/180));
motorCommand = 0;
}
//print out touch vector
//for( int o = 0; o < 7; o++){
// std::cout << touches[o];
//}
//std::cout << std::endl;
}
//}
//dsSetColor (0,1,1);
//dsSetTexture (DS_WOOD);
//dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
//dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
//dsSetColor (1,1,1);
//for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
// dBodyGetRotation(body[i]),0.02f,RADIUS);
//dVector3 ss;
//dGeomBoxGetLengths (ground_box,ss);
//dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
if(drawingOn){
drawBox(0);
drawCylinder(1);
drawCylinder(2);
drawCylinder(3);
drawCylinder(4);
drawCylinder(5);
drawCylinder(6);
drawEnvironment();
if(capturing){ CaptureFrame(timerCount);}
}
for(int o=0; o<6; o++){ if(!pause){touches[o] = 0;} }//reset touches array
if(timerCount==1000){
timerCount = 0;
//Delete the robot! YAAAH
FitnessSave();
deleteRobot();
destroyEnvironment();
//create the robot YAAAH
createRobot();
createEnvironment();
for(int i=0; i < 6; i++){
prevOutputs[i] = bias;}
}
}
int main (int argc, char **argv)
{
//int i;
//dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (space);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-.5);
ground = dCreatePlane (space,0,0,1,0);
// srand(23); //seed 1 = dancing
//// environment
//ground_box = dCreateBox (space,2,1.5,1);
//dMatrix3 R;
//dRFromAmd
//xisAndAngle (R,0,1,0,-0.15);
//dGeomSetPosition (ground_box,2,0,-0.34);
//dGeomSetRotation (ground_box,R);
//// run simulation
//seed the random number generator
//;
createEnvironment();
//create parents and children and whatnot
createParent();
copyParentToWeights();
createRobot();
if(drawingOn){
dsSimulationLoop (argc,argv,352,288,&fn);
std::cout << "BE WARY OF BEST WEIGHTS BEING OVERWRITTEN! MAKE COPIES!" << std::endl;
}else{
for(globalcounter=0; globalcounter < globalgenerations; globalcounter++){
for(int eachRobot =0; eachRobot < 1000; eachRobot++){
simLoop(0);
}
if(globalcounter == ((int)globalgenerations/10)){ mutationProb = .4;std::cout << "mut change .4\n";}
if(globalcounter == ((globalgenerations/10)+1*(globalgenerations/10))){ mutationProb == .3; std::cout << "mut change .3\n";}
if(globalcounter == ((globalgenerations/10)+2*(globalgenerations/10))){ mutationProb == .2;std::cout << "mut change .2\n";}
if(globalcounter == ((globalgenerations/10)+3*(globalgenerations/10))){ mutationProb == .1;std::cout << "mut change .1\n";}
if(globalcounter == ((globalgenerations/10)+4*(globalgenerations/10))){ mutationProb == .05;std::cout << "mut change .05\n";}
createChild();
copyChildToWeights();
for(int i=0; i < 6; i++){
prevOutputs[i] = bias;}
}
writeBestFitnessFiles();
}
globalcounter=0;
outofevo = true;
if(!drawingOn){
createRobot();
drawingOn=true;
dsSimulationLoop(argc,argv,352,288,&fn);
std::cout << "BE WARY OF BEST WEIGHTS BEING OVERWRITTEN! MAKE COPIES!" << std::endl;
deleteRobot();
}
destroyEnvironment();
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
//return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment