/*
* Find Playground in image and calc its Extrinsics
*/
bool PlaygroundDetector::detect(const cv::Mat &image, Playground &playground, Contours &candidateContours, aruco::CameraParameters &cameraParameters)
{
  // pg not valid
  playground.id = -1;

  // get all contours from color thres image
  Contours contours;
  findContours(image, contours);
  
  // search for L shaped contours
  filterContours(contours, candidateContours);
  
    if(candidateContours.size() < 4) return false;
  
  // combine exatly 4 L-contours to one rectangle with 4 corners
  std::vector<cv::Point2f> corners; // max length: 4
  extractPlayGroundCorners(candidateContours, corners);
  
    if(corners.size() != 4) return false;
  
  playground.resize(4); 
  std::copy(corners.begin(), corners.end(), playground.begin());
  
  // playground valid
  playground.id = 0;
  
  // calc translation and rotation
  playground.calculateExtrinsics(cameraParameters);
  
  return true;
}
示例#2
0
文件: main.cpp 项目: dowei14/ai4-esn
    // set up Playground
    void setup_Playground(GlobalData& global)
    {

        // inner Platform
        double length_pg = 0.0;
        double width_pg = 9.5;
        double height_pg = 1.0;

        Playground* playground = new Playground(odeHandle, osgHandle.changeColor(Color(0.6,0.0,0.6)),
                                                osg::Vec3(length_pg /*length*/, width_pg /*width*/, height_pg/*height*/), /*factorxy = 1*/1, /*createGround=true*/true /*false*/);

        playground->setPosition(osg::Vec3(0,0,0.0));
        global.obstacles.push_back(playground);

        // DSW: outer_pg = outter platform
        double length_outer_pg = 22.0;
        double width_outer_pg = 5.0;
        double height_outer_pg = 1.0;

        Playground* outer_playground = new Playground(odeHandle, osgHandle.changeColor(Color(0.6,0.0,0.6)),
                osg::Vec3(length_outer_pg /*length*/, width_outer_pg /*width*/, height_outer_pg/*height*/), /*factorxy = 1*/1, /*createGround=true*/true /*false*/);
        outer_playground->setPosition(osg::Vec3(0,0,0.0));
        global.obstacles.push_back(outer_playground);

        // DSW: outer_pg2 = walls
        double length_outer_pg2 = 32.0;
        double width_outer_pg2 = 1.0;
        double height_outer_pg2 = 2.0;

        Playground* outer_playground2 = new Playground(odeHandle, osgHandle.changeColor(Color(0.6,0.0,0.6)),
                osg::Vec3(length_outer_pg2 /*length*/, width_outer_pg2 /*width*/, height_outer_pg2/*height*/), /*factorxy = 1*/1, /*createGround=true*/true /*false*/);
        outer_playground2->setPosition(osg::Vec3(0,0,0.0));
        global.obstacles.push_back(outer_playground2);
    }
示例#3
0
  // starting function (executed once at the beginning of the simulation loop)
  void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
  {
    setCameraHomePos(Pos(-26.6849, 17.3789, 16.7798),  Pos(-120.46, -24.7068, 0));
    setCameraMode(Static);

    global.odeConfig.setParam("noise",0.0);
    global.odeConfig.setParam("realtimefactor",1);
    global.odeConfig.setParam("gravity",0);

    Playground* playground = new Playground(odeHandle, osgHandle,
                                            osg::Vec3(20 , 0.2, .5));
    playground->setPosition(osg::Vec3(0,0,0));
    global.obstacles.push_back(playground);


    b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1, 1, 1));
    b1->setPosition(Vec3(0,0,1.1));
    global.obstacles.push_back(b1);
    rs = new RaySensor(0.1,5, RaySensor::drawAll);
    rs->setInitData(odeHandle, osgHandle, ROTM(M_PI,0,1,0) * TRANSM(0,0,-.5));
    rs->init(b1->getMainPrimitive());

    b2 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1, 1, 1));
    b2->setPosition(Vec3(2,0,1.1));
    global.obstacles.push_back(b2);
    ir = new IRSensor(1.0, 0.1,2, RaySensor::drawAll);
    ir->setInitData(odeHandle, osgHandle, ROTM(M_PI,0,1,0) * TRANSM(0,0,-.5));
    ir->init(b2->getMainPrimitive());

  }
