double
SpeedControlLogic(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
	double v_scl = simulator_config->max_v;
	//todo transformar esses valores em parametros no ini

	double a_scl = 0.1681;
	double b_scl = -0.0049;
	double safety_factor = 1;

	double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);

	double v_cmd = fabs(simulator_config->target_v);

	double v_cmd_max = carmen_fmax(v_scl, (kt + simulator_config->delta_t  - a_scl) / b_scl);

	double k_max_scl = carmen_fmin(simulator_config->maximum_steering_command_curvature, a_scl + b_scl * v_cmd);

	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);
	if (fabs(kt_dt) >= k_max_scl)
		v_cmd = fabs(safety_factor * v_cmd_max);

	if(pose.v != 0)
		return v_cmd * (pose.v / fabs(pose.v));
	else
		return 0;
}
double
compute_curvature(double phi, carmen_simulator_ackerman_config_t *simulator_config)
{
	double curvature;

	curvature = carmen_get_curvature_from_phi(phi, simulator_config->v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);

	return (curvature);
}
Robot_State
TrajectoryLookupTable::predict_next_pose(Robot_State &robot_state, Command &requested_command,
		double full_time_interval, double *distance_traveled, double delta_t)
{
	int n = floor(full_time_interval / delta_t);
	double remaining_time = full_time_interval - ((double) n * delta_t);
	Robot_State achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo

	double curvature = carmen_get_curvature_from_phi(
			requested_command.phi, requested_command.v,
			GlobalState::robot_config.understeer_coeficient,
			GlobalState::robot_config.distance_between_front_and_rear_axles);

	double new_curvature = carmen_get_curvature_from_phi(
			achieved_robot_state.v_and_phi.phi, achieved_robot_state.v_and_phi.v,
			GlobalState::robot_config.understeer_coeficient,
			GlobalState::robot_config.distance_between_front_and_rear_axles);

	double max_curvature_change = GlobalState::robot_config.maximum_steering_command_rate * delta_t;

	// Euler method
	for (int i = 0; i < n; i++)
	{
		double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
				new_curvature, curvature, max_curvature_change);

		if (distance_traveled)
			*distance_traveled += dist_walked;
	}

	if (remaining_time > 0.0)
	{
		double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
				new_curvature, curvature, max_curvature_change);

		if (distance_traveled)
			*distance_traveled += dist_walked;
	}

	achieved_robot_state.pose.theta = carmen_normalize_theta(achieved_robot_state.pose.theta);
	robot_state = achieved_robot_state;

	return (achieved_robot_state);
}
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;
}
Exemplo n.º 5
0
carmen_ackerman_traj_point_t
carmen_libcarmodel_recalc_pos_ackerman(carmen_ackerman_traj_point_t robot_state, double target_v, double target_phi,
		double full_time_interval, double *distance_traveled, double delta_t, carmen_robot_ackerman_config_t robot_config)
{
	double a = (target_v - robot_state.v) / full_time_interval;
	int n = floor(full_time_interval / delta_t);
	double remaining_time = full_time_interval - ((double) n * delta_t);
	carmen_ackerman_traj_point_t achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo

	double curvature = carmen_get_curvature_from_phi(target_phi, target_v,
			robot_config.understeer_coeficient,	robot_config.distance_between_front_and_rear_axles);

	double new_curvature = carmen_get_curvature_from_phi(achieved_robot_state.phi, achieved_robot_state.v,
			robot_config.understeer_coeficient,	robot_config.distance_between_front_and_rear_axles);

	double max_curvature_change = robot_config.maximum_steering_command_rate * delta_t;

	// Euler method
	for (int i = 0; i < n; i++)
	{
		double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
				new_curvature, curvature, max_curvature_change, robot_config);

		if (distance_traveled)
			*distance_traveled += dist_walked;
	}

	if (remaining_time > 0.0)
	{
		double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
				new_curvature, curvature, max_curvature_change, robot_config);

		if (distance_traveled)
			*distance_traveled += dist_walked;
	}

	achieved_robot_state.theta = carmen_normalize_theta(achieved_robot_state.theta);
	robot_state = achieved_robot_state;

	return (achieved_robot_state);
}
carmen_ackerman_path_point_t
motionModel(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
	pose.x += pose.v * simulator_config->delta_t * cos(pose.theta);
	pose.y +=  pose.v * simulator_config->delta_t * sin(pose.theta);
	pose.theta += pose.v * simulator_config->delta_t * carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
	pose.theta = carmen_normalize_theta(pose.theta);
	//todo como tratar o delay?

	pose = DynamicsResponse(pose, simulator_config);
	return pose;
}
Exemplo n.º 7
0
bool
Obstacle_Detection::is_obstacle_path2(Robot_State &robot_state, const Command &requested_command, double full_time_interval)
{
	double delta_t = 0.09;

	int n = floor(full_time_interval / delta_t);
	double remaining_time = full_time_interval - ((double) n * delta_t);
	Robot_State achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo

#ifdef OLD_STEERING_CONTROL
	double curvature = carmen_get_curvature_from_phi(
			requested_command.phi, requested_command.v,
			GlobalState::robot_config.understeer_coeficient,
			GlobalState::robot_config.distance_between_front_and_rear_axles);

	double new_curvature = carmen_get_curvature_from_phi(
			achieved_robot_state.v_and_phi.phi, achieved_robot_state.v_and_phi.v,
			GlobalState::robot_config.understeer_coeficient,
			GlobalState::robot_config.distance_between_front_and_rear_axles);

	double max_curvature_change = GlobalState::robot_config.desired_steering_command_rate * delta_t;
#else
	double max_phi_velocity = GlobalState::max_phi_velocity;
	double max_phi_acceleration = GlobalState::max_phi_acceleration;

	double phi_velocity = 0.0;
	// O codigo abaixo assume phi_velocity == 0.0 no início do full_time_interval
	double t_fim_descida;
	double t_fim_plato;
	double t_fim_subida;
	Ackerman::compute_intermediate_times(t_fim_subida, t_fim_plato, t_fim_descida, max_phi_acceleration, robot_state.v_and_phi.phi, requested_command.phi,
			full_time_interval,	max_phi_velocity);
#endif

	// Euler method
	for (int i = 0; i < n; i++)
	{
#ifdef OLD_STEERING_CONTROL
		Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, delta_t,
				new_curvature, curvature, max_curvature_change);
#else
		double t = (double) i * delta_t;
		Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, phi_velocity,
				t, delta_t,
				t_fim_descida, t_fim_plato, t_fim_subida,
				max_phi_velocity, max_phi_acceleration);
#endif

		if (car_border_verification(achieved_robot_state.pose))
			return true;
	}

	if (remaining_time > 0.0)
	{
#ifdef OLD_STEERING_CONTROL
		Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, delta_t,
				new_curvature, curvature, max_curvature_change);
#else
		double t = (double) n * delta_t;
		Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, phi_velocity,
				t, remaining_time,
				t_fim_descida, t_fim_plato, t_fim_subida,
				max_phi_velocity, max_phi_acceleration);
#endif

		if (car_border_verification(achieved_robot_state.pose))
			return true;
	}

	return false;
}