bool XMLBoundingShape::init(const lpzrobots::OdeHandle& _odeHandle, const lpzrobots::OsgHandle& osgHandle,
      double scale, char mode) {
  odeHandle = OdeHandle(_odeHandle);
  parentSpace = odeHandle.space;
  odeHandle.createNewSimpleSpace(parentSpace,true);
  char primitiveMode = mode & ~Primitive::Body; // never create any body for the primitives (TODO: compound body)
  if (!(mode & Primitive::Body)) {
    attachedToParentBody = false;
  }
  for EACHCHILDNODE(boundingBoxNode, node) {
    if (node->getNodeType() == DOMNode::ELEMENT_NODE) {
      Primitive* primitive = 0;
      if (XMLHelper::matchesName(node,XMLDefinitions::boxNode))
        primitive = new Box(VALOFNODE(node, XMLDefinitions::lengthAtt) * scale, VALOFNODE(node, XMLDefinitions::widthAtt) * scale, VALOFNODE(node, XMLDefinitions::heightAtt) * scale);
      else if(XMLHelper::matchesName(node,XMLDefinitions::sphereNode))
        primitive = new Sphere(VALOFNODE(node,XMLDefinitions::radiusAtt));
      else if(XMLHelper::matchesName(node,XMLDefinitions::cylinderNode))
        primitive = new Cylinder(VALOFNODE(node,XMLDefinitions::radiusAtt) * scale, VALOFNODE(node,XMLDefinitions::heightAtt) * scale);
      else if(XMLHelper::matchesName(node,XMLDefinitions::capsuleNode))
        primitive = new Capsule(VALOFNODE(node,XMLDefinitions::radiusAtt) * scale, VALOFNODE(node,XMLDefinitions::heightAtt) * scale);
      if (primitive!=0) {
        XMLPrimitiveFactory::setMaterial(boundingBoxNode, primitive);
        XMLPrimitiveFactory::setMaterial(node,primitive);
        const Vec3 rot = XMLHelper::getRotation(node);
        const Vec3 pos = XMLHelper::getPosition(node);
        if (mode & Primitive::Body) {  // use Transforms to attach the Primitives to the body
          std::cout << "BoundingShape body mode!" << std::endl;
          Primitive* Trans = new lpzrobots::Transform(parent, primitive, osgRotate(rot[0]*M_PI/180.0f,rot[1]*M_PI/180.0f,rot[2]*M_PI/180.0f)
                   *osg::Matrix::translate(scale*pos[0],scale*pos[1],scale*pos[2]));
          Trans->init(odeHandle, 0, osgHandle.changeColor(Color(1.0,0,0,0.3)),primitiveMode);
        }
        else {
          std::cout << "BoundingShape geom only mode!" << std::endl;
          primitive->init(odeHandle, 0, osgHandle.changeColor(Color(1.0,0,0,0.3)), primitiveMode);
          boundingPrimitiveList.push_back(primitive);
          boundingPrimitivePoseList.push_back(osgRotate(rot[0]*M_PI/180.0f,rot[1]*M_PI/180.0f,rot[2]*M_PI/180.0f)
                   *osg::Matrix::translate(scale*pos[0],scale*pos[1],scale*pos[2]));
        }
        active = true;
        std::cout << "Primitive for BoundingShape created!" << std::endl;
      }
    }
  }
  return active;
}
Ejemplo n.º 2
0
    /**
     * starting function (executed once at the beginning of the simulation loop)
     */
    virtual void start(const lpzrobots::OdeHandle& odeHandle,
        const lpzrobots::OsgHandle& osgHandle,
        lpzrobots::GlobalData& global) {
      // set initial camera position
      setCameraHomePos(
          lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
          lpzrobots::Pos(178.866, -7.43884, 0));

      // set simulation parameters
      global.odeConfig.setParam("controlinterval", 30);
      global.odeConfig.setParam("simstepsize", 0.001);

      // add playground
      lpzrobots::Playground* playground 
        = new lpzrobots::Playground(odeHandle, osgHandle, 
                                    osg::Vec3(10, 0.2, 0.3)); 
      playground->setTexture(0,0,lpzrobots::TextureDescr("Images/wall_bw.jpg",-1.5,-3));
      playground->setPosition(osg::Vec3(0,0,.0));
      global.obstacles.push_back(playground);

      // Add Hexabot robot
      HEXABOT::HexabotConf hexabotConf = lpzrobots::Hexabot::getDefaultConf();
      lpzrobots::OdeHandle rodeHandle = odeHandle;
      rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
      hexabot = new lpzrobots::Hexabot(
          rodeHandle,
          osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
          hexabotConf, "Hexabot");

      // put hexabot a little bit in the air
      hexabot->place(osg::Matrix::translate(.0, .0, 1.));

      controller = new TripodGait18DOF();
      // create wiring
      One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());

      // create agent and init it with controller, robot and wiring
      lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
      agent->init(controller, hexabot, wiring);

      // create a fixed joint to hold the robot in the air at the beginning
      robotfixator = new lpzrobots::FixedJoint(
          hexabot->getMainPrimitive(),
          global.environment);
      robotfixator->init(odeHandle, osgHandle, false);

      // inform global variable over everything that happened:
      global.configs.push_back(hexabot);
      global.agents.push_back(agent);
      global.configs.push_back(controller);

      std::cout << "\n\n"
          << "################################\n"
          << "#   Press x to free hexabot!    #\n"
          << "################################\n"
          << "\n\n" << std::endl;
    }
