def generateUnivectorField(r_x: int, r_y: int, ball_pos: Tuple[int, int], obs_pos: List[Tuple[int, int]], de: float = de, v_obs: list = v_obstacle(), v_rob: list = v_robot(), ko: float = ko, delta: float = delta, d_min: float = d_min) -> float: ball_x, ball_y = ball_pos d_ball_x, d_ball_y = math_utils.delta_axis(ball_x, ball_y, r_x, r_y) theta = phiR(d_ball_x, d_ball_y) phi_tuf = phiTuf(theta, d_ball_x, d_ball_y, de) obstacle = closestObstacle(r_x, r_y, obs_pos) obs_x, obs_y = obstacle robot_obs_x, robot_obs_y = math_utils.delta_axis(obs_x, obs_y, r_x, r_y) R = math_utils.norm(robot_obs_x, robot_obs_y) robot_obs_dist = math_utils.norm(robot_obs_x, robot_obs_y) phi_auf = phiAuf(obs_x, obs_y, r_x, r_y, robot_obs_dist, v_obs, v_rob, ko) phi_composed = phiComposed(phi_tuf, phi_auf, R, obstacle, delta, d_min) return Nh(phi_composed)
def repulsive(r_x: int, r_y: int, __, obs_pos: Tuple[int, int]) -> List[float]: obs_x, obs_y = obs_pos[0] delta_x, delta_y = math_utils.delta_axis(obs_x, obs_y, r_x, r_y) phi_r = univector.phiR(delta_x, delta_y) return univector.Nh(phi_r)
def phiAuf(obs_x: int, obs_y: int, r_x: int, r_y: int, r_o_dist: float, v_obs: list = v_obstacle(), v_rob: list = v_robot(), ko: float = ko) -> float: # Avoid Obstacles ''' Returns an avoidance coefficient, relative to the obstacle, considering the obstacle's position and velocity, as well as the robot's position and velocity ''' obstacle_position = np.array([obs_x, obs_y]) s_vec = ko * (v_obs - v_rob) s_norm = math_utils.norm(s_vec[0], s_vec[1]) obs_robot_dist = r_o_dist if obs_robot_dist >= s_norm: p_line_obs = obstacle_position + s_vec else: p_line_obs = obstacle_position + obs_robot_dist * s_vec / s_norm delta_x, delta_y = math_utils.delta_axis(p_line_obs[0], p_line_obs[1], r_x, r_y) phi_auf = phiR(delta_x, delta_y) return math_utils.wrapToPi(phi_auf)
def moveToGoal(r_x: int, r_y: int, ball_pos: Tuple[int, int]) -> List[float]: ball_x, ball_y = ball_pos delta_x, delta_y = math_utils.delta_axis(ball_x, ball_y, r_x, r_y) theta = univector.phiR(delta_x, delta_y) phi_tuf = univector.phiTuf(theta, delta_x, delta_y) return univector.Nh(phi_tuf)
def avoidObstacles(r_x: int, r_y: int, __, obs_pos: Tuple[int, int]) -> List[float]: obs_x, obs_y = obs_pos[0] robot_obs_x, robot_obs_y = math_utils.delta_axis(obs_x, obs_y, r_x, r_y) robot_obs_dist = math_utils.norm(robot_obs_x, robot_obs_y) phi_auf = univector.phiAuf(obs_x, obs_y, r_x, r_y, robot_obs_dist) return univector.Nh(phi_auf)
def hyperbolic(r_x: int, r_y: int, ball_pos: Tuple[int, int]) -> List[float]: ball_x, ball_y = ball_pos delta_x, delta_y = math_utils.delta_axis(ball_x, ball_y, r_x, r_y) theta = univector.phiR(delta_x, delta_y) rho = math_utils.norm(delta_x, delta_y) phi_h = univector.phiH(rho, theta) return univector.Nh(phi_h)
def composition(r_x: int, r_y: int, ball_pos: Tuple[int, int], obs_pos: List[Tuple[int, int]]) -> List[float]: ball_x, ball_y = ball_pos d_ball_x, d_ball_y = math_utils.delta_axis(ball_x, ball_y, r_x, r_y) theta = univector.phiR(d_ball_x, d_ball_y) phi_tuf = univector.phiTuf(theta, d_ball_x, d_ball_y) obstacle = univector.closestObstacle(r_x, r_y, obs_pos) obs_x, obs_y = obstacle robot_obs_x, robot_obs_y = math_utils.delta_axis(obs_x, obs_y, r_x, r_y) R = math_utils.norm(robot_obs_x, robot_obs_y) robot_obs_dist = math_utils.norm(robot_obs_x, robot_obs_y) phi_auf = univector.phiAuf(obs_x, obs_y, r_x, r_y, robot_obs_dist) phi_composed = univector.phiComposed(phi_tuf, phi_auf, R, obstacle) return univector.Nh(phi_composed)
def closestObstacle(x: int, y: int, obstacles: List[Tuple[int, int]]) -> Tuple[int, int]: ''' Given an obstacles list, return the obstacle closest to the robot ''' last_ro = 0 count = 0 for obstacle in obstacles: obs_x, obs_y = obstacle delta_x, delta_y = math_utils.delta_axis(x, y, obs_x, obs_y) if not count: last_ro = math_utils.norm(delta_x, delta_y) if (math_utils.norm(delta_x, delta_y) <= last_ro): closer_obs = obstacle last_ro = math_utils.norm(delta_x, delta_y) count += 1 return closer_obs