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