示例#4
0
  // 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.setParam("noise",0.05);
    global.odeConfig.setParam("controlinterval",1);
    global.odeConfig.setParam("gravity:",1); // normally at -9.81
    // initialization

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

    for(int i=0; i<5; i++){
      PassiveSphere* s =
        new PassiveSphere(odeHandle,
                          osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2);
      s->setPosition(Pos(i*0.5-2, i*0.5, 1.0));
      s->setTexture("Images/dusty.rgb");
      global.obstacles.push_back(s);
    }

    OdeAgent* agent;
    AbstractWiring* wiring;
//    OdeRobot* robot;
    AbstractController *controller;

    CaterPillar* myCaterPillar;
    CaterPillarConf myCaterPillarConf = DefaultCaterPillar::getDefaultConf();
    //******* R A U P E  *********/
    myCaterPillarConf.segmNumber=6;
    myCaterPillarConf.jointLimit=M_PI/4;
    myCaterPillarConf.motorPower=0.8;
    myCaterPillarConf.frictionGround=0.04;
    myCaterPillar = new CaterPillar ( odeHandle, osgHandle.changeColor(Color(0.0f,1.0f,0.0f)), myCaterPillarConf, "Raupe1");
    ((OdeRobot*) myCaterPillar)->place(Pos(-5,-5,0.2));

    InvertMotorNStepConf invertnconf = InvertMotorNStep::getDefaultConf();
    invertnconf.cInit=2.0;
    controller = new InvertMotorNStep(invertnconf);
    wiring = new One2OneWiring(new ColorUniformNoise(0.1));
    agent = new OdeAgent( global, plotoptions );
    agent->init(controller, myCaterPillar, wiring);
    global.agents.push_back(agent);
    global.configs.push_back(controller);
    global.configs.push_back(myCaterPillar);
    myCaterPillar->setParam("gamma",/*gb");
      global.obstacles.push_back(s)0.0000*/ 0.0);

  }
示例#5
0
    /// 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);
    }
