/************************************************************************/ /************************************************************************/ // sidp1-execute.c: follow a policy for the marble maze. /************************************************************************/ /************************************************************************/ #include "stdafx.h" /* Windows stuff, make empty stdafx.h under Linux */ /************************************************************************/ #include #include #include #include #include "maze1.h" /************************************************************************/ /* DEFINES */ #define N_STATES 4 #define N_ACTIONS 2 /* Status flags for values: These are shoved into floating point values */ /* _TEST stuff is way to test for flags */ #define VERY_BIG_VALUE 1e30 #define VERY_BIG_VALUE_TEST 1e29 #define IN_GOAL 0.0 #define IN_GOAL_TEST 1e-7 /* Number of steps puck has to remain in goal to be "in goal" */ #define MIN_N_STEPS_IN_GOAL 100 #define MAX_STEPS_IN_TRAJECTORY_SEGMENT 30 /* Velocity range we look at */ #define MAX_VELOCITY 0.4 /* Force range we look at. */ #define MAX_FORCE 1.0 /* Need 60x60 on board2 to get one hypercube fully in goal */ #define X_RESOLUTION 119 #define Y_RESOLUTION 97 #define XD_RESOLUTION 9 #define YD_RESOLUTION 9 #define FORCE_X_RESOLUTION 9 #define FORCE_Y_RESOLUTION 9 /* #define X_RESOLUTION 51 #define Y_RESOLUTION 51 #define XD_RESOLUTION 3 #define YD_RESOLUTION 3 #define FORCE_X_RESOLUTION 3 #define FORCE_Y_RESOLUTION 3 */ /************************************************************************/ /* General Globals */ float timestep = 0.0166666; /* Used for displaying minimum values */ float overall_maximum_minimum_value = 0; /************************************************************************/ /* GLUT/OPENGL Globals */ float view[4] = { -1.0, -1.0, 1.0, 1.0 }; GLsizei window_widthi, window_heighti; GLfloat window_widthf, window_heightf; float mouse_display[2] = { 0.0, 0.0 }; #define MAX_WINDOW_SIZE 400 /************************************************************************/ /* Globals */ BOARD a_board; BOARD *the_board; /************************************************************************/ /* The dp data structure */ typedef struct leaf { float value; float action[N_ACTIONS]; } LEAF; typedef struct vel_array { LEAF a[XD_RESOLUTION][YD_RESOLUTION]; } VEL_ARRAY; typedef struct map { int resolution[N_STATES]; int u_resolution[N_ACTIONS]; float min[N_STATES]; float max[N_STATES]; float increments[N_STATES]; float u_min[N_ACTIONS]; float u_max[N_ACTIONS]; VEL_ARRAY *a[X_RESOLUTION][Y_RESOLUTION]; int status[X_RESOLUTION][Y_RESOLUTION]; float one_step_cost[X_RESOLUTION][Y_RESOLUTION]; float temp[X_RESOLUTION][Y_RESOLUTION]; float display[X_RESOLUTION][Y_RESOLUTION][3]; } MAP; MAP a_map; MAP *the_map = NULL; /************************************************************************/ /************************************************************************/ void flush_graphics() { glutPostRedisplay(); glutMainLoopEvent(); } /************************************************************************/ void display() { int i, j; MAP *m; float x, y; m = the_map; glClear( GL_COLOR_BUFFER_BIT ); for( i = 0; i < m->resolution[XX]; i++ ) { x = (float) i; for( j = 0; j < m->resolution[YY]; j++ ) { y = (float) j; glColor3fv( &(m->display[i][j][0]) ); glRectf( x, y, x+1, y+1 ); } } glutSwapBuffers(); } /************************************************************************/ void init_display( MAP *m ) { int i, j; float increment, angle; view[LEFT] = 0; view[BOTTOM] = 0; view[RIGHT] = X_RESOLUTION; view[TOP] = Y_RESOLUTION; glMatrixMode( GL_PROJECTION ); glLoadIdentity(); gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM], view[TOP] ); } /************************************************************************/ /* This needs to be fixed. */ void reshape( GLsizei w, GLsizei h ) { window_widthi = w; window_heighti = h; window_widthf = (GLfloat) w; window_heightf = (GLfloat) h; glMatrixMode( GL_PROJECTION ); glLoadIdentity(); /* if ( w <= h ) gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM]*window_heightf/window_widthf, view[TOP]*window_heightf/window_widthf ); else gluOrtho2D( view[LEFT]*window_widthf/window_heightf, view[RIGHT]*window_widthf/window_heightf, view[BOTTOM], view[TOP] ); */ gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM], view[TOP] ); /* glMatrixMode( GL_MODELVIEW ); */ glViewport( 0, 0, w, h ); } /************************************************************************/ void idle() { glutPostRedisplay(); } /************************************************************************/ /************************************************************************/ void keypress( unsigned char key, int x, int y ) { if ( key == 'q' || key == 'Q' || key == '\27' ) exit( 0 ); mouse_display[XX] = x; mouse_display[YY] = y; } /************************************************************************/ void mouse( int button, int state, int x, int y ) { char b, d; if ( button == GLUT_LEFT_BUTTON ) b = 'L'; else if ( button == GLUT_RIGHT_BUTTON ) b = 'R'; else b = 'M'; if ( state == GLUT_UP ) d = 'U'; else d = 'D'; mouse_display[XX] = x; mouse_display[YY] = y; } /************************************************************************/ void mouse_motion( int x, int y ) { mouse_display[XX] = x; mouse_display[YY] = y; } /************************************************************************/ int init_glut( int argc, char **argv, float ratio ) { int wx, wy; if ( ratio < 1.0 ) { wx = MAX_WINDOW_SIZE; wy = wx*ratio; } else { wy = MAX_WINDOW_SIZE; wx = wy/ratio; } glutInit( &argc, argv ); glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE ); glutInitWindowPosition( 0, 0 ); glutInitWindowSize( wx, wy ); glutCreateWindow( "sidp1" ); glutDisplayFunc( display ); glutReshapeFunc( reshape ); glutIdleFunc( idle ); glutKeyboardFunc( keypress ); glutMouseFunc( mouse ); /* glutMotionFunc( mouse_motion ); glutPassiveMotionFunc( mouse_motion ); */ return TRUE; } /************************************************************************/ /************************************************************************/ void init_map( BOARD *b, MAP *m ) { int i, j; m->resolution[XX] = X_RESOLUTION; m->resolution[YY] = Y_RESOLUTION; m->resolution[XD] = XD_RESOLUTION; m->resolution[YD] = YD_RESOLUTION; m->u_resolution[XX] = FORCE_X_RESOLUTION; m->u_resolution[YY] = FORCE_Y_RESOLUTION; m->min[XX] = b->board.p[0]; m->min[YY] = b->board.p[1]; m->min[XD] = -MAX_VELOCITY; m->min[YD] = -MAX_VELOCITY; m->max[XX] = b->board.p[2]; m->max[YY] = b->board.p[3]; m->max[XD] = MAX_VELOCITY; m->max[YD] = MAX_VELOCITY; m->u_min[XX] = -MAX_FORCE; m->u_min[YY] = -MAX_FORCE; m->u_max[XX] = MAX_FORCE; m->u_max[YY] = MAX_FORCE; for( i = 0; i < N_STATES; i++ ) { m->increments[i] = (m->max[i] - m->min[i])/(m->resolution[i] - 1); } for( i = 0; i < m->resolution[XX]; i++ ) { for( j = 0; j < m->resolution[YY]; j++ ) { m->a[i][j] = NULL; m->status[i][j] = 0; m->one_step_cost[i][j] = 0; m->display[i][j][0] = 0.0; m->display[i][j][1] = 0.0; m->display[i][j][2] = 0.0; } } flush_graphics(); } /************************************************************************/ int indices_to_state( MAP *m, int *indices, float *state ) { int i; for( i = 0; i < N_STATES; i++ ) { state[i] = m->min[i] + indices[i]*m->increments[i]; } return TRUE; } /************************************************************************/ int state_to_indices( MAP *m, float *state, int *indices ) { int i; for( i = 0; i < N_STATES; i++ ) { indices[i] = (state[i] - m->min[i])/m->increments[i]; } return TRUE; } /************************************************************************/ int indices_to_action( MAP *m, int *indices, float *action ) { int i; for( i = 0; i < N_ACTIONS; i++ ) { action[i] = m->u_min[i] + indices[i]*(m->u_max[i] - m->u_min[i])/(m->u_resolution[i] - 1); } return TRUE; } /************************************************************************/ void sidp1_map_obstacles( BOARD *b, MAP *m ) { int ix, iy; int indices[N_STATES]; float state[N_STATES]; indices[XD] = 0; indices[YD] = 0; for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { indices[XX] = ix; indices[YY] = iy; indices_to_state( m, indices, state ); if ( in_obstacle( b, state ) ) { m->status[ix][iy] = OBSTACLE; m->display[ix][iy][0] = 1.0; m->display[ix][iy][1] = 1.0; m->display[ix][iy][2] = 1.0; } else { m->display[ix][iy][0] = 0.0; m->display[ix][iy][1] = 0.0; m->display[ix][iy][2] = 0.0; } } } flush_graphics(); } /************************************************************************/ void sidp1_map_holes( BOARD *b, MAP *m ) { int ix, iy; int indices[N_STATES]; float state[N_STATES]; indices[XD] = 0; indices[YD] = 0; for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->status[ix][iy] != 0 ) { continue; } indices[XX] = ix; indices[YY] = iy; indices_to_state( m, indices, state ); if ( in_hole( b, state ) ) { m->status[ix][iy] = HOLE; m->display[ix][iy][0] = 0.0; m->display[ix][iy][1] = 0.0; m->display[ix][iy][2] = 1.0; } } } flush_graphics(); } /************************************************************************/ void initialize_value_function( BOARD *b, MAP *m ) { int i, j; int ix, iy, ixd, iyd; float state[N_STATES]; int indices[N_STATES]; VEL_ARRAY *va; for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->status[ix][iy] != 0 ) { continue; } va = (VEL_ARRAY *) malloc( sizeof( VEL_ARRAY ) ); if ( va == NULL ) { fprintf( stderr, "Allocation error in initialize_value_function\n" ); exit( -1 ); } for( ixd = 0; ixd < m->resolution[XD]; ixd++ ) { for( iyd = 0; iyd < m->resolution[YD]; iyd++ ) { va->a[ixd][iyd].value = VERY_BIG_VALUE; } } indices[XX] = ix; indices[YY] = iy; indices_to_state( m, indices, state ); if ( in_goal( b, state ) ) { /* Make sure box around goal in velocity space */ m->status[ix][iy] = GOAL; state[XD] = 0; state[YD] = 0; state_to_indices( m, state, indices ); for( ixd = indices[XD] - 1; ixd <= indices[XD] + 1; ixd++ ) { if ( ixd < 0 ) continue; if ( ixd > m->resolution[XD] ) continue; for( iyd = indices[YD] - 1; iyd <= indices[YD] + 1; iyd++ ) { if ( iyd < 0 ) continue; if ( iyd > m->resolution[YD] ) continue; va->a[ixd][iyd].value = IN_GOAL; /* printf( "Goal: %d %d %d %d\n", ix, iy, ixd, iyd ); */ m->display[ix][iy][0] = 1.0; } } } m->a[ix][iy] = va; } } flush_graphics(); } /************************************************************************/ int grow_obstacle( MAP *m, int jx, int jy ) { int ix, iy; float biggest_value = -VERY_BIG_VALUE; float decay; for( ix = jx - 1; ix <= jx + 1; ix++ ) { if ( ix < 0 ) continue; if ( ix >= X_RESOLUTION ) continue; for( iy = jy - 1; iy <= jy + 1; iy++ ) { if ( iy < 0 ) continue; if ( iy >= Y_RESOLUTION ) continue; if ( m->status[ix][iy] == OBSTACLE ) continue; if ( biggest_value < m->temp[ix][iy] ) { biggest_value = m->temp[ix][iy]; } } } /* Trying to get same decay rate per meter for different resolutions. Need to take into account board size, etc. */ decay = 1.0 - 0.1*(100.0/m->resolution[XX]); m->one_step_cost[jx][jy] = decay*biggest_value; m->display[jx][jy][0] = m->one_step_cost[jx][jy]; m->display[jx][jy][1] = 0; m->display[jx][jy][2] = m->one_step_cost[jx][jy]; } /************************************************************************/ void calculate_one_step_costs( MAP *m ) { /* Here is where we expand holes */ int i, j; int ix, iy; int n_growth_cycles; /* First copy the holes */ for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->status[ix][iy] == HOLE ) m->one_step_cost[ix][iy] = 1.0; } } /* Trying to grow obstacles same distance independent of resolution. Need to take into account size of board. */ n_growth_cycles = 5*(m->resolution[XX]/100.0); if ( n_growth_cycles < 1 ) n_growth_cycles = 1; for( i = 0; i < n_growth_cycles; i++ ) { /* Copy to temp. */ for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { m->temp[ix][iy] = m->one_step_cost[ix][iy]; } } /* Grow obstacles */ printf( "Growing obstacles.\n" ); for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->status[ix][iy] != 0 ) continue; grow_obstacle( m, ix, iy ); } } flush_graphics(); } } /************************************************************************/ void init_indices( int sweep_control, int i, int *indices, int *limits ) { if ( ((sweep_control >> i) & 1) == 0 ) indices[i] = 0; else indices[i] = limits[i] - 1; } /************************************************************************/ int test_indices( int sweep_control, int i, int *indices, int *limits ) { if ( ((sweep_control >> i) & 1) == 0 ) { if ( indices[i] < limits[i] ) return 1; } else { if ( indices[i] >= 0 ) return 1; } return 0; } /************************************************************************/ void increment_indices( int sweep_control, int i, int *indices ) { if ( ((sweep_control >> i) & 1) == 0 ) indices[i] += 1; else indices[i] -= 1; } /************************************************************************/ void goal_policy( BOARD *b, float *state, float *u ) { float x_d, y_d; float position_gain = 10.0; float velocity_gain = 5.0; x_d = (b->goal.p[0] + b->goal.p[2])/2; y_d = (b->goal.p[1] + b->goal.p[3])/2; u[XX] = -position_gain*(state[XX] - x_d) - velocity_gain*state[XD]; u[YY] = -position_gain*(state[YY] - y_d) - velocity_gain*state[YD]; } /************************************************************************/ /* Figure out which of goal states is kept in goal by "goal policy" */ identify_goal_states( BOARD *b, MAP *m ) { int indices[N_STATES]; int status; float state[N_STATES]; float next_state[N_STATES]; float u[N_ACTIONS]; int count; for( indices[XX] = 0; indices[XX] < m->resolution[XX]; indices[XX]++ ) for( indices[YY] = 0; indices[YY] < m->resolution[YY]; indices[YY]++ ) { if ( m->status[indices[XX]][indices[YY]] != GOAL ) continue; for( indices[XD] = 0; indices[XD] < m->resolution[XD]; indices[XD]++ ) for( indices[YD] = 0; indices[YD] < m->resolution[YD]; indices[YD]++) { indices_to_state( m, indices, state ); /* printf( "Goal testing: %d %d %d %d: %g %g %g %g\n", indices[0], indices[1], indices[2], indices[3], state[0], state[1], state[2], state[3] ); */ if ( set_state( b, state, &status ) < 0 ) { fprintf( stderr, "Couldn't initialize state: %d %d %d %d: %g %g %g %g\n", indices[0], indices[1], indices[2], indices[3], state[0], state[1], state[2], state[3] ); exit( -1 ); } for( count = 0; count < MIN_N_STEPS_IN_GOAL; count++ ) { goal_policy( b, state, u ); if ( apply_action( b, u, timestep, state, &status ) < 0 ) break; if ( !in_goal( b, state ) ) break; /* printf( "%g %g %g %g\n", state[0], state[1], state[2], state[3] ); */ } if ( count >= MIN_N_STEPS_IN_GOAL ) { if ( m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value == IN_GOAL ) { /* printf( "Point %d %d %d %d already labelled as goal.\n", indices[0], indices[1], indices[2], indices[3] ); */ } else { m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value = IN_GOAL; /* printf( "Point %d %d %d %d labelled as goal.\n", indices[0], indices[1], indices[2], indices[3] ); */ } } else { /* if ( m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value == IN_GOAL ) { printf( "Point %d %d %d %d incorrectly labelled as goal.\n", indices[0], indices[1], indices[2], indices[3] ); } else { printf( "Point %d %d %d %d exits goal.\n", indices[0], indices[1], indices[2], indices[3] ); } */ m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value = VERY_BIG_VALUE; } /* printf( "Press return to continue.\n" ); getchar(); */ } } } /************************************************************************/ float get_value( MAP *m, int *indices ) { int i; VEL_ARRAY *va; /* printf( "%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3] ); */ for ( i = 0; i < N_STATES; i++ ) if ( indices[i] < 0 || indices[i] >= m->resolution[i] ) return VERY_BIG_VALUE; va = m->a[indices[XX]][indices[YY]]; if ( va == NULL ) return VERY_BIG_VALUE; return va->a[indices[XD]][indices[YD]].value; } /************************************************************************/ int count_valid_values( MAP *m, int *indices ) { int bits; int i; int indices2[N_STATES]; int count = 0; /* Check all corners of a hypercube */ for ( bits = 0; bits < 16; bits++ ) { for ( i = 0; i < N_STATES; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } if ( get_value( m, indices2 ) < VERY_BIG_VALUE_TEST ) count++; } return count; } /************************************************************************/ int hypercube_valid( MAP *m, int *indices ) { int bits; int i; int indices2[N_STATES]; /* Check all corners of a hypercube */ for ( bits = 0; bits < 16; bits++ ) { for ( i = 0; i < N_STATES; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } if ( get_value( m, indices2 ) >= VERY_BIG_VALUE_TEST ) return 0; } return 1; } /************************************************************************/ count_valid_hypercubes( MAP *m ) { int indices[N_STATES]; int indices2[N_STATES]; int count = 0; for( indices[XX] = 0; indices[XX] < m->resolution[XX]; indices[XX]++ ) for( indices[YY] = 0; indices[YY] < m->resolution[YY]; indices[YY]++ ) for( indices[XD] = 0; indices[XD] < m->resolution[XD]; indices[XD]++ ) for( indices[YD] = 0; indices[YD] < m->resolution[YD]; indices[YD]++) { if ( hypercube_valid( m, indices ) ) { count++; /* printf( "Good: %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3] ); */ } } printf( "%d valid hypercubes.\n", count ); } /************************************************************************/ /* We will interpolate over the hypercube whose 0,0,0,... corner is indices. Is the next_indices out of this hypercube? */ int out_of_box( MAP *m, int *indices, int *next_indices ) { int i; for( i = 0; i < N_STATES; i++ ) { if ( next_indices[i] > indices[i] ) return 1; if ( next_indices[i] < indices[i] - 1 ) return 1; } return 0; } /************************************************************************/ float state_distance2( float *s1, float *s2 ) { int i; float diff; float d2 = 0; for( i = 0; i < N_STATES; i++ ) { diff = s1[i] - s2[i]; d2 += diff*diff; } return d2; } /************************************************************************/ /* We are going to interpolate the V values over the hypercube Distance weighted. */ float interpolate_v_distance_weighted( MAP *m, float *state ) { int i; int bits; int indices[N_STATES]; int indices2[N_STATES]; float state2[N_STATES]; float value; float weight; float sum_values = 0; float sum_weights = 0; int count = 0; state_to_indices( m, state, indices ); /* Check all corners of a hypercube */ for ( bits = 0; bits < 16; bits++ ) { for ( i = 0; i < N_STATES; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } value = get_value( m, indices2 ); /* printf( "%d %d %d %d: %g\n", indices2[0], indices2[1], indices2[2], indices2[3], value ); */ if ( value < VERY_BIG_VALUE_TEST ) { indices_to_state( m, indices2, state2 ); weight = 1/state_distance2( state, state2 ); sum_weights += weight; sum_values += weight*value; count++; } } if ( count == 0 ) return VERY_BIG_VALUE; return sum_values/sum_weights; } /************************************************************************/ /* We are going to interpolate the V values over the hypercube N-linear interpolation. What do we do about values not filled in yet?: give up. */ float interpolate_v( MAP *m, float *state ) { int i; int bits; int indices[N_STATES]; int indices2[N_STATES]; float value; float values[16]; float values2[8]; float values3[4]; float values4[2]; float reference_state[N_STATES]; float w0, w1; int valid_values; state_to_indices( m, state, indices ); valid_values = count_valid_values( m, indices ); if ( valid_values == 0 ) return VERY_BIG_VALUE; /* if ( valid_values < 16 ) return VERY_BIG_VALUE; */ if ( valid_values < 16 ) return interpolate_v_distance_weighted( m, state ); /* Check all corners of a hypercube: Give up if any values missing. */ for ( bits = 0; bits < 16; bits++ ) { for ( i = 0; i < N_STATES; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } value = get_value( m, indices2 ); /* printf( "%d %d %d %d: %g\n", indices2[0], indices2[1], indices2[2], indices2[3], value ); */ if ( value >= VERY_BIG_VALUE_TEST ) return VERY_BIG_VALUE; values[bits] = value; } indices_to_state( m, indices, reference_state ); w0 = (state[XX] - reference_state[XX])/m->increments[XX]; w1 = 1 - w0; for( i = 0; i < 8; i++ ) { values2[i] = w1*values[2*i] + w0*values[2*i+1]; } w0 = (state[YY] - reference_state[YY])/m->increments[YY]; w1 = 1 - w0; for( i = 0; i < 4; i++ ) { values3[i] = w1*values2[2*i] + w0*values2[2*i+1]; } w0 = (state[XD] - reference_state[XD])/m->increments[XD]; w1 = 1 - w0; for( i = 0; i < 2; i++ ) { values4[i] = w1*values3[2*i] + w0*values3[2*i+1]; } w0 = (state[YD] - reference_state[YD])/m->increments[YD]; w1 = 1 - w0; /* printf( "Press return to continue.\n" ); getchar(); */ return w1*values4[0] + w0*values4[1]; } /************************************************************************/ float apply_u_until_exit( BOARD *b, MAP *m, int *indices, float *u ) { float initial_state[N_STATES]; float one_step_costs = 0; float final_value; float next_state[N_STATES]; int next_indices[N_STATES]; int status; int i; indices_to_state( m, indices, initial_state ); if ( set_state( b, initial_state, &status ) < 0 ) { fprintf( stderr, "Couldn't initialize state: %d %d %d %d: %g %g %g %g\n", indices[0], indices[1], indices[2], indices[3], initial_state[0], initial_state[1], initial_state[2], initial_state[3] ); exit( -1 ); } for( i = 0; i < MAX_STEPS_IN_TRAJECTORY_SEGMENT; i++ ) { if ( apply_action( b, u, timestep, next_state, &status ) < 0 ) return VERY_BIG_VALUE; state_to_indices( m, next_state, next_indices ); /* add up one step costs */ one_step_costs += m->one_step_cost[next_indices[XX]][next_indices[YY]] + 1.0/m->resolution[XX]; /* want penalty on commands? */ if ( out_of_box( m, indices, next_indices ) ) { final_value = interpolate_v( m, next_state ); return final_value + one_step_costs; } } /* Didn't make it out of cell */ return VERY_BIG_VALUE; } /************************************************************************/ int update_cell( BOARD *b, MAP *m, int *indices ) { float best_value; float best_u[N_ACTIONS]; float action[N_ACTIONS]; float best_action[N_ACTIONS]; int u_indices[N_ACTIONS]; float value; int iux, iuy; int i; /* printf( "update_cell: %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3] ); */ if ( m->a[indices[XX]][indices[YY]] == NULL ) { fprintf( stderr, "NULL vel_array at %d %d\n", indices[XX], indices[YY] ); exit( -1 ); } /* This is a bug best_value = m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value; */ best_value = VERY_BIG_VALUE; if ( best_value == IN_GOAL ) return 0; /* Try different u */ for( iux = 0; iux < FORCE_X_RESOLUTION; iux++ ) { for( iuy = 0; iuy < FORCE_Y_RESOLUTION; iuy++ ) { u_indices[XX] = iux; u_indices[YY] = iuy; indices_to_action( m, u_indices, action ); value = apply_u_until_exit( b, m, indices, action ); if ( value < best_value ) { best_value = value; for( i = 0; i < N_ACTIONS; i++ ) best_action[i] = action[i]; } } } if ( best_value != m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value ) { /* printf( "update_cell: %d %d %d %d: %g %g\n", indices[0], indices[1], indices[2], indices[3], m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value, best_value ); */ m->a[indices[XX]][indices[YY]]->a[indices[XD]][indices[YD]].value = best_value; for( i = 0; i < N_ACTIONS; i++ ) m->a[indices[XX]][indices[YY]]->a[indices[XD]][indices[YD]].action[i] = best_action[i]; return 1; } return 0; } /************************************************************************/ void display_n_values( MAP *m, int *indices ) { int n_valid_hypercubes = 0; int n_values = 0; float value; float minimum_value = VERY_BIG_VALUE; if ( m->status[indices[XX]][indices[YY]] != 0 ) return; if ( m->a[indices[XX]][indices[YY]] == NULL ) return; for( init_indices( 0, 2, indices, m->resolution ); test_indices( 0, 2, indices, m->resolution ); increment_indices( 0, 2, indices ) ) { for( init_indices( 0, 3, indices, m->resolution ); test_indices( 0, 3, indices, m->resolution ); increment_indices( 0, 3, indices ) ) { value = m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value; if ( value < VERY_BIG_VALUE_TEST ) { n_values++; if ( value < minimum_value ) minimum_value = value; } if ( count_valid_values( m, indices ) == 16 ) n_valid_hypercubes++; } } m->display[indices[XX]][indices[YY]][0] = ((float) n_values)/(m->resolution[XD]*m->resolution[YD]); m->display[indices[XX]][indices[YY]][1] = ((float) n_valid_hypercubes)/(m->resolution[XD]*m->resolution[YD]); m->display[indices[XX]][indices[YY]][2] = 0; if ( minimum_value < VERY_BIG_VALUE_TEST ) if ( minimum_value > overall_maximum_minimum_value ) overall_maximum_minimum_value = minimum_value; } /************************************************************************/ void update_progress_display( MAP *m ) { int indices[N_STATES]; for( init_indices( 0, 0, indices, m->resolution ); test_indices( 0, 0, indices, m->resolution ); increment_indices( 0, 0, indices ) ) { for( init_indices( 0, 1, indices, m->resolution ); test_indices( 0, 1, indices, m->resolution ); increment_indices( 0, 1, indices ) ) { display_n_values( m, indices ); } } flush_graphics(); } /************************************************************************/ void display_minimum_value( MAP *m, int *indices ) { float value; float minimum_value = VERY_BIG_VALUE; if ( m->status[indices[XX]][indices[YY]] != 0 ) return; if ( m->a[indices[XX]][indices[YY]] == NULL ) return; for( init_indices( 0, 2, indices, m->resolution ); test_indices( 0, 2, indices, m->resolution ); increment_indices( 0, 2, indices ) ) { for( init_indices( 0, 3, indices, m->resolution ); test_indices( 0, 3, indices, m->resolution ); increment_indices( 0, 3, indices ) ) { value = m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value; if ( value < VERY_BIG_VALUE_TEST ) { if ( value < minimum_value ) minimum_value = value; } } } m->display[indices[XX]][indices[YY]][0] = 0.0; m->display[indices[XX]][indices[YY]][1] = minimum_value/overall_maximum_minimum_value; m->display[indices[XX]][indices[YY]][2] = 0; } /************************************************************************/ void display_minimum_values( MAP *m ) { int indices[N_STATES]; for( init_indices( 0, 0, indices, m->resolution ); test_indices( 0, 0, indices, m->resolution ); increment_indices( 0, 0, indices ) ) { for( init_indices( 0, 1, indices, m->resolution ); test_indices( 0, 1, indices, m->resolution ); increment_indices( 0, 1, indices ) ) { display_minimum_value( m, indices ); } } flush_graphics(); } /************************************************************************/ dump_value_function( MAP *m, char *filename ) { FILE *stream; int ix, iy; int count = 0; stream = fopen( filename, "w" ); if ( stream == NULL ) { fprintf( stderr, "Can't open file %s for write.\n", filename ); exit( -1 ); } for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->a[ix][iy] == NULL ) continue; if ( fwrite( m->a[ix][iy], sizeof( VEL_ARRAY ), 1, stream ) < 1 ) { fprintf( stderr, "fwrite error at %d %d\n", ix, iy ); exit( -1 ); } count++; } } fclose( stream ); printf( "%d velocity arrays written.\n", count ); } /************************************************************************/ read_value_function( MAP *m, char *filename ) { FILE *stream; int ix, iy; int count = 0; stream = fopen( filename, "r" ); if ( stream == NULL ) { fprintf( stderr, "Can't open file %s for read.\n", filename ); exit( -1 ); } for( ix = 0; ix < m->resolution[XX]; ix++ ) { for( iy = 0; iy < m->resolution[YY]; iy++ ) { if ( m->a[ix][iy] == NULL ) continue; if ( fread( m->a[ix][iy], sizeof( VEL_ARRAY ), 1, stream ) < 1 ) { fprintf( stderr, "fread error at %d %d\n", ix, iy ); exit( -1 ); } count++; } } fclose( stream ); printf( "%d velocity arrays read.\n", count ); } /************************************************************************/ void print_vel_array( MAP *m, float *state ) { int indices[N_STATES]; int i; state_to_indices( m, state, indices ); printf( "cell: %g %g %g %g: %d %d %d %d\n", state[0], state[1], state[2], state[3], indices[0], indices[1], indices[2], indices[3] ); printf( "status: %d\n", m->status[indices[XX]][indices[YY]] ); for ( i = 0; i < N_STATES; i++ ) if ( indices[i] < 0 || indices[i] >= m->resolution[i] ) { printf( "out of bounds\n" ); return; } if ( m->a[indices[XX]][indices[YY]] == NULL ) { printf( "No VEL_ARRAY\n" ); return; } if ( m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value >= VERY_BIG_VALUE_TEST ) { printf( "value: VERY_BIG_VALUE\n" ); } printf( "v: %g; u: %g %g\n", m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].value, m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].action[0], m->a[indices[XX]][indices[YY]] ->a[indices[XD]][indices[YD]].action[1] ); } /************************************************************************/ int n_sweeps = 0; int do_sweeps( BOARD *b, MAP *m ) { int sweep_control; int indices[N_STATES]; int n_changes = 0; /* Reversing the order of these indices on every sweep will speed up the value function calculation */ for( sweep_control = 1; sweep_control < 16; sweep_control++ ) { n_changes = 0; for( init_indices( sweep_control, 0, indices, m->resolution ); test_indices( sweep_control, 0, indices, m->resolution ); increment_indices( sweep_control, 0, indices ) ) { /* printf( "sweep %d %d: %d\n", n_sweeps, sweep_control, indices[XX] ); */ for( init_indices( sweep_control, 1, indices, m->resolution ); test_indices( sweep_control, 1, indices, m->resolution ); increment_indices( sweep_control, 1, indices ) ) { if ( m->status[indices[XX]][indices[YY]] != 0 ) continue; /* printf( "sweep %d %d: %d %d\n", n_sweeps, sweep_control, indices[XX], indices[YY] ); */ for( init_indices( sweep_control, 2, indices, m->resolution ); test_indices( sweep_control, 2, indices, m->resolution ); increment_indices( sweep_control, 2, indices ) ) { for( init_indices( sweep_control, 3, indices, m->resolution ); test_indices( sweep_control, 3, indices, m->resolution ); increment_indices( sweep_control, 3, indices ) ) { n_changes += update_cell( b, m, indices ); } } display_n_values( m, indices ); flush_graphics(); } } if ( n_changes == 0 ) return n_changes; n_sweeps++; printf( "Done sweep %d %d: %d changes.\n", n_sweeps, sweep_control, n_changes ); count_valid_hypercubes( m ); update_progress_display( m ); dump_value_function( m, "sidp1.value" ); } return n_changes; } /************************************************************************/ /* This version only considerers cells on corners of hypercube surrounding query */ int apply_policy_hypercube( MAP *m, float *state, float *action, int *indices ) { int indices2[N_STATES]; float state2[N_STATES]; float best_distance = VERY_BIG_VALUE; float best_action[N_ACTIONS]; int best_indices[N_STATES]; int i; int bits; VEL_ARRAY *va; float distance; int is_bad_flag = 0; state_to_indices( m, state, indices ); /* printf( "\napply_policy: %g %g %g %g: %d %d %d %d\n", state[0], state[1], state[2], state[3], indices[0], indices[1], indices[2], indices[3] ); */ /* Check all corners of a hypercube */ for ( bits = 0; bits < 16; bits++ ) { for ( i = 0; i < N_STATES; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } /* printf( " checking %d %d %d %d: ", indices2[0], indices2[1], indices2[2], indices2[3] ); */ for ( i = 0; i < N_STATES; i++ ) if ( indices2[i] < 0 || indices2[i] >= m->resolution[i] ) { is_bad_flag = 1; /* printf( "out of bounds.\n" ); */ } if ( is_bad_flag ) continue; va = m->a[indices2[XX]][indices2[YY]]; if ( va == NULL ) { /* printf( "no VEL_ARRAY\n" ); */ continue; } if ( m->status[indices2[XX]][indices2[YY]] == OBSTACLE || m->status[indices2[XX]][indices2[YY]] == HOLE ) { /* printf( "obstacle or hole\n" ); */ continue; } if ( va->a[indices2[XD]][indices2[YD]].value >= VERY_BIG_VALUE_TEST ) { /* printf( "bad value.\n" ); */ continue; } indices_to_state( m, indices2, state2 ); distance = state_distance2( state, state2 ); /* printf( "distance: %g; %g %g\n", distance, va->a[indices[XD]][indices[YD]].action[0], va->a[indices[XD]][indices[YD]].action[1] ); */ if ( distance < best_distance ) { best_distance = distance; for( i = 0; i < N_ACTIONS; i++ ) best_action[i] = va->a[indices[XD]][indices[YD]].action[i]; for( i = 0; i < N_STATES; i++ ) best_indices[i] = indices2[i]; } } if ( best_distance >= VERY_BIG_VALUE_TEST ) return -1; for( i = 0; i < N_ACTIONS; i++ ) action[i] = best_action[i]; for( i = 0; i < N_STATES; i++ ) indices[i] = best_indices[i]; /* printf( "Best: %d %d %d %d: %g %g\n\n", indices[0], indices[1], indices[2], indices[3], action[0], action[1] ); */ return TRUE; } /************************************************************************/ /* This version checks for nearest point including all velocities */ int apply_policy_all_velocity( MAP *m, float *state, float *action, int *indices ) { int indices2[N_STATES]; float state2[N_STATES]; float best_distance = VERY_BIG_VALUE; float best_action[N_ACTIONS]; int best_indices[N_STATES]; int i; int bits; VEL_ARRAY *va; float distance; int is_bad_flag = 0; state_to_indices( m, state, indices ); /* printf( "\napply_policy: %g %g %g %g: %d %d %d %d\n", state[0], state[1], state[2], state[3], indices[0], indices[1], indices[2], indices[3] ); */ /* For surrounding positions. */ for ( bits = 0; bits < 4; bits++ ) { for ( i = 0; i < 2; i++ ) { if ( ((bits >> i) & 1) == 0 ) indices2[i] = indices[i]; else indices2[i] = indices[i] + 1; } if ( indices2[XX] < 0 || indices2[XX] >= m->resolution[XX] || indices2[YY] < 0 || indices2[YY] >= m->resolution[YY] ) continue; if ( m->status[indices2[XX]][indices2[YY]] == OBSTACLE || m->status[indices2[XX]][indices2[YY]] == HOLE ) continue; for( indices2[XD] = 0; indices2[XD] < m->resolution[XD]; ++(indices2[XD]) ) for( indices2[YD] = 0; indices2[YD] < m->resolution[YD]; ++(indices2[YD]) ) { va = m->a[indices2[XX]][indices2[YY]]; if ( va == NULL ) continue; if ( va->a[indices2[XD]][indices2[YD]].value >= VERY_BIG_VALUE_TEST ) continue; indices_to_state( m, indices2, state2 ); distance = state_distance2( state, state2 ); if ( distance < best_distance ) { best_distance = distance; for( i = 0; i < N_ACTIONS; i++ ) best_action[i] = va->a[indices[XD]][indices[YD]].action[i]; for( i = 0; i < N_STATES; i++ ) best_indices[i] = indices2[i]; } } } if ( best_distance >= VERY_BIG_VALUE_TEST ) return -1; for( i = 0; i < N_ACTIONS; i++ ) action[i] = best_action[i]; for( i = 0; i < N_STATES; i++ ) indices[i] = best_indices[i]; /* printf( "Best: %d %d %d %d: %g %g\n\n", indices[0], indices[1], indices[2], indices[3], action[0], action[1] ); */ return TRUE; } /************************************************************************/ int compute_action( BOARD *b, MAP *m, float *state, float *action, float *next_state, int *indices ) { int status; if ( apply_policy_all_velocity( m, state, action, indices ) < 0 ) return -1; if ( set_state( b, state, &status ) < 0 ) return -1; if ( apply_action( b, action, timestep, next_state, &status ) < 0 ) return -1; return TRUE; } /************************************************************************/ int plan_trajectory( BOARD *b, MAP *m, float *state, char *filename ) { FILE *stream; float action[N_ACTIONS]; float next_state[N_STATES]; int indices[N_STATES]; int status; int i, j; stream = fopen( filename, "w" ); if ( stream == NULL ) { fprintf( stderr, "Can't open file %s for write.\n", filename ); exit( -1 ); } for( i = 0; /* i < 1000 */ 1; i++ ) { /* Not handling goal quite right, could exit again. */ if ( in_goal( b, state ) ) { printf( "in goal.\n" ); goal_policy( b, state, action ); } else { if ( compute_action( b, m, state, action, next_state, indices ) < 0 ) break; } printf( "%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", state[XX], state[YY], state[XD], state[YD], action[XX], action[YY] ); fprintf( stream, "%7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", state[XX], state[YY], state[XD], state[YD], action[XX], action[YY] ); /* print_vel_array( m, state ); */ if ( in_goal( b, state ) ) { fclose( stream ); return TRUE; } m->display[indices[XX]][indices[YY]][0] = 1.0; m->display[indices[XX]][indices[YY]][1] = 0.0; m->display[indices[XX]][indices[YY]][2] = 0.0; for( j = 0; j < N_STATES; j++ ) state[j] = next_state[j]; flush_graphics(); } printf( "Trajectory failed to reach goal.\n" ); fclose( stream ); return -1; } /************************************************************************/ /************************************************************************/ int main( int argc, char **argv ) { int wx, wy; float ratio; BOARD *b; MAP *m; int n_changes; int ix, iy; float start_state[N_STATES]; /* Set up a board */ b = the_board = &a_board; initialize_maze( "board0", b ); srand( 2 ); /* Set up planning map */ m = the_map = &a_map; init_glut( argc, argv, ( b->board.p[TOP] - b->board.p[BOTTOM] ) /( b->board.p[RIGHT] - b->board.p[LEFT] ) ); init_display( m ); b->random_force_0 = 0.0; init_map( b, m ); sidp1_map_obstacles( b, m ); sidp1_map_holes( b, m ); initialize_value_function( b, m ); calculate_one_step_costs( m ); identify_goal_states( b, m ); read_value_function( m, "sidp1-execute.value" ); update_progress_display( m ); count_valid_hypercubes( m ); /* printf( "Press return to continue.\n" ); getchar(); */ display_minimum_values( m ); start_state[XX] = b->start[XX]; start_state[YY] = b->start[YY]; start_state[XD] = 0; start_state[YD] = 0; /* board0 starts */ /* start_state[XX] = 0.015; start_state[YY] = 0.015; start_state[XX] = 0.25; start_state[YY] = 0.015; start_state[XX] = 0.015; start_state[YY] = 0.21; */ /* board1 starts */ /* start_state[XX] = 0.053; start_state[YY] = 0.050; */ /* board2 starts */ /* start_state[XX] = 0.215; start_state[YY] = 0.150; start_state[XX] = 0.216; start_state[YY] = 0.150; start_state[XX] = 0.175; start_state[YY] = 0.115; start_state[XX] = 0.180; start_state[YY] = 0.110; start_state[XX] = 0.190; start_state[YY] = 0.110; start_state[XX] = 0.195; start_state[YY] = 0.110; start_state[XX] = 0.195; start_state[YY] = 0.100; start_state[XX] = 0.195; start_state[YY] = 0.090; start_state[XX] = 0.190; start_state[YY] = 0.100; start_state[XX] = 0.086; start_state[YY] = 0.160; start_state[XX] = 0.045; start_state[YY] = 0.170; start_state[XX] = 0.035; start_state[YY] = 0.145; */ print_vel_array( m, start_state ); plan_trajectory( b, m, start_state, "sidp1-execute.new" ); printf( "DONE\n" ); glutMainLoop(); return 0; } /************************************************************************/