C++ OdeAgent::init方法代码示例

  /// 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(8.85341, -11.2614, 3.32813),  Pos(33.5111, -7.0144, 0));

    // initialization
    global.odeConfig.noise=0.1;

//     Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(7.0, 0.2, 1.5));
//     playground->setPosition(Pos(0,0,0)); // playground positionieren und generieren
//     global.obstacles.push_back(playground);

    Arm2SegmConf arm_conf=Arm2Segm::getDefaultConf();
    Arm2Segm* vehicle = new Arm2Segm(odeHandle, osgHandle, arm_conf, "arm");
    ((OdeRobot* )vehicle)->place(Position(0,0,0));
    //AbstractController *controller = new InvertNChannelController(10);
    AbstractController *controller = new SineController();
    global.configs.push_back(vehicle);

    One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
    OdeAgent* agent = new OdeAgent(global);
    agent->init(controller, vehicle, wiring);
    global.agents.push_back(agent);

    global.configs.push_back(controller);

  }

  /**
   * restart() is called at the second and all following starts of the cylce
   * The end of a cycle is determined by (simulation_time_reached==true)
   * @param the odeHandle
   * @param the osgHandle
   * @param globalData
   * @return if the simulation should be restarted; this is false by default
   */
  virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
  {
    // for demonstration: just repositionize the robot and restart 10 times
    if (this->currentCycle==10)
      return false; // don't restart, just quit
  //  vehicle->place(Pos(currentCycle,0,0));
    if (agent!=0) {
      OdeAgentList::iterator itr = find(global.agents.begin(),global.agents.end(),agent);
      if (itr!=global.agents.end())
      {
        global.agents.erase(itr);
      }
        delete agent;
        agent = 0;
    }

    Nimm2Conf c = Nimm2::getDefaultConf();
       c.force   = 4;
       c.bumper  = true;
       c.cigarMode  = true;
       // c.irFront = true;
       OdeRobot* vehicle2 = new Nimm2(odeHandle, osgHandle, c, "Nimm2");
       vehicle2->place(Pos(0,6+currentCycle*1.5,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 InvertNChannelController(10);
       AbstractController *controller = new InvertMotorSpace(10);
       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, vehicle2, wiring);
       global.agents.push_back(agent);
    // restart!
    return 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 );

  }

    /// 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)
    {
        // Initial position and orientation of the camera (use 'p' in graphical window to find out)
        setCameraHomePos(Pos(-14,14, 10),  Pos(-135, -24, 0));
        // Some simulation parameters can be set here
        global.odeConfig.setParam("controlinterval", 1);
        global.odeConfig.setParam("gravity", -9.8);

        /** New robot instance */
        // Get the default configuration of the robot
        DifferentialConf conf = Differential::getDefaultConf();
        // Values can be modified locally
        conf.wheelMass = .5;
        // Instantiating the robot
        OdeRobot* robot = new Differential(odeHandle, osgHandle, conf, "Differential robot");
        // Placing the robot in the scene
        ((OdeRobot*)robot)->place(Pos(.0, .0, .2));
        // Instantiatign the controller

        AbstractController* controller = new BasicController("Basic Controller", "$ID$");
        // Create the wiring with color noise
        AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(.1));
        // Create Agent
        OdeAgent* agent = new OdeAgent(global);
        // Agent initialisation
        agent->init(controller, robot, wiring);
        // Adding the agent to the agents list
        global.agents.push_back(agent);
        global.configs.push_back(agent);

        /** Environment and obstacles */
        // New playground
        Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(15., .2, 1.2), 1);
        // Set colours
        playground->setGroundColor(Color(.784, .784, .0));
        playground->setColor(Color(1., .784, .082, .3));
        // Set position
        playground->setPosition(osg::Vec3(.0, .0, .1));
        // Adding playground to obstacles list
        global.obstacles.push_back(playground);

        // Add a new box obstacle (or use 'o' to drop random obstacles)
        //PassiveBox* box = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1., 1., 1.), 2.);
        //box->setPose(osg::Matrix::translate(-.5, 4., .7));
        //global.obstacles.push_back(box);
    }

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

  virtual void addCallback(GlobalData& global, bool draw, bool pause, bool control) {
    if(!control || pause) return;
    if(counter == maxCounter){
      counter = 0;
      if(sphere) {
        // measure final speed
        double dat[3];
        speedsensor->sense(global);
        speedsensor->get(dat,3);
        Matrix s(1,3,dat);
        logfile << s << endl;
        cout << "End:   " << s << endl;
        if(runcounter>=startAngles.size()){
          this->pause=true;
          simulation_time_reached=true;
          return;
        }

        // destroy sphere
        global.agents.pop_back();
        delete sphere;
        delete controller;
        delete wiring;
        delete agent;
      }
      // create new sphere
      sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(0,0.0,2.0)),
                                        conf, "Sphere_test_sat", 0.3);

      // we will accelerate the sphere in positive x direction
      //  we place it the way that all (straight) rolling modes are possible
      double theta = startAngles[runcounter].val(0,0);
      double omega = startAngles[runcounter].val(1,0);
      sphere->place ( osg::Matrix::rotate(omega, 0,0,1)*osg::Matrix::rotate(theta, 1,0,0));

      //      sphere->place ( osg::Matrix::rotate(M_PI/2, 0,1,0));
      //      sphere->place ( osg::Matrix::rotate(M_PI/2, 0,0,1));

      speedsensor->init(sphere->getMainPrimitive());

      //      controller = new SatNetControl(networkfilename);
      controller = new SatNetControl_NoY(networkfilename);

      wiring = new One2OneWiring ( new ColorUniformNoise(0.20) );
      agent = new OdeAgent ( plotoptions );
      agent->init ( controller , sphere , wiring );
      //      agent->setTrackOptions(TrackRobot(true, true, false, true, "ZSens", 50));
      global.agents.push_back ( agent );

      runcounter++;
    } else if(counter > 1 && counter < accelerationTime){
      // speed up sphere
      double power = startAngles[runcounter-1].val(2,0);
      dBodyAddTorque ( sphere->getMainPrimitive()->getBody() , 0, power, 0 );
    }else if(counter == accelerationTime){ // measure start speed
      double dat[3];
      speedsensor->sense(global);
      speedsensor->get(dat,3);
      Matrix s(1,3,dat);
      logfile << s;
      cout << "Run: " << runcounter << "/" << startAngles.size() << endl;
      cout << "Start: " << s <<  endl;

    }

    counter++;
  }

    // starting function (executed once at the beginning of the simulation loop)
    virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
    {

        std::cout << "\n begin restart " << currentCycle << "\n";

        std::cout<<"Current Cycle"<<this->currentCycle<<std::endl;

        // remove agents
        while (global.agents.size() > 0)
        {
            OdeAgent* agent = *global.agents.begin();

            AbstractController* controller = agent->getController();

            OdeRobot* robot = agent->getRobot();
            AbstractWiring* wiring = agent->getWiring();

            global.configs.erase(std::find(global.configs.begin(),
                                           global.configs.end(), controller));

            delete robot;
            delete wiring;

            global.agents.erase(global.agents.begin());

        }
        // clean the playgrounds
        while (global.obstacles.size() > 0)
        {
            std::vector<AbstractObstacle*>::iterator iter =
                global.obstacles.begin();
            delete (*iter);
            global.obstacles.erase(iter);
        }
        boxPrimitives.clear();
        relative_sensor_obst.clear();
        grippables.clear();

        ///////////////Recreate Robot Start//////////////////////////////////////////////////////////////////////////////////////
        /**************************************************************************************************
        ***			Set up Environment
        **************************************************************************************************/
        setup_Playground(global);

        /**************************************************************************************************
        ***			Set up 4 landmark and 1 goal spheres
        **************************************************************************************************/

        generate_spheres(global);

        /**************************************************************************************************
        ***			Set up 3 pushable boxes and add the first one as graspable
        ************************************************************************************************/

        generate_boxes(global);
        grippables.push_back(boxPrimitives[0]);
//		grippables.push_back(boxPrimitives[1]);
//		grippables.push_back(boxPrimitives[2]);

        /**************************************************************************************************
        ***			Set up robot and controller
        **************************************************************************************************/

        //1) Activate IR sensors
        FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();

        ///2) relative sensors
        for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
            fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
        }
        vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);

        /****Initial position of Nimm4******/
        Pos pos(0.0 , 0.0 , 1.0);
        //setting position and orientation
        vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));

        // only set new grippables otherwise keep old controller
        qcontroller->setGrippablesAndVehicle(vehicle,grippables);
        global.configs.push_back(qcontroller);

        // create pointer to one2onewiring
        AbstractWiring*  wiring = new One2OneWiring(new ColorUniformNoise(0.1));

        // create pointer to agent
        OdeAgent* agent = new OdeAgent(global);

        agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
        global.agents.push_back(agent);


        std::cout << "\n end restart " << currentCycle << "\n";
        // restart!

        qcontroller->setReset(false);


        return 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));

        // initialization
        // - set noise to 0.1
        // - register file chess.ppm as a texture called chessTexture (used for the wheels)
        global.odeConfig.noise=0.05;
        //    global.odeConfig.setParam("gravity", 0);
        global.odeConfig.setParam("controlinterval", 5);
        global.odeConfig.setParam("simstepsize", 0.01);

        // use Playground as boundary:
        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);
        /*
             for(int i=0; i<50; i++){
               PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
               s->setPosition(osg::Vec3(-4+(i/10),-4+(i%10),1));
               global.obstacles.push_back(s);
             }
        */
        OdeRobot* nimm2;
        //    AbstractController* contrl;
        AbstractWiring* wiring;
        OdeAgent* agent;

        Nimm2Conf conf = Nimm2::getDefaultConf();
        conf.speed=10;
        conf.force=1.0;
        //conf.bumper=true;
        //conf.cigarMode=true;
        //conf.irFront=true;
        //conf.irBack=true;
        wiring = new One2OneWiring(new ColorUniformNoise(0.1));
        //controller = new InvertMotorNStep();
        controller = new InvertNChannelController(10);
        //          controller = new InvertMotorSpace(10);
        agent = new OdeAgent(global);
        nimm2 = new Nimm2(odeHandle, osgHandle, conf, "Nimm2Yellow");
        nimm2->setColor(Color(1.0,1.0,0));
        global.configs.push_back(controller);
        agent->init(controller, nimm2, wiring);
        /*          controller->setParam("adaptrate", 0.000);
              controller->setParam("nomupdate", 0.0005);
              controller->setParam("epsA", 0.01);
              controller->setParam("epsC", 0.05);
              controller->setParam("rootE", 0);
              controller->setParam("steps", 2);
              controller->setParam("s4avg", 5);
              controller->setParam("s4del", 5);
              controller->setParam("factorB",0);*/
        controller->setParam("eps",0.1);
        controller->setParam("factor_a",0.01);
        nimm2->place(Pos(2.5,1.26,0));
        global.agents.push_back(agent);







    }