示例#6
0
/*
* Draw playground, color according to lock / newly detected
*/
void HUD::drawPlayground(const Playground &playground, const Contours &candidateContours, bool highlight, bool pglock)
{
  // show detected single edges 
  if(candidateContours.size() > 0) {
    cv::drawContours(image, candidateContours, -1, cv::Scalar(0,0,255), 1);
  }

  cv::Scalar color = (pglock) ? cv::Scalar(0,255,0) : cv::Scalar(0,134,209);
  int lineSize = (highlight) ? 4 : 1;
  playground.draw(image,color,lineSize);
  
}
示例#7
0
文件: main.cpp 项目: Kazade/Spindash
int main(int argc, char* argv[]) {
    Playground app;
    return app.run();
}
示例#8
0
	// starting function (executed once at the beginning of the simulation loop)
	void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
	{

		// set initial camera position
		setCameraHomePos(Pos(-1.14383, 10.1945, 42.7865),  Pos(179.991, -77.6244, 0));


		// initialization simulation parameters

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


		//5) - set 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)

		// odeHandle and osgHandle are global references
		// vec3 == length, width, height


		double length_pg = 35;//28;//0.0; //45, 32, 22
		double width_pg = 0.5;//0.0;  //0.2
		double height_pg = 0.5;//0.0; //0.5

		Playground* playground = new Playground(odeHandle, osgHandle.changeColor(Color(0.6,0.0,0.6)),
				osg::Vec3(length_pg /*length*/, width_pg /*width*/, height_pg/*height*/), /*factorxy = 1*/1, /*createGround=true*/true /*false*/);
		playground->setPosition(osg::Vec3(4,0,0.0)); // playground positionieren und generieren
		// register playground in obstacles list
		global.obstacles.push_back(playground);

//    Playground* playground =
//      new Playground(odeHandle, osgHandle.changeColor(Color(0.88f,0.4f,0.26f,0.2f)),osg::Vec3(18, 0.2, 2.0));
//    playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
//    Substance substance;
//    substance.toRubber(40);
//    playground->setGroundSubstance(substance);
//    global.obstacles.push_back(playground);

//    for(int i=0; i<0; i++)
//    {
//      PassiveSphere* s =
//        new PassiveSphere(odeHandle,
//                          osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2);
//      s->setTexture("Images/dusty.rgb");
//      s->setPosition(Pos(i*0.5-2, i*0.5, 1.0));
//      global.obstacles.push_back(s);
//    }

		if(!ico_learning_task)
		{

		  for (int j=0;j<4;j++)
		  {
		    for(int i=0; i<4; i++)
		    {
		      PassiveBox* b =
		          new PassiveBox(odeHandle,
		              osgHandle.changeColor(Color(1.0f,0.2f,0.2f,0.5f)), osg::Vec3(1.5+i*0.01,1.5+i*0.01,1.5+i*0.01),40.0);
		      b->setTexture("Images/light_chess.rgb");
		      b->setPosition(Pos(i*4-5, -5+j*4, 1.0));
		      global.obstacles.push_back(b);
		    }
		  }

		}

		//6) - add passive spheres as TARGET!
		// - 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

		PassiveSphere* s1;

		double target_z_position = 1.5;

		if(ico_learning_task)
		{
		  target_z_position = 0.5;
		}


		for (int i=0; i < number_spheres; i++){
			s1 = new PassiveSphere(odeHandle, osgHandle, 0.5);

			//Target 1
			if(random_positon_spheres)
			{
				/*******Generating Random Position****/
				random_position_S  = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x; // Input 0 (m) between -2.4 and 2.4
				std::cout<<"\n\n\n\n\n"<<"Inital Random Target X position"<<" = "<<-1*random_position_S<<"\t"<<"Inital Random Target Y position"<<" = "<<random_position_S<<"\n\n\n\n";
				/************************************/
				if (i==0) s1->setPosition(osg::Vec3(-1*random_position_S,random_position_S,target_z_position/*2*/));
			}
			else
			{
				//position_S = 0.0;// -2.0---------------------------------------------------------------------------TEST
				if (i==0) s1->setPosition(osg::Vec3(-10.0 /*position_S*/, position_S, target_z_position/*0*/));
			}

			//Target 2

			//if (i==1) s1->setPosition(osg::Vec3(15-11.0 /*position_S*/, position_S+10, 0.5/*0*/));
			if (i==1) s1->setPosition(osg::Vec3(0.0 /*position_S*/, position_S+5, target_z_position/*0*/));

			//Target 3
			//if (i==2) s1->setPosition(osg::Vec3(15-11.0 /*position_S*/, position_S-10, 0.5/*0*/));
			if (i==2) s1->setPosition(osg::Vec3(0.0 /*position_S*/, position_S-5, target_z_position/*0*/));


			//Target 4
			if (i==3) s1->setPosition(osg::Vec3(-10.0 /*position_S*/, position_S, target_z_position/*0*/));
			//if (i==3) s1->setPosition(osg::Vec3(-10, 10,2/*2*/));

			s1->setTexture("Images/dusty.rgb");
			//Target 1
			if (i==0){ s1->setColor(Color(1,0,0)); }
			//Target 2
			if (i==1){ s1->setColor(Color(0,1,0)); }
			//Target 3
			if (i==2){ s1->setColor(Color(0,0,1)); }
			//Target 4
			if (i==3){ s1->setColor(Color(1,1,0)); }
			obst.push_back(s1);
			global.obstacles.push_back(s1);
			// fix sphere (in actual position) to simulation
			//fixator.push_back(new  FixedJoint(s1->getMainPrimitive(), global.environment));  //create pointer
			//fixator.at(i)->init(odeHandle, osgHandle);
			fixator2 = new  FixedJoint(s1->getMainPrimitive(), global.environment);
			fixator2->init(odeHandle, osgHandle);
		}


		//		int number_spheres = 4;
		//		PassiveSphere* s1;
		//		for (int i=0; i < number_spheres; i++){
		//			s1 = new PassiveSphere(odeHandle, osgHandle, 0.5);
		//			if (i==0) s1->setPosition(osg::Vec3(-10,-10,2));
		//			if (i==1) s1->setPosition(osg::Vec3( 10, 10,2));
		//			if (i==2) s1->setPosition(osg::Vec3( 10, -10,2));
		//			if (i==3) s1->setPosition(osg::Vec3(-10, 10,2));
		//
		//			s1->setTexture("Images/dusty.rgb");
		//			if (i==0){ s1->setColor(Color(1,0,0)); }
		//			if (i==1){ s1->setColor(Color(0,1,0)); }
		//			if (i==2){ s1->setColor(Color(0,0,1)); }
		//			if (i==3){ s1->setColor(Color(1,1,0)); }
		//			obst.push_back(s1);
		//			global.obstacles.push_back(s1);
		//			// fix sphere (in actual position) to simulation
		//			fixator.push_back(  new  FixedJoint(s1->getMainPrimitive(), global.environment));  //create pointer
		//			fixator.at(i)->init(odeHandle, osgHandle);
		//		}

		//7) - add passive box as OBSTACLES
		// - create pointer to Box (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


		PassiveBox* b1;
		double length = 10.0;//1.5 /*for learning*/; //3.5 /*for*/;//testing//---------------------------------------------------------------------------TEST
		double width = 1.0;
		double height = 1.0;

		if(ico_learning_task)
		{
		  obstacle_on = false;
		}


		if(obstacle_on)
		{
			for (int i=0; i < number_boxes /*SET NUMBER OF OBSTACLES*/; i++){
				b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height /*size*/));
				//b1->setTexture("dusty.rgb");
				b1->setColor(Color(1,0,0));
				//b1->setPosition(osg::Vec3(/*-4.5+*/i*4.5,3+i,0)); // Fixed robot position
				//osg::Matrix pose;
				//			pose.setTrans(osg::Vec3(/*-4.5+*/i*4.5,3+i,0));
				b1->setPose(osg::Matrix::rotate(0.3*(M_PI/2)*i, 0,0, 1) * osg::Matrix::translate(/*-4.5+*/i*5,5+i,0.5) /* pose*/);
				//b1->setPose(osg::Matrix::rotate(/*-0.5*(M_PI/4)*/ (M_PI/2), 0,0, 1) * osg::Matrix::translate(/*i*4.5+*/pos_obstacle_x, pos_obstacle_y+i*-pos_obstacle_y*2.0/*0+i*/,height/2) /* pose*/);
				global.obstacles.push_back(b1);
				fixator.push_back(  new  FixedJoint(b1->getMainPrimitive(), global.environment));  //create pointer
				fixator.at(i)->init(odeHandle, osgHandle);

			}




		}


		////////////////////////////////////Call this set up and Control//////////////////////////////////////////////
		bool ac_ico_robot = true;
		if (ac_ico_robot)
		{

			//0)

			//qcontroller->setReset(1);

			//1) Activate IR sensors
		  FourWheeledConf fconf =FourWheeledRPos::getDefaultConf();

			///2) relative sensors
			for (int i=0; i < number_spheres; i++){
				fconf.rpos_sensor_references.push_back(obst.at(i)->getMainPrimitive());
			}

			OdeRobot* vehicle3 = new FourWheeledRPos(odeHandle, osgHandle, fconf);



			/****Initial position of Nimm4******/

			if(random_positon_frist)
			{
				/*******Generating Random Position****/
				//srand (time(NULL));
				random_position  = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x; // Input 0 (m) between -2.4 and 2.4
				/************************************/
				do
				{
					int r = rand();
					std::cout << "random value 1: " << r << std::endl;
					random_position  = ((MAX_x-MIN_x)*((float)r/RAND_MAX))+MIN_x;
					//std::cout<<"\n\n\n\n\n"<<"Inital Random_Robot X position"<<" = "<<random_position<<"\t"<<"Inital Random_Robot Y position"<<" = "<<-1*random_position<<"\n\n\n\n";

				}while(abs(abs(position_S) - abs(random_position)) <= 4);
				//while(abs(abs(random_position_S) - abs(random_position)) <= 4);

				std::cout<<"\n\n"<<"Inital Random X position"<<" = "<<random_position<<"\t"<<"Inital Random Y position"<<" = "<<-1*random_position<<"\n\n";
				//vehicle3->place(Pos(position_x-10.0 /* 20 random_position*/  /*+x = to left (sphere in front of robot), -x = to right sphere behind robot*/, 0.0 /*-1*random_position*/ /*+y = to up, -y = to down*/,0.0/*z*/));
				vehicle3->place(Pos(position_x-15.0 /* 20 random_position*/  /*+x = to left (sphere in front of robot), -x = to right sphere behind robot*/, 0.0 /*-1*random_position*/ /*+y = to up, -y = to down*/,0.0/*z*/));
			}
			else
			{

				/*******Generating Random Position****/
				//srand (time(NULL));
				int r = rand();
				std::cout << "random value 2: " << r << std::endl;
				random_or  = ((MAX_or-MIN_or)*((float)r/RAND_MAX))+MIN_or; // between -pi/3 (60 deg) and pi/3 (60 deg)
				/************************************/

				Pos pos(position_x-5.0/*, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/);

				std::cout << "random_or1: " << random_or << std::endl;

				//setting position and orientation
				vehicle3->place(osg::Matrix::rotate(random_or, 0, 0, 1) *osg::Matrix::translate(pos));
				//setting only position
				//vehicle3->place(Pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/));


			}


			qcontroller = new EmptyController("1","1");

			global.configs.push_back(qcontroller);

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

			// create pointer to agent

			plotoptions.push_back(PlotOption(NoPlot /*select "File" to save signals, "NoPlot" to not save*/));
			OdeAgent* agent3 = new OdeAgent(plotoptions);



			if(drawtrace_on)
			{
				TrackRobot* track3 = new TrackRobot(/*bool trackPos*/true,
						/*bool trackSpeed*/false,
						/*bool trackOrientation*/false,
						/*bool displayTrace*/true //,
				/*const char *scene="", int interval=1*/);
				agent3->setTrackOptions(*track3);
			}

			agent3->init(qcontroller, vehicle3, wiring3);///////////// Initial controller!!!
			global.agents.push_back(agent3);

		}

		//bool random_controlled_robot =false;


		showParams(global.configs);
	}
