Пример #1
0
	Primitives OutdoorEnv::getAllPrimitives()	{
		Primitives result;
		if (r_sky->getBool()) {
#if 1
			result.push_back(m_skybox12);
			result.push_back(m_skybox34);
			result.push_back(m_skybox5);
#else
			result.push_back(m_skydome);
#endif
		}

		if (r_water->getBool()) {
			result.push_back(m_oceanMesh);
		}
		return result;
	}
Пример #2
0
    // generate 3 goal boxes return Primitives(vector of Primitive*) of boxes to add to grippables
    void generate_boxes(GlobalData& global)
    {
        double length = 1.38;
        double width = 1.38;
        double height = 0.9;
        Substance material(5.0,10.0,99.0,1.0);
        double mass = 0.1;
        PassiveBox* b1;
        b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b1->setColor(Color(0,1,0));
        b1->setSubstance(material);
        b1->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(-5,0,1.5));
        global.obstacles.push_back(b1);

        PassiveBox* b2;
        b2 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b2->setColor(Color(1,0,0));
        b2->setSubstance(material);
        b2->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(5,0,1.5));
        global.obstacles.push_back(b2);

        PassiveBox* b3;
        b3 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b3->setColor(Color(1,0,0));
        b3->setSubstance(material);
        b3->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(0,-5,1.5));
        global.obstacles.push_back(b3);

        boxPrimitives.push_back(b1->getMainPrimitive());
        boxPrimitives.push_back(b2->getMainPrimitive());
        boxPrimitives.push_back(b3->getMainPrimitive());

        // adding boxes to obstacle vector so it can be connected to sensors
        relative_sensor_obst.push_back(b1);
        relative_sensor_obst.push_back(b2);
        relative_sensor_obst.push_back(b3);

    }
Пример #3
0
    // 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;

    }
Пример #4
0
    // 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);


    }