/**************************Reset Function***************************************************************/
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;
//Temporary variable storing the pointer to the controller if the latter should not be resetted
AbstractController* temp_controller;
//OdeAgent* temp_agent;
// We would like to have 10 runs!
// after it we must clean all and return false because we don't want a new restart
if (this->currentCycle == repeat_number) // This will be delete all after finish simulation!!!!!!
{
//clean robots
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 controller;///////////////////////////////////////////////This will finally delete which is OK
delete robot;
delete wiring;
delete (agent);
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);
}
std::cout << "end.";
return false; // don't restart, just quit
}
// Now we must delete all robots and agents from the simulation and create new robots and agents.
// BUT NOT CONTROLLER for learning
while (global.agents.size() > 0)
{
// std::cout << "\n MAIN WHILE LOOP" << currentCycle << "\n";
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;
//DON'T delete controller if DON't want to reset controller!------------------------------------------------------(FRANK)
if(delete_controller)
{
delete controller;
//this calls destroy:
} else { // instead save the pointer in a temporary variable
temp_controller = controller;
// temp_agent = agent;
}
//Need to add put the current controller to temp controller?????----STILL NOT IMPLEMENT-----------------------------(FRANK)
//delete (agent);
global.agents.erase(global.agents.begin());
// std::cout << "\n END OF MAIN WHILE LOOP" << currentCycle << "\n";
}
///////////////Recreate Robot Start//////////////////////////////////////////////////////////////////////////////////////
//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, "FourWheeled");
if(random_positon)
{
/*******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
{
random_positionx = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x;
random_positiony = ((MAX_y-MIN_y)*((float)rand()/RAND_MAX))+MIN_y;
//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(random_position_S) - abs(random_positionx)) <= 4 || abs(abs(pos_obstacle_x) - abs(random_positionx)) <= 1.5 || abs(random_positiony) <= 0.5);
std::cout<<"\n\n"<<"Reset Inital Random X position"<<" = "<<random_positionx<<"\t"<<"Reset Inital Random Y position"<<" = "<<random_positiony<<"\n\n";
vehicle3->place(Pos(random_positionx/*10 +x = to left, -x = to right*/, random_positiony /* +y = to close, -y = to far*/,0/*z*/));
}
else
{
vehicle3->place(Pos(position_x-5.0/*x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/));
}
if(random_orientation)
{
/*******Generating Random Position****/
//srand (time(NULL));
int r = rand();
std::cout << "random value 3: " << 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/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/);
std::cout << "random_or2: " << 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*/));
}
else
{
Pos pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/);
//setting position and orientation
vehicle3->place(osg::Matrix::rotate(0.0, 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*/));
}
if(repeat_experiment)
{
qcontroller = (EmptyController*)temp_controller;
}
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);
//OdeAgent* agent3 = new OdeAgent(global);
agent3->init(qcontroller, vehicle3, wiring3);
if(drawtrace_on)
{
TrackRobot* track3 = new TrackRobot(/*bool trackPos*/true/*true*/,
/*bool trackSpeed*/false,
/*bool trackOrientation*/false,
/*bool displayTrace*/ true //,
/*const char *scene="", int interval=1*/);
agent3->setTrackOptions(*track3);
}
global.agents.push_back(agent3);
std::cout << "\n end restart " << currentCycle << "\n";
// restart!
return true;
}
// 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;
}