示例#9
0
文件: main2.cpp 项目: humm/playful
  // starting function (executed once at the beginning of the simulation loop)
  void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) 
  {
    setCameraHomePos(Pos(-16.4509, 15.6927, 12.5683),  Pos(-133.688, -26.4496, 0));
    setCameraMode(Static);
    global.odeConfig.setParam("noise",0.05);
    global.odeConfig.setParam("realtimefactor",2);
    //  global.odeConfig.setParam("gravity", 0);

    Playground* playground;
    if(env==ElipticBasin){
      playground = new Playground(odeHandle, osgHandle, 
				  osg::Vec3(20, 0.2, height+1.f), 2);
    }else{
      playground = new Playground(odeHandle, osgHandle, 
				  osg::Vec3(20, 0.2, height+0.3f), 1);
    }
    playground->setColor(Color(.1,0.7,.1));
    playground->setTexture("");
    playground->setPosition(osg::Vec3(0,0,0.01f)); // playground positionieren und generieren
    global.obstacles.push_back(playground);

    int numpassive=0;
    switch(env){
    case ThreeBump:
      {
        TerrainGround* terrainground = 
          new TerrainGround(odeHandle, osgHandle.changeColor(Color(1.0f,1.0,1.0)),
                            "terrains/macrospheresLMH_64.ppm","terrains/macrospheresTex_256.ppm", 
                            20, 20, height, OSGHeightField::LowMidHigh);
        terrainground->setPose(osg::Matrix::translate(0, 0, 0.1));
        global.obstacles.push_back(terrainground);
        addRobot(odeHandle, osgHandle, global, 0);
        addRobot(odeHandle, osgHandle, global, 1);
        numpassive=4;
      }
      break;
    case SingleBasin:
      // at Radius 3.92 height difference of 0.5 and at 6.2 height difference of 1 
      // ./start -single -track -f 2 -r 1297536669

    case ElipticBasin:
      // ./start -eliptic -f 5 -track -r 1297628680
      {
        TerrainGround* terrainground = 
          new TerrainGround(odeHandle, osgHandle.changeColor(Color(1.0f,1.0f,1.0f)),
			    //                        "terrains/dip128_flat.ppm","terrains/dip128_flat_texture.ppm", 
                            "terrains/dip128.ppm","terrains/dip128_texture.ppm", 
                            20, env == SingleBasin ? 20 : 40, height, OSGHeightField::Red);
        terrainground->setPose(osg::Matrix::translate(0, 0, 0.1));
        global.obstacles.push_back(terrainground);
        addRobot(odeHandle, osgHandle, global, 3);
      }
      break;
    }
        

    // 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< numpassive; i+=1){
      PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.5,0.1);
      s1->setPosition(osg::Vec3(-8+2*i,-2,height+0.5));
      s1->setTexture("Images/dusty.rgb");
      global.obstacles.push_back(s1);
    }
  
  }
