// Only works for vectors with nonzero components
Grid::Grid(Vector3f center, Vector3f step, Vector3f gridMax) {
Vector3f shiftToCenter = center - gridMax / 2;
this->numSteps= Vector3i(gridMax(0)/step(0), gridMax(1)/step(1), gridMax(2)/step(2)) + Vector3i(1, 1, 1);
for (int i = 0; i < numSteps(0); i++) {
for (int j = 0; j < numSteps(1); j++) {
for (int k = 0; k < numSteps(2); k++) {
vertices.push_back(Vector3f(i*step(0), j*step(1), k*step(2)) + shiftToCenter);
}
}
}
}
void Job::jobFinished(Report& report, bool b)
{
setStatus(b ? Success : Error);
emit progress(numSteps());
emit finished();
report.setStatus(i18nc("@info/plain job status (error, warning, ...)", "%1: %2", description(), statusText()));
}
void Grid::addTriangles(vector_tri *triangles, float (*func)(Vector3f)) {
for (int i = 0; i < numSteps(0)-1; i++) {
for (int j = 0; j < numSteps(1)-1; j++) {
for (int k = 0; k < numSteps(2)-1; k++) {
int ic = numSteps(1)*numSteps(2);
int jc = numSteps(2);
Vector3f *v1 = &vertices[i*ic+j*jc+k];
Vector3f *v2 = &vertices[i*ic+j*jc+k+1];
Vector3f *v3 = &vertices[(i+1)*ic+j*jc+k+1];
Vector3f *v4 = &vertices[(i+1)*ic+j*jc+k];
Vector3f *v5 = &vertices[i*ic+(j+1)*jc+k];
Vector3f *v6 = &vertices[i*ic+(j+1)*jc+k+1];
Vector3f *v7 = &vertices[(i+1)*ic+(j+1)*jc+k+1];
Vector3f *v8 = &vertices[(i+1)*ic+(j+1)*jc+k];
Tetrahedron t1(v2, v1, v5, v4);
Tetrahedron t2(v2, v4, v5, v8);
Tetrahedron t3(v2, v5, v6, v8);
Tetrahedron t4(v3, v6, v7, v8);
Tetrahedron t5(v3, v2, v4, v8);
Tetrahedron t6(v3, v2, v6, v8);
//Tetrahedron t1(v5, v6, v8, v1);
//Tetrahedron t2(v7, v6, v3, v8);
//Tetrahedron t3(v2, v1, v6, v3);
//Tetrahedron t4(v4, v8, v3, v1);
//Tetrahedron t5(v1, v3, v6, v8);
t1.addTriangles(triangles, func);
t2.addTriangles(triangles, func);
t3.addTriangles(triangles, func);
t4.addTriangles(triangles, func);
t5.addTriangles(triangles, func);
t6.addTriangles(triangles, func);
}
}
}
}
void Simulator::runThreadOnHouse(int houseIndex)
{
string houseFileName = houseFileNames[houseIndex];
// read house file and run all algorithms on the specific house
unique_ptr<House> house = make_unique<House>();
int handle = readHouseFile(houseIndex, houseFileName, houseErrors.get(), house.get()); // pass ptr without releasing (house will be updated)
if (handle) {
// error
isValidHouses[houseIndex] = false; // (default is false anyway)
return;
}
numOfWorkingHouses++; // atomic increase
isValidHouses[houseIndex] = true;
// automatic win for each algorithm if the dirt in the house == 0
if (house->sumOfDirt == 0) {
map<string, int> autoWinScore;
autoWinScore["simulation_steps"] = 0;
autoWinScore["winner_num_steps"] = 0;
autoWinScore["this_num_steps"] = 0;
autoWinScore["sum_dirt_in_house"] = 0;
autoWinScore["dirt_collected"] = 0;
autoWinScore["is_back_in_docking"] = 1;
autoWinScore["actual_position_in_competition"] = 1;
auto nameIterator = registrar.getAlgorithmNames().begin();
for (int i = 0; i < numOfAlgorithms; i++) {
if (score_function != NULL) {
scores[*nameIterator][houseIndex] = (*score_function)(autoWinScore);
}
else {
scores[*nameIterator][houseIndex] = score(autoWinScore);
}
}
return; // that's all
}
// new instance of all algorithms
AlgorithmRegistrar& curRegistrar = AlgorithmRegistrar::getInstance();
auto algorithms = curRegistrar.getAlgorithms();
auto& algorithmNames = curRegistrar.getAlgorithmNames();
auto nameIterator = algorithmNames.begin();
vector<bool> if_end(numOfAlgorithms);
vector<bool> into_wall(numOfAlgorithms);
vector<int> curBattery(numOfAlgorithms);
vector<int> numSteps(numOfAlgorithms);
vector<int> positionInComp(numOfAlgorithms, 10); // default position is 10
vector<Direction> lastMoves(numOfAlgorithms, Direction::Stay);
int simulation_num_steps = 0;
int max_steps = house->maxSteps;
int batteryCapacity = (config.find("BatteryCapacity"))->second;
int batteryConsumptionRate = (config.find("BatteryConsumptionRate"))->second;
int batteryRechargeRate = (config.find("BatteryRechargeRate"))->second;
for (int i = 0; i < numOfAlgorithms; i++)
curBattery[i] = batteryCapacity;
bool is_winner = false;
int winner_num_steps = -1;
int cur_stage_winners = 0;
int cur_position = 1;
int finished = 0;
bool already_alerted_more_steps = false;
int algIndex;
// make a copy of the current house for every algorithm and assign a sensor to it
unique_ptr<House[]> curHouses = make_unique<House[]>(numOfAlgorithms);
vector<Sensor> sensors;
for (int l = 0; l < numOfAlgorithms; l++) {
copyHouse(curHouses[l], house.get());
sensors.emplace_back(Sensor(&curHouses[l]));
}
int i = 0;
for (auto& algorithm : algorithms)
{
algorithm->setSensor(sensors[i]);
algorithm->setConfiguration(config);
i++;
}
if (DEBUG) {
// print the house with D (no R) - for debugging
string space = " ";
if (house->rows > 60 || house->cols > 40)
space = "";
if (house->matrix != NULL) {
for (int i = 0; i < house->rows; i++) {
for (int j = 0; j < house->cols; j++) {
cout << house->matrix[i][j] << space;
}
cout << endl;
}
}
// print with robot R (no D)
cout << endl;
printHouseWithRobot(curHouses[0]); // house == curHouses[0] == ... == curHouses[numOfAlgorithms-1]
}
while (true) {
simulation_num_steps++;
if (SHOW_SIMULATION_HOUSES) {
getchar();
cout << "Step " << simulation_num_steps << endl;
}
// simulate one step for each algorithm
algIndex = -1;
nameIterator = algorithmNames.begin();
for (auto& algorithm : algorithms) {
// increase for the next algorithm
algIndex++;
if (if_end[algIndex] == true) {
nameIterator++;
continue;
}
// pass last move of the algorithm and update the new one
Direction direction = algorithm->step(lastMoves[algIndex]);
lastMoves[algIndex] = direction;
// if leaving docking station -> load battery
if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] == 'D') {
curBattery[algIndex] = MIN(batteryCapacity, curBattery[algIndex] + batteryRechargeRate);
}
// consume battery only if did not start the move from the docking station
// staying or starting the move from the docking station does not consume battery
if (curHouses[algIndex].robot != curHouses[algIndex].docking)
curBattery[algIndex] -= batteryConsumptionRate;
// make the step on the current house of the algorithm
switch (direction)
{
case static_cast<Direction>(0) :
curHouses[algIndex].robot.col++;
break;
case static_cast<Direction>(1) :
curHouses[algIndex].robot.col--;
break;
case static_cast<Direction>(2) :
curHouses[algIndex].robot.row++;
break;
case static_cast<Direction>(3) :
curHouses[algIndex].robot.row--;
break;
default:
break;
// do nothing for 'Stay'
}
//cleaning dust when entering a cell
if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] > '0' && curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] <= '9') {
curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] = curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] - 1;
curHouses[algIndex].sumOfDirt--;
}
// create a snapshot of the current house if desired && there was no previous error with creating a folder for the current alg+home
// note that we do take a snapshot of the case in which a robot gets into a wall (that's ok as amir said)
if (isFlagVideoUp && !curHouses[algIndex].folderError) {
curHouses[algIndex].montage(*nameIterator, videoErrors);
}
// walked into a wall -> stop the algorithm from running. its score will be zero
if (curHouses[algIndex].matrix[curHouses[algIndex].robot.row][curHouses[algIndex].robot.col] == 'W') {
into_wall[algIndex] = true;
if_end[algIndex] = true;
finished++;
// make the error note to be printed later (at the end after all other errors)
int index = static_cast<int>((*nameIterator).find(".so"));
string name = (*nameIterator).substr(0, index);
string wallError = "Algorithm ";
wallError += name;
wallError += " when running on House ";
index = static_cast<int>(curHouses[algIndex].houseFileName.find_last_of('.'));
name = (curHouses[algIndex].houseFileName).substr(0, index);
name = name.substr(6, name.size() - 6);
wallError += name;
wallError += " went on a wall in step ";
wallError += to_string(simulation_num_steps);
walkingIntoWallsErrors[houseIndex] = wallError;
algorithmIntoWall = true;
if (DEBUG)
cout << INTO_WALL << endl;
nameIterator++;
continue;
}
// for debug purpose
if (SHOW_SIMULATION_HOUSES) {
cout << "Robot(" << (*nameIterator) << ") Battery: " << curBattery[algIndex] << endl;
printHouseWithRobot(curHouses[algIndex]);
}
if (curHouses[algIndex].sumOfDirt == 0 && curHouses[algIndex].robot == curHouses[algIndex].docking) {
if (DEBUG)
cout << "Robot wins (cleaned the whole house in the limited time)." << endl; // for debug purpose
if_end[algIndex] = true;
cur_stage_winners++;
if (!is_winner) {
is_winner = true;
winner_num_steps = simulation_num_steps;
max_steps = MIN(max_steps, simulation_num_steps + config["MaxStepsAfterWinner"]);
}
finished++;
positionInComp[algIndex] = cur_position;
numSteps[algIndex] = simulation_num_steps;
nameIterator++;
continue;
}
if (curBattery[algIndex] <= 0) {
if (DEBUG)
cout << BATTERY_DEAD << endl; // for debug purpose
if_end[algIndex] = true;
finished++;
nameIterator++;
continue;
}
nameIterator++;
}
// finished one step of each algorithm
if (cur_stage_winners > 0)
cur_position = cur_position + cur_stage_winners;
cur_stage_winners = 0;
// - MAX-STEPS-AFTER-WINNNER ALERT -
if (!already_alerted_more_steps) {
// let all the other algorithms (that did not win in the this last move) know 'MaxStepsAfterWinner'
// alret them only at the first round when some algorithm wins
// the condition is true ONLY on the first round when some algorithm wins
if (winner_num_steps == simulation_num_steps) {
// if someone wins, max_steps is already updated in the loop, so it's simply a subtraction
int alert_more_steps = max_steps - simulation_num_steps;
algIndex = 0;
for (auto& algorithm : algorithms) {
// alert only algorithms that did not win / die (they're battery finished before charging)
if (if_end[algIndex] == false) {
algorithm->aboutToFinish(alert_more_steps);
}
algIndex++;
}
already_alerted_more_steps = true;
if (DEBUG)
cout << endl << "ALERT TO ALL ALGORITHMS: more steps = " << alert_more_steps << endl;
}
// other case -> no one won but there are 'maxstepsafterwinner' more steps till the end
// alert all algorithms
else if (!is_winner && ((max_steps - simulation_num_steps) == config["MaxStepsAfterWinner"])) {
algIndex = 0;
for (auto& algorithm : algorithms) {
// alert only algorithms that did not die (they're battery finished before charging / went into a wall)
if (if_end[algIndex] == false) {
algorithm->aboutToFinish(config["MaxStepsAfterWinner"]);
}
algIndex++;
}
already_alerted_more_steps = true;
if (DEBUG)
cout << endl << "ALERT TO ALL ALGORITHMS: more steps = " << config["MaxStepsAfterWinner"] << endl;
}
}
// end of game
if (finished == numOfAlgorithms || simulation_num_steps == max_steps) {
if (DEBUG)
cout << NO_MORE_MOVES << endl; // for debug purpose
for (algIndex = 0; algIndex < numOfAlgorithms; algIndex++) {
// if didn't change -> he didn't win. set number of steps to simulation steps
if (numSteps[algIndex] == 0) {
numSteps[algIndex] = simulation_num_steps;
}
}
break;
}
}
// score the algorithms on the house
// if none won -> winner num steps = simulation num steps
// call the right score method (Score.h or calc_score from the loaded score_formula.so)
// (call calc_score if score_loaded==true (field in Simulator.h) and call the score method in Score.h when it's false)
if (winner_num_steps == -1)
winner_num_steps = simulation_num_steps;
nameIterator = algorithmNames.begin();
for (int algIndex = 0; algIndex < numOfAlgorithms; algIndex++) {
int is_back_in_docking = (curHouses[algIndex].robot == curHouses[algIndex].docking) ? true : false;
if (into_wall[algIndex] == true) // if walked into a wall, score=0
scores[*nameIterator][houseIndex] = 0;
else {
map<string, int> score_params;
score_params["simulation_steps"] = simulation_num_steps;
score_params["winner_num_steps"] = winner_num_steps;
score_params["this_num_steps"] = numSteps[algIndex];
score_params["sum_dirt_in_house"] = curHouses[algIndex].initialSumOfDirt;
score_params["dirt_collected"] = curHouses[algIndex].initialSumOfDirt - curHouses[algIndex].sumOfDirt;
score_params["is_back_in_docking"] = is_back_in_docking;
if (curHouses[algIndex].sumOfDirt == 0 && is_back_in_docking) {
score_params["actual_position_in_competition"] = positionInComp[algIndex];
}
else {
score_params["actual_position_in_competition"] = 10;
}
if (score_function != NULL) {
scores[*nameIterator][houseIndex] = (*score_function)(score_params);
}
else {
scores[*nameIterator][houseIndex] = score(score_params);
}
if (scores[*nameIterator][houseIndex] == -1)
isErrorInScoreCalc = true;
}
nameIterator++;
}
// create the videos for all the algorithms on the house
if (isFlagVideoUp)
{
algIndex = 0;
nameIterator = algorithmNames.begin();
string simulationDir;
string imagesExpression;
while (nameIterator != algorithmNames.end()) {
// make a video only when the folder creation was successfull
if (!curHouses[algIndex].folderError) {
// image creation error
if (curHouses[algIndex].imageErrors > 0) {
string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": the creation of " + to_string(curHouses[algIndex].imageErrors) + " images was failed";
videoErrors.push_back(error_msg);
}
// make a video only when at least one snapshot was created successfully
if (curHouses[algIndex].createVideo) {
simulationDir = "simulations/" + *nameIterator + "_" + curHouses[algIndex].houseFileName + "/";
imagesExpression = simulationDir + "image%5d.jpg";
if (Encoder::encode(imagesExpression, *nameIterator + "_" + curHouses[algIndex].houseFileName + ".mpg")) { // video creation fail
string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": video file creation failed";
videoErrors.push_back(error_msg);
}
}
// remove the folder with all its content (images)
removeDirectory(curHouses[algIndex].imagesDirPath);
// optional: if removing folder fails -> add to errors
/*
if (removeDirectory(curHouses[algIndex].imagesDirPath)) {
string error_msg = "Error: In the simulation " + *nameIterator + ", " + curHouses[algIndex].houseFileName + ": removing folder " + curHouses[algIndex].imagesDirPath + " failed";
videoErrors.push_back(error_msg);
}
*/
}
nameIterator++;
algIndex++;
}
}
if (DEBUG)
getchar();
}
int main(int argc, const char *argv[])
{
Json::Value options;
char usage[] = "Usage: main [jobNum] config1 [config2 ...]";
if (argc <= 1) {
std::cerr << "Too few arguments" << std::endl;
std::cerr << usage << std::endl;
return 1;
} else if ((std::string(argv[1]) == "-h") || (std::string(argv[1]) == "--help")) {
std::cout << usage << std::endl;
return 0;
}
unsigned int configStart = 1;
int jobNum = -1;
// try to interpret the first arg as a job number
bool isJobNum = true;
for (int i = 0; argv[1][i] != '\0'; i++) {
if (!isdigit(argv[1][i])) {
isJobNum = false;
break;
}
}
if (isJobNum) {
jobNum = atoi(argv[1]);
configStart++;
}
for (int i = configStart; i < argc; i++) {
if (! readJson(argv[i],options)) {
return 1;
}
}
int numTrials = options.get("trials",1).asUInt();
int startTrial = 0;
int origNumTrials = numTrials;
unsigned int numTrialsPerJob = options.get("trialsPerJob",1).asUInt();
unsigned int maxNumStepsPerEpisode = options.get("maxNumStepsPerEpisode",10000).asUInt();
if (jobNum < 0) {
jobNum = 0;
} else {
startTrial = jobNum * numTrialsPerJob;
numTrials = min((int)numTrialsPerJob,numTrials-startTrial);
}
if (numTrials <= 0) {
std::cerr << "ERROR: insufficient number of trials: " << numTrials << std::endl;
std::cerr << "Calculated from: jobNum: " << jobNum << " numTrialsPerJob: " << numTrialsPerJob << " numTrials: " << origNumTrials << std::endl;
std::cerr << "Start trial should be: " << startTrial << std::endl;
return 1;
}
replaceOptsDir(options);
if (jobNum == 0)
saveConfig(options);
replaceOptsJob(options,boost::lexical_cast<std::string>(jobNum));
unsigned int numEpisodes = options.get("numEpisodesPerTrial",1).asUInt();
bool displayDescriptionQ = options["verbosity"].get("description",true).asBool();
bool displaySummaryQ = options["verbosity"].get("summary",true).asBool();
bool displayObsQ = options["verbosity"].get("observation",true).asBool();
bool displayStepsPerEpisodeQ = options["verbosity"].get("stepsPerEpisode",true).asBool();
bool displayStepsPerTrialQ = options["verbosity"].get("stepsPerTrial",true).asBool();
std::string saveFilename = options["save"].get("results","").asString();
bool saveResultsQ = (saveFilename != "");
bool randomizeSeedQ = options.get("randomizeSeed",false).asBool();
// running for fixed lengths
unsigned int numStepsPerEpisode = options.get("numStepsPerEpisode",0).asUInt();
bool runForFixedLength = (numStepsPerEpisode != 0);
// get the output DT information
unsigned int outputDTSteps = options["verbosity"].get("dtsteps",0).asUInt();
std::string outputDTFilename = options["verbosity"].get("dtfile","").asString();
bool outputDTCSVQ = (outputDTFilename != "");
boost::shared_ptr<OutputDT> outputDT;
boost::shared_ptr<std::vector<Action::Type> > actions;
Observation obs;
double startTime = getTime();
std::vector<std::vector<unsigned int> > numSteps(numTrials,std::vector<unsigned int>(numEpisodes,0));
std::vector<std::vector<unsigned int> > numCaptures(numTrials,std::vector<unsigned int>(numEpisodes,0));
std::vector<std::vector<unsigned int> > *results = &numSteps;
if (runForFixedLength)
results = &numCaptures;
std::cout << "Running for " << numTrials << " trials" << std::endl;
unsigned int trialNum;
unsigned int randomSeed;
for (int trial = 0; trial < numTrials; trial++) {
trialNum = trial + startTrial;
if (randomizeSeedQ)
randomSeed = getTime() * 1000000 + 1000 * getpid() + trialNum; // hopefully random enough
else
randomSeed = trialNum;
//std::cout << "RANDOM SEED: " << randomSeed << std::endl;
Json::Value trialOptions(options);
replaceOptsTrial(trialOptions,trialNum);
boost::shared_ptr<World> world = createWorldAgents(randomSeed,trialNum,trialOptions);
boost::shared_ptr<const WorldModel> model = world->getModel();
std::cout << "Ad hoc agent ind: " << model->getAdhocInd() << std::endl;
// INITIALIZATION
if (trial == 0) {
if (displayDescriptionQ)
std::cout << world->generateDescription() << std::endl;
if (outputDTCSVQ) {
// set up the actions
actions = boost::shared_ptr<std::vector<Action::Type> >(new std::vector<Action::Type>(model->getNumAgents()));
// create models for the DT csv output if required
std::vector<std::string> modelNames;
//modelNames.push_back("GR");
//modelNames.push_back("TA");
//modelNames.push_back("GP");
//modelNames.push_back("PD");
outputDT = boost::shared_ptr<OutputDT>(new OutputDT(outputDTFilename,model->getDims(),model->getNumAgents()-1,modelNames,true,false,outputDTSteps));
}
}
if (outputDTCSVQ) {
if (outputDT->hasCollectedSufficientData()) {
std::cout << "WARNING: collected sufficient data, stopping with " << trial << " trials" << std::endl;
numSteps.resize(trial);
break;
}
}
if (displayStepsPerTrialQ)
std::cout << "trial " << std::setw(2) << trialNum << ": " << std::flush;
for (unsigned int episode = 0; episode < numEpisodes; episode++) {
world->randomizePositions();
world->restartAgents();
if (outputDTCSVQ) {
// for the first step, add the observation, since it keeps a history of 1
world->generateObservation(obs);
outputDT->saveStep(trial,numSteps[trial][episode],obs,*actions);
}
while (!model->isPreyCaptured()) {
numSteps[trial][episode]++;
// check end conditions
if (runForFixedLength) {
if (numSteps[trial][episode] > numStepsPerEpisode)
break;
} else {
if (numSteps[trial][episode] > maxNumStepsPerEpisode) {
std::cerr << "TRIAL " << trial << " EPISODE " << episode << " TOO LONG" << std::endl;
break;
}
}
if (displayObsQ) {
world->generateObservation(obs);
std::cout << obs << std::endl;
}
world->step(actions);
if (outputDTCSVQ){
world->generateObservation(obs); // should follow world->step so that we can extract the observed actions of the previous step
outputDT->saveStep(trial,numSteps[trial][episode],obs,*actions);
}
// if we want to run for a fixed length and the prey is captured, find a new position for the prey
if (runForFixedLength && model->isPreyCaptured()) {
//std::cout << "Prey is captured, generating new position" << std::endl;
world->randomizePreyPosition();
numCaptures[trial][episode]++;
}
} // while the episode lasts
if (displayObsQ) {
world->generateObservation(obs);
std::cout << obs << std::endl;
}
if (displayStepsPerEpisodeQ)
std::cout << std::setw(3) << (*results)[trial][episode] << " " << std::flush;
}
if (displayStepsPerTrialQ)
displayStepsPerTrial(displayStepsPerEpisodeQ,(*results)[trial]);
} // end for trial
double endTime = getTime();
// optionally display the summary
if (displaySummaryQ)
displaySummary(endTime-startTime,*results);
// optionally save the results
if (saveResultsQ)
saveResults(saveFilename,startTrial,*results);
// optionally finialize the saving of data for the DT
if (outputDTCSVQ)
outputDT->finalizeSave(randomSeed);
return 0;
}