void
NFQ_train(struct fann *qlearning_ann, int num_data, state *s, action *a, carmen_simulator_ackerman_config_t *simulator_config)
{
int k = 0; int b; int N = num_data - 1;
double gamma = 0.3, target;
static struct fann_train_data *ann_data;
static double max_atan_curvature;
if (ann_data == NULL)
{
initialize_ann_data(&ann_data, N*3, 4, 1);
max_atan_curvature = atan(compute_curvature(carmen_degrees_to_radians(30.0), simulator_config));
}
do
{
target = c(s[k], a[k], s[k+1]) + gamma * min_b_Q(&b, qlearning_ann, s[k+1], a[k+1], simulator_config);
ann_data->input[k][0] = s[k].atan_desired_curvature / max_atan_curvature;
ann_data->input[k][1] = s[k].atan_current_curvature / max_atan_curvature;
ann_data->input[k][2] = (double) s[k].b / 6.0;
ann_data->input[k][3] = a[k].steering_command / 100.0;
ann_data->output[k][0] = target;
//fprintf(stdout, "[%d] d: %.5lf c: %.5lf b: %.5lf (%d') s: %.5lf t: %.5lf\n",
// k, ann_data->input[k][0], ann_data->input[k][1], ann_data->input[k][2]*10, b, ann_data->input[k][3]*100, ann_data->output[k][0]);
k++;
} while(k < N);
//(ann, data, max_epochs, epochs_between_reports, desired_error)
fann_train_on_data(qlearning_ann, ann_data, 200, 0, 0.001);
}
double
compute_new_phi_with_ann_new(carmen_simulator_ackerman_config_t *simulator_config)
{
static double steering_command = 0.0;
double atan_current_curvature, atan_desired_curvature;
static fann_type steering_ann_input[NUM_STEERING_ANN_INPUTS];
fann_type *steering_ann_output __attribute__ ((unused));
static struct fann *steering_ann = NULL;
if (steering_ann == NULL)
{
steering_ann = fann_create_from_file("steering_ann.net");
if (steering_ann == NULL)
{
printf("Error: Could not create steering_ann\n");
exit(1);
}
init_steering_ann_input(steering_ann_input);
}
atan_current_curvature = atan(compute_curvature(simulator_config->phi, simulator_config));
atan_desired_curvature = atan(compute_curvature(simulator_config->target_phi, simulator_config));
compute_steering_with_qlearning(&steering_command,
atan_desired_curvature, atan_current_curvature, simulator_config->delta_t, simulator_config);
build_steering_ann_input(steering_ann_input, steering_command, atan_current_curvature);
steering_ann_output = fann_run(steering_ann, steering_ann_input);
// Alberto: O ganho de 1.05 abaixo foi necessario pois a rede nao estava gerando curvaturas mais extremas
// que nao aparecem no treino, mas apenas rodando livremente na simulacao
//simulator_config->phi = 1.05 * get_phi_from_curvature(tan(steering_ann_output[0]), simulator_config);
simulator_config->phi += steering_command / 500.0;
simulator_config->phi = carmen_clamp(carmen_degrees_to_radians(-30.0), simulator_config->phi , carmen_degrees_to_radians(30.0));
if (simulator_config->delta_t != 0 && atan_desired_curvature != 0.0)
fprintf(stdout, "d_phi: %.5lf phi: %.5lf\n", simulator_config->target_phi, simulator_config->phi);
return (simulator_config->phi);
}
double
compute_new_phi(carmen_simulator_ackerman_config_t *simulator_config)
{
double current_curvature;
double desired_curvature;
double delta_curvature;
double minimum_time_to_reach_desired_curvature;
current_curvature = compute_curvature(simulator_config->phi, simulator_config);
desired_curvature = compute_curvature(simulator_config->target_phi, simulator_config);
delta_curvature = desired_curvature - current_curvature;
minimum_time_to_reach_desired_curvature = fabs(delta_curvature / simulator_config->maximum_steering_command_rate);
if (minimum_time_to_reach_desired_curvature > simulator_config->delta_t)
{
if (delta_curvature < 0.0)
current_curvature -= simulator_config->maximum_steering_command_rate * simulator_config->delta_t;
else
current_curvature += simulator_config->maximum_steering_command_rate * simulator_config->delta_t;
}
else
current_curvature = desired_curvature;
if (fabs(current_curvature) > simulator_config->maximum_capable_curvature)
{
current_curvature = current_curvature / fabs(current_curvature);
current_curvature *= simulator_config->maximum_capable_curvature;
}
simulator_config->phi = get_phi_from_curvature(current_curvature, simulator_config);
if (fabs(simulator_config->phi) > simulator_config->max_phi)
{
simulator_config->phi = simulator_config->phi/fabs(simulator_config->phi);
simulator_config->phi *= simulator_config->max_phi;
}
//printf("desired_phi = %lf, phi = %lf\n", simulator_config->target_phi, simulator_config->phi);
return (simulator_config->phi);
}
double
compute_new_phi_with_ann(carmen_simulator_ackerman_config_t *simulator_config)
{
static double steering_command = 0.0;
double atan_current_curvature;
double atan_desired_curvature;
static fann_type steering_ann_input[NUM_STEERING_ANN_INPUTS];
fann_type *steering_ann_output;
static struct fann *steering_ann = NULL;
if (steering_ann == NULL)
{
steering_ann = fann_create_from_file("steering_ann.net");
if (steering_ann == NULL)
{
printf("Error: Could not create steering_ann\n");
exit(1);
}
init_steering_ann_input(steering_ann_input);
}
atan_current_curvature = atan(compute_curvature(simulator_config->phi, simulator_config));
atan_desired_curvature = atan(compute_curvature(simulator_config->target_phi, simulator_config));
carmen_ford_escape_hybrid_steering_PID_controler(&steering_command,
atan_desired_curvature, atan_current_curvature, simulator_config->delta_t);
build_steering_ann_input(steering_ann_input, steering_command, atan_current_curvature);
steering_ann_output = fann_run(steering_ann, steering_ann_input);
// Alberto: O ganho de 1.05 abaixo foi necessario pois a rede nao estava gerando curvaturas mais extremas
// que nao aparecem no treino mas apenas rodando livremente na simulacao
simulator_config->phi = 1.05 * get_phi_from_curvature(tan(steering_ann_output[0]), simulator_config);
return (simulator_config->phi);
}
EXPORT void initialize_curvature(
Array3<double> level_set, // level-set field
Array3<double> curvature, // interface curvature
Array3<double> unsmoothed_curvature, // interface curvature without smoothing
int number_primary_cells_i, // number of primary (pressure) cells in x1 direction
int number_primary_cells_j, // number of primary (pressure) cells in x2 direction
int number_primary_cells_k, // number of primary (pressure) cells in x3 direction
double mesh_width_x1, // grid spacing in x1 direction (uniform)
double mesh_width_x2, // grid spacing in x2 direction (uniform)
double mesh_width_x3, // grid spacing in x3 direction (uniform)
int apply_curvature_smoothing, // =1, apply curvature smoothing
// =0, use unsmoothed curvature
int number_curvature_smoothing_steps, // number of iterations applied in the
// curvature smoothing algorithm
int apply_curvature_smoothing_filter // =1, apply curvature smoothing filter
// =0, do not apply curvature smoothing filter
)
{
/* compute the curvature */
compute_curvature( level_set, unsmoothed_curvature,
number_primary_cells_i, number_primary_cells_j, number_primary_cells_k,
mesh_width_x1, mesh_width_x2, mesh_width_x3);
/* smooth the curvature if necessary */
if(apply_curvature_smoothing)
{
/* apply smoothing algorithm */
smooth_curvature(level_set, curvature, unsmoothed_curvature,
number_primary_cells_i, number_primary_cells_j, number_primary_cells_k,
mesh_width_x1, mesh_width_x2, mesh_width_x3,
apply_curvature_smoothing_filter, number_curvature_smoothing_steps);
}
else
{
/* don't bother with smoothing */
copy_cell_centered_field(unsmoothed_curvature, curvature,
number_primary_cells_i, number_primary_cells_j, number_primary_cells_k);
}
}
double
run_qlearning_network(struct fann *qlearning_ann,
double atan_desired_curvature, double atan_current_curvature, double steering_command, double b, carmen_simulator_ackerman_config_t *simulator_config)
{
fann_type ann_input[4], *ann_output;
static double max_atan_curvature;
static int first_time = 1;
if (first_time)
{
max_atan_curvature = atan(compute_curvature(carmen_degrees_to_radians(30.0), simulator_config));
first_time = 0;
}
ann_input[0] = atan_desired_curvature / max_atan_curvature;
ann_input[1] = atan_current_curvature / max_atan_curvature;
ann_input[2] = b / 6.0;
ann_input[3] = steering_command / 100.0;
ann_output = fann_run(qlearning_ann, ann_input);
return ann_output[0];
}
