Beispiel #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(-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();
      dc.useId=true;
      dc.useFirstD=true;
      // dc.derivativeScale = 1.0;
      AbstractWiring* drw = new DerivativeWiring(dc, new ColorUniformNoise(0.1));
      if(!useDerivativeWiring){ wiring = new WiringSequence(fbw, drw); // TODO booster bei derivative wiring ???
     }
    }

    AbstractController *controller = new Sox(.7, false);
    //AbstractController *controller = new SineController();
    controller->setParam("Logarithmic", 1);
    controller->setParam("epsC",0.05);
    if ( hippo) controller->setParam("epsC",0.1);
    controller->setParam("epsA",0.01);
    controller->setParam("damping",0.0001);

    controller->setParam("period",300);
    controller->setParam("phaseshift",0);

    AbstractController* cont= new GroupController(controller,3); // 3 context sensor

    OdeAgent* agent = new OdeAgent(global);
    agent->init(cont, robot, wiring);
    if(!hippo){
    // add an operator to keep robot from falling over
      agent->addOperator(new LimitOrientationOperator(Axis(0,0,1), Axis(0,0,1),
                                                      M_PI*0.35, 10));
    }

    //agent->setTrackOptions(TrackRobot(true,true,false,true,"bodyheight",20)); // position and speed tracking every 20 steps
    global.agents.push_back(agent);
    global.configs.push_back(agent);

    }// Several dogs end

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


  }