示例#10
0
  // starting function (executed once at the beginning of the simulation loop)
  void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
  {
    int num_barrels=1;
    int num_barrels_test=0;

    sensor=0;
    setCameraMode(Follow);

    bool normalplayground=false;

    //    setCameraHomePos(Pos(-0.497163, 11.6358, 3.67419),  Pos(-179.213, -11.6718, 0));
    setCameraHomePos(Pos(-2.60384, 13.1299, 2.64348),  Pos(-179.063, -9.7594, 0));
    // initialization
    global.odeConfig.setParam("noise",0.1);
    //  global.odeConfig.setParam("gravity",-10);
    global.odeConfig.setParam("controlinterval",4);
    global.odeConfig.setParam("realtimefactor",5);

    // add a new parameter to be configured on the console
    global.odeConfig.addParameterDef("friction", &friction, 0.1, "rolling friction coefficient");


    if(normalplayground){
      Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(20, 0.01, 0.01 ), 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.05));
      global.obstacles.push_back(playground);
    }


    /* * * * 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;

      conf.addSensor(new SpeedSensor(10, SpeedSensor::Translational, Sensor::X ));
      conf.irAxis1=false;
      conf.irAxis2=false;
      conf.irAxis3=false;
      conf.axesShift = 0;
      // conf.axesShift = conf.diameter/2 - conf.pendularrange/2;
      sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
                                    conf, "Barrel1", 0.4);

      sphere1->place (osg::Matrix::rotate(M_PI/2, 1,0,0)*osg::Matrix::translate(0,0,0.2));

      // InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
      // cc.cInit=1;
      // //    cc.useSD=true;
      // controller = new InvertMotorNStep(cc);

      // controller->setParam("steps", 2);
      // controller->setParam("adaptrate", 0.0);
      // controller->setParam("nomupdate", 0.00);
      // controller->setParam("epsC", 0.03);
      // controller->setParam("epsA", 0.05);
      // controller->setParam("rootE", 0);
      // controller->setParam("logaE", 0);
      //controller = new PiMax();
      //controller->setParam("epsC", 0.001);

      // controller = new Sox();
      // controller->setParam("epsC", 0.5);
      controller = new ROSController("Test");

      AbstractWiring* wiring = new SelectiveOne2OneWiring(new ColorUniformNoise(), new select_from_to(0,1));
      //      OdeAgent* agent = new OdeAgent ( PlotOption(File, Robot, 1) );
      OdeAgent* agent = new OdeAgent ();
      agent->init ( controller , sphere1 , wiring );
      //  agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50));
      global.agents.push_back ( agent );
      global.configs.push_back ( controller );
    }


    /* * * * TEST BARRELS * * * */
    for(int i=0; i< num_barrels_test; i++){
      global.odeConfig.setParam("realtimefactor",1);
      //****************
      Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
      conf.pendularrange  = 0.15;
      conf.motorsensor=true;
      conf.irAxis1=false;
      conf.irAxis2=false;
      conf.irAxis3=false;
      conf.spheremass   = 1;
      sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
                                    conf, "Barrel1", 0.4);
      sphere1->place ( osg::Matrix::rotate(M_PI/2, 1,0,0));

      controller = new SineController();
      controller->setParam("sinerate", 15);
      controller->setParam("phaseshift", 0.45);

      //       DerivativeWiringConf dc = DerivativeWiring::getDefaultConf();
      //       dc.useId=true;
      //       dc.useFirstD=false;
      //       AbstractWiring* wiring = new DerivativeWiring(dc,new ColorUniformNoise());
      AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise());
      OdeAgent* agent = new OdeAgent ();
      agent->init ( controller , sphere1 , wiring );
      //  agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50));
      global.agents.push_back ( agent );
      global.configs.push_back ( controller );
    }



  }
