void prediction(int action) {
int current_state, previous_state;
for (current_state = 0; current_state < nb_state; current_state++) {
current_localization[current_state] = 0.0;
for (previous_state = 0; previous_state < nb_state; previous_state++) {
current_localization[current_state] += dynamic_model(current_state, previous_state, action) * previous_localization[previous_state];
}
}
}
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;
}