Ejemplo n.º 3
0
    /**
     * starting function (executed once at the beginning of the simulation loop)
     */
    virtual void start(const lpzrobots::OdeHandle& odeHandle,
        const lpzrobots::OsgHandle& osgHandle,
        lpzrobots::GlobalData& global) {

      // set initial camera position
      setCameraHomePos(
          lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
          lpzrobots::Pos(178.866, -7.43884, 0));

      // set simulation parameters
      global.odeConfig.setParam("controlinterval", 10);
      global.odeConfig.setParam("simstepsize", 0.01);

      // Add amosII robot
      lpzrobots::AmosIIConf myAmosIIConf = lpzrobots::AmosII::getDefaultConf();
      myAmosIIConf.rubberFeet = true;
      amos = new lpzrobots::AmosII(
          odeHandle,
          osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
          myAmosIIConf, "AmosII");

      // put amos a little bit in the air
      amos->place(osg::Matrix::translate(.0, .0, 0.5));

      controller = new ExampleController();
      // create wiring
      One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());

      // create agent and init it with controller, robot and wiring
      lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
      agent->init(controller, amos, wiring);

      // add playground
      lpzrobots::Playground* playground
        = new lpzrobots::Playground(odeHandle, osgHandle,
                                    osg::Vec3(10, 0.2, 0.3));
      playground->setPosition(osg::Vec3(0,0,0));
      global.obstacles.push_back(playground);

      // create a fixed joint to hold the robot in the air at the beginning
      robotfixator = new lpzrobots::FixedJoint(
          amos->getMainPrimitive(),
          global.environment);
      robotfixator->init(odeHandle, osgHandle, false);

      // inform global variable over everything that happened:
      global.configs.push_back(amos);
      global.agents.push_back(agent);
      global.configs.push_back(controller);

      std::cout << "\n\n"
          << "################################\n"
          << "#   Press x to free amosII!    #\n"
          << "################################\n"
          << "\n\n" << std::endl;
    }
