コード例 #1
0
  // starting function (executed once at the beginning of the simulation loop)
  void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
  {
    setCameraHomePos(Pos(-26.6849, 17.3789, 16.7798),  Pos(-120.46, -24.7068, 0));
    setCameraMode(Static);

    global.odeConfig.setParam("noise",0.0);
    global.odeConfig.setParam("realtimefactor",1);
    global.odeConfig.setParam("gravity",0);

    Playground* playground = new Playground(odeHandle, osgHandle,
                                            osg::Vec3(20 , 0.2, .5));
    playground->setPosition(osg::Vec3(0,0,0));
    global.obstacles.push_back(playground);


    b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1, 1, 1));
    b1->setPosition(Vec3(0,0,1.1));
    global.obstacles.push_back(b1);
    rs = new RaySensor(0.1,5, RaySensor::drawAll);
    rs->setInitData(odeHandle, osgHandle, ROTM(M_PI,0,1,0) * TRANSM(0,0,-.5));
    rs->init(b1->getMainPrimitive());

    b2 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1, 1, 1));
    b2->setPosition(Vec3(2,0,1.1));
    global.obstacles.push_back(b2);
    ir = new IRSensor(1.0, 0.1,2, RaySensor::drawAll);
    ir->setInitData(odeHandle, osgHandle, ROTM(M_PI,0,1,0) * TRANSM(0,0,-.5));
    ir->init(b2->getMainPrimitive());

  }
コード例 #2
0
  void addCallback(GlobalData& globalData, bool draw, bool pause, bool control) {
    if(globalData.sim_step < 4) return;
    rs->update();
    if(control){
      rs->sense(globalData);
    }
    if(rs->getList().front()!=1.0)
      cerr << "ERROR Ray: " << rs->getList().front() << "!= 1.0" <<endl;
    if(control){ // second call in the same timestep
      rs->sense(globalData);
      if(rs->getList().front()!=1.0)
      cerr << "ERROR Ray: " << rs->getList().front() << "!= 1.0 (second call)" <<endl;
    }

    ir->update();
    if(control){
      ir->sense(globalData);
    }
    double p_last = sin(0.5*(globalData.time-(globalData.odeConfig.simStepSize*globalData.odeConfig.controlInterval)));
    double v = ir->getList().front();
    double s = (2-(p_last+1))/2;
    if(fabs(v - s) > 10e-4){
      cerr << "ERROR IR:  " << v << "!= " << s <<  endl;
    }

    double p = sin(0.5*globalData.time);
    b2->setPosition(Vec3(2,0,p+1.1));
  }
コード例 #3
0
  // 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


  }
コード例 #4
0
  // 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);
//     }



  }
コード例 #5
0
ファイル: main.cpp プロジェクト: pmanoonpong/gorobots_edu
	// 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);
	}
コード例 #6
0
ファイル: main.cpp プロジェクト: sinusoidplus/lpzrobots
    // 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);

        }
    }
コード例 #7
0
ファイル: main.cpp プロジェクト: amr1985/playful
  /// 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

  }
コード例 #8
0
  // 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;


  }
コード例 #9
0
ファイル: main.cpp プロジェクト: amr1985/playful
  // 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);
//     }

  }