示例#11
0
文件: main.cpp 项目: amr1985/playful
  /// 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(1.86452, 21.1998, 8.7964),  Pos(174.993, -10.688, 0));
    setCameraMode(Follow);

    global.odeConfig.setParam("controlinterval",1);
    global.odeConfig.setParam("gravity", -9.81); 

    global.odeConfig.setParam("noise", 0);
    global.odeConfig.setParam("realtimefactor", 1.);

    int sliderwheelies = 1;
    int snakes = 0;

    Playground* playground = new Playground(odeHandle, osgHandle, 
			    osg::Vec3(120, 0.2, 2.5),.5,true);
    //    playground->setColor(Color(1,0.2,0,0.1));
    playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
    global.obstacles.push_back(playground);
    

    // Creation of Obstacle
    PassiveBox* b = 
      new  PassiveBox(odeHandle, 
                      osgHandle.changeColor(Color(180.0 / 255.0, 180.0 / 255.0, 130.0 / 255.0)), 
                      osg::Vec3(4.0, 20.0, .8), 0);
    b->setTexture(0,TextureDescr("Images/playfulmachines.rgb",1,1));
    b->setPosition(Pos(-32, 0, 0)); 
    global.obstacles.push_back(b);    
    


      /******* S L I D E R - W H E E L I E *********/
    for(int i=0; i<sliderwheelies; i++){
      SliderWheelieConf mySliderWheelieConf = SliderWheelie::getDefaultConf();
      mySliderWheelieConf.segmNumber=12;
      mySliderWheelieConf.jointLimitIn=M_PI/3;
      mySliderWheelieConf.frictionGround=0.5;
      mySliderWheelieConf.motorPower=8;
      mySliderWheelieConf.motorDamp=0.05;
      mySliderWheelieConf.sliderLength=0.5;
      mySliderWheelieConf.segmLength=1.4;
      OdeRobot* robot = new SliderWheelie(odeHandle, osgHandle, mySliderWheelieConf, "Slider Armband");
      robot->place(Pos(0,0,2.0)); 

      //controller = new Sos(1.0);
      controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint");
      AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); 
      OdeAgent* agent = new OdeAgent(global);
      // only the first controller is exported to guilogger and Co
      agent->addInspectable(((OneControllerPerChannel*)controller)->getControllers()[0]);
      agent->init(controller, robot, wiring);
      if(track) 
        agent->setTrackOptions(TrackRobot(true,true,false,false,"split_control",1));
      global.agents.push_back(agent);
      global.configs.push_back(agent);
    }


    //****** SNAKES **********/
    for(int i=0; i<snakes; i++){

      //****************/
      SchlangeConf conf = Schlange::getDefaultConf();
      // conf.segmMass   = .2;
      // conf.segmLength= .5// 0.8;
      //   conf.segmDia=.6;
      // conf.motorPower=.5;
      conf.segmNumber = 12+2*i;//-i/2; 
      conf.jointLimit=conf.jointLimit* 1.6;
      conf.frictionJoint=0.02;
      conf.useServoVel=true;

      SchlangeServo2* schlange1;
      if (i==0) {
	schlange1 = 

	  new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)),
			       conf, "S1");
      } else {
	schlange1 = 
	  new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)),
			       conf, "S2");
      }
      //Positionieren und rotieren 
      schlange1->place(osg::Matrix::rotate(M_PI/2,0, 1, 0)*
		        osg::Matrix::translate(-.7+0.7*i,0,(i+1)*(.2+conf.segmNumber)/2.0/*+2*/));
                     // osg::Matrix::translate(5-i,2 + i*2,height+2));
      schlange1->setTexture("Images/whitemetal_farbig_small.rgb");
      if (i==0) {
	schlange1->setHeadColor(Color(1.0,0,0));
      } else {
	schlange1->setHeadColor(Color(0,1.0,0));
      }      
      OneControllerPerChannel *controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint");
      AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); 

      OdeAgent* agent = new OdeAgent(global);
      agent->addInspectable(controller->getControllers()[0]);
      agent->init(controller, schlange1, wiring);
      global.agents.push_back(agent);
      global.configs.push_back(agent);
 
    }//creation of snakes End

  }
示例#12
0
  // 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);


  }
示例#13
0
  // starting function (executed once at the beginning of the simulation loop/first cycle)
  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(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
    // initialization
    // - set noise to 0.1
    global.odeConfig.noise=0.05;
    // set realtimefactor to maximum
    global.odeConfig.setParam("realtimefactor", 0);

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

    // odeHandle and osgHandle are global references
    // vec3 == length, width, height
    Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(32, 0.2, 0.5));
    playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren
    // register playground in obstacles list
    global.obstacles.push_back(playground);

    // 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.force   = 4;
    c.bumper  = true;
    c.cigarMode  = true;
    // c.irFront = true;
    vehicle = new Nimm2(odeHandle, osgHandle, c, "Nimm2");
    vehicle->place(Pos(0,0,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
    agent = new OdeAgent(global);
    agent->init(controller, vehicle, wiring);
    global.agents.push_back(agent);


  }