Exemple #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

  }
Exemple #2
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.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);

        }
    }