371 lines
11 KiB
C
371 lines
11 KiB
C
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdbool.h>
|
|
|
|
#include <math.h>
|
|
|
|
// for sleep !
|
|
/*
|
|
#ifdef __linux__
|
|
#include <unistd.h>
|
|
#elif _WIN32
|
|
#include <windows.h>
|
|
#endif
|
|
*/
|
|
|
|
#include "ftest/ftest.h"
|
|
#include "ftest/ftest_array.h"
|
|
#include "fmock/fmock.h"
|
|
|
|
|
|
//#include "permutation_t/permutation_t.h"
|
|
#include "neuron_t/neuron_t.h"
|
|
|
|
#include "vehicle.h"
|
|
#include "learn_to_drive.h"
|
|
|
|
TEST(create_coordenate){
|
|
coordinate * coord = create_coordinate(3);
|
|
coord->x[0]=0;
|
|
coord->x[1]=1.2;
|
|
coord->x[0]=0.8;
|
|
|
|
free_coordinate(coord);
|
|
}
|
|
|
|
TEST(create_blocks){
|
|
size_t nb_block = 7;
|
|
size_t dim= 2;
|
|
struct blocks * zn = create_blocks(nb_block, dim);
|
|
|
|
for(size_t i=0; i<nb_block; ++i){
|
|
for(size_t j=0; j<dim; ++j){
|
|
zn->lower_bound_block[i]->x[j] = 0;
|
|
zn->upper_bound_block[i]->x[j] = (i+1)*(j+1);
|
|
}
|
|
}
|
|
|
|
|
|
for(size_t i=0; i<nb_block; ++i){
|
|
LOG("%s"," || (");
|
|
for(size_t j=0; j<dim; ++j)
|
|
LOG(" %f ", zn->lower_bound_block[i]->x[j]);
|
|
LOG("%s",")\\ (");
|
|
for(size_t j=0; j<dim; ++j)
|
|
LOG(" %f ", zn->upper_bound_block[i]->x[j]);
|
|
LOG("%s",")");
|
|
|
|
}
|
|
LOG("\n");
|
|
|
|
free_blocks(zn);
|
|
|
|
}
|
|
|
|
TEST(is_in_blocks){
|
|
size_t nb_block = 3;
|
|
size_t dim= 2;
|
|
struct blocks * zn = create_blocks(nb_block, dim);
|
|
|
|
copy_coordinate(zn->lower_bound_block[0], (float[]){0,0});
|
|
copy_coordinate(zn->upper_bound_block[0], (float[]){2,2});
|
|
copy_coordinate(zn->lower_bound_block[1], (float[]){2,0});
|
|
copy_coordinate(zn->upper_bound_block[1], (float[]){4,4});
|
|
copy_coordinate(zn->lower_bound_block[2], (float[]){0,4});
|
|
copy_coordinate(zn->upper_bound_block[2], (float[]){6,7});
|
|
|
|
coordinate *coord = create_coordinate(2);
|
|
|
|
copy_coordinate(coord, (float[]){1,1});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
|
|
copy_coordinate(coord, (float[]){1,5});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
|
|
copy_coordinate(coord, (float[]){3,3});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
|
|
copy_coordinate(coord, (float[]){1,3});
|
|
EXPECT_FALSE(is_in_blocks(zn, coord));
|
|
|
|
print2D_blocks(zn,0.4,0.6, '.');
|
|
|
|
|
|
free_blocks(zn);
|
|
free_coordinate(coord);
|
|
|
|
|
|
}
|
|
|
|
|
|
TEST(print_blocks_withPoint){
|
|
size_t nb_block = 7;
|
|
size_t dim= 2;
|
|
struct blocks * zn = create_blocks(nb_block, dim);
|
|
|
|
/*
|
|
copy_coordinate(zn->lower_bound_block[0], (float[]){0,0});
|
|
copy_coordinate(zn->upper_bound_block[0], (float[]){8,2});
|
|
copy_coordinate(zn->lower_bound_block[1], (float[]){8,0});
|
|
copy_coordinate(zn->upper_bound_block[1], (float[]){12,4});
|
|
copy_coordinate(zn->lower_bound_block[2], (float[]){0,4});
|
|
copy_coordinate(zn->upper_bound_block[2], (float[]){14,7});
|
|
copy_coordinate(zn->lower_bound_block[3], (float[]){15,2});
|
|
copy_coordinate(zn->upper_bound_block[3], (float[]){18,7});
|
|
*/
|
|
|
|
copy_coordinate(zn->lower_bound_block[0], (float[]){0,0});
|
|
copy_coordinate(zn->upper_bound_block[0], (float[]){2,7});
|
|
copy_coordinate(zn->lower_bound_block[1], (float[]){2,0});
|
|
copy_coordinate(zn->upper_bound_block[1], (float[]){4,2});
|
|
copy_coordinate(zn->lower_bound_block[2], (float[]){4,1});
|
|
copy_coordinate(zn->upper_bound_block[2], (float[]){8,3});
|
|
copy_coordinate(zn->lower_bound_block[3], (float[]){8,0});
|
|
copy_coordinate(zn->upper_bound_block[3], (float[]){16,2});
|
|
copy_coordinate(zn->lower_bound_block[4], (float[]){16,0});
|
|
copy_coordinate(zn->upper_bound_block[4], (float[]){18,7});
|
|
copy_coordinate(zn->lower_bound_block[5], (float[]){6,7});
|
|
copy_coordinate(zn->upper_bound_block[5], (float[]){18,9});
|
|
copy_coordinate(zn->lower_bound_block[6], (float[]){2,6});
|
|
copy_coordinate(zn->upper_bound_block[6], (float[]){6,8});
|
|
|
|
|
|
coordinate *coord = create_coordinate(2);
|
|
|
|
copy_coordinate(coord, (float[]){1,1});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
|
|
print2D_blocks_withPoint(zn,0.24,0.48, '#',coord);
|
|
//print2D_blocks(zn,0.14,0.28, '#');
|
|
|
|
copy_coordinate(coord, (float[]){1,5});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
// print2D_blocks_withPoint(zn,0.24,0.48, '#',coord);
|
|
|
|
copy_coordinate(coord, (float[]){2,3});
|
|
EXPECT_TRUE(is_in_blocks(zn, coord));
|
|
// print2D_blocks_withPoint(zn,0.24,0.48, '#',coord);
|
|
|
|
copy_coordinate(coord, (float[]){7,5});
|
|
EXPECT_FALSE(is_in_blocks(zn, coord));
|
|
|
|
|
|
|
|
free_blocks(zn);
|
|
free_coordinate(coord);
|
|
|
|
|
|
}
|
|
|
|
#if 0
|
|
|
|
TEST(first_vehicle){
|
|
size_t nb_block = 7;
|
|
size_t dim= 2;
|
|
struct blocks * path = create_blocks(nb_block, dim);
|
|
|
|
copy_coordinate(path->lower_bound_block[0], (float[]){0,0});
|
|
copy_coordinate(path->upper_bound_block[0], (float[]){2,7});
|
|
copy_coordinate(path->lower_bound_block[1], (float[]){2,0});
|
|
copy_coordinate(path->upper_bound_block[1], (float[]){4,2});
|
|
copy_coordinate(path->lower_bound_block[2], (float[]){4,0.5});
|
|
copy_coordinate(path->upper_bound_block[2], (float[]){8,3});
|
|
copy_coordinate(path->lower_bound_block[3], (float[]){8,0});
|
|
copy_coordinate(path->upper_bound_block[3], (float[]){16,2});
|
|
copy_coordinate(path->lower_bound_block[4], (float[]){16,0});
|
|
copy_coordinate(path->upper_bound_block[4], (float[]){18,7});
|
|
copy_coordinate(path->lower_bound_block[5], (float[]){6,7});
|
|
copy_coordinate(path->upper_bound_block[5], (float[]){18,9});
|
|
copy_coordinate(path->lower_bound_block[6], (float[]){2,6});
|
|
copy_coordinate(path->upper_bound_block[6], (float[]){6,8});
|
|
|
|
update_bounds_limits_blocks(path);
|
|
|
|
struct vehicle *vhcl = create_vehicle(path);
|
|
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
free_vehicle(vhcl);
|
|
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
TEST(circle_path_vehicle){
|
|
size_t nb_block = 7;
|
|
size_t dim= 2;
|
|
struct blocks * path = create_blocks(nb_block, dim);
|
|
|
|
copy_coordinate(path->lower_bound_block[0], (float[]){0,3});
|
|
copy_coordinate(path->upper_bound_block[0], (float[]){4,7});
|
|
copy_coordinate(path->lower_bound_block[1], (float[]){1,0});
|
|
copy_coordinate(path->upper_bound_block[1], (float[]){10,3});
|
|
copy_coordinate(path->lower_bound_block[2], (float[]){10,0.5});
|
|
copy_coordinate(path->upper_bound_block[2], (float[]){14,5});
|
|
copy_coordinate(path->lower_bound_block[3], (float[]){14,2});
|
|
copy_coordinate(path->upper_bound_block[3], (float[]){18,7});
|
|
copy_coordinate(path->lower_bound_block[4], (float[]){11,7});
|
|
copy_coordinate(path->upper_bound_block[4], (float[]){17,10});
|
|
copy_coordinate(path->lower_bound_block[5], (float[]){8,6});
|
|
copy_coordinate(path->upper_bound_block[5], (float[]){11,9.75});
|
|
copy_coordinate(path->lower_bound_block[6], (float[]){1,7});
|
|
copy_coordinate(path->upper_bound_block[6], (float[]){8,9.75});
|
|
|
|
update_bounds_limits_blocks(path);
|
|
|
|
struct vehicle *vhcl = create_vehicle(path);
|
|
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
step_vehicle(vhcl, CENTER);
|
|
Sleep(200);
|
|
print_vehicle_n_path(vhcl, 0.2,0.4);
|
|
|
|
free_vehicle(vhcl);
|
|
|
|
|
|
}
|
|
|
|
TEST(reward_list){
|
|
struct status_qlearning * l_reward = create_status_qlearning();
|
|
|
|
free_status_qlearning(l_reward);
|
|
}
|
|
|
|
#if 1
|
|
TEST(first_learn_vehicle){
|
|
size_t nb_block = 7;
|
|
size_t dim= 2;
|
|
struct blocks * path = create_blocks(nb_block, dim);
|
|
|
|
|
|
|
|
#if 1
|
|
copy_coordinate(path->lower_bound_block[0], (float[]){0,3});
|
|
copy_coordinate(path->upper_bound_block[0], (float[]){4,7});
|
|
copy_coordinate(path->lower_bound_block[1], (float[]){1,0});
|
|
copy_coordinate(path->upper_bound_block[1], (float[]){10,3});
|
|
copy_coordinate(path->lower_bound_block[2], (float[]){10,0.5});
|
|
copy_coordinate(path->upper_bound_block[2], (float[]){14,5});
|
|
copy_coordinate(path->lower_bound_block[3], (float[]){14,2});
|
|
copy_coordinate(path->upper_bound_block[3], (float[]){18,7});
|
|
copy_coordinate(path->lower_bound_block[4], (float[]){11,7});
|
|
copy_coordinate(path->upper_bound_block[4], (float[]){17,10});
|
|
copy_coordinate(path->lower_bound_block[5], (float[]){8,6});
|
|
copy_coordinate(path->upper_bound_block[5], (float[]){11,9.75});
|
|
copy_coordinate(path->lower_bound_block[6], (float[]){1,7});
|
|
copy_coordinate(path->upper_bound_block[6], (float[]){8,9.75});
|
|
|
|
#else
|
|
|
|
|
|
copy_coordinate(path->lower_bound_block[0], (float[]){0,0});
|
|
copy_coordinate(path->upper_bound_block[0], (float[]){2,7});
|
|
copy_coordinate(path->lower_bound_block[1], (float[]){2,0});
|
|
copy_coordinate(path->upper_bound_block[1], (float[]){4,2});
|
|
copy_coordinate(path->lower_bound_block[2], (float[]){4,0.5});
|
|
copy_coordinate(path->upper_bound_block[2], (float[]){8,3});
|
|
copy_coordinate(path->lower_bound_block[3], (float[]){8,0});
|
|
copy_coordinate(path->upper_bound_block[3], (float[]){16,2});
|
|
copy_coordinate(path->lower_bound_block[4], (float[]){16,0});
|
|
copy_coordinate(path->upper_bound_block[4], (float[]){18,7});
|
|
copy_coordinate(path->lower_bound_block[5], (float[]){6,7});
|
|
copy_coordinate(path->upper_bound_block[5], (float[]){18,9});
|
|
copy_coordinate(path->lower_bound_block[6], (float[]){2,6});
|
|
copy_coordinate(path->upper_bound_block[6], (float[]){6,8});
|
|
#endif
|
|
|
|
update_bounds_limits_blocks(path);
|
|
|
|
struct vehicle *car = create_vehicle(path);
|
|
|
|
config_layers *pconf = create_config_layers_from_OneD(4,(size_t[]){3,24,24,3}); /* 3 input , 3 target; 2 hidden layer with 24 neurons each */
|
|
|
|
bool randomize=true;
|
|
float minR = 0, maxR = 1;
|
|
int randomRange = 500;
|
|
size_t nb_prod_thread = 2;
|
|
size_t nb_calc_thread = 4;
|
|
float learning_rate = 0.001;
|
|
struct networks_qlearning *nnetworks = create_nework_qlearning(
|
|
pconf,
|
|
randomize, minR, maxR, randomRange,
|
|
nb_prod_thread, nb_calc_thread,
|
|
learning_rate
|
|
);
|
|
|
|
struct status_qlearning *qlstatus = create_status_qlearning ();
|
|
struct delay_params *dly = create_delay_params (
|
|
200/*size_t delay_between_episodes*/,
|
|
20/*size_t delay_between_games*/
|
|
);
|
|
|
|
struct qlearning_params *qlparams = create_qlearning_params (
|
|
0.95/*float gamma*/,
|
|
learning_rate,
|
|
0 /* (not used!)float discount_factor*/,
|
|
0.99/*float exploration_factor*/,
|
|
20/*long int nb_training_before_update_weight_in_target*/,
|
|
10000/*size_t number_episodes*/
|
|
);
|
|
struct print_params *pprint = create_print_params(
|
|
0.2/*float scale_x*/,0.4 /*float scale_y*/,
|
|
dly/*struct delay_params * dly_p*/
|
|
);
|
|
|
|
struct RL_agent *rlAgent = create_RL_agent (
|
|
nnetworks /*struct networks_qlearning * networks*/,
|
|
car /*struct vehicle * car*/,
|
|
qlstatus /*struct status_qlearning * status*/,
|
|
pprint /*struct print_params * pprint*/,
|
|
qlparams/*struct qlearning_params *qlearnParams*/
|
|
);
|
|
|
|
learn_to_drive(rlAgent);
|
|
|
|
free_RL_agent(rlAgent);
|
|
|
|
|
|
|
|
|
|
}
|
|
#endif
|
|
|
|
|
|
int main(int argc, char **argv){
|
|
|
|
|
|
run_all_tests_args(argc, argv);
|
|
|
|
return 0;
|
|
}
|