// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(-0.497163, 11.6358, 3.67419), Pos(-179.213, -11.6718, 0)); // initialization global.odeConfig.setParam("noise",0.0); // global.odeConfig.setParam("gravity",-10); global.odeConfig.setParam("controlinterval",2); global.odeConfig.setParam("realtimefactor",1); global.odeConfig.setParam("realtimefactor",0); global.odeConfig.setParam("drawinterval",2000); runcounter=0; maxCounter=10000; // maxCounter=500; counter = maxCounter; sphere=0; // we want to start the sphere in different initial conditions and // see how the sat networks will end up driving the robot string fn=string(networkfilename) + string("scan.log"); logfile.open(fn.c_str(),ofstream::out); logfile << "# speed scan for sat network " << networkfilename << endl; logfile << "#C sx sy sz ex ey ez" << endl; // generate start speeds on 3 spheres double stepsize; double powers[3] = {5,10,15}; // double powers[3] = {5,15,30}; startAngles.push_back(Matrix(3,1)); // add zero initial condition //{ double theta =0; for(int p=0; p < 3; p++){ // stepsize=M_PI/(6.0*(p+1.0)); // 663 initial condictions stepsize=M_PI/(3.0*(p+1.0)); // 172 initial conditions for(double theta=-M_PI/2; theta < M_PI/2; theta +=stepsize){ // { double omega=0; for(double omega=-M_PI; omega < M_PI; omega += (stepsize/cos(theta)) ){ Matrix m(3,1); m.val(0,0)=theta; m.val(1,0)=omega; m.val(2,0)=powers[p]; startAngles.push_back(m); } } } speedsensor = new SpeedSensor(5, SpeedSensor::RotationalRel); //**************** conf = Sphererobot3Masses::getDefaultConf(); conf.pendularrange = 0.15; conf.motorpowerfactor = 150; conf.motorsensor=false; // conf.spheremass = 1; conf.spheremass = 0.3; conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection)); }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0)); setCameraMode(Follow); global.odeConfig.setParam("noise",0.1); global.odeConfig.setParam("controlinterval",4); global.odeConfig.setParam("realtimefactor",4); // global.odeConfig.setParam("gravity", 0); // no gravity for(int i=0; i<0; i++){ PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5); s->setPosition(osg::Vec3(5,0,i*3)); global.obstacles.push_back(s); } // Spherical Robot with axis orientation sensors: Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf(); conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection)); // regular behaviour conf.motorsensor=false; conf.diameter=1.0; conf.pendularrange= 0.25; conf.motorpowerfactor = 150; // conf.diameter=1.0; // conf.pendularrange= 0.35; sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)), conf, "Sphere1", 0.2); ((OdeRobot*)sphere1)->place ( Pos( 0 , 0 , 0.1 )); global.configs.push_back ( sphere1 ); controller = new Sox(.8,true); controller->setParam("epsA",0.3); // model learning rate controller->setParam("epsC",1); // controller learning rate controller->setParam("causeaware",0.4); controller->setParam("pseudo",2); global.configs.push_back ( controller ); One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() ); OdeAgent* agent = new OdeAgent ( global ); agent->init ( controller , sphere1 , wiring ); if(track) agent->setTrackOptions(TrackRobot(true, true, true, false, "zaxis", 20)); global.agents.push_back ( agent ); }
// starting function (executed once at the beginning of the simulation loop) void OpenLoopSim::start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { int num_barrels=1; setCameraHomePos(Pos(-1.29913, 6.06201, 1.36947), Pos(-165.217, -9.32904, 0)); setCameraMode(Follow); // initialization global.odeConfig.setParam("noise",0); // the simulation runs with 100Hz and we control every second global.odeConfig.setParam("controlinterval",2); // add a new parameter to be configured on the console global.odeConfig.addParameterDef("friction", &friction, 0.0, "rolling friction coefficient"); global.odeConfig.addParameterDef("color", &color, 0, "color of noise (correlation length)"); /* * * * BARRELS * * * */ for(int i=0; i< num_barrels; i++){ //**************** Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf(); conf.pendularrange = 0.3;//0.15; conf.motorpowerfactor = 200;//150; conf.motorsensor=false; conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y)); conf.spheremass = 1; robot = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)), conf, "Barrel", 0.4); robot->place (osg::Matrix::rotate(M_PI/2, 1,0,0) /* rotate to laying position */ * osg::Matrix::rotate(M_PI/4, 0,1,0) /* both axis at 45Deg*/ * osg::Matrix::translate(0,0,.15+2*i)); // place at 0,0 and in some height controller = new SineController(); controller->setParam("period", 300); controller->setParam("phaseshift", 1); noisegen = new ColorUniformNoise(); wiring = new MotorNoiseWiring(noisegen, 0); OdeAgent* agent = new OdeAgent ( global ); agent->init ( controller , robot , wiring ); global.agents.push_back ( agent ); global.configs.push_back ( agent ); } }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0)); // initialization // - set global noise sensor to 0.05 global.odeConfig.setParam("noise",0.05); // global.odeConfig.setParam("gravity", 0); // no gravity // use Playground as boundary: // - create pointer to playground (odeHandle contains things like world and space the // playground should be created in; odeHandle is generated in simulation.cpp) // - setting initial position of the playground: setPosition(osg::Vec3(double x, double y, double z)) // - push playground to the global list of obstacles (global list comes from simulation.cpp) OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(10, 0.2, 1), 12); playground->setPosition(osg::Vec3(0,0,0)); // place and create playground global.obstacles.push_back(playground); // add passive spheres as obstacles // - create pointer to sphere (with odehandle, osghandle and // optional parameters radius and mass,where the latter is not used here) ) // - set Pose(Position) of sphere // - add sphere to list of obstacles for(int i=0; i<8; i++){ PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5); s->setPosition(osg::Vec3(5,0,i*3)); global.obstacles.push_back(s); } // Spherical Robot with axis (gyro) sensors: // - get default configuration for robot // - create pointer to spherical robot (with odeHandle, osgHandle and configuration) // - place robot (unfortunatelly there is a type cast necessary, which is not quite understandable) Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf(); conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection)); conf.diameter=1.0; conf.pendularrange= 0.35; robot = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)), conf, "Spherical", 0.2); (robot)->place ( Pos( 0 , 0 , 0.1 )); // Selforg - Controller (Note there are several: use Sos or Sox now) // create pointer to controller // set some parameters // push controller in global list of configurables controller = new InvertMotorSpace(10); controller->setParam("epsA",0.05); // model learning rate controller->setParam("epsC",0.2); // controller learning rate controller->setParam("rootE",3); // model and contoller learn with square rooted error global.configs.push_back ( controller ); // SineController (produces just sine waves) // controller = new SineController(); // controller->setParam("sinerate", 40); // controller->setParam("phaseshift", 0.0); // create pointer to one2onewiring which uses colored-noise One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() ); // create pointer to agent // initialize pointer with controller, robot and wiring // push agent in globel list of agents OdeAgent* agent = new OdeAgent ( global ); agent->init ( controller , robot , wiring ); if(track){ // the following line will enables a position tracking of the robot, which is written into a file // you can customize what is logged with the TrackRobotConf TrackRobotConf tc = TrackRobot::getDefaultConf(); tc.scene = "zaxis"; tc.displayTrace = true; agent->setTrackOptions(TrackRobot(tc)); } global.agents.push_back ( agent ); }
void addRobot(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global, int i){ Color col; Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf(); conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection)); conf.diameter=1.0; conf.pendularrange= 0.30; // 0.15; conf.motorpowerfactor = 150; conf.spheremass = 1; conf.motorsensor=false; conf.addSensor(new SpeedSensor(5, SpeedSensor::RotationalRel)); //SphererobotArms* sphere = new SphererobotArms ( odeHandle, conf); switch(i){ case 0: col.r()=0; col.g()=1; col.b()=0.1; sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 1", 0.4); sphere->place ( osg::Matrix::translate(9.5 , 0 , height+1 )); break; case 1: col.r()=1; col.g()=0.2; col.b()=0; sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 2", 0.4); sphere->place ( osg::Matrix::translate( 2 , -2 , height+1 )); break; case 3: col.r()=0; col.g()=0; col.b()=1; sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere" + string(envnames[env]), 0.4); sphere->place ( osg::Matrix::translate( 0 , 0 , .5 )); break; default: case 2: col.r()=0; col.g()=1; col.b()=0.4; sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 3", 0.4); sphere->place ( osg::Matrix::translate( double(rand())/RAND_MAX*10 , 0 , height+1 )); break; } Sox* sox = new Sox(.8,true); sox->setParam("epsC", 0.2); sox->setParam("epsA", 0.2); if(env==ElipticBasin){ sox->setParam("epsC", 0.3); sox->setParam("epsA", 0.3); } sox->setParam("Logarithmic", 0); sox->setParam("sense", 0.5); AbstractController *controller = new GroupController(sox, 3); // AbstractWiring* wiring = new One2OneWiring ( new ColorUniformNoise() ); AbstractWiring* wiring = new SelectiveOne2OneWiring(new WhiteUniformNoise(), new select_from_to(0,2), AbstractWiring::Robot); OdeAgent* agent; if(i==0 || i==3 ){ agent = new OdeAgent (global); } else agent = new OdeAgent (global, PlotOption(NoPlot)); agent->init ( controller , sphere , wiring ); if(track) agent->setTrackOptions(TrackRobot(true,true,true,false,"",2)); global.agents.push_back ( agent ); //global.configs.push_back ( controller ); global.configs.push_back ( sphere); }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { int num_barrels=1; int num_barrels_test=0; sensor=0; setCameraMode(Follow); bool normalplayground=false; // setCameraHomePos(Pos(-0.497163, 11.6358, 3.67419), Pos(-179.213, -11.6718, 0)); setCameraHomePos(Pos(-2.60384, 13.1299, 2.64348), Pos(-179.063, -9.7594, 0)); // initialization global.odeConfig.setParam("noise",0.1); // global.odeConfig.setParam("gravity",-10); global.odeConfig.setParam("controlinterval",4); global.odeConfig.setParam("realtimefactor",5); // add a new parameter to be configured on the console global.odeConfig.addParameterDef("friction", &friction, 0.1, "rolling friction coefficient"); if(normalplayground){ Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(20, 0.01, 0.01 ), 1); playground->setGroundColor(Color(255/255.0,200/255.0,0/255.0)); playground->setGroundTexture("Images/really_white.rgb"); playground->setColor(Color(255/255.0,200/255.0,21/255.0, 0.1)); playground->setPosition(osg::Vec3(0,0,0.05)); global.obstacles.push_back(playground); } /* * * * BARRELS * * * */ for(int i=0; i< num_barrels; i++){ //**************** Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf(); conf.pendularrange = 0.3;//0.15; conf.motorpowerfactor = 200;//150; conf.motorsensor=false; conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y)); conf.spheremass = 1; conf.addSensor(new SpeedSensor(10, SpeedSensor::Translational, Sensor::X )); conf.irAxis1=false; conf.irAxis2=false; conf.irAxis3=false; conf.axesShift = 0; // conf.axesShift = conf.diameter/2 - conf.pendularrange/2; sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)), conf, "Barrel1", 0.4); sphere1->place (osg::Matrix::rotate(M_PI/2, 1,0,0)*osg::Matrix::translate(0,0,0.2)); // InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); // cc.cInit=1; // // cc.useSD=true; // controller = new InvertMotorNStep(cc); // controller->setParam("steps", 2); // controller->setParam("adaptrate", 0.0); // controller->setParam("nomupdate", 0.00); // controller->setParam("epsC", 0.03); // controller->setParam("epsA", 0.05); // controller->setParam("rootE", 0); // controller->setParam("logaE", 0); //controller = new PiMax(); //controller->setParam("epsC", 0.001); // controller = new Sox(); // controller->setParam("epsC", 0.5); controller = new ROSController("Test"); AbstractWiring* wiring = new SelectiveOne2OneWiring(new ColorUniformNoise(), new select_from_to(0,1)); // OdeAgent* agent = new OdeAgent ( PlotOption(File, Robot, 1) ); OdeAgent* agent = new OdeAgent (); agent->init ( controller , sphere1 , wiring ); // agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50)); global.agents.push_back ( agent ); global.configs.push_back ( controller ); } /* * * * TEST BARRELS * * * */ for(int i=0; i< num_barrels_test; i++){ global.odeConfig.setParam("realtimefactor",1); //**************** Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf(); conf.pendularrange = 0.15; conf.motorsensor=true; conf.irAxis1=false; conf.irAxis2=false; conf.irAxis3=false; conf.spheremass = 1; sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)), conf, "Barrel1", 0.4); sphere1->place ( osg::Matrix::rotate(M_PI/2, 1,0,0)); controller = new SineController(); controller->setParam("sinerate", 15); controller->setParam("phaseshift", 0.45); // DerivativeWiringConf dc = DerivativeWiring::getDefaultConf(); // dc.useId=true; // dc.useFirstD=false; // AbstractWiring* wiring = new DerivativeWiring(dc,new ColorUniformNoise()); AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise()); OdeAgent* agent = new OdeAgent (); agent->init ( controller , sphere1 , wiring ); // agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50)); global.agents.push_back ( agent ); global.configs.push_back ( controller ); } }
// starting function (executed once at the beginning of the simulation loop) void NormalSim::start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { int num_barrels=1; setCameraHomePos(Pos(-1.29913, 6.06201, 1.36947), Pos(-165.217, -9.32904, 0)); setCameraMode(Follow); // initialization global.odeConfig.setParam("noise",0.01); // global.odeConfig.setParam("gravity",-10); global.odeConfig.setParam("controlinterval",1); global.odeConfig.setParam("cameraspeed",200); // add a new parameter to be configured on the console global.odeConfig.addParameterDef("friction", &friction, 0.05, "rolling friction coefficient"); /* * * * BARRELS * * * */ for(int i=0; i< num_barrels; i++){ //**************** Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf(); conf.pendularrange = 0.3;//0.15; conf.motorpowerfactor = 200;//150; conf.motorsensor=false; conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y)); conf.spheremass = 1; robot = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)), conf, "Barrel", 0.4); robot->place (osg::Matrix::rotate(M_PI/2, 1,0,0) /* rotate to laying postion */ * osg::Matrix::rotate(M_PI/4, 0,1,0) /* both axis at 45Deg*/ * osg::Matrix::translate(0,0,.15+2*i)); if(mode==NoLearn){ controller = new ClosedLoopController(); } else { controller = new CtrlOutline(); } AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise()); // OdeAgent* agent = new OdeAgent ( PlotOption(File, Robot, 1) ); OdeAgent* agent = new OdeAgent ( global ); agent->init ( controller , robot , wiring ); // controller is now initialized and we can modify the matrices Matrix C(2,2); switch(mode){ case NoLearn: break; case LearnContHS: controller->setParam("TLE",0); controller->setParam("epsA",0.1); controller->setParam("epsC",0); controller->setParam("creativity",0); C.val(0,0)= .7; C.val(0,1)= .7; C.val(1,0)= -.4; C.val(1,1)= .4; if(dynamic_cast<CtrlOutline*>(controller)){ dynamic_cast<CtrlOutline*>(controller)->setC(C); } break; case LearnHS: controller->setParam("TLE",0); controller->setParam("epsA",0.1); controller->setParam("epsC",0.1); controller->setParam("creativity",0); global.odeConfig.setParam("friction", 0.1); break; case LearnHK: setParam("friction", 0.1); controller->setParam("TLE",1); break; // // controller is now initialized and we can modify the matrices // Matrix C(2,2); // C.val(0,0)= -5; // C.val(0,1)= -5; // C.val(1,0)= 5; // C.val(1,1)= -5; // if(dynamic_cast<CtrlOutline*>(controller)){ // dynamic_cast<CtrlOutline*>(controller)->setC(C); // } } // agent->setTrackOptions(TrackRobot(true, false, false, "", 50)); global.agents.push_back ( agent ); global.configs.push_back ( controller ); } }