void qlearning_integrator(double *steering_command, int b) { int i; double steering = 0.0; exhaustive_value_to_steering_mapping mapping[] = { //{0, -50.000}, {1, -25.000}, {2, -12.500}, {3, -06.250}, {4, -03.125}, //{5, 00.000}, //{6, 03.125}, {7, 06.250}, {8, 12.500}, {9, 25.000}, {10, 50.000} {0, -50.0}, {1, -20.0}, {2, -06.0}, {3, 0}, {4, 06.0}, {5, 20.0}, {6, 50.0} }; for (i = 0; i <= 6; i ++) { if (b == mapping[i].exhaustive_value) { steering = mapping[i].steering_output; break; } } *steering_command += steering; *steering_command = carmen_clamp(-100.0, *steering_command, 100.0); }
double predict_next_pose_step(carmen_ackerman_traj_point_p new_robot_state, double target_v, double a, double delta_t, double &achieved_curvature, const double &desired_curvature, double &max_curvature_change, carmen_robot_ackerman_config_t robot_config) { carmen_ackerman_traj_point_t initial_robot_state = *new_robot_state; double delta_curvature = fabs(achieved_curvature - desired_curvature); double command_curvature_signal = (achieved_curvature < desired_curvature) ? 1.0 : -1.0; achieved_curvature = achieved_curvature + command_curvature_signal * fmin(delta_curvature, max_curvature_change); new_robot_state->phi = carmen_get_phi_from_curvature(achieved_curvature, initial_robot_state.v, robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles); new_robot_state->phi = carmen_clamp(-robot_config.max_phi, new_robot_state->phi, robot_config.max_phi); double v0 = initial_robot_state.v; double s = v0 * delta_t + 0.5 * a * delta_t * delta_t; double move_x = s * cos(initial_robot_state.theta); double move_y = s * sin(initial_robot_state.theta); new_robot_state->x += move_x; new_robot_state->y += move_y; new_robot_state->theta += s * tan(new_robot_state->phi) / robot_config.distance_between_front_and_rear_axles; new_robot_state->v = target_v; return sqrt(move_x * move_x + move_y * move_y); }
// EQUATION 1 void update_plant_input_u(double atan_desired_curvature, double atan_current_curvature, double delta_t, rl_data* data) { data->variables.U[2] = data->variables.U[1]; //Update the past u values data->variables.U[1] = data->variables.U[0]; double integral_t_1 = data->variables.error_order[1]; double u_t; // u(t) -> actuation in time t if (delta_t == 0.0) { data->variables.U[0] = 0; return; } calculate_pid_errors(atan_desired_curvature, atan_current_curvature, delta_t, data); u_t = (data->variables.pid_params[0] * data->variables.error_order[0]) + (data->variables.pid_params[1] * data->variables.error_order[1]) + (data->variables.pid_params[2] * data->variables.error_order[2]); data->variables.error[2] = data->variables.error[1]; data->variables.error[1] = data->variables.error[0]; data->variables.error[0] = data->variables.error_order[0]; // Anti windup if ((u_t < -100.0) || (u_t > 100.0)) data->variables.error_order[1] = integral_t_1; u_t = carmen_clamp(-100.0, u_t, 100.0); data->variables.U[0] = -u_t;//carmen_libpid_steering_PID_controler(atan_desired_curvature, atan_current_curvature, delta_t); }
void carmen_robot_send_base_velocity_command(void) { IPC_RETURN_TYPE err; static carmen_base_velocity_message v; v.host = carmen_get_host(); if (collision_avoidance) { #ifndef COMPILE_WITHOUT_LASER_SUPPORT if (use_laser) { command_tv = carmen_clamp(carmen_robot_laser_min_rear_velocity(), command_tv, carmen_robot_laser_max_front_velocity()); } #endif if (use_sonar) command_tv = carmen_clamp(carmen_robot_sonar_min_rear_velocity(), command_tv, carmen_robot_sonar_max_front_velocity()); if (use_bumper && carmen_robot_bumper_on()) { command_tv = 0; command_rv = 0; } } if (!carmen_robot_config.allow_rear_motion && command_tv < 0) command_tv = 0.0; v.tv = carmen_clamp(-carmen_robot_config.max_t_vel, command_tv, carmen_robot_config.max_t_vel); v.rv = carmen_clamp(-carmen_robot_config.max_r_vel, command_rv, carmen_robot_config.max_r_vel); v.timestamp = carmen_get_time(); err = IPC_publishData(CARMEN_BASE_VELOCITY_NAME, &v); carmen_test_ipc(err, "Could not publish", CARMEN_BASE_VELOCITY_NAME); }
carmen_ackerman_path_point_t DynamicsResponse(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config) { double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles); double kt_dt = carmen_get_curvature_from_phi(simulator_config->target_phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles); double dk_dt_cmd = carmen_clamp(0 , kt_dt - kt, simulator_config->maximum_steering_command_rate); //todo descobrir qual é o comando minimo de curvatura, se existir double dv_dt_cmd; simulator_config->target_v = SpeedControlLogic(pose, simulator_config); pose.phi = get_phi_from_curvature(carmen_clamp(-simulator_config->maximum_steering_command_curvature, kt + dk_dt_cmd * simulator_config->delta_t, simulator_config->maximum_steering_command_curvature), simulator_config); if (pose.v >= 0) dv_dt_cmd = carmen_clamp(-simulator_config->maximum_deceleration_forward, simulator_config->target_v - pose.v, simulator_config->maximum_acceleration_forward); else dv_dt_cmd = carmen_clamp(-simulator_config->maximum_deceleration_reverse, simulator_config->target_v - pose.v, simulator_config->maximum_acceleration_reverse); pose.v = pose.v + dv_dt_cmd * simulator_config->delta_t; return pose; }
double compute_new_velocity(carmen_simulator_ackerman_config_t *simulator_config) { double minimum_time_to_reach_desired_velocity; double acceleration; int signal_target_v, signal_v, command_signal; double target_v, time, time_rest; signal_target_v = simulator_config->target_v >= 0 ? 1 : -1; signal_v = simulator_config->v >= 0 ? 1 : -1; target_v = simulator_config->target_v; if (signal_target_v != signal_v) { target_v = 0; } acceleration = get_acceleration(simulator_config->v, target_v, simulator_config); minimum_time_to_reach_desired_velocity = fabs((target_v - simulator_config->v) / acceleration); // s = v / a, i.e., s = (m/s) / (m/s^2) command_signal = simulator_config->v < target_v ? 1 : -1; time = fmin(simulator_config->delta_t, minimum_time_to_reach_desired_velocity); simulator_config->v += command_signal * acceleration * time; if (signal_target_v != signal_v) { time_rest = simulator_config->delta_t - time; target_v = simulator_config->target_v; acceleration = get_acceleration(simulator_config->v, target_v, simulator_config); minimum_time_to_reach_desired_velocity = fabs((target_v - simulator_config->v) / acceleration); // s = v / a, i.e., s = (m/s) / (m/s^2) command_signal = simulator_config->v < target_v ? 1 : -1; time = fmin(time_rest, minimum_time_to_reach_desired_velocity); simulator_config->v += command_signal * acceleration * time; } simulator_config->v = carmen_clamp( -simulator_config->maximum_speed_reverse, simulator_config->v, simulator_config->maximum_speed_forward); return (simulator_config->v); }
void carmen_robot_send_base_velocity_command(void) { IPC_RETURN_TYPE err; char *host; static carmen_base_velocity_message v; static int first = 1; if(first) { host = carmen_get_tenchar_host_name(); strcpy(v.host, host); first = 0; } v.tv = carmen_clamp(-carmen_robot_config.max_t_vel, command_tv, carmen_robot_config.max_t_vel); v.rv = carmen_clamp(-carmen_robot_config.max_r_vel, command_rv, carmen_robot_config.max_r_vel); v.timestamp = carmen_get_time_ms(); err = IPC_publishData(CARMEN_BASE_VELOCITY_NAME, &v); carmen_test_ipc(err, "Could not publish", CARMEN_BASE_VELOCITY_NAME); }
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); }
void carmen_conventional_build_costs(carmen_robot_config_t *robot_conf, carmen_map_point_t *robot_posn, carmen_navigator_config_t *navigator_conf) { int x_index = 0, y_index = 0; double value; double *cost_ptr; float *map_ptr; double resolution; int index; int x, y; int x_start, y_start; int x_end, y_end; double robot_distance; carmen_verbose("Building costs..."); if (x_size != carmen_planner_map->config.x_size || y_size != carmen_planner_map->config.y_size) { free(costs); costs = NULL; free(utility); utility = NULL; } x_size = carmen_planner_map->config.x_size; y_size = carmen_planner_map->config.y_size; if (costs == NULL) { costs = (double *)calloc(x_size*y_size, sizeof(double)); carmen_test_alloc(costs); for (index = 0; index < x_size*y_size; index++) costs[index] = carmen_planner_map->complete_map[index]; } resolution = carmen_planner_map->config.resolution; if (robot_posn && navigator_conf) { x_start = robot_posn->x - navigator_conf->map_update_radius/ robot_posn->map->config.resolution; x_start = carmen_clamp(0, x_start, x_size); y_start = robot_posn->y - navigator_conf->map_update_radius/ robot_posn->map->config.resolution; y_start = carmen_clamp(0, y_start, y_size); x_end = robot_posn->x + navigator_conf->map_update_radius/ robot_posn->map->config.resolution; x_end = carmen_clamp(0, x_end, x_size); y_end = robot_posn->y + navigator_conf->map_update_radius/ robot_posn->map->config.resolution; y_end = carmen_clamp(0, y_end, y_size); cost_ptr = costs+x_start*y_size; map_ptr = carmen_planner_map->complete_map+x_start*y_size; for (x_index = x_start; x_index < x_end; x_index++) { for (y_index = y_start; y_index < y_end; y_index++) cost_ptr[y_index] = map_ptr[y_index]; cost_ptr += y_size; map_ptr += y_size; } } else { x_start = 0; y_start = 0; x_end = x_size; y_end = y_size; for (index = 0; index < x_size*y_size; index++) costs[index] = carmen_planner_map->complete_map[index]; } /* Initialize cost function to match map, where empty cells have MIN_COST cost and filled cells have MAX_UTILITY cost */ for (x_index = x_start; x_index < x_end; x_index++) { cost_ptr = costs+x_index*y_size+y_start; for (y_index = y_start; y_index < y_end; y_index++) { value = *(cost_ptr); if (value >= 0 && value < MIN_COST) value = MIN_COST; else value = MAX_UTILITY; *(cost_ptr++) = value; } } /* Loop through cost map, starting at top left, updating cost of cell as max of itself and most expensive neighbour less the cost downgrade. */ for (x_index = x_start; x_index < x_end; x_index++) { cost_ptr = costs+x_index*y_size+y_start; for (y_index = y_start; y_index < y_end; y_index++, cost_ptr++) { if (x_index < 1 || x_index >= x_size-1 || y_index < 1 || y_index >= y_size-1) continue; for (index = 0; index < NUM_ACTIONS; index++) { x = x_index + carmen_planner_x_offset[index]; y = y_index + carmen_planner_y_offset[index]; value = *(costs+x*y_size+y) - resolution*MAX_UTILITY; if (value > *cost_ptr) *cost_ptr = value; } } } /* Loop through cost map again, starting at bottom right, updating cost of cell as max of itself and most expensive neighbour less the cost downgrade. */ for (x_index = x_end-1; x_index >= x_start; x_index--) { cost_ptr = costs+x_index*y_size+y_end-1; for (y_index = y_end-1; y_index >= y_start; y_index--, cost_ptr--) { if (x_index < 1 || x_index >= x_size-1 || y_index < 1 || y_index >= y_size-1) continue; for (index = 0; index < NUM_ACTIONS; index++) { x = x_index + carmen_planner_x_offset[index]; y = y_index + carmen_planner_y_offset[index]; value = *(costs+x*y_size+y) - resolution*MAX_UTILITY; if (value > *cost_ptr) *cost_ptr = value; } } } robot_distance = robot_conf->width/2*MAX_UTILITY; /* Clamp any cell that's closer than the robot width to be impassable. Also, rescale cost function so that the max cost is 0.5 */ for (x_index = x_start; x_index < x_end; x_index++) { cost_ptr = costs+x_index*y_size+y_start; for (y_index = y_start; y_index < y_end; y_index++) { value = *(cost_ptr); if (value < MAX_UTILITY - robot_distance) { value = value / (2*MAX_UTILITY); if (value < MIN_COST) value = MIN_COST; } else value = 1.0; *(cost_ptr++) = value; } } if (robot_posn != NULL) { x_index = robot_posn->x / carmen_planner_map->config.resolution; y_index = robot_posn->y / carmen_planner_map->config.resolution; if (x_index >= 0 && x_index < x_size && y_index >= 0 && y_index < y_size) *(costs+x_index*y_size+y_index) = MIN_COST; } carmen_verbose("done\n"); }