create deepReinforcementLearning repo, test print vehicle
This commit is contained in:
@@ -0,0 +1,24 @@
|
||||
#include "learn_to_drive.h"
|
||||
|
||||
|
||||
float reLU(float x){
|
||||
if(x>0) return x;
|
||||
return 0;
|
||||
}
|
||||
|
||||
float d_reLU(float x){
|
||||
if (x>0) return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
float L2(float t, float o){
|
||||
return (o - t) * (o - t)/2;
|
||||
}
|
||||
|
||||
float D_L2(float t, float o){
|
||||
return (o - t);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
#ifndef __LEARNING_VEHICLE__C_H____
|
||||
#define __LEARNING_VEHICLE__C_H____
|
||||
|
||||
//#include <pthread.h>
|
||||
|
||||
#include "neuron_t/neuron_t.h"
|
||||
|
||||
#include "list_t/list_t.h"
|
||||
|
||||
|
||||
#include "vehicle.h"
|
||||
|
||||
//float reLU(float x);
|
||||
|
||||
//float d_reLU(float x);
|
||||
|
||||
|
||||
struct qlearning_params {
|
||||
double learning_rate;
|
||||
double discount_factor;
|
||||
double exploration_factor;
|
||||
};
|
||||
|
||||
|
||||
struct reward_lists {
|
||||
struct main_list_TYPE_L_INT * list_main_cumul;
|
||||
struct main_list_TYPE_L_INT * list_target_cumul;
|
||||
struct main_list_TYPE_L_INT * progress_best_cumul;
|
||||
};
|
||||
|
||||
struct delay_params {
|
||||
size_t delay_between_episodes;
|
||||
size_t delay_between_games;
|
||||
};
|
||||
|
||||
struct networks_qlearning {
|
||||
config_layers *config;
|
||||
neurons_TYPE_FLOAT *main_net;
|
||||
neurons_TYPE_FLOAT *target_net;
|
||||
neurons_TYPE_FLOAT *best_net;
|
||||
};
|
||||
|
||||
struct RL_agent {
|
||||
struct networks_qlearning * networks;
|
||||
struct vehicle * car;
|
||||
struct reward_lists * rewards;
|
||||
struct delay_params * delay;
|
||||
struct qlearning_params *qlearnParams;
|
||||
|
||||
};
|
||||
|
||||
struct networks_qlearning * create_nework_qlearning(
|
||||
struct config_layers * config,
|
||||
bool randomize, float minR, float maxR, int randomRange
|
||||
);
|
||||
struct reward_lists * create_reward_lists ();
|
||||
struct delay_params * create_delay_params (
|
||||
size_t delay_between_episodes,
|
||||
size_t delay_between_games
|
||||
);
|
||||
|
||||
struct qlearning_params (
|
||||
double learning_rate,
|
||||
double discount_factor,
|
||||
double exploration_factor
|
||||
);
|
||||
|
||||
struct RL_agent * create_RL_agent (
|
||||
struct networks_qlearning * networks,
|
||||
struct vehicle * car,
|
||||
struct reward_lists * rewards,
|
||||
struct delay_params * delay,
|
||||
struct qlearning_params *qlearnParams
|
||||
);
|
||||
|
||||
void free_networks_qlearning (struct networks_qlearning * networks);
|
||||
void free_reward_lists(struct reward_lists *rwd_l);
|
||||
void free_delay_params (struct delay_params *dly_p);
|
||||
void free_qlearning_params(struct qlearning_params *q_params);
|
||||
void free_RL_agent(struct RL_agent *rlAgent);
|
||||
|
||||
void copy_weight_in_networks_from_main_to_target(struct networks_qlearning * networks);
|
||||
void copy_weight_in_networks_from_main_to_best(struct networks_qlearning * networks);
|
||||
|
||||
|
||||
#endif /* __LEARNING_VEHICLE__C_H____ */
|
||||
@@ -0,0 +1,548 @@
|
||||
#include "vehicle.h"
|
||||
|
||||
#define NB_ACTION 3
|
||||
#define NB_SENSORS 3
|
||||
|
||||
//#define LEFT 0
|
||||
//#define CENTER 1
|
||||
//#define RIGHT 2
|
||||
|
||||
#define LIMIT_DISTANCE ((float)1)/50
|
||||
|
||||
#define REWARD_STOP -1000
|
||||
#define REWARD_CONTINUE 100
|
||||
#define THRESHOLD_REWARD 10000
|
||||
|
||||
|
||||
struct game_status * create_game_status(){
|
||||
struct game_status * status = malloc(sizeof(struct game_status));
|
||||
status->state = 0;
|
||||
status->reward = 0;
|
||||
status->cumulative_reward = 0;
|
||||
status->done = false;
|
||||
}
|
||||
|
||||
struct coordinate * create_coordinate(size_t dim_size){
|
||||
struct coordinate * ret_coord = malloc(sizeof(struct coordinate));
|
||||
ret_coord->dimension_size = dim_size;
|
||||
ret_coord->x = malloc(dim_size * sizeof(float));
|
||||
|
||||
return ret_coord;
|
||||
}
|
||||
struct blocks * create_blocks(size_t nb_blocks, size_t dim_size){
|
||||
struct blocks * ret_blocks = malloc(sizeof(struct blocks));
|
||||
ret_blocks->nb_blocks = nb_blocks;
|
||||
ret_blocks->dimension_size = dim_size;
|
||||
ret_blocks->lower_bound_block = malloc(nb_blocks * sizeof(struct coordinate *));
|
||||
ret_blocks->upper_bound_block = malloc(nb_blocks * sizeof(struct coordinate *));
|
||||
ret_blocks->bounds_all_blocks = NULL;
|
||||
ret_blocks->all_updated = false;
|
||||
ret_blocks->marker = malloc(nb_blocks * sizeof(bool));
|
||||
|
||||
for(size_t i=0; i<nb_blocks; ++i){
|
||||
ret_blocks->lower_bound_block[i] = create_coordinate(dim_size);
|
||||
ret_blocks->upper_bound_block[i] = create_coordinate(dim_size);
|
||||
ret_blocks->marker[i] = false;
|
||||
}
|
||||
|
||||
return ret_blocks;
|
||||
}
|
||||
|
||||
struct sensors * create_sensors(size_t nb_values){
|
||||
struct sensors * ret_sensors = malloc(sizeof(struct sensors));
|
||||
ret_sensors->nb_values = nb_values;
|
||||
ret_sensors->value = malloc(nb_values * sizeof(float));
|
||||
|
||||
return ret_sensors;
|
||||
}
|
||||
|
||||
struct vehicle * create_vehicle(struct blocks *path){
|
||||
struct vehicle * ret_vehicle = malloc(sizeof(struct vehicle));
|
||||
|
||||
ret_vehicle->coord = create_coordinate(2);
|
||||
ret_vehicle->sensor = create_sensors(NB_SENSORS);
|
||||
ret_vehicle->path = path;
|
||||
|
||||
ret_vehicle->status = create_game_status();
|
||||
|
||||
reset(ret_vehicle);
|
||||
|
||||
return ret_vehicle;
|
||||
|
||||
}
|
||||
|
||||
void free_game_status(struct game_status *status){
|
||||
free(status);
|
||||
}
|
||||
|
||||
void free_coordinate(struct coordinate *coord){
|
||||
free(coord->x);
|
||||
free(coord);
|
||||
}
|
||||
|
||||
void free_blocks(struct blocks *blk){
|
||||
for(size_t i=0; i<blk->nb_blocks; ++i){
|
||||
free_coordinate(blk->lower_bound_block[i]);
|
||||
free_coordinate(blk->upper_bound_block[i]);
|
||||
}
|
||||
free(blk->lower_bound_block);
|
||||
free(blk->upper_bound_block);
|
||||
if(blk->bounds_all_blocks != NULL){
|
||||
free_coordinate(blk->bounds_all_blocks[0]);
|
||||
free_coordinate(blk->bounds_all_blocks[1]);
|
||||
free(blk->bounds_all_blocks);
|
||||
}
|
||||
free(blk->marker);
|
||||
free(blk);
|
||||
}
|
||||
|
||||
void free_sensors(struct sensors *snsr){
|
||||
free(snsr->value);
|
||||
free(snsr);
|
||||
}
|
||||
|
||||
void free_vehicle(struct vehicle * vhcl){
|
||||
free_coordinate(vhcl->coord);
|
||||
free_blocks(vhcl->path);
|
||||
free_sensors(vhcl->sensor);
|
||||
free_game_status(vhcl->status);
|
||||
|
||||
free(vhcl);
|
||||
}
|
||||
|
||||
|
||||
float scalar_product(struct coordinate *coord1, struct coordinate *coord2){
|
||||
size_t dimension_size = coord1->dimension_size;
|
||||
if(coord1->dimension_size > coord2->dimension_size )
|
||||
dimension_size = coord2->dimension_size;
|
||||
|
||||
float scalar = 0;
|
||||
for(size_t i=0; i<dimension_size; ++i){
|
||||
scalar += (coord1->x[i] * coord2->x[i]);
|
||||
}
|
||||
|
||||
return scalar;
|
||||
}
|
||||
|
||||
float vector_norm2(struct coordinate * coord){
|
||||
float scalar = scalar_product(coord, coord);
|
||||
return (float)sqrt(scalar);
|
||||
}
|
||||
|
||||
bool is_in_block_index(struct blocks *blk, size_t index, struct coordinate *coord){
|
||||
if(blk->dimension_size != coord->dimension_size) return false;
|
||||
for(size_t j=0; j<blk->dimension_size; ++j){
|
||||
if( ((blk->lower_bound_block[index])->x[j] > coord->x[j] ) ||
|
||||
((blk->upper_bound_block[index])->x[j] < coord->x[j] ) ){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
int is_in_blocks(struct blocks *blk, struct coordinate *coord){
|
||||
if(blk->dimension_size != coord->dimension_size) return 0;
|
||||
size_t count_in = 0;
|
||||
for(size_t i=0; i<blk->nb_blocks; ++i){
|
||||
count_in = 0;
|
||||
for(size_t j=0; j<blk->dimension_size; ++j){
|
||||
if( ((blk->lower_bound_block[i])->x[j] <= coord->x[j] ) &&
|
||||
((blk->upper_bound_block[i])->x[j] >= coord->x[j] ) ){
|
||||
++count_in;
|
||||
}
|
||||
}
|
||||
if(count_in == blk->dimension_size )
|
||||
return (i+1);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void printCoordinate(struct coordinate *coord, char *msg){
|
||||
for(size_t i=0; i<coord->dimension_size; ++i){
|
||||
printf(" %f,", coord->x[i]);
|
||||
}
|
||||
printf("{%s} [ %ld ]\n", msg, coord->dimension_size);
|
||||
}
|
||||
|
||||
void copy_coordinate(struct coordinate *coord, float *x){
|
||||
for(size_t i=0; i<coord->dimension_size; ++i){
|
||||
coord->x[i] = x[i];
|
||||
}
|
||||
}
|
||||
|
||||
struct coordinate ** bounds_limits_blocks(struct blocks *blk){ /* min x , max y */
|
||||
|
||||
struct coordinate **bounds_coord = malloc(2 * sizeof(struct coordinate*)) ;
|
||||
bounds_coord[0] = create_coordinate(blk->dimension_size);
|
||||
bounds_coord[1] = create_coordinate(blk->dimension_size);
|
||||
|
||||
copy_coordinate(bounds_coord[0], blk->lower_bound_block[0]->x);
|
||||
copy_coordinate(bounds_coord[1], blk->upper_bound_block[1]->x);
|
||||
|
||||
for(size_t i=1; i<blk->nb_blocks; ++i){
|
||||
for(size_t j=0; j<blk->dimension_size; ++j){
|
||||
bounds_coord[0]->x[j] = MIN(bounds_coord[0]->x[j], blk->lower_bound_block[i]->x[j]);
|
||||
bounds_coord[0]->x[j] = MIN(bounds_coord[0]->x[j], blk->upper_bound_block[i]->x[j]);
|
||||
bounds_coord[1]->x[j] = MAX(bounds_coord[1]->x[j], blk->lower_bound_block[i]->x[j]);
|
||||
bounds_coord[1]->x[j] = MAX(bounds_coord[1]->x[j], blk->upper_bound_block[i]->x[j]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
return bounds_coord;
|
||||
|
||||
}
|
||||
|
||||
void update_bounds_limits_blocks(struct blocks * blk){
|
||||
if(blk->all_updated == false){
|
||||
blk->all_updated = true;
|
||||
|
||||
if(blk->bounds_all_blocks != NULL){
|
||||
free_coordinate(blk->bounds_all_blocks[0]);
|
||||
free_coordinate(blk->bounds_all_blocks[1]);
|
||||
free(blk->bounds_all_blocks);
|
||||
}
|
||||
blk->bounds_all_blocks = bounds_limits_blocks(blk);
|
||||
}
|
||||
}
|
||||
|
||||
void print2D_blocks(struct blocks *blk, float scale_x, float scale_y, char pad){
|
||||
if(blk->dimension_size == 2){
|
||||
//struct coordinate ** bounds_coord = bounds_limits_blocks(blk);
|
||||
update_bounds_limits_blocks(blk);
|
||||
struct coordinate ** bounds_coord = blk->bounds_all_blocks;
|
||||
|
||||
for(int i=0; i<(bounds_coord[0]->dimension_size); ++i){
|
||||
printf(" x[%d]: %f <= %f \n",i, bounds_coord[0]->x[i],bounds_coord[1]->x[i]);
|
||||
}
|
||||
|
||||
struct coordinate * coord = create_coordinate(2);
|
||||
// int offset_space = 0;
|
||||
// int offset = 2;
|
||||
//for(coord->x[1] = bounds_coord[0]->x[1]; coord->x[1] < bounds_coord[1]->x[1]; coord->x[1]+=scale_y )
|
||||
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y )
|
||||
//for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > 0; coord->x[1]-=scale_y )
|
||||
{
|
||||
// printf("%*c",offset_space,' ');
|
||||
// offset_space += offset;
|
||||
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x )
|
||||
//for(coord->x[0] = 0; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x )
|
||||
{
|
||||
/*if(is_in_blocks(blk, coord))
|
||||
printf("%c",pad);
|
||||
*/
|
||||
int in = is_in_blocks(blk,coord);
|
||||
if(in)
|
||||
printf("%d",in);
|
||||
else
|
||||
printf(" ");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
free_coordinate(coord);
|
||||
|
||||
// free_coordinate(bounds_coord[0]);
|
||||
// free_coordinate(bounds_coord[1]);
|
||||
// free(bounds_coord);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct blocks * block_neighbord_Point(struct coordinate *coord, float *radius ){
|
||||
struct blocks * blk = create_blocks(1, coord->dimension_size);
|
||||
for(size_t i=0; i<blk->dimension_size; ++i){
|
||||
blk->lower_bound_block[0]->x[i] = coord->x[i]-radius[i] ;
|
||||
blk->upper_bound_block[0]->x[i] = coord->x[i]+radius[i] ;
|
||||
}
|
||||
|
||||
return blk;
|
||||
}
|
||||
|
||||
void print2D_blocks_withPoint(struct blocks *blk, float scale_x, float scale_y, char pad, struct coordinate *coordPoint){
|
||||
if(blk->dimension_size == 2){
|
||||
//struct coordinate ** bounds_coord = bounds_limits_blocks(blk);
|
||||
update_bounds_limits_blocks(blk);
|
||||
struct coordinate ** bounds_coord = blk->bounds_all_blocks;
|
||||
|
||||
struct coordinate * coord = create_coordinate(2);
|
||||
|
||||
float *radius = malloc(2 * sizeof(float));
|
||||
//radius[0] = MIN(scale_x, scale_y);
|
||||
//radius[1] = radius[0];
|
||||
radius[0]=scale_x;
|
||||
radius[1]=scale_y;
|
||||
|
||||
struct blocks * blk_point = block_neighbord_Point(coordPoint, radius);
|
||||
//for(coord->x[1] = bounds_coord[0]->x[1]; coord->x[1] < bounds_coord[1]->x[1]; coord->x[1]+=scale_y )
|
||||
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y ){
|
||||
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x ){
|
||||
if(is_in_blocks(blk_point, coord))
|
||||
printf("\033[0;31m"); // red
|
||||
if(is_in_blocks(blk, coord))
|
||||
printf("%c",pad);
|
||||
/*int in = is_in_blocks(blk,coord);
|
||||
if(in)
|
||||
printf("%d",in);
|
||||
*/
|
||||
else
|
||||
printf(" ");
|
||||
//printf("\033[37;01m"); // white gras
|
||||
printf("\033[0;37m"); // white
|
||||
//printf("\033[0;30m"); // black
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
free_coordinate(coord);
|
||||
|
||||
free_blocks(blk_point);
|
||||
free(radius);
|
||||
}
|
||||
}
|
||||
|
||||
void print2D_blocks_indexOne_withPoint(struct blocks *blk, float scale_x, float scale_y, struct coordinate *coordPoint){
|
||||
if(blk->dimension_size == 2){
|
||||
update_bounds_limits_blocks(blk);
|
||||
struct coordinate ** bounds_coord = blk->bounds_all_blocks;
|
||||
|
||||
struct coordinate * coord = create_coordinate(2);
|
||||
|
||||
float *radius = malloc(2 * sizeof(float));
|
||||
radius[0]=scale_x;
|
||||
radius[1]=scale_y;
|
||||
|
||||
struct blocks * blk_point = block_neighbord_Point(coordPoint, radius);
|
||||
for(coord->x[1] = bounds_coord[1]->x[1]; coord->x[1] > bounds_coord[0]->x[1]; coord->x[1]-=scale_y ){
|
||||
for(coord->x[0] = bounds_coord[0]->x[0]; coord->x[0] < bounds_coord[1]->x[0]; coord->x[0]+=scale_x ){
|
||||
if(is_in_blocks(blk_point, coord))
|
||||
printf("\033[0;31m"); // red
|
||||
int in = is_in_blocks(blk,coord);
|
||||
if(in)
|
||||
printf("%d",in);
|
||||
else
|
||||
printf(" ");
|
||||
printf("\033[0;37m"); // white
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
free_coordinate(coord);
|
||||
|
||||
free_blocks(blk_point);
|
||||
free(radius);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void goto_xy(int x, int y)
|
||||
{
|
||||
printf("%c[%d;%df", 0x1B, y, x);
|
||||
}
|
||||
|
||||
|
||||
static struct winsize w;
|
||||
|
||||
void init_win(){
|
||||
ioctl(1, TIOCGWINSZ, &w);
|
||||
}
|
||||
|
||||
void print_vehicle_n_path(struct vehicle *v, float scale_x, float scale_y){
|
||||
static bool first = true;
|
||||
if(first){
|
||||
first = false;
|
||||
init_win();
|
||||
char pad[w.ws_col+1];
|
||||
int i=0;
|
||||
for(i=0; i<w.ws_col+1; ++i) pad[i]=' ';
|
||||
pad[i]='\0';
|
||||
for(i=0; i<w.ws_row / 2 ; ++i) printf("%s\n",pad);;
|
||||
|
||||
}
|
||||
//goto_xy(0,w.ws_row - lines);
|
||||
goto_xy(0,0);
|
||||
//printf("lines : %d , row : %d : diff: %d\n",lines, w.ws_row , w.ws_row - lines);
|
||||
print2D_blocks_indexOne_withPoint(v->path, scale_x, scale_y, v->coord);
|
||||
//printf("lines print : %d\n",lines);
|
||||
printf("\nlog : %s \n",v->status->log);
|
||||
}
|
||||
|
||||
void move_vehicle(struct vehicle *v){
|
||||
v->coord->x[0] += v->speed * cos(v->direction * M_PI / 180);
|
||||
v->coord->x[1] += v->speed * sin(v->direction * M_PI / 180);
|
||||
}
|
||||
|
||||
float distance2_coordinate(struct coordinate *c0, struct coordinate *c1){
|
||||
if(c0->dimension_size != c1->dimension_size) return 0;
|
||||
float d=0, tmp;
|
||||
for(size_t i=0; i<c0->dimension_size; ++i){
|
||||
tmp = (c0->x[i] - c1->x[i]);
|
||||
d += tmp * tmp;
|
||||
}
|
||||
return sqrt(d);
|
||||
}
|
||||
|
||||
#define SENSOR_VALUE_CALCULATE(position, deviation)\
|
||||
direction_radian = (v->direction + deviation) * M_PI / 180;\
|
||||
while( is_in_blocks(v->path, diStep_sensor )){\
|
||||
diStep_sensor->x[0] += step_sensor * cos(direction_radian);\
|
||||
diStep_sensor->x[1] += step_sensor * sin(direction_radian);\
|
||||
}\
|
||||
v->sensor->value[position] = (MIN(49,(distance2_coordinate(diStep_sensor, v->coord)))) / 50;\
|
||||
|
||||
//v->sensor->value[position] = (MIN(49,(int)(distance2_coordinate(diStep_sensor, v->coord)/10))) / 50;\
|
||||
|
||||
void read_sensor(struct vehicle *v){
|
||||
float step_sensor = ((float)1)/SUBDIVISION;
|
||||
struct coordinate * diStep_sensor = create_coordinate(2);
|
||||
copy_coordinate(diStep_sensor, v->coord->x);
|
||||
|
||||
// count the number of step until we go out of the path = distance
|
||||
// center sensor
|
||||
float direction_radian ;
|
||||
SENSOR_VALUE_CALCULATE(CENTER,0);
|
||||
|
||||
copy_coordinate(diStep_sensor, v->coord->x);
|
||||
// right sensor
|
||||
SENSOR_VALUE_CALCULATE(RIGHT,45);
|
||||
|
||||
copy_coordinate(diStep_sensor, v->coord->x);
|
||||
// left sensor
|
||||
SENSOR_VALUE_CALCULATE(LEFT, -45);
|
||||
|
||||
free_coordinate(diStep_sensor);
|
||||
|
||||
}
|
||||
|
||||
#define ADD_CHAR_LOG(status, c)\
|
||||
do{\
|
||||
if(status->cur_log >= LOG_LENTH){\
|
||||
status->cur_log = 0;\
|
||||
}\
|
||||
status->log[(status->cur_log)++]=c;\
|
||||
}while(0); \
|
||||
\
|
||||
|
||||
|
||||
#define ADD_CHAR_LOG_ENDED(status, c)\
|
||||
do{\
|
||||
if(status->cur_log >= LOG_LENTH){\
|
||||
status->cur_log = 0;\
|
||||
}\
|
||||
status->log[(status->cur_log)++]=c;\
|
||||
status->log[(status->cur_log)]='\0';\
|
||||
}while(0); \
|
||||
\
|
||||
|
||||
void add_string_log_M(struct game_status *status, char *str ){
|
||||
size_t i;
|
||||
for(i=0; i < strlen(str) - 1; ++i){
|
||||
ADD_CHAR_LOG(status, str[i]);
|
||||
}
|
||||
ADD_CHAR_LOG_ENDED(status, str[i]);
|
||||
}
|
||||
|
||||
|
||||
void add_string_log(struct game_status *status, char *str ){
|
||||
size_t i;
|
||||
for(i=0; i < strlen(str) - 1; ++i){
|
||||
if(status->cur_log >= LOG_LENTH){
|
||||
status->cur_log = 0;
|
||||
}
|
||||
status->log[(status->cur_log)++]=str[i];
|
||||
}
|
||||
if(status->cur_log >= LOG_LENTH){
|
||||
status->cur_log = 0;
|
||||
}
|
||||
status->log[(status->cur_log)++]=str[i];
|
||||
status->log[(status->cur_log)]='\0';
|
||||
|
||||
}
|
||||
|
||||
void step(struct vehicle *v, int action){
|
||||
float action_value[NB_ACTION]={-3,0,3}; // [LEFT, CENTER, RIGHT]
|
||||
v->direction = v->direction + action_value[action % 3];
|
||||
v->speed = ((float)1)/2;
|
||||
move_vehicle(v);
|
||||
read_sensor(v);
|
||||
struct game_status *status = v->status;
|
||||
status->state = v->sensor->value[LEFT]* 2500 +
|
||||
v->sensor->value[CENTER]* 50 +
|
||||
v->sensor->value[RIGHT] ;
|
||||
status->reward = 0;
|
||||
status->done =false;
|
||||
struct blocks * path = v->path;
|
||||
printf(" center : %f vs %f direction: %f\n",v->sensor->value[CENTER], LIMIT_DISTANCE, v->direction);
|
||||
if( v->sensor->value[CENTER]<= LIMIT_DISTANCE ){
|
||||
status->reward = REWARD_STOP;
|
||||
status->done = true;
|
||||
}
|
||||
else{
|
||||
bool breaked = false;
|
||||
long prec, next;
|
||||
char msg[48];
|
||||
for(long i=0; i< path->nb_blocks; ++i){
|
||||
//prec = (i-1)%(path->nb_blocks);
|
||||
prec = (i + path->nb_blocks - 1 )%(path->nb_blocks);
|
||||
next = (i + 1)%(path->nb_blocks);
|
||||
printf("i:%ld, prec:%ld, next:%ld: maker %d, prec marker %d\n",i,prec,next, path->marker[i], path->marker[prec]);
|
||||
if(is_in_block_index(path, i, v->coord)){
|
||||
if(path->marker[i] == false && path->marker[prec] == true){
|
||||
path->marker[i]=true;
|
||||
path->marker[prec]=false;
|
||||
status->reward = REWARD_CONTINUE;
|
||||
status->done = false;
|
||||
sprintf(msg," %ld,",i);
|
||||
add_string_log(status, msg);
|
||||
}
|
||||
if(path->marker[next] == true){
|
||||
status->reward = REWARD_STOP;
|
||||
status->done = true;
|
||||
add_string_log(status, "| reverse |");
|
||||
}
|
||||
breaked = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(breaked == false){
|
||||
if(status->cumulative_reward > THRESHOLD_REWARD){
|
||||
status->reward = REWARD_CONTINUE;
|
||||
status->done = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
status->cumulative_reward += status->reward;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void reset(struct vehicle *v){
|
||||
struct blocks * path = v->path;
|
||||
long int i;
|
||||
for(i=0; i<(path->nb_blocks -1); ++i)
|
||||
path->marker[i] = false;
|
||||
path->marker[i] = true;
|
||||
v->status->cumulative_reward = 0;
|
||||
sprintf(v->status->log,"\n");
|
||||
v->status->cur_log = 0;
|
||||
srand(time(NULL));
|
||||
int random;
|
||||
int diff;
|
||||
diff = path->upper_bound_block[0]->x[0] - path->lower_bound_block[0]->x[0];
|
||||
random = rand() % diff;
|
||||
v->coord->x[0] = path->lower_bound_block[0]->x[0] + random;
|
||||
diff = path->upper_bound_block[0]->x[1] - path->lower_bound_block[0]->x[1];
|
||||
random = rand() % diff;
|
||||
v->coord->x[1] = path->lower_bound_block[0]->x[1] + random;
|
||||
random = rand() % 50;
|
||||
v->direction = random - 25;
|
||||
//v->direction = 15;
|
||||
v->speed = 1;
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
#ifndef __VEHICLE__C_H__
|
||||
#define __VEHICLE__C_H__
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "tools_t/tools_t.h"
|
||||
#include "dimension_t/dimension_t.h"
|
||||
|
||||
#define LOG_LENTH 128
|
||||
|
||||
#define LEFT 0
|
||||
#define CENTER 1
|
||||
#define RIGHT 2
|
||||
|
||||
#define SUBDIVISION 10
|
||||
|
||||
|
||||
struct game_status {
|
||||
long state;
|
||||
long reward;
|
||||
long cumulative_reward;
|
||||
bool done;
|
||||
char log[LOG_LENTH];
|
||||
int cur_log;
|
||||
};
|
||||
|
||||
struct coordinate {
|
||||
size_t dimension_size;
|
||||
float *x;
|
||||
};
|
||||
|
||||
/*
|
||||
+-----------------------+ <-- upper_bound_block (coordinate (6,5) for example)
|
||||
| |
|
||||
| |
|
||||
| BLOCK |
|
||||
| |
|
||||
| |
|
||||
+-----------------------+
|
||||
^
|
||||
|
|
||||
lower_bound_block (coordinate ( 0,0 ) for example
|
||||
|
||||
*/
|
||||
struct blocks {
|
||||
size_t nb_blocks;
|
||||
struct coordinate **lower_bound_block;
|
||||
struct coordinate **upper_bound_block;
|
||||
struct coordinate **bounds_all_blocks;
|
||||
bool all_updated;
|
||||
size_t dimension_size;
|
||||
bool *marker;
|
||||
//float step: // size of subdivision of the lowest large
|
||||
};
|
||||
|
||||
|
||||
struct sensors {
|
||||
size_t nb_values;
|
||||
float *value;
|
||||
};
|
||||
|
||||
struct vehicle {
|
||||
struct coordinate *coord;
|
||||
float direction;
|
||||
float speed;
|
||||
struct sensors *sensor;
|
||||
struct blocks *path;
|
||||
struct game_status *status;
|
||||
};
|
||||
|
||||
struct game_status * greate_game_status();
|
||||
struct coordinate * create_coordinate(size_t dim_size);
|
||||
struct blocks * create_blocks(size_t nb_blocks, size_t dim_size);
|
||||
|
||||
struct sensors * create_sensors(size_t nb_values);
|
||||
struct vehicle * create_vehicle(
|
||||
struct blocks *path
|
||||
);
|
||||
|
||||
void free_game_status(struct game_status *status);
|
||||
void free_coordinate(struct coordinate *coord);
|
||||
void free_blocks(struct blocks *blk);
|
||||
|
||||
void free_sensors(struct sensors *snsr);
|
||||
|
||||
void free_vehicle(struct vehicle * vhcl);
|
||||
|
||||
void update_bounds_limits_blocks(struct blocks * blk);
|
||||
|
||||
int is_in_blocks(struct blocks *blk, struct coordinate *coord);
|
||||
|
||||
void copy_coordinate(struct coordinate *coord, float *x);
|
||||
|
||||
void move_vehicle(struct vehicle *v);
|
||||
void read_sensor(struct vehicle *v);
|
||||
void step(struct vehicle *v, int action);
|
||||
|
||||
void reset(struct vehicle *v);
|
||||
|
||||
void print2D_blocks_indexOne_withPoint(struct blocks *blk, float scale_x, float scale_y, struct coordinate *coordPoint);
|
||||
void print_vehicle_n_path(struct vehicle *v, float scale_x, float scale_y);
|
||||
|
||||
float distance2_coordinate(struct coordinate *c0, struct coordinate *c1);
|
||||
|
||||
void print2D_blocks(struct blocks *blk, float scale_x, float scale_y, char pad);
|
||||
void print2D_blocks_withPoint(struct blocks *blk, float scale_x, float scale_y, char pad, struct coordinate *coordPoint);
|
||||
|
||||
#endif /* __VEHICLE__C_H__ */
|
||||
@@ -0,0 +1,88 @@
|
||||
|
||||
|
||||
|
||||
|
||||
NAME_TEST=is_good
|
||||
CC=gcc
|
||||
ROOT_DIR=$(PWD)
|
||||
YTESTDIR=$(PWD)/../../ytest_t
|
||||
YTOOLDIR=$(PWD)/../../ytools_t
|
||||
YPERMDIR=$(PWD)/../../ypermutation_t
|
||||
DIMDIR=$(PWD)/../../dimension_t
|
||||
|
||||
TENSDIR=$(PWD)/../../tensor_t
|
||||
|
||||
NEURODIR=$(PWD)/../../neuron_t
|
||||
INCLUDE_DIR=$(PWD)/../src/deepQlearning
|
||||
CFLAGS=-I$(INCLUDE_DIR) -I$(NEURODIR)/src -I$(YPERMDIR)/src -I$(YTESTDIR)/include_ytest/include -I$(DIMDIR)/src -I$(TENSDIR)/src -I$(YTOOLDIR)/include #"-D DEBUG=1"
|
||||
LDFLAGS=-L$(YTESTDIR) -lytest -lOpenCL -lm -lpthread #-lcurses
|
||||
|
||||
#SRC_DIR=$(ROOT_DIR)/src
|
||||
#SRC=$(wildcard */*/*.c)
|
||||
SRC=$(wildcard **/**/*.c)
|
||||
#HEADS=$(OBJS:.o=.h)
|
||||
TEST_DIR=$(PWD)
|
||||
|
||||
EXECSRC=$(NAME_TEST).c
|
||||
#EXECSRC=openF.c
|
||||
|
||||
EXEC=launch_$(NAME_TEST)_m
|
||||
|
||||
NEUROSRC=$(NEURODIR)/src/neuron_t/neuron_t.c
|
||||
NEUROSRC_O=$(NEUROSRC:.c=.o)
|
||||
|
||||
|
||||
TENSRC=$(TENSDIR)/src/tensor_t/tensor_t.c
|
||||
TENSRC_O=$(TENSRC:.c=.o)
|
||||
|
||||
VEHICLESRC=$(INCLUDE_DIR)/vehicle.c
|
||||
VEHICLESRC_O=$(VEHICLESRC:.c=.o)
|
||||
|
||||
|
||||
TOOLSRC_O=$(YTOOLDIR)/src/tools_t/tools_t.o
|
||||
|
||||
PERMSRC_O=$(YPERMDIR)/src/permutation_t/permutation_t.o
|
||||
|
||||
DIMSRC_O=$(DIMDIR)/src/dimension_t/dimension_t.o
|
||||
|
||||
TOPTARGETS := all clean
|
||||
|
||||
DEPS=$(DIMDIR) $(YPERMDIR) $(YTESTDIR) $(TENSDIR) $(NEURODIR) $(YTOOLDIR)
|
||||
|
||||
OBJ=$(VEHICLESRC_O) $(DIMSRC_O) $(PERMSRC_O) $(TENSRC_O) $(NEUROSRC_O) $(TOOLSRC_O)
|
||||
|
||||
LIB_YTEST=$(YTESTDIR)/libytest.so
|
||||
|
||||
|
||||
$(TOPTARGETS): $(DEPS)
|
||||
|
||||
$(DEPS):
|
||||
$(MAKE) -C $@ $(MAKECMDGOALS)
|
||||
|
||||
|
||||
#PERMSRC_O=$(PERMSRC:.c=.o)
|
||||
#SETTSRC_O=$(PWD)/../src/set_theoric_t/set_theoric_t.o
|
||||
#SETTSRC_O=$(SETTSRC:.c=.o)
|
||||
#TOOLSRC=$(TOOLDIR)/src/tools_t/tools_t.c
|
||||
#TOOLSRC_O=$(TOOLSRC:.c=.o)
|
||||
|
||||
|
||||
all: $(EXEC) $(LIB_YTEST)
|
||||
|
||||
$(EXEC): $(EXECSRC) $(OBJ)
|
||||
$(CC) -o $@ $^ $(CFLAGS) $(LDFLAGS)
|
||||
|
||||
$(VEHICLESRC_O): $(VEHICLESRC) $(TOOLSRC_O) $(DIMSRC_O)
|
||||
$(CC) -o $@ -c $< $(CFLAGS)
|
||||
|
||||
|
||||
.PHONY: clean mrproper
|
||||
|
||||
clean:
|
||||
rm -f $(OBJ)
|
||||
|
||||
mrproper: clean
|
||||
rm -f $(EXEC)
|
||||
|
||||
run: $(EXEC)
|
||||
$(EXEC) -h
|
||||
@@ -0,0 +1,210 @@
|
||||
#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"
|
||||
|
||||
TEST(create_coordenate){
|
||||
struct 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});
|
||||
|
||||
struct 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});
|
||||
|
||||
|
||||
struct 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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
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,1});
|
||||
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(vhcl, CENTER);
|
||||
sleep(2);
|
||||
print_vehicle_n_path(vhcl, 0.2,0.4);
|
||||
|
||||
step(vhcl, CENTER);
|
||||
sleep(2);
|
||||
print_vehicle_n_path(vhcl, 0.2,0.4);
|
||||
|
||||
step(vhcl, CENTER);
|
||||
sleep(2);
|
||||
print_vehicle_n_path(vhcl, 0.2,0.4);
|
||||
|
||||
step(vhcl, CENTER);
|
||||
sleep(2);
|
||||
print_vehicle_n_path(vhcl, 0.2,0.4);
|
||||
|
||||
free_vehicle(vhcl);
|
||||
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
|
||||
|
||||
run_all_tests_args(argc, argv);
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user