Ejemplo n.º 4
0
  /**
   * starting function (executed once at the beginning of the simulation loop)
   */
  virtual void start(const lpzrobots::OdeHandle& odeHandle,
      const lpzrobots::OsgHandle& osgHandle,
      lpzrobots::GlobalData& global) {
    // set initial camera position
    setCameraHomePos(
        lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
        lpzrobots::Pos(178.866, -7.43884, 0));

    // set simulation parameters
    global.odeConfig.setParam("controlinterval", 10);
    global.odeConfig.setParam("simstepsize", 0.01);
    global.odeConfig.setParam("noise", 0.3);

    // add playground
    lpzrobots::Playground* playground
    = new lpzrobots::Playground(odeHandle, osgHandle,
        osg::Vec3(10, 0.2, 0.3));
    playground->setTexture(0,0,lpzrobots::TextureDescr("Images/wall_bw.jpg",-1.5,-3));
    playground->setPosition(osg::Vec3(0,0,.0));
    global.obstacles.push_back(playground);

    //add obstacle
    PassiveBox* b1;
    double length = 3;
    double width = 0.2;
    double height = 1.0;
    bool obstacle_active = false;
    if(obstacle_active)
    {
      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.5*(M_PI/2), 0,0, 1) * osg::Matrix::translate(/*-4.5+*/i*4.5,3+i,0.5) /* pose*/);
      b1->setPose(osg::Matrix::rotate(/*-0.5*(M_PI/4)*/ (M_PI/2), 0,0, 1) * osg::Matrix::translate(2.0, 0.0,0.0) /* pose*/);
      global.obstacles.push_back(b1);
      lpzrobots::FixedJoint* fixator = new  lpzrobots::FixedJoint(b1->getMainPrimitive(), global.environment);
      fixator->init(odeHandle, osgHandle);
    }


    //----------create a sphere as the target by Ren-----------------------------
    //the first sphere
    lpzrobots::PassiveSphere* s1 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
    s1->setPosition(osg::Vec3(3.0, 0.0, 0.1));
    s1->setTexture("Images/dusty.rgb");
    s1->setColor(lpzrobots::Color(1,0,0));
    obst.push_back(s1);
    global.obstacles.push_back(s1);
    lpzrobots::FixedJoint* fixator1 = new  lpzrobots::FixedJoint(s1->getMainPrimitive(), global.environment);
    fixator1->init(odeHandle, osgHandle);

    //the second sphere
    lpzrobots::PassiveSphere* s2 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
    s2->setPosition(osg::Vec3(0.0, 3.0, 0.1));
    s2->setTexture("Images/dusty.rgb");
    s2->setColor(lpzrobots::Color(0,1,0));
    obst.push_back(s2);
    global.obstacles.push_back(s2);
    lpzrobots::FixedJoint* fixator2 = new  lpzrobots::FixedJoint(s2->getMainPrimitive(), global.environment);
    fixator2->init(odeHandle, osgHandle);

    //the third sphere
    lpzrobots::PassiveSphere* s3 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
    s3->setPosition(osg::Vec3(0.0, -3.0, 0.1));
    s3->setTexture("Images/dusty.rgb");
    s3->setColor(lpzrobots::Color(0,0,1));
    obst.push_back(s3);
    global.obstacles.push_back(s3);
    lpzrobots::FixedJoint* fixator3 = new  lpzrobots::FixedJoint(s3->getMainPrimitive(), global.environment);
    fixator3->init(odeHandle, osgHandle);

    //----------create a sphere as the target by Ren-----------------------------

    // Add amosII robot
    dungbeetleConf myAmosIIConf = dungbeetle::getDefaultConf(1.0 /*_scale*/,1 /*_useShoulder*/,1 /*_useFoot*/,1 /*_useBack*/);
    myAmosIIConf.rubberFeet = true;
    lpzrobots::OdeHandle rodeHandle = odeHandle;
    rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
    //------------------- Link the sphere to the Goal Sensor by Ren---------------
    for(unsigned int i = 0; i<obst.size(); i++)
    {
      myAmosIIConf.GoalSensor_references.push_back(obst.at(i)->getMainPrimitive());
    }
    //------------------- Link the sphere to the Goal Sensor by Ren---------------
    amos = new dungbeetle(
        rodeHandle,
        osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
        myAmosIIConf, "AmosII");

    // define the usage of the individual legs
    amos->setLegPosUsage(amos->L0, amos->LEG);
    amos->setLegPosUsage(amos->L1, amos->LEG);
    amos->setLegPosUsage(amos->L2, amos->LEG);
    amos->setLegPosUsage(amos->R0, amos->LEG);
    amos->setLegPosUsage(amos->R1, amos->LEG);
    amos->setLegPosUsage(amos->R2, amos->LEG);

    // put amos a little bit in the air
    amos->place(osg::Matrix::translate(.0, .0, 0.5));

    controller = new EmptyController();
    // create wiring
    One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());

    // create agent and init it with controller, robot and wiring
    lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
    agent->init(controller, amos, wiring);

    // create a fixed joint to hold the robot in the air at the beginning
    robotfixator = new lpzrobots::FixedJoint(
        amos->getMainPrimitive(),
        global.environment);
    robotfixator->init(odeHandle, osgHandle, false);

    // inform global variable over everything that happened:
    global.configs.push_back(amos);
    global.agents.push_back(agent);
    global.configs.push_back(controller);

    std::cout << "\n\n"
        << "################################\n"
        << "#   Press x to free amosII!    #\n"
        << "################################\n"
        << "\n\n" << std::endl;

  }
Ejemplo n.º 5
0
  /**
   * starting function (executed once at the beginning of the simulation loop)
   */
  virtual void start(const lpzrobots::OdeHandle& odeHandle,
      const lpzrobots::OsgHandle& osgHandle,
      lpzrobots::GlobalData& global) {
    // set initial camera position
    setCameraHomePos(
        lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
        lpzrobots::Pos(178.866, -7.43884, 0));

    // set simulation parameters
    global.odeConfig.setParam("controlinterval", 10);
    global.odeConfig.setParam("simstepsize", 0.01);
    global.odeConfig.setParam("noise", sim_noise);
	
	// declare terrain params
	double terrain_roughness;
	double terrain_slip;
	double terrain_hardness;
	double terrain_elasticity;
	double terrain_height;
	Color terrain_color;
	
	int terrain_type = terrain_type_g;
	
	// define individual terrain parameters here
	switch(terrain_type) {
		case 1:	// concrete
			setTitle("concrete");
			terrain_roughness = 10.0;
			terrain_slip = 0.0;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.0;
			terrain_color = Color(156.0/255.0,159.0/255.0,166.0/255.0);
			break;
		case 2:	// mud
			setTitle("mud");
			terrain_roughness = 0.5;
			terrain_slip = 5.0;
			terrain_hardness = 0.5;
			terrain_elasticity = 0.5;
			terrain_height = 0.02;
			terrain_color = Color(100.0/255.0,100.0/255.0,100.0/255.0);
			break;
		case 3:	// ice
			setTitle("ice");
			terrain_roughness = 0.0;
			terrain_slip = 100.0;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.0;
			terrain_color = Color(215.0/255.0,227.0/255.0,255.0/255.0);
			break;
		case 4:	// sand
			setTitle("sand");
			terrain_roughness = 1.0;
			terrain_slip = 0.1;
			terrain_hardness = 30.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.02;
			terrain_color = Color(242.0/255.0,238.0/255.0,124.0/255.0);
			break;
		case 5:	// gravel
			setTitle("gravel");
			terrain_roughness = 7.0;
			terrain_slip = 0.1;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.03;
			terrain_color = Color(115.0/255.0,127.0/255.0,156.0/255.0);
			break;
		case 6:	// grass
			setTitle("grass");
			terrain_roughness = 5.0;
			terrain_slip = 0.0;
			terrain_hardness = 30.0;
			terrain_elasticity = 0.6;
			terrain_height = 0.05;
			terrain_color = Color(35.0/255.0,150.0/255.0,20.0/255.0);
			break;
		case 7:	// swamp
			setTitle("swamp");
			terrain_roughness = 0.0;
			terrain_slip = 5.0;
			terrain_hardness = 0.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.1;
			terrain_color = Color(50.0/255.0,75.0/255.0,50.0/255.0);
			break;
		case 8:	// rock
			setTitle("rock");
			terrain_roughness = 10.0;
			terrain_slip = 0.0;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.1;
			terrain_color = Color(110.0/255.0,90.0/255.0,60.0/255.0);
			break;
		case 9:	// tiles
			setTitle("tiles");
			terrain_roughness = 5.0;
			terrain_slip = 30.0;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.0;
			terrain_color = Color(250.0/255.0,200.0/255.0,150.0/255.0);
			break;
		case 10: // snow
			setTitle("snow");
			terrain_roughness = 0.0;
			terrain_slip = 80.0;
			terrain_hardness = 20.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.02;
			terrain_color = Color(255.0/255.0,255.0/255.0,255.0/255.0);
			break;
		case 11: // rubber
			setTitle("rubber");
			terrain_roughness = 8.0;
			terrain_slip = 0.0;
			terrain_hardness = 80.0;
			terrain_elasticity = 2.0;
			terrain_height = 0.0;
			terrain_color = Color(0.0/255.0,0.0/255.0,0.0/255.0);
			break;
		case 12: // carpet
			setTitle("carpet");
			terrain_roughness = 3.0;
			terrain_slip = 0.0;
			terrain_hardness = 40.0;
			terrain_elasticity = 0.3;
			terrain_height = 0.02;
			terrain_color = Color(135.0/255.0,100.0/255.0,150.0/255.0);
			break;
		case 13: // wood
			setTitle("wood");
			terrain_roughness = 6.0;
			terrain_slip = 0.0;
			terrain_hardness = 80.0;
			terrain_elasticity = 0.2;
			terrain_height = 0.02;
			terrain_color = Color(90.0/255.0,65.0/255.0,0.0/255.0);
			break;
		case 14: // plastic
			setTitle("plastic");
			terrain_roughness = 1.0;
			terrain_slip = 2.0;
			terrain_hardness = 60.0;
			terrain_elasticity = 0.5;
			terrain_height = 0.0;
			terrain_color = Color(150.0/255.0,250.0/255.0,190.0/255.0);
			break;
		case 15: // foam
			setTitle("foam");
			terrain_roughness = 5.0;
			terrain_slip = 0.0;
			terrain_hardness = 0.0;
			terrain_elasticity = 0.7;
			terrain_height = 0.05;
			terrain_color = Color(220.0/255.0,230.0/255.0,150.0/255.0);
			break;
		default:
			setTitle("default");
			terrain_roughness = 10.0;
			terrain_slip = 0.0;
			terrain_hardness = 100.0;
			terrain_elasticity = 0.0;
			terrain_height = 0.0;
			terrain_color = Color(255.0/255.0,255.0/255.0,255.0/255.0);
	}
	
	if (terrain_noise) {
		// Adding noise
		terrain_roughness += fRand(-10.0*tn_std_prcnt, 10.0*tn_std_prcnt);
		terrain_slip  += fRand(-10.0*tn_std_prcnt, 10.0*tn_std_prcnt);
		terrain_hardness += fRand(-100.0*tn_std_prcnt, 100.0*tn_std_prcnt);
		terrain_elasticity += fRand(-2.0*tn_std_prcnt, 2.0*tn_std_prcnt);
		terrain_height += fRand(-0.1*tn_std_prcnt, 0.1*tn_std_prcnt);
	}
	
	// limits
	terrain_roughness = max(0.0, terrain_roughness);
	terrain_slip = max(0.0, terrain_slip);
	terrain_hardness = max(0.0, terrain_hardness);
	terrain_elasticity = max(0.0, terrain_elasticity);
	terrain_height = max(0.0, terrain_height);
	
	cout << terrain_roughness << ", " << terrain_slip << ", " << terrain_hardness << ", " << terrain_elasticity << ", " << terrain_height << endl;
	
	//**************Change Material substance*********//
	//Substance roughterrainSubstance(1.0,0.0,/*100.0 friction*/100.0,0.0); //(roughness,slip,hardness,elasticity)
	Substance roughterrainSubstance(terrain_roughness, terrain_slip, terrain_hardness, terrain_elasticity); //(roughness(0:smooth/1:rough), slip(0:friction/100:sliding), hardness(0:soft/100:hard), elasticity(0:hard/1:elastic))
	OdeHandle oodeHandle = odeHandle;
	oodeHandle.substance = roughterrainSubstance;
	//**************Change Material substance*********//

	TerrainGround* terrainground =
			new TerrainGround(oodeHandle, osgHandle.changeColor(terrain_color),
								"rough1.ppm","", /*AREA-SIZE-X->*/25, /*AREA-SIZE-Y->*/25, /*HEIGHT->*/terrain_height);
	terrainground->setPose(osg::Matrix::translate(0, 0, 0.005));
	global.obstacles.push_back(terrainground);

	
    /* ########################## Add amosII robot ########################## */
    lpzrobots::AmosIIConf myAmosIIConf = lpzrobots::AmosII::getDefaultConf(1.0 /*_scale*/,1 /*_useShoulder*/,1 /*_useFoot*/,1 /*_useBack*/);
    myAmosIIConf.rubberFeet = true;
    lpzrobots::OdeHandle rodeHandle = odeHandle;
    rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
    
    amos = new lpzrobots::AmosII(
        rodeHandle,
        osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
        myAmosIIConf, "AmosII");

    // define the usage of the individual legs
    amos->setLegPosUsage(amos->L0, amos->LEG);
    amos->setLegPosUsage(amos->L1, amos->LEG);
    amos->setLegPosUsage(amos->L2, amos->LEG);
    amos->setLegPosUsage(amos->R0, amos->LEG);
    amos->setLegPosUsage(amos->R1, amos->LEG);
    amos->setLegPosUsage(amos->R2, amos->LEG);

    // put amos on a random position in the map
    double init_pos_x = fRand(0.0, 5.0);
    double init_pos_y = fRand(0.0, 5.0);
    cout << init_pos_x << "," << init_pos_y;
    amos->place(osg::Matrix::translate(init_pos_x, init_pos_y, 0.1));

    controller = new EmptyController();
    // create wiring
    One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());

    // create agent and init it with controller, robot and wiring
    lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
    agent->init(controller, amos, wiring);
	
	
    // create a fixed joint to hold the robot in the air at the beginning
    robotfixator = new lpzrobots::FixedJoint(
        amos->getMainPrimitive(),
        global.environment);
    robotfixator->init(odeHandle, osgHandle, false);
    

    // inform global variable over everything that happened:
    global.configs.push_back(amos);
    global.agents.push_back(agent);
    global.configs.push_back(controller);
   	
   	// remove robotfixator
    if (robotfixator) {
      delete robotfixator;
      robotfixator = NULL;
    }
  }