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

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

  }

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

        }
    }