void Box2DSystem::addWallsOnScreen()
{
float width = meters(window.getSize().x);
float height = meters(window.getSize().y);
float halfwidth = width / 2;
float halfheight = height / 2;
const float wallsz = 15_px;
createStaticBox(halfwidth, wallsz, halfwidth, wallsz); //Top wall
createStaticBox(wallsz, halfheight, wallsz, halfheight); //Left wall
createStaticBox(width-wallsz, halfheight, wallsz, halfheight);//Right wall
createStaticBox(halfwidth, height-wallsz, halfwidth, wallsz); //Bottom wall
}
void singlePlanner::displayPlan(const string &color)
{
printf("\n===============================\n");
cout<<"DISPLAY PLAN"<<endl;
int indexFirstObsSet = 0;
if (robot == "icubSim")
indexFirstObsSet = 2; // Do not display robot and table
else
indexFirstObsSet = 1; // Do not display robot
if (controlPoint == "End-effector")
{
for (int j=indexFirstObsSet; j<obsSet.size(); j++) // Do not display the robot itself
{
createStaticBox(obsSet[j],"obstacle");
}
}
// for (int i=0; i<bestTraj.size(); i++)
// {
// createStaticSphere(0.03, bestTraj[i], color);
// }
if (controlPoint == "End-effector")
createStaticBox(target,"goal");
}