// 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(-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)
  {
    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),
//.........这里部分代码省略.........

  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(-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",1);
    global.odeConfig.setParam("noise",0.01);
    global.odeConfig.setParam("realtimefactor",1);
    global.odeConfig.setParam("gravity", -6);
    //    global.odeConfig.setParam("cameraspeed", 250);
    //  int chessTexture = dsRegisterTexture("chess.ppm");

    bool useDerivativeWiring=false;

    // environemnt (type is set in constructor)
    env.numSpheres  = 0;
    env.numBoxes    = 0;
    env.numCapsules = 0;
    env.numSeeSaws  = 0;
    env.roughness   = 1.0;

    // use Playground as boundary:
    if(barriers){
      env.widthground = 4;
      env.distance    = 6;
      env.numgrounds  = 5;
      env.height      = 0.1;
      env.heightincrease =0.1;
    }else {
      env.widthground = 32;
      env.height      = 1.0;
    }

    env.create(odeHandle, osgHandle, global);
    global.configs.push_back(&env);


    // Boxpile* boxpile = new Boxpile(odeHandle, osgHandle, Pos(10,10,0.2),
    //                                100,0, Pos(0.3,0.3,0.05), Pos(0.3,0.3,0.05)
    //                                );
    // boxpile->setColor("wall");
    // boxpile->setPose(ROTM(M_PI/5.0,0,0,1)*TRANSM(0, 0 ,0.2));
    // global.obstacles.push_back(boxpile);



    for (int i=0; i< 1/*2*/; i++){ //Several dogs

      //    VierBeinerConf conf = VierBeiner::getDefaultConf();
    VierBeinerConf conf = VierBeiner::getDefaultConfVelServos();

    conf.dampingFactor = .0;
    conf.powerFactor = 1.3;
    if(air) conf.powerFactor = 0.3;
    if (hippo) conf.powerFactor = 1.5;

    conf.hipJointLimit = M_PI/3.0;
    if ( barriers)  conf.hipJointLimit = M_PI/2.5;

    conf.kneeJointLimit = M_PI/3;
    conf.legNumber = 4; /* for the dog's sake use only even numbers */
    conf.useBigBox = false;
    if(hippo) conf.useBigBox = false;
    conf.drawstupidface=true;
    conf.hippo = hippo;
    //    conf.onlyMainParameters = false; // all parameters

    OdeHandle doghandle = odeHandle;
    doghandle.substance.toRubber(10);
    VierBeiner* dog = new VierBeiner(doghandle, osgHandle,conf, "Dog");
    std::list<Sensor*> sensors;
    sensors.push_back(new SpeedSensor(1,SpeedSensor::TranslationalRel));
    AddSensors2RobotAdapter* robot =
      new AddSensors2RobotAdapter(odeHandle,osgHandle,dog, sensors);

    //dog->place(osg::Matrix::translate(0,0,0.15));
    robot->place(osg::Matrix::translate(0,0,.5 + 4*i));

    if(air || barriers){
      Primitive* trunk = dog->getMainPrimitive();
      fixator = new FixedJoint(trunk, global.environment);
      fixator->init(odeHandle, osgHandle);
    }

    // create pointer to one2onewiring
    //AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));

    double booster = 0.05;
    if(air) booster=0;

    AbstractWiring* wiring;
    if(!useDerivativeWiring){
      wiring = new ForceBoostWiring(new ColorUniformNoise(0.1), booster);
    }else{
      ////////////////
      AbstractWiring* fbw = new ForceBoostWiring(new NoNoise(), booster);
      DerivativeWiringConf dc = DerivativeWiring::getDefaultConf();
//.........这里部分代码省略.........

  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 );


  }

    // starting function (executed once at the beginning of the simulation loop)
    void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
    {

        /**************************************************************************************************
        ***			Camera Position
        **************************************************************************************************/
        //setCameraHomePos(Pos(0, 40, 10),  Pos(0, 0, 0)); // viewing full scene from side
        setCameraHomePos(Pos(0, 5, 5),  Pos(0, 0, 0));
        //setCameraHomePos(Pos(0, 20, 20),  Pos(0, 0, 0));

        /**************************************************************************************************
        ***			Simulation Parameters
        **************************************************************************************************/
        //1) - set noise to 0.1
        global.odeConfig.noise= 0.0;//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);

        /**************************************************************************************************
        ***			Set up Environment
        **************************************************************************************************/

        setup_Playground(global);
        Substance material(5.0,10.0,99.0,1.0);
        this->setGroundSubstance(material);
        /**************************************************************************************************
        ***			Set up 4 landmark and 1 goal spheres
        **************************************************************************************************/

        // not being used atm
        //generate_spheres(global);

        /**************************************************************************************************
        ***			Set up 3 pushable boxes and add the first one as graspable
        ************************************************************************************************/

        generate_boxes(global);
        grippables.push_back(boxPrimitives[0]);
//		grippables.push_back(boxPrimitives[1]);
//		grippables.push_back(boxPrimitives[2]);

        /**************************************************************************************************
        ***			Set up robot and controller
        **************************************************************************************************/

        //1) Activate IR sensors
        FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();

        ///2) relative sensors
        for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
            fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
        }
        vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);

        /****Initial position of Nimm4******/
        Pos pos(0.0 , 0.0 , 1.0);
        //setting position and orientation
        vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));

        qcontroller = new ASLController("1","1");
//		qcontroller = new FSMController("1","1", vehicle, grippables);
        qcontroller->setGrippablesAndVehicle(vehicle,grippables);
        global.configs.push_back(qcontroller);

        // create pointer to one2onewiring
        AbstractWiring*  wiring = new One2OneWiring(new ColorUniformNoise(0.1));

        // create pointer to agent
        OdeAgent* agent = new OdeAgent(global);

        agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
        global.agents.push_back(agent);


    }