int main()
{
//Motion planning algorithm parameters
glc::Parameters alg_params;
alg_params.res=16;
alg_params.control_dim = 2;
alg_params.state_dim = 2;
alg_params.depth_scale = 100;
alg_params.dt_max = 5.0;
alg_params.max_iter = 50000;
alg_params.time_scale = 20;
alg_params.partition_scale = 40;
alg_params.x0 = glc::vctr({0.0,0.0});
//Create a dynamic model
SingleIntegrator dynamic_model(alg_params.dt_max);
//Create the control inputs
ControlInputs2D controls(alg_params.res);
//Create the cost function
ArcLength performance_objective(4);
//Create instance of goal region
glc::vctr xg({10.0,10.0});
SphericalGoal goal(xg.size(),0.25,4);
goal.setGoal(xg);
//Create the obstacles
PlanarDemoObstacles obstacles(4);
//Create a heuristic for the current goal
EuclideanHeuristic heuristic(xg,goal.getRadius());
glc::GLCPlanner planner(&obstacles,
&goal,
&dynamic_model,
&heuristic,
&performance_objective,
alg_params,
controls.points);
//Run the planner and print solution
glc::PlannerOutput out;
planner.plan(out);
if(out.solution_found){
std::vector<glc::nodePtr> path = planner.pathToRoot(true);
glc::splinePtr solution = planner.recoverTraj( path );
glc::splineTensor coef(solution->coefficient_array);
glc::printSpline( solution , 20, "Solution");
glc::trajectoryToFile("shortest_path.txt","../plots/",solution,500);
glc::nodesToFile("shortest_path_nodes.txt","../plots/",planner.domain_labels);
}
return 0;
}
void KdTree::buildObstacleTree()
{
deleteObstacleTree(obstacleTree_);
std::vector<Obstacle*> obstacles(sim_->obstacles_.size());
for (size_t i = 0; i < sim_->obstacles_.size(); ++i) {
obstacles[i] = sim_->obstacles_[i];
}
obstacleTree_ = buildObstacleTreeRecursive(obstacles);
}
void TestAgent::obstacles()
{
std::auto_ptr<Kernel::Model> model(new Kernel::Model()) ;
model->init() ;
Kernel::Object* system = model->createObject() ;
system->addTrait(new Model::StellarSystem()) ;
system->addTrait(new Model::Positioned()) ;
Kernel::Object* ship1 = Model::createShip(system) ;
Kernel::Object* agent1 = Model::createAI(ship1) ;
Kernel::Object* ship2 = Model::createShip(system) ;
Kernel::Object* agent2 = Model::createAI(ship2) ;
Implementation::Agent* agent_controller1 = getAgentController(agent1) ;
std::set<Implementation::Vehicle*> obstacles(agent_controller1->getObstacles()) ;
CPPUNIT_ASSERT_EQUAL((unsigned int)1,obstacles.size()) ;
Implementation::Agent* agent_controller2 = getAgentController(agent2) ;
CPPUNIT_ASSERT(agent_controller2->getVehicle()) ;
CPPUNIT_ASSERT(obstacles.find(agent_controller2->getVehicle())!=obstacles.end()) ;
}
void RobotEnvironment::generateRandomScene(unsigned int& numObstacles)
{
scene_ = std::make_shared<frapu::Scene>();
std::vector<frapu::ObstacleSharedPtr> obstacles(numObstacles);
std::uniform_real_distribution<double> uniform_dist(-1.0, 4.0);
std::uniform_real_distribution<double> uniform_distZ(3.0, 6.0);
for (size_t i = 0; i < numObstacles; i++) {
double rand_x = uniform_dist(randomEngine_);
double rand_y = uniform_dist(randomEngine_);
double rand_z = uniform_distZ(randomEngine_);
frapu::TerrainSharedPtr terrain = std::make_shared<frapu::TerrainImpl>("t" + std::to_string(i),
0.0,
0.0,
false,
true);
std::string box_name = "b" + std::to_string(i);
obstacles[i] = std::make_shared<frapu::BoxObstacle>(box_name,
rand_x,
rand_y,
rand_z,
0.25,
0.25,
0.25,
terrain);
std::vector<double> diffuseColor( {0.5, 0.5, 0.5, 0.5});
static_cast<frapu::ObstacleImpl*>(obstacles[obstacles.size() - 1].get())->setStandardColor(diffuseColor, diffuseColor);
std::vector<double> dims( {rand_x, rand_y, rand_z, 0.25, 0.25, 0.25});
robot_->addBox(box_name, dims);
}
scene_->setObstacles(obstacles);
cout << "random scene created " << obstacles.size();
}
