// 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 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("gravity:",1); // normally at -9.81 // initialization Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(25, 0.2, 1.5)); playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren global.obstacles.push_back(playground); for(int i=0; i<5; i++){ PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2); s->setPosition(Pos(i*0.5-2, i*0.5, 1.0)); s->setTexture("Images/dusty.rgb"); global.obstacles.push_back(s); } OdeAgent* agent; AbstractWiring* wiring; // OdeRobot* robot; AbstractController *controller; CaterPillar* myCaterPillar; CaterPillarConf myCaterPillarConf = DefaultCaterPillar::getDefaultConf(); //******* R A U P E *********/ myCaterPillarConf.segmNumber=6; myCaterPillarConf.jointLimit=M_PI/4; myCaterPillarConf.motorPower=0.8; myCaterPillarConf.frictionGround=0.04; myCaterPillar = new CaterPillar ( odeHandle, osgHandle.changeColor(Color(0.0f,1.0f,0.0f)), myCaterPillarConf, "Raupe1"); ((OdeRobot*) myCaterPillar)->place(Pos(-5,-5,0.2)); InvertMotorNStepConf invertnconf = InvertMotorNStep::getDefaultConf(); invertnconf.cInit=2.0; controller = new InvertMotorNStep(invertnconf); wiring = new One2OneWiring(new ColorUniformNoise(0.1)); agent = new OdeAgent( global, plotoptions ); agent->init(controller, myCaterPillar, wiring); global.agents.push_back(agent); global.configs.push_back(controller); global.configs.push_back(myCaterPillar); myCaterPillar->setParam("gamma",/*gb"); global.obstacles.push_back(s)0.0000*/ 0.0); }
void ContactSensor::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle, Primitive* reference, bool createSphere, const osg::Matrix pose, bool colorObject){ assert(reference); this->colorObject = colorObject; this->reference = reference; value = 0; lastvalue = -1; if(createSphere){ sensorBody = new Sphere(size); transform = new Transform(reference, sensorBody, pose); origColor = osgHandle.getColor("joint"); transform->init(odeHandle, 0, osgHandle.changeColor(origColor)); transform->substance.setCollisionCallback(contactCollCallbackNoCol,this); }else{ reference->substance.setCollisionCallback(contactCollCallback,this); if(reference->getOSGPrimitive()) origColor = reference->getOSGPrimitive()->getColor(); else colorObject = false; } update(); initialised = true; };
// 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 ); }
void Sound::render(const OsgHandle& osgHandle){ if(!visual){ visual = new OSGSphere((intensity+1.0)/4.0); visual->init(osgHandle.changeColor(Color(255-int((frequency+1.0)*128.0), 0,int((frequency+1.0)*128.0),0.4))); visual->setMatrix(osg::Matrix::translate(pos.x(), pos.y(), pos.z()+1)); } }
void Mesh::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle, char mode) { // 20100307; guettler: sometimes the Geom is created later (XMLBoundingShape), // if no body is created, this Mesh seems to be static. Then the BoundingShape must not attach // any Primitive to the body of the Mesh by a Transform. //assert(mode & Body || mode & Geom); if (!substanceManuallySet) substance = odeHandle.substance; this->mode=mode; double r=0.01; QMP_CRITICAL(7); if (mode & Draw){ osgmesh->init(osgHandle); r = osgmesh->getRadius(); } else { osgmesh->virtualInit(osgHandle); } if (r<0) r=0.01; if (mode & Body){ body = dBodyCreate (odeHandle.world); // Todo: use compound bounding box mass instead setMass(mass, mode & Density); } // read boundingshape file // const osg::BoundingSphere& bsphere = osgmesh->getGroup()->getBound(); // 20100307; guettler: if no Geom, don't create any Geom or Boundings (this is used e.g. for Meshes loaded from XML) if (mode & Geom) { short drawBoundingMode; if (osgHandle.drawBoundings) drawBoundingMode=Primitive::Geom | Primitive::Draw; else drawBoundingMode=Primitive::Geom; boundshape = new BoundingShape(filename+".bbox" ,this); if(!boundshape->init(odeHandle, osgHandle.changeColor(Color(1,0,0,0.3)), scale, drawBoundingMode)){ printf("use default bounding box, because bbox file not found!\n"); Primitive* bound = new Sphere(r); Transform* trans = new Transform(this,bound,Pose::translate(0.0f,0.0f,0.0f)); trans->init(odeHandle, 0, osgHandle.changeColor(Color(1,0,0,0.3)),drawBoundingMode); osgmesh->setMatrix(Pose::translate(0.0f,0.0f,osgmesh->getRadius())*getPose()); // set obstacle higher } } QMP_END_CRITICAL(7); }
void TmpPrimitive::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){ Color mcolor(color); if(useColorName){ mcolor = osgHandle.getColor(colorname); mcolor.alpha() = alpha; } item->init(odeHandle, mass, osgHandle.changeColor(mcolor), mode); item->setPose(pose); initialized=true; }
void TmpJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){ Color mcolor(color); if(useColorName){ mcolor = osgHandle.getColor(colorname); mcolor.alpha() = alpha; } joint->init(odeHandle, osgHandle.changeColor(mcolor), withVisual, visualSize, ignoreColl); initialized=true; }
void TmpDisplayItem::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){ Color mcolor(color); if(useColorName){ mcolor = osgHandle.getColor(colorname); mcolor.alpha() = alpha; } item->init(osgHandle.changeColor(mcolor), quality); item->setMatrix(pose); initialized=true; }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(-1.64766, 4.48823, 1.71381), Pos(-158.908, -10.5863, 0)); // initialization // - set noise to 0.0 // - register file chess.ppm as a texture called chessTexture (used for the wheels) global.odeConfig.setParam("controlinterval",2); global.odeConfig.setParam("noise",0.1); global.odeConfig.setParam("realtimefactor",1); global.odeConfig.setParam("gravity", -3); // use Playground as boundary: playground = new Playground(odeHandle, osgHandle, osg::Vec3(10, .2, 1)); playground->setPosition(osg::Vec3(0,0,0.2)); global.obstacles.push_back(playground); playground = new OctaPlayground(odeHandle, osgHandle); playground->setPosition(osg::Vec3(15,0,0.2)); global.obstacles.push_back(playground); // add passive spheres as obstacles for (int i=0; i< 1/*2*/; i+=1){ PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.3); // s1->setPosition(osg::Vec3(-4.5+i*4.5,0,0)); s1->setPosition(osg::Vec3(0,0,1+i*5)); s1->setTexture("Images/dusty.rgb"); global.obstacles.push_back(s1); } b = new OSGBoxTex(5,1,2); b->setTexture(0,TextureDescr("Images/dusty.rgb",1,1)); b->setTexture(1,TextureDescr("Images/tire_full.rgb",3,1)); b->setTexture(2,TextureDescr("Images/whitemetal_farbig_small.rgb",1,1)); b->setTexture(3,TextureDescr("Images/wall.rgb",1,1)); b->setTexture(4,TextureDescr("Images/really_white.rgb",1,1)); b->setTexture(5,TextureDescr("Images/light_chess.rgb",-1,-1)); b->init(osgHandle.changeColor(Color(1,1,0))); b->setMatrix(osg::Matrix::translate(0,-2,2)); b2 = new OSGBox(5,1,2); b2->setTexture(TextureDescr("Images/light_chess.rgb",1,1)); b2->init(osgHandle); b2->setMatrix(osg::Matrix::translate(7,0,2)); }
// 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 ); } }
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) { setCameraHomePos(Pos(-6.32561, 5.12705, 3.17278), Pos(-130.771, -17.7744, 0)); global.odeConfig.setParam("noise", 0.05); global.odeConfig.setParam("controlinterval", 2); global.odeConfig.setParam("cameraspeed", 250); global.odeConfig.setParam("gravity", -6); setParam("UseQMPThread", false); // use Playground as boundary: AbstractGround* playground = new Playground(odeHandle, osgHandle, osg::Vec3(8, 0.2, 1), 1); // // playground->setColor(Color(0,0,0,0.8)); playground->setGroundColor(Color(2,2,2,1)); playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren global.obstacles.push_back(playground); Boxpile* boxpile = new Boxpile(odeHandle, osgHandle); boxpile->setColor("wall"); boxpile->setPose(ROTM(M_PI/5.0,0,0,1)*TRANSM(0, 0,0.2)); global.obstacles.push_back(boxpile); // global.obstacles.push_back(playground); // double diam = .90; // OctaPlayground* playground3 = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(/*Diameter*/4.0*diam, 5,/*Height*/ .3), 12, // false); // // playground3->setColor(Color(.0,0.2,1.0,1)); // playground3->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren // global.obstacles.push_back(playground3); controller=0; // addParameter("gamma_s",&teacher); global.configs.push_back(this); for(int i=0; i< bars; i++) { PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.,0.,0.)), osg::Vec3(1,10,0.3+i*.1),10); b->setPosition(osg::Vec3(10+i*7,0,0)); global.obstacles.push_back(b); } /******* H E X A P O D *********/ int numhexapods = 1; for ( int ii = 0; ii< numhexapods; ii++) { HexapodConf myHexapodConf = Hexapod::getDefaultConf(); myHexapodConf.coxaPower = 1.5; myHexapodConf.tebiaPower = 0.8; myHexapodConf.coxaJointLimitV = .9; // M_PI/8; // angle range for vertical dir. of legs myHexapodConf.coxaJointLimitH = 1.3; //M_PI/4; myHexapodConf.tebiaJointLimit = 1.8; // M_PI/4; // +- 45 degree myHexapodConf.percentageBodyMass = .5; myHexapodConf.useBigBox = false; myHexapodConf.tarsus = true; myHexapodConf.numTarsusSections = 1; myHexapodConf.useTarsusJoints = true; // myHexapodConf.numTarsusSections = 2; OdeHandle rodeHandle = odeHandle; rodeHandle.substance.toRubber(20); vehicle = new Hexapod(rodeHandle, osgHandle.changeColor("Green"), myHexapodConf, "Hexapod_" + std::itos(teacher*10000)); // on the top vehicle->place(osg::Matrix::rotate(M_PI*1,1,0,0)*osg::Matrix::translate(0,0,1.5+ 2*ii)); // normal position // vehicle->place(osg::Matrix::translate(0,0,0)); // InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); // cc.cInit=1.0; // cc.useS=false; // cc.someInternalParams=true; // InvertMotorNStep *semox = new InvertMotorNStep(cc); // semox->setParam("steps", 1); // semox->setParam("continuity", 0.005); // semox->setParam("teacher", teacher); SoMLConf sc = SoML::getDefaultConf(); sc.useHiddenContr=true; sc.useHiddenModel=false; sc.someInternalParams=false; sc.useS=false; SoML* soml = new SoML(sc); soml->setParam("epsC",0.105); soml->setParam("epsA",0.05); Sox* sox = new Sox(1.2, false); sox->setParam("epsC",0.105); sox->setParam("epsA",0.05); sox->setParam("Logarithmic",1); SeMoXConf cc = SeMoX::getDefaultConf(); //cc.cInit=.95; cc.cInit=.99; cc.modelExt=false; cc.someInternalParams=true; SeMoX* semox = new SeMoX(cc); DerInfConf dc = DerInf::getDefaultConf(); dc.cInit=.599; dc.someInternalParams=false; AbstractController* derinf = new DerInf(dc); derinf->setParam("epsC",0.1); derinf->setParam("epsA",0.05); AbstractController* sine = 0; if(useSineController) { // sine = new SineController(~0, SineController::Sine); sine = new SineController(~0, SineController::Impulse); // // // // // motorpower 20 sine->setParam("period", 30); sine->setParam("phaseshift", 0.5); sine->setParam("amplitude", 0.5); } 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); if(useSineController) { controller = sine; } else { // controller = semox; controller = sox; // controller = soml; // controller = derinf; } One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1)); // the feedbackwiring feeds here 75% of the motor actions as inputs and only 25% of real inputs // 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->init(controller, vehicle, wiring); // add an operator to keep robot from falling over agent->addOperator(new LimitOrientationOperator(Axis(0,0,1), Axis(0,0,1), M_PI*0.5, 30)); if(track) { TrackRobotConf c = TrackRobot::getDefaultConf(); c.displayTrace = true; c.scene = ""; c.interval = 1; c.trackSpeed = false; c.displayTraceThickness = 0.01; agent->setTrackOptions(TrackRobot(c)); } if(tracksegm) { TrackRobotConf c = TrackRobot::getDefaultConf(); Color col = osgHandle.getColor("joint"); c.displayTrace = true; c.scene = "segm"; c.interval = 1; c.displayTraceThickness = 0.02; col.alpha() = 0.5; agent->addTracking(5, TrackRobot(c), col); agent->addTracking(8, TrackRobot(c), col); } global.agents.push_back(agent); global.configs.push_back(agent); //agent->startMotorBabblingMode(5000); // this->getHUDSM()->setColor(Color(1.0,1.0,0)); // this->getHUDSM()->setFontsize(18); // this->getHUDSM()->addMeasure(teacher,"gamma_s",ID,1); } }
// 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 start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(-16.4509, 15.6927, 12.5683), Pos(-133.688, -26.4496, 0)); setCameraMode(Static); global.odeConfig.setParam("noise",0.05); global.odeConfig.setParam("realtimefactor",2); // global.odeConfig.setParam("gravity", 0); Playground* playground; if(env==ElipticBasin){ playground = new Playground(odeHandle, osgHandle, osg::Vec3(20, 0.2, height+1.f), 2); }else{ playground = new Playground(odeHandle, osgHandle, osg::Vec3(20, 0.2, height+0.3f), 1); } playground->setColor(Color(.1,0.7,.1)); playground->setTexture(""); playground->setPosition(osg::Vec3(0,0,0.01f)); // playground positionieren und generieren global.obstacles.push_back(playground); int numpassive=0; switch(env){ case ThreeBump: { TerrainGround* terrainground = new TerrainGround(odeHandle, osgHandle.changeColor(Color(1.0f,1.0,1.0)), "terrains/macrospheresLMH_64.ppm","terrains/macrospheresTex_256.ppm", 20, 20, height, OSGHeightField::LowMidHigh); terrainground->setPose(osg::Matrix::translate(0, 0, 0.1)); global.obstacles.push_back(terrainground); addRobot(odeHandle, osgHandle, global, 0); addRobot(odeHandle, osgHandle, global, 1); numpassive=4; } break; case SingleBasin: // at Radius 3.92 height difference of 0.5 and at 6.2 height difference of 1 // ./start -single -track -f 2 -r 1297536669 case ElipticBasin: // ./start -eliptic -f 5 -track -r 1297628680 { TerrainGround* terrainground = new TerrainGround(odeHandle, osgHandle.changeColor(Color(1.0f,1.0f,1.0f)), // "terrains/dip128_flat.ppm","terrains/dip128_flat_texture.ppm", "terrains/dip128.ppm","terrains/dip128_texture.ppm", 20, env == SingleBasin ? 20 : 40, height, OSGHeightField::Red); terrainground->setPose(osg::Matrix::translate(0, 0, 0.1)); global.obstacles.push_back(terrainground); addRobot(odeHandle, osgHandle, global, 3); } break; } // 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 // - set a texture for the sphere // - add sphere to list of obstacles for (int i=0; i< numpassive; i+=1){ PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.5,0.1); s1->setPosition(osg::Vec3(-8+2*i,-2,height+0.5)); s1->setTexture("Images/dusty.rgb"); global.obstacles.push_back(s1); } }
void playground_with_ramps_and_agents(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global){ Playground* playground1 = new Playground(odeHandle, osgHandle, osg::Vec3(20.5, 0.2, 2.0),0.05, true); // playground1->setColor(Color(0,0.8,0,0.2)); // playground1->setTexture("Images/really_white.rgb"); playground1->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren // global.obstacles.push_back(playground1); playground1->setColor(Color(1,0,0)); // Playground* playground2 = new Playground(odeHandle, osgHandle, osg::Vec3(20.5, 0.2, 2.0),0.05); // playground2->setColor(Color(0,0.8,0,0.2)); // playground2->setTexture("Images/really_white.rgb"); // playground2->setPosition(osg::Vec3(0,1.4,0)); // playground positionieren und generieren // global.obstacles.push_back(playground2); Box* box = new Box(3, 2.6 ,0.1); box->init(odeHandle, 0, osgHandle, Primitive::Geom | Primitive::Draw); box->setPose(osg::Matrix::rotate(-M_PI/6,osg::Vec3(0,1,0)) * osg::Matrix::translate(9.0,0.7,0.4)); box->update(); // box = new Box(3, 2.6 ,0.1); // box->init(odeHandle, 0, osgHandle, Primitive::Geom | Primitive::Draw); // box->setPose(osg::Matrix::rotate(M_PI/6,osg::Vec3(0,1,0)) * osg::Matrix::translate(-9.0,0.7,0.4)); // box->update(); Box* b = new Box(1,2,3); b->init(odeHandle, 0, osgHandle.changeColor(Color(0,1,1)), Primitive::Geom | Primitive::Draw); b->setPose(osg::Matrix::translate(0.0f,0.0f,-0.05f)); b->setTexture("Images/greenground.rgb",true,true); delete b; b = new Box(1,2,3); b->init(odeHandle, 0, osgHandle.changeColor(Color(0,1,1)), Primitive::Geom | Primitive::Draw); b->setPose(osg::Matrix::translate(0.0f,0.0f,-0.05f)); b->setTexture("Images/greenground.rgb",true,true); // ForcedSphereConf conf; // ForcedSphere* sphere1; // ForcedSphere* sphere2; // AbstractWiring* wiring; // OdeAgent* agent; // //////// AGENT 1 // conf = ForcedSphere::getDefaultConf(); // // conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::OnlyZAxis)); // RelativePositionSensor* s = new RelativePositionSensor(4,1,RelativePositionSensor::X); // s->setReference(playground1->getMainPrimitive()); // conf.addSensor(s); // conf.radius = 0.5; // conf.drivenDimensions = ForcedSphere::X; // sphere1 = new ForcedSphere ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)), // conf, "Agent1"); // ((OdeRobot*)sphere1)->place ( Pos( 0 , 0 , 0.5 )); // // controller = new InvertMotorSpace(50); // controller = new InvertMotorNStep(); // controller->setParam("epsA",0.005); // model learning rate // controller->setParam("epsC",0.02); // controller learning rate // // controller->setParam("rootE",3); // model and contoller learn with square rooted error // controller->setParam("factorB",0); // controller->setParam("noiseB",0); // controller->setParam("adaptrate",0.0); // controller->setParam("noiseY",0.0); // global.configs.push_back ( controller ); // //controller = new SineController(); // //global.configs.push_back ( controller ); // // wiring = new One2OneWiring ( new ColorUniformNoise() ); // DerivativeWiringConf wc = DerivativeWiring::getDefaultConf(); // wc.useId=false; // wc.useSecondD=true; // wc.eps=1; // wc.derivativeScale=100; // wiring = new DerivativeWiring ( wc, new ColorUniformNoise()); // agent = new OdeAgent ( plotoptions ); // agent->init ( controller , sphere1 , wiring ); // global.agents.push_back ( agent ); //////// AGENT 2 // conf = ForcedSphere::getDefaultConf(); // // conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::OnlyZAxis)); // s = new RelativePositionSensor(4,1,RelativePositionSensor::X); // s->setReference(playground2->getMainPrimitive()); // conf.addSensor(s); // conf.radius = 0.5; // conf.drivenDimensions = ForcedSphere::X; // sphere2 = new ForcedSphere ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)), // conf, "Agent2"); // ((OdeRobot*)sphere2)->place ( Pos( 0 , 1.4 , 0.5 )); // controller = new InvertMotorSpace(50); // 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 ); // // wiring = new One2OneWiring ( new ColorUniformNoise() ); // wiring = new DerivativeWiring ( wc, new ColorUniformNoise()); // agent = new OdeAgent (std::list<PlotOption>()); // agent->init ( controller , sphere2 , wiring ); // global.agents.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(46.8304, -1.4434, 19.3963), Pos(88.9764, -26.2964, 0)); OdeHandle elast = odeHandle; elast.substance.toMetal(0.8); // initialization // - set global noise to 0.1 global.odeConfig.setParam("noise",0.01); // global.odeConfig.setParam("gravity", 0); // no gravity Playground* playground1 = new Playground(elast, osgHandle, osg::Vec3(30.0, 0.2, 1.0), 1, true); playground1->setColor(Color(0.88f,0.4f,0.26f,0.2f)); playground1->setTexture("Images/really_white.rgb"); playground1->setGroundColor(Color(200/255.0,174.0/255.0,21.0/255.0)); playground1->setGroundTexture("Images/really_white.rgb"); playground1->setPosition(osg::Vec3(0,0,0.0)); // playground positionieren und generieren global.obstacles.push_back(playground1); // Agents numrobots = 2; ForcedSphereConf conf; ForcedSphere** spheres = new ForcedSphere*[numrobots]; Sensor** sensors = new Sensor*[numrobots]; controllers = new AbstractController*[numrobots]; AbstractWiring* wiring; OdeAgent* agent; for(int i=0; i<numrobots; i++){ conf = ForcedSphere::getDefaultConf(); // conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::OnlyZAxis)); // RelativePositionSensor* s = new RelativePositionSensor(4,1,Sensor::X | Sensor::Y); // s->setReference(playground1->getMainPrimitive()); SpeedSensor* s = new SpeedSensor(5,SpeedSensor::Translational, Sensor::X | Sensor::Y); conf.addSensor(s); conf.addSensor(new SoundSensor()); conf.addMotor(new Speaker(-1)); conf.maxForce = 10; conf.speedDriven = true; conf.maxSpeed = 5; conf.radius = 0.5; conf.cylinderBody=true; ForcedSphere* sphere1; sphere1 = new ForcedSphere ( elast, osgHandle.changeColor(Color(i==0,i==1,i==2)), conf, "Agent1"); ((OdeRobot*)sphere1)->place ( Pos( 2*i , 0 , 0.1 )); InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); cc.useSD=true; controller = new InvertMotorNStep(cc); controller->setParam("epsA",0.03); // model learning rate controller->setParam("epsC",0.01); // controller learning rate controller->setParam("rootE",3); // model and contoller learn with square rooted error controller->setParam("factorB",0.2); controller->setParam("noiseB",0.01); controller->setParam("s4avg",10); controller->setParam("adaptrate",0.000); controller->setParam("nomupdate",0.001); controller->setParam("noiseY",0.0); // controller = new SineController(); // controller = new InvertNChannelController(10,1.2); // controller->setParam("eps",0.2); global.configs.push_back ( controller ); wiring = new One2OneWiring ( new ColorUniformNoise() ); // DerivativeWiringConf wc = DerivativeWiring::getDefaultConf(); // wc.useId=false; // wc.useSecondD=true; // wc.eps=1; // wc.derivativeScale=100; // wiring = new DerivativeWiring ( wc, new ColorUniformNoise()); agent = new OdeAgent ( i==0 ? plotoptions : std::list<PlotOption>() ); agent->init ( controller , sphere1 , wiring ); global.agents.push_back ( agent ); spheres[i]=sphere1; sensors[i]=s; controllers[i]=controller; } // connect them // let robot 2 actually persive robot 1 // sensors[1]->init(spheres[0]->getMainPrimitive()); myspeaker=0; // myspeaker = new Speaker(1); // myspeaker->init(playground1->getMainPrimitive()); }
/// 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) { setCameraHomePos(Pos(-0.0114359, 6.66848, 0.922832), Pos(178.866, -7.43884, 0)); // 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("cameraspeed", 250); // use Playground as boundary: // playground = new Playground(odeHandle, osgHandle, osg::Vec3(8, 0.2, 1), 1); // // playground->setColor(Color(0,0,0,0.8)); // playground->setGroundColor(Color(2,2,2,1)); // playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren // global.obstacles.push_back(playground); controller=0; addParameter("k",&k); addParameter("gamma_s",&teacher); global.configs.push_back(this); for(int i=0; i< bars; i++){ PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.,0.,0.)), osg::Vec3(1,10,0.3+i*.1),0.0); b->setPosition(osg::Vec3(10+i*7,0,0)); global.obstacles.push_back(b); } double h = 0.; RandGen rgen; rgen.init(2); for(int i=0; i<stairs; i++){ do{ h+=(rgen.rand()-.5)*0.6; // values between (-.25.25) }while(h<0); PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.3,0.3,0.3)), osg::Vec3(1,10,h),0.0); b->setPosition(osg::Vec3(5+i,0,0)); global.obstacles.push_back(b); } /******* 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.showCenter = false; vehicle = new SliderWheelie(odeHandle, osgHandle.changeColor(Color(1,222/255.0,0)), mySliderWheelieConf, "sliderWheelie_" + std::itos(teacher*10000)); vehicle->place(Pos(0,0,.1)); global.configs.push_back(vehicle); // InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); // cc.cInit=1.0; // cc.useS=false; // cc.someInternalParams=true; // InvertMotorNStep *semox = new InvertMotorNStep(cc); // semox->setParam("steps", 1); // semox->setParam("continuity", 0.005); // semox->setParam("teacher", teacher); SeMoXConf cc = SeMoX::getDefaultConf(); //cc.cInit=.95; cc.cInit=.5; cc.modelExt=false; // cc.someInternalParams=true; cc.someInternalParams=false; SeMoX* semox = new SeMoX(cc); // AbstractController* controller = new SineController(~0, SineController::Sine); // local variable! // // // motorpower 20 // controller->setParam("period", 300); // controller->setParam("phaseshift", 0.3); 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); // AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1)); //AbstractWiring* wiring = new MotorAsSensors(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->init(controller, vehicle, wiring); if(track) agent->setTrackOptions(TrackRobot(true,false,false, false, change != 0 ? std::itos(change).c_str() : "uni", 50)); global.agents.push_back(agent); global.configs.push_back(controller); this->getHUDSM()->setColor(Color(1.0,1.0,0)); this->getHUDSM()->setFontsize(18); this->getHUDSM()->addMeasure(teacher,"gamma_s",ID,1); this->getHUDSM()->addMeasure(k_double,"k",ID,1); setCMC(k); blink=0; }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos(Pos(-5.44372, 7.37141, 3.31768), Pos(-142.211, -21.1623, 0)); // 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("gravity", 0); // use Playground as boundary: // playground = new Playground(odeHandle, osgHandle, osg::Vec3(8, 0.2, 1), 1); // // playground->setColor(Color(0,0,0,0.8)); // playground->setGroundColor(Color(2,2,2,1)); // playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren // global.obstacles.push_back(playground); 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, "sliderWheelie_" + std::itos(teacher*10000)); vehicle->place(Pos(0,0,0.1)); global.configs.push_back(vehicle); // create pointer to controller // push controller in global list of configurables InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); cc.cInit=1.0; cc.useS=false; cc.someInternalParams=true; controller = new InvertMotorNStep(cc); // AbstractController* controller = new SineController(~0, SineController::Sine); // local variable! // // // motorpower 20 // controller->setParam("period", 300); // controller->setParam("phaseshift", 0.3); controller->setParam("adaptrate", 0.000); if(useSym) { controller->setParam("epsC", 0.1); controller->setParam("epsA", 0.1); } else { controller->setParam("epsC", 0.1); controller->setParam("epsA", 0.1); } controller->setParam("rootE", 3); controller->setParam("steps", 1); controller->setParam("s4avg", 1); controller->setParam("continuity", 0.005); controller->setParam("teacher", teacher); // One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1)); AbstractWiring* wiring = new FeedbackWiring(new ColorUniformNoise(0.1), FeedbackWiring::Motor, 0.75); //plotoptions.push_back(PlotOption(GuiLogger,Robot,5)); OdeAgent* agent = new OdeAgent(global); agent->init(controller, vehicle, wiring); if(track) agent->setTrackOptions(TrackRobot(true,false,false, false, change < 50 ? std::itos(change).c_str() : "uni", 50)); global.agents.push_back(agent); global.configs.push_back(controller); }
// 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(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 }
// 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 ); } }
// 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); // } }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { D=0; setCameraHomePos(Pos(-1.55424, 10.0881, 1.58559), Pos(-170.16, -7.29053, 0)); // 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("gravity", 0); for(int i=0; i< 2; i++){ PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.,0.,0.)), osg::Vec3(1,10,0.3+i*.1),10); b->setPosition(osg::Vec3(30+i*7,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, "sliderWheelie_" + std::itos(teacher*10000)); vehicle->place(Pos(0,0,2)); global.configs.push_back(vehicle); // InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); // cc.cInit=1.0; // cc.useS=false; // cc.someInternalParams=true; // InvertMotorNStep *semox = new InvertMotorNStep(cc); // semox->setParam("steps", 1); // semox->setParam("continuity", 0.005); // semox->setParam("teacher", teacher); SeMoXConf cc = SeMoX::getDefaultConf(); cc.cInit=1.2; cc.modelExt=false; cc.someInternalParams=true; SeMoX* semox = new SeMoX(cc); // AbstractController* controller = new SineController(~0, SineController::Sine); // local variable! // // // motorpower 20 // controller->setParam("period", 300); // controller->setParam("phaseshift", 0.3); 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, change < 50 ? std::itos(change).c_str() : "uni", 50)); global.agents.push_back(agent); global.configs.push_back(controller); this->getHUDSM()->setColor(Color(1.0,1.0,0)); this->getHUDSM()->setFontsize(18); this->getHUDSM()->addMeasure(teacher,"Gamma_s",ID,1); this->getHUDSM()->addMeasure(D,"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); // } }
// starting function (executed once at the beginning of the simulation loop) void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { // first: position(x,y,z) second: view(alpha,beta,gamma) // gamma=0; // alpha == horizontal angle // beta == vertical angle setCameraHomePos(Pos(-0.18, 20.36, 13.63), Pos(-179.93, -34.37, 0)); // initialization // - set noise to 0.1 global.odeConfig.noise=0.1; global.odeConfig.setParam("controlinterval", 10); // 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) bool labyrint=false; bool squarecorridor=false; bool normalplayground=true; bool boxes = true; if(normalplayground){ // Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(12, 0.2, 0.5)); Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(17, 0.2, 1.0)); playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren // register playground in obstacles list global.obstacles.push_back(playground); } if(squarecorridor){ Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(15, 0.2, 1.2 ), 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.1)); playground->setTexture(""); global.obstacles.push_back(playground); // // inner playground playground = new Playground(odeHandle, osgHandle,osg::Vec3(10, 0.2, 1.2), 1, false); playground->setColor(Color(255/255.0,200/255.0,0/255.0, 0.1)); playground->setPosition(osg::Vec3(0,0,0.1)); playground->setTexture(""); global.obstacles.push_back(playground); } if(labyrint){ double radius=7.5; Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(radius*2+1, 0.2, 5 ), 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.1)); playground->setTexture(""); global.obstacles.push_back(playground); int obstanz=30; OsgHandle rotOsgHandle = osgHandle.changeColor(Color(255/255.0, 47/255.0,0/255.0)); OsgHandle gruenOsgHandle = osgHandle.changeColor(Color(0,1,0)); for(int i=0; i<obstanz; i++){ PassiveBox* s = new PassiveBox(odeHandle, (i%2)==0 ? rotOsgHandle : gruenOsgHandle, osg::Vec3(random_minusone_to_one(0)+1.2, random_minusone_to_one(0)+1.2 ,1),5); s->setPose(osg::Matrix::translate(radius/(obstanz+10)*(i+10),0,i) * osg::Matrix::rotate(2*M_PI/obstanz*i,0,0,1)); global.obstacles.push_back(s); } } if(boxes) { for (int i=0; i<= 2; i+=2){ PassiveBox* s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4); s1->setTexture("Images/dusty.rgb"); s1->setPosition(osg::Vec3(-5+i*5,0,0)); global.obstacles.push_back(s1); Joint* fixator; Primitive* p = s1->getMainPrimitive(); fixator = new FixedJoint(p, global.environment); fixator->init(odeHandle, osgHandle); s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4); s1->setTexture("Images/dusty.rgb"); s1->setPosition(osg::Vec3(0,-5+i*5,0)); global.obstacles.push_back(s1); p = s1->getMainPrimitive(); fixator = new FixedJoint(p, global.environment); fixator->init(odeHandle, osgHandle); s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4); s1->setTexture("Images/dusty.rgb"); s1->setPosition(osg::Vec3(-3.5+i*3.5,-3.5+i*3.5,0)); global.obstacles.push_back(s1); p = s1->getMainPrimitive(); fixator = new FixedJoint(p, global.environment); fixator->init(odeHandle, osgHandle); s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4); s1->setTexture("Images/dusty.rgb"); s1->setPosition(osg::Vec3(-3.5+i*3.5,3.5-i*3.5,0)); global.obstacles.push_back(s1); p = s1->getMainPrimitive(); fixator = new FixedJoint(p, global.environment); fixator->init(odeHandle, osgHandle); } } // 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 // - set a texture for the sphere // - add sphere to list of obstacles for (int i=0; i < 0/*2*/; i++){ PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.5); s1->setPosition(osg::Vec3(-4.5+i*4.5,0,0)); s1->setTexture("Images/dusty.rgb"); global.obstacles.push_back(s1); } // use Nimm2 vehicle as robot: // - get default configuration for nimm2 // - activate bumpers, cigar mode and infrared front sensors of the nimm2 robot // - create pointer to nimm2 (with odeHandle, osg Handle and configuration) // - place robot Nimm2Conf c = Nimm2::getDefaultConf(); c.bumper = false;//true; c.cigarMode = false;//true; c.irFront = true; c.irBack = true; //c.irSide = true; c.irRange = 1.2;//2;//3; c.force=2; c.speed=8; OdeRobot* vehicle = new Nimm2(odeHandle, osgHandle, c, "Nimm2"); vehicle->place(Pos(0,0,0.5)); // vehicle->place(Pos(0,6.25,0)); // use Nimm4 vehicle as robot: // - create pointer to nimm4 (with odeHandle and osg Handle and possible other settings, see nimm4.h) // - place robot //OdeRobot* vehicle = new Nimm4(odeHandle, osgHandle, "Nimm4"); //vehicle->place(Pos(0,1,0)); // create pointer to controller // push controller in global list of configurables AbstractController *controller = new LayeredController(10); controller->setParam("eps",0.1); controller->setParam("factor_a",0.1); controller->setParam("eps_hebb",0.03); global.configs.push_back(controller); // create pointer to one2onewiring One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1)); // 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, vehicle, wiring); agent->setTrackOptions(TrackRobot(true, false, false, false, "hebbh" ,1)); global.agents.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 start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) { setCameraHomePos ( Pos ( 0, 0, 7), Pos ( -0.0828247 , -89.9146 , 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(11, 0.2, 1), 12); playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren global.obstacles.push_back(playground); //**************** ComponentConf cConf = Component::getDefaultConf (); cConf.max_force = 2; cConf.speed = 10; //adding the controller for the component-connections InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf(); cc.cInit=1.2; AbstractController* controller; DerivativeWiringConf c = DerivativeWiring::getDefaultConf (); DerivativeWiring* wiring; OdeAgent* agent; body = new SimpleComponent ( odeHandle , osgHandle , cConf ); wheel[0] = new SimpleComponent ( odeHandle , osgHandle , cConf ); wheel[1] = new SimpleComponent ( odeHandle , osgHandle , cConf ); wheel[2] = new SimpleComponent ( odeHandle , osgHandle , cConf ); wheel[3] = new SimpleComponent ( odeHandle , osgHandle , cConf ); body->setSimplePrimitive ( new Capsule ( 0.1 , 1 ) ); body->getMainPrimitive ()->init ( odeHandle , 0.5 , osgHandle.changeColor ( Color ( 2, 156/255.0, 0, 1.0f ) ) ); body->getMainPrimitive()->getOSGPrimitive()->setTexture("Images/wood.rgb"); for ( int n = 0; n < 4; n++ ) { wheel[n]->setSimplePrimitive ( (Primitive*) new Sphere ( 0.3 ) ); wheel[n]->getMainPrimitive ()->init ( odeHandle , 0.2 , osgHandle.changeColor ( Color ( 0.8,0.8,0.8 )) ); wheel[n]->getMainPrimitive()->getOSGPrimitive()->setTexture("Images/wood.rgb"); } body->place ( osg::Matrix::rotate ( M_PI/2 , osg::Vec3 (1 , 0 , 0 ))*osg::Matrix::translate ( 0 , 0 , 0.5 ) ); wheel[0]->place ( Pos ( -0.45 , -0.5 , 0.5 ) ); wheel[1]->place ( Pos ( 0.45 , -0.5 , 0.5 ) ); wheel[2]->place ( Pos ( -0.45 , 0.5 , 0.5 ) ); wheel[3]->place ( Pos ( 0.45 , 0.5 , 0.5 ) ); for ( int n = 0; n < 4; n++ ) { Axis axis = Axis ( ( wheel[n<2?0:2]->getPosition () - wheel[n<2?1:3]->getPosition ()).toArray() ); HingeJoint* j1 = new HingeJoint ( body->getMainPrimitive () , wheel[n]->getMainPrimitive () , osg::Vec3 (0,0.5*( n<2?-1:1),0.5) , axis ); j1->init ( odeHandle , osgHandle , true , 1 ); body->addSubcomponent ( wheel[n] , j1 , false ); } //controller controller = new InvertMotorNStep ( cc ); controller->setParam ("adaptrate", 0.0); controller->setParam ("epsC", 0.1); controller->setParam ("epsA", 0.01); controller->setParam ("rootE", 3); controller->setParam ("steps", 2); controller->setParam ("s4avg", 2); controller->setParam ("factorB",0); // controller = new SineController ( 18 ); //wiring wiring = new DerivativeWiring ( c , new ColorUniformNoise () ); //agent agent = new OdeAgent ( plotoptions ); agent->init ( controller , body , wiring ); global.agents.push_back ( agent ); global.configs.push_back ( controller ); }