295 Evo Final. Code from several years ago

295evo.cpp

/*************************************************************************
 *                                                                       *
 * 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;
}