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");
}
