// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(6.5701, 17.3534, 11.7749), Pos(159.449, -30.0839, 0)); // initialization // - set noise to 0.1 // - register file chess.ppm as a texture called chessTexture (used for the wheels) global.odeConfig.noise=0.1; // global.odeConfig.setParam("gravity", 0); // int chessTexture = dsRegisterTexture("chess.ppm"); // 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 geometry for each wall of playground: // setGeometry(double length, double width, double height) // - setting initial position of the playground: setPosition(double x, double y, double z) // - push playground in the global list of obstacles(globla list comes from simulation.cpp) OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(8.0f, 0.2, 6.0f), 12, // number of corners true); // if ground is created (only create one) // true); // if ground is created (only create one) // when you not set this texture, a brown wooden texture // is used, the setColor would overlay with the wooden texture playground->setColor(Color(1.0,0.5,0.0,0.2)); // when you not set this texture, a brown wooden texture // is used, the setColor would overlay with the wooden texture playground->setTexture("Images/really_white.rgb"); playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren global.obstacles.push_back(playground); OctaPlayground* playground2 = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(1.0f, 0.2, 2.5f), 12, // number of corners false); // if ground is created (only create one) playground2->setColor(Color(1.0,0.5,0.0,0.1)); // when you not set this texture, a brown wooden texture // is used, the setColor would overlay with the wooden texture playground2->setTexture("Images/really_white.rgb"); playground2->setPosition(osg::Vec3(0.0,0.0,0)); // playground positionieren und generieren global.obstacles.push_back(playground2); // Creation of passive boxes // add passive spheres, boxes and capsules as obstacles // - create pointer to sphere, box or capsule (with odehandle, osghandle and // optional parameters radius and mass,where the latter is not used here) ) // - set Pose(Position) of sphere, box or capsule // - set a texture for the sphere, box or capsule // - add sphere, box or capsule to list of obstacles // note that the dependent header file has to be include above // ("passivesphere.h", "passivebox.h" and/or "passivecapsule.h" int n=10; int m=3; for (int j=0;j<m;j++) { for(int i=0; i<n; i++){ PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), /*Diameter=*/.2+i*0.03,/*Mass=*/.5); s->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n)+0.05f))*(3.0f+2.0f*j), cos(2*M_PI*(((float)i)/((float)n)+0.05f))*(3.0f+2.0f*j), 0.8f+2.0f*j)); s->setTexture("Images/dusty.rgb"); global.obstacles.push_back(s); } } n=8; m=3; for (int j=0;j<m;j++) { for(int i=0; i<n; i++){ PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), osg::Vec3(0.4+i*0.1, 0.8-i*0.03, 0.4), 0.2+i*0.1); if (i==1) b->setTexture("Images/light_chess.rgb"); else b->setTexture("Images/dusty.rgb"); b->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n))+0.1f)*(3.0f+2.0f*j), cos(2*M_PI*(((float)i)/((float)n))+0.1f)*(3.0f+2.0f*j), 3.0f+2.0f*j)); global.obstacles.push_back(b); } } n=15; m=3; for (int j=0;j<m;j++) { for(int i=0; i<n; i++){ PassiveCapsule* b = new PassiveCapsule(odeHandle, osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2f+0.03*i,1.0f-i*0.04); b->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n)))*(3.0f+2.0f*j), cos(2*M_PI*(((float)i)/((float)n)))*(3.0f+2.0f*j), 5.0f+2.0f*j)); b->setTexture("Images/dusty.rgb"); global.obstacles.push_back(b); } } // Creation of spherical robots: for(int i=0; i<0; i++){ OdeRobot* sphere1; Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf(); conf.diameter=2; conf.irAxis1=true; conf.irAxis2=true; conf.irAxis3=true; sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(0.0+.1*i,0.0,1.0)), conf, "Sphere1", 0.4); // sphere1 = new ForcedSphere(odeHandle, osgHandle, "FSphere"); // sphere1->place ( Pos(-3,1/2,3+2*i)); sphere1->place ( Pos(0,0,7+2*i)); AbstractController* controller = new InvertMotorNStep(); controller->setParam("steps", 2); controller->setParam("adaptrate", 0.005); controller->setParam("nomupdate", 0.01); controller->setParam("epsC", 0.01); controller->setParam("epsA", 0.005); controller->setParam("rootE", 3); DerivativeWiringConf c = DerivativeWiring::getDefaultConf(); c.useId = true; c.useFirstD = true; DerivativeWiring* wiring = new DerivativeWiring ( c , new ColorUniformNoise() ); // One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() ); OdeAgent* agent = new OdeAgent ( plotoptions ); 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 ); } //creation of snakes for(int i=0; i<1; i++){ //****************/ SchlangeConf conf = Schlange::getDefaultConf(); conf.motorPower=.3; conf.segmNumber =10-i/2; conf.segmDia = 0.15; // diameter of a snake element // conf.jointLimit=conf.jointLimit*3; conf.jointLimit=conf.jointLimit*2.0; conf.frictionJoint=0.002; SchlangeServo2* schlange1 = //new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.8, 0.3, 0.5)), new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(1.0, 1.0, 1.0)), conf, "S1"); //Positionieren und rotieren schlange1->place(osg::Matrix::rotate(M_PI/2, 0, 1, 0)* osg::Matrix::translate(2*i,i,conf.segmNumber/2+2)); if (i==0) { // schlange1->setTexture("Images/whitemetal_tiefgruen.rgb"); // schlange1->setHeadTexture("Images/whitemetal_tiefrot.rgb"); } else { // schlange1->setTexture("Images/whitemetal_tiefrot.rgb"); // schlange1->setHeadTexture("Images/whitemetal_tiefgruen.rgb"); } //AbstractController *controller = new InvertNChannelController(100/*,true*/); // AbstractController *controller = new InvertMotorSpace(100/*,true*/); AbstractController *controller = new InvertMotorNStep(); // AbstractController *controller = new invertmotornstep(); //AbstractController *controller = new SineController(); // AbstractController *controller = new InvertMotorNStep(); // AbstractController *controller = new SineController(); // AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); //Only this line for one2Onewiring // AbstractWiring* wiring = new DerivativeWiring(c, new ColorUniformNoise(0.1)); DerivativeWiringConf c = DerivativeWiring::getDefaultConf(); c.useId = true; c.useFirstD = true; // c.derivativeScale=10; DerivativeWiring* wiring = new DerivativeWiring ( c , new ColorUniformNoise() ); OdeAgent* agent = new OdeAgent(global); agent->init(controller, schlange1, wiring); global.agents.push_back(agent); global.configs.push_back(controller); global.configs.push_back(schlange1); global.odeConfig.setParam("controlinterval",2); global.odeConfig.setParam("gravity", -2); controller->setParam("steps",2); controller->setParam("epsC",0.01); controller->setParam("epsA",0.01); controller->setParam("adaptrate",0.005); controller->setParam("logaE",3); // controller->setParam("desens",0.0); controller->setParam("s4delay",3.0); // controller->setParam("s4avg",1.0); controller->setParam("factorB",0.0); // controller->setParam("zetaupdate",0.1); }//creation of snakes End }
/// start() is called at the start and should create all the object (obstacles, agents...). virtual void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global){ setCameraHomePos(Pos(1.86452, 21.1998, 8.7964), Pos(174.993, -10.688, 0)); setCameraMode(Follow); global.odeConfig.setParam("controlinterval",1); global.odeConfig.setParam("gravity", -9.81); global.odeConfig.setParam("noise", 0); global.odeConfig.setParam("realtimefactor", 1.); int sliderwheelies = 1; int snakes = 0; Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(120, 0.2, 2.5),.5,true); // playground->setColor(Color(1,0.2,0,0.1)); playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren global.obstacles.push_back(playground); // Creation of Obstacle PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(180.0 / 255.0, 180.0 / 255.0, 130.0 / 255.0)), osg::Vec3(4.0, 20.0, .8), 0); b->setTexture(0,TextureDescr("Images/playfulmachines.rgb",1,1)); b->setPosition(Pos(-32, 0, 0)); global.obstacles.push_back(b); /******* S L I D E R - W H E E L I E *********/ for(int i=0; i<sliderwheelies; i++){ SliderWheelieConf mySliderWheelieConf = SliderWheelie::getDefaultConf(); mySliderWheelieConf.segmNumber=12; mySliderWheelieConf.jointLimitIn=M_PI/3; mySliderWheelieConf.frictionGround=0.5; mySliderWheelieConf.motorPower=8; mySliderWheelieConf.motorDamp=0.05; mySliderWheelieConf.sliderLength=0.5; mySliderWheelieConf.segmLength=1.4; OdeRobot* robot = new SliderWheelie(odeHandle, osgHandle, mySliderWheelieConf, "Slider Armband"); robot->place(Pos(0,0,2.0)); //controller = new Sos(1.0); controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint"); AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); OdeAgent* agent = new OdeAgent(global); // only the first controller is exported to guilogger and Co agent->addInspectable(((OneControllerPerChannel*)controller)->getControllers()[0]); agent->init(controller, robot, wiring); if(track) agent->setTrackOptions(TrackRobot(true,true,false,false,"split_control",1)); global.agents.push_back(agent); global.configs.push_back(agent); } //****** SNAKES **********/ for(int i=0; i<snakes; i++){ //****************/ SchlangeConf conf = Schlange::getDefaultConf(); // conf.segmMass = .2; // conf.segmLength= .5// 0.8; // conf.segmDia=.6; // conf.motorPower=.5; conf.segmNumber = 12+2*i;//-i/2; conf.jointLimit=conf.jointLimit* 1.6; conf.frictionJoint=0.02; conf.useServoVel=true; SchlangeServo2* schlange1; if (i==0) { schlange1 = new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)), conf, "S1"); } else { schlange1 = new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)), conf, "S2"); } //Positionieren und rotieren schlange1->place(osg::Matrix::rotate(M_PI/2,0, 1, 0)* osg::Matrix::translate(-.7+0.7*i,0,(i+1)*(.2+conf.segmNumber)/2.0/*+2*/)); // osg::Matrix::translate(5-i,2 + i*2,height+2)); schlange1->setTexture("Images/whitemetal_farbig_small.rgb"); if (i==0) { schlange1->setHeadColor(Color(1.0,0,0)); } else { schlange1->setHeadColor(Color(0,1.0,0)); } OneControllerPerChannel *controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint"); AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); OdeAgent* agent = new OdeAgent(global); agent->addInspectable(controller->getControllers()[0]); agent->init(controller, schlange1, wiring); global.agents.push_back(agent); global.configs.push_back(agent); }//creation of snakes End }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { // set initial camera position setCameraHomePos(Pos(-1.14383, 10.1945, 42.7865), Pos(179.991, -77.6244, 0)); // initialization simulation parameters //1) - set noise to 0.1 global.odeConfig.noise= 0.02;//0.05; //2) - set controlinterval -> default = 1 global.odeConfig.setParam("controlinterval", 1);/*update frequency of the simulation ~> amos = 20*/ //3) - set simulation setp size global.odeConfig.setParam("simstepsize", 0.01); /*stepsize of the physical simulation (in seconds)*/ //Update frequency of simulation = 1*0.01 = 0.01 s ; 100 Hz //4) - set gravity if not set then it uses -9.81 =earth gravity //global.odeConfig.setParam("gravity", -9.81); //5) - set 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 geometry for each wall of playground: // setGeometry(double length, double width, double height) // - setting initial position of the playground: setPosition(double x, double y, double z) // - push playground in the global list of obstacles(globla list comes from simulation.cpp) // odeHandle and osgHandle are global references // vec3 == length, width, height double length_pg = 35;//28;//0.0; //45, 32, 22 double width_pg = 0.5;//0.0; //0.2 double height_pg = 0.5;//0.0; //0.5 Playground* playground = new Playground(odeHandle, osgHandle.changeColor(Color(0.6,0.0,0.6)), osg::Vec3(length_pg /*length*/, width_pg /*width*/, height_pg/*height*/), /*factorxy = 1*/1, /*createGround=true*/true /*false*/); playground->setPosition(osg::Vec3(4,0,0.0)); // playground positionieren und generieren // register playground in obstacles list global.obstacles.push_back(playground); // Playground* playground = // new Playground(odeHandle, osgHandle.changeColor(Color(0.88f,0.4f,0.26f,0.2f)),osg::Vec3(18, 0.2, 2.0)); // playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren // Substance substance; // substance.toRubber(40); // playground->setGroundSubstance(substance); // global.obstacles.push_back(playground); // for(int i=0; i<0; i++) // { // PassiveSphere* s = // new PassiveSphere(odeHandle, // osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2); // s->setTexture("Images/dusty.rgb"); // s->setPosition(Pos(i*0.5-2, i*0.5, 1.0)); // global.obstacles.push_back(s); // } if(!ico_learning_task) { for (int j=0;j<4;j++) { for(int i=0; i<4; i++) { PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(1.0f,0.2f,0.2f,0.5f)), osg::Vec3(1.5+i*0.01,1.5+i*0.01,1.5+i*0.01),40.0); b->setTexture("Images/light_chess.rgb"); b->setPosition(Pos(i*4-5, -5+j*4, 1.0)); global.obstacles.push_back(b); } } } //6) - add passive spheres as TARGET! // - 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 // - set a texture for the sphere // - add sphere to list of obstacles PassiveSphere* s1; double target_z_position = 1.5; if(ico_learning_task) { target_z_position = 0.5; } for (int i=0; i < number_spheres; i++){ s1 = new PassiveSphere(odeHandle, osgHandle, 0.5); //Target 1 if(random_positon_spheres) { /*******Generating Random Position****/ random_position_S = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x; // Input 0 (m) between -2.4 and 2.4 std::cout<<"\n\n\n\n\n"<<"Inital Random Target X position"<<" = "<<-1*random_position_S<<"\t"<<"Inital Random Target Y position"<<" = "<<random_position_S<<"\n\n\n\n"; /************************************/ if (i==0) s1->setPosition(osg::Vec3(-1*random_position_S,random_position_S,target_z_position/*2*/)); } else { //position_S = 0.0;// -2.0---------------------------------------------------------------------------TEST if (i==0) s1->setPosition(osg::Vec3(-10.0 /*position_S*/, position_S, target_z_position/*0*/)); } //Target 2 //if (i==1) s1->setPosition(osg::Vec3(15-11.0 /*position_S*/, position_S+10, 0.5/*0*/)); if (i==1) s1->setPosition(osg::Vec3(0.0 /*position_S*/, position_S+5, target_z_position/*0*/)); //Target 3 //if (i==2) s1->setPosition(osg::Vec3(15-11.0 /*position_S*/, position_S-10, 0.5/*0*/)); if (i==2) s1->setPosition(osg::Vec3(0.0 /*position_S*/, position_S-5, target_z_position/*0*/)); //Target 4 if (i==3) s1->setPosition(osg::Vec3(-10.0 /*position_S*/, position_S, target_z_position/*0*/)); //if (i==3) s1->setPosition(osg::Vec3(-10, 10,2/*2*/)); s1->setTexture("Images/dusty.rgb"); //Target 1 if (i==0){ s1->setColor(Color(1,0,0)); } //Target 2 if (i==1){ s1->setColor(Color(0,1,0)); } //Target 3 if (i==2){ s1->setColor(Color(0,0,1)); } //Target 4 if (i==3){ s1->setColor(Color(1,1,0)); } obst.push_back(s1); global.obstacles.push_back(s1); // fix sphere (in actual position) to simulation //fixator.push_back(new FixedJoint(s1->getMainPrimitive(), global.environment)); //create pointer //fixator.at(i)->init(odeHandle, osgHandle); fixator2 = new FixedJoint(s1->getMainPrimitive(), global.environment); fixator2->init(odeHandle, osgHandle); } // int number_spheres = 4; // PassiveSphere* s1; // for (int i=0; i < number_spheres; i++){ // s1 = new PassiveSphere(odeHandle, osgHandle, 0.5); // if (i==0) s1->setPosition(osg::Vec3(-10,-10,2)); // if (i==1) s1->setPosition(osg::Vec3( 10, 10,2)); // if (i==2) s1->setPosition(osg::Vec3( 10, -10,2)); // if (i==3) s1->setPosition(osg::Vec3(-10, 10,2)); // // s1->setTexture("Images/dusty.rgb"); // if (i==0){ s1->setColor(Color(1,0,0)); } // if (i==1){ s1->setColor(Color(0,1,0)); } // if (i==2){ s1->setColor(Color(0,0,1)); } // if (i==3){ s1->setColor(Color(1,1,0)); } // obst.push_back(s1); // global.obstacles.push_back(s1); // // fix sphere (in actual position) to simulation // fixator.push_back( new FixedJoint(s1->getMainPrimitive(), global.environment)); //create pointer // fixator.at(i)->init(odeHandle, osgHandle); // } //7) - add passive box as OBSTACLES // - create pointer to Box (with odehandle, osghandle and // optional parameters radius and mass,where the latter is not used here) ) // - set Pose(Position) of sphere // - set a texture for the sphere // - add sphere to list of obstacles PassiveBox* b1; double length = 10.0;//1.5 /*for learning*/; //3.5 /*for*/;//testing//---------------------------------------------------------------------------TEST double width = 1.0; double height = 1.0; if(ico_learning_task) { obstacle_on = false; } if(obstacle_on) { for (int i=0; i < number_boxes /*SET NUMBER OF OBSTACLES*/; i++){ b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height /*size*/)); //b1->setTexture("dusty.rgb"); b1->setColor(Color(1,0,0)); //b1->setPosition(osg::Vec3(/*-4.5+*/i*4.5,3+i,0)); // Fixed robot position //osg::Matrix pose; // pose.setTrans(osg::Vec3(/*-4.5+*/i*4.5,3+i,0)); b1->setPose(osg::Matrix::rotate(0.3*(M_PI/2)*i, 0,0, 1) * osg::Matrix::translate(/*-4.5+*/i*5,5+i,0.5) /* pose*/); //b1->setPose(osg::Matrix::rotate(/*-0.5*(M_PI/4)*/ (M_PI/2), 0,0, 1) * osg::Matrix::translate(/*i*4.5+*/pos_obstacle_x, pos_obstacle_y+i*-pos_obstacle_y*2.0/*0+i*/,height/2) /* pose*/); global.obstacles.push_back(b1); fixator.push_back( new FixedJoint(b1->getMainPrimitive(), global.environment)); //create pointer fixator.at(i)->init(odeHandle, osgHandle); } } ////////////////////////////////////Call this set up and Control////////////////////////////////////////////// bool ac_ico_robot = true; if (ac_ico_robot) { //0) //qcontroller->setReset(1); //1) Activate IR sensors FourWheeledConf fconf =FourWheeledRPos::getDefaultConf(); ///2) relative sensors for (int i=0; i < number_spheres; i++){ fconf.rpos_sensor_references.push_back(obst.at(i)->getMainPrimitive()); } OdeRobot* vehicle3 = new FourWheeledRPos(odeHandle, osgHandle, fconf); /****Initial position of Nimm4******/ if(random_positon_frist) { /*******Generating Random Position****/ //srand (time(NULL)); random_position = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x; // Input 0 (m) between -2.4 and 2.4 /************************************/ do { int r = rand(); std::cout << "random value 1: " << r << std::endl; random_position = ((MAX_x-MIN_x)*((float)r/RAND_MAX))+MIN_x; //std::cout<<"\n\n\n\n\n"<<"Inital Random_Robot X position"<<" = "<<random_position<<"\t"<<"Inital Random_Robot Y position"<<" = "<<-1*random_position<<"\n\n\n\n"; }while(abs(abs(position_S) - abs(random_position)) <= 4); //while(abs(abs(random_position_S) - abs(random_position)) <= 4); std::cout<<"\n\n"<<"Inital Random X position"<<" = "<<random_position<<"\t"<<"Inital Random Y position"<<" = "<<-1*random_position<<"\n\n"; //vehicle3->place(Pos(position_x-10.0 /* 20 random_position*/ /*+x = to left (sphere in front of robot), -x = to right sphere behind robot*/, 0.0 /*-1*random_position*/ /*+y = to up, -y = to down*/,0.0/*z*/)); vehicle3->place(Pos(position_x-15.0 /* 20 random_position*/ /*+x = to left (sphere in front of robot), -x = to right sphere behind robot*/, 0.0 /*-1*random_position*/ /*+y = to up, -y = to down*/,0.0/*z*/)); } else { /*******Generating Random Position****/ //srand (time(NULL)); int r = rand(); std::cout << "random value 2: " << r << std::endl; random_or = ((MAX_or-MIN_or)*((float)r/RAND_MAX))+MIN_or; // between -pi/3 (60 deg) and pi/3 (60 deg) /************************************/ Pos pos(position_x-5.0/*, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/); std::cout << "random_or1: " << random_or << std::endl; //setting position and orientation vehicle3->place(osg::Matrix::rotate(random_or, 0, 0, 1) *osg::Matrix::translate(pos)); //setting only position //vehicle3->place(Pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/)); } qcontroller = new EmptyController("1","1"); global.configs.push_back(qcontroller); // create pointer to one2onewiring AbstractWiring* wiring3 = new One2OneWiring(new ColorUniformNoise(0.1)); // create pointer to agent plotoptions.push_back(PlotOption(NoPlot /*select "File" to save signals, "NoPlot" to not save*/)); OdeAgent* agent3 = new OdeAgent(plotoptions); if(drawtrace_on) { TrackRobot* track3 = new TrackRobot(/*bool trackPos*/true, /*bool trackSpeed*/false, /*bool trackOrientation*/false, /*bool displayTrace*/true //, /*const char *scene="", int interval=1*/); agent3->setTrackOptions(*track3); } agent3->init(qcontroller, vehicle3, wiring3);///////////// Initial controller!!! global.agents.push_back(agent3); } //bool random_controlled_robot =false; showParams(global.configs); }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(-0.777389, 6.34573, 1.83396), Pos(-170.594, -5.10046, 0)); setCameraMode(Follow); // initialization // - set noise to 0.1 // - register file chess.ppm as a texture called chessTexture (used for the wheels) global.odeConfig.setParam("noise", 0.05); global.odeConfig.setParam("controlinterval", 1); global.odeConfig.setParam("realtimefactor", 1); // global.odeConfig.setParam("gravity", 0); for(int i=0; i< 4; i++){ PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(.6, .6, .4)), osg::Vec3(1,10,0.3+i*0.02),0); b->setTexture(0,TextureDescr("Images/playfulmachines.rgb",1,1)); b->setPosition(osg::Vec3(-75+i*30,0,0)); global.obstacles.push_back(b); } controller=0; /******* S L I D E R - w H E E L I E *********/ SliderWheelieConf mySliderWheelieConf = SliderWheelie::getDefaultConf(); mySliderWheelieConf.segmNumber = segmnum; mySliderWheelieConf.motorPower = 5; mySliderWheelieConf.jointLimitIn = M_PI/3; // mySliderWheelieConf.frictionGround=0.5; // mySliderWheelieConf.segmLength=1.4; mySliderWheelieConf.sliderLength = 0; mySliderWheelieConf.motorType = SliderWheelieConf::CenteredServo; //mySliderWheelieConf.drawCenter = false; vehicle = new SliderWheelie(odeHandle, osgHandle.changeColor(Color(1,222/255.0,0)), mySliderWheelieConf, "Armband"); vehicle->place(Pos(0,0,.1)); SeMoXConf cc = SeMoX::getDefaultConf(); cc.cInit=1.1; cc.modelExt=false; cc.someInternalParams=false; SeMoX* semox = new SeMoX(cc); if(useSym){ semox->setParam("epsC", 0.1); semox->setParam("epsA", 0.1); }else{ semox->setParam("epsC", 0.1); semox->setParam("epsA", 0.1); } semox->setParam("rootE", 3); semox->setParam("s4avg", 1); semox->setParam("gamma_cont", 0.005); semox->setParam("gamma_teach", teacher); //controller=semox; controller = new CrossMotorCoupling( semox, semox, 0.4); // One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1)); AbstractWiring* wiring = new FeedbackWiring(new ColorUniformNoise(0.1), FeedbackWiring::Motor, 0.75); //global.plotoptions.push_back(PlotOption(GuiLogger,Robot,5)); OdeAgent* agent = new OdeAgent(global); agent->addCallbackable(&stats); agent->init(controller, vehicle, wiring); if(track) agent->setTrackOptions(TrackRobot(true,false,false, false, "uni", 20)); global.agents.push_back(agent); global.configs.push_back(agent); thisConfig.controller=controller; global.configs.push_back(&thisConfig); this->getHUDSM()->setColor(osgHandle.getColor("hud")); this->getHUDSM()->setFontsize(16); // this->getHUDSM()->setColor(Color(1.0,1.0,0)); // this->getHUDSM()->setFontsize(18); this->getHUDSM()->addMeasure(teacher,"Gamma",ID,1); this->getHUDSM()->addMeasure(thisConfig.D_display,"D",ID,1); // if(useSym){ // int k= 0; // std::list<int> perm; // int len = controller->getMotorNumber(); // for(int i=0; i<len; i++){ // perm.push_back((i+k+(len)/2)%len); // } // CMC cmc = controller->getPermutationCMC(perm); // controller->setCMC(cmc); // } }