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