def planning_from_origin(self, gx, gy, gtheta):
D = hypot(gx, gy)
d = D * self.max_curvature
theta = mod2pi(atan2(gy, gx))
alpha = mod2pi(-theta)
beta = mod2pi(gtheta - theta)
best_cost = float("inf")
bt, bp, bq, best_mode = None, None, None, None
for planner in self.planners:
t, p, q, mode = planner(alpha, beta, d)
if t is None:
continue
cost = abs(t) + abs(p) + abs(q)
if best_cost > cost:
bt, bp, bq, best_mode = t, p, q, mode
best_cost = cost
lengths = [bt, bp, bq]
x_list, y_list, theta_list, directions = self.generate_local_course(
sum(lengths),
lengths,
best_mode,
)
return x_list, y_list, theta_list, best_mode, best_cost
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
rec = (6.0 - dist**2 + 2.0 * cos_a_b + 2.0 * dist *
(sin_b - sin_a)) / 8.0
if abs(rec) > 1.0:
return None, None, None
p_lrl = mod2pi(2 * pi - acos(rec))
t_lrl = mod2pi(-alpha - atan2(cos_a - cos_b, dist + sin_a - sin_b) +
p_lrl / 2.0)
q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))
return t_lrl, p_lrl, q_lrl
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
rec = (6.0 - dist**2 + 2.0 * cos_a_b + 2.0 * dist *
(sin_a - sin_b)) / 8.0
if abs(rec) > 1.0:
return None, None, None
p_rlr = mod2pi(2 * pi - acos(rec))
t_rlr = mod2pi(alpha - atan2(cos_a - cos_b, dist - sin_a + sin_b) +
mod2pi(p_rlr / 2.0))
q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))
return t_rlr, p_rlr, q_rlr
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
p_rsl = -2 + dist**2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
if p_rsl < 0:
return None, None, None
else:
p_rsl = sqrt(p_rsl)
rec = atan2(cos_a + cos_b, dist - sin_a - sin_b) - atan2(2.0, p_rsl)
t_rsl = mod2pi(alpha - rec)
q_rsl = mod2pi(beta - rec)
return t_rsl, p_rsl, q_rsl
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
p_lsr = -2 + dist**2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
if p_lsr < 0:
return None, None, None
else:
p_lsr = sqrt(p_lsr)
rec = atan2(-cos_a - cos_b, dist + sin_a + sin_b) - atan2(-2.0, p_lsr)
t_lsr = mod2pi(-alpha + rec)
q_lsr = mod2pi(-mod2pi(beta) + rec)
return t_lsr, p_lsr, q_lsr
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
p_rsr = 2 + dist**2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
if p_rsr < 0:
return None, None, None
else:
p_rsr = sqrt(p_rsr)
denominate = dist - sin_a + sin_b
t_rsr = mod2pi(alpha - atan2(cos_a - cos_b, denominate))
q_rsr = mod2pi(-beta + atan2(cos_a - cos_b, denominate))
return t_rsr, p_rsr, q_rsr
def fit(self, alpha, beta, dist):
sin_a, sin_b, cos_a, cos_b, cos_a_b = self._calc_sines(alpha, beta)
p_lsl = 2 + dist**2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
if p_lsl < 0:
return (
None,
None,
None,
)
else:
p_lsl = sqrt(p_lsl)
denominate = dist + sin_a - sin_b
t_lsl = mod2pi(-alpha + atan2(cos_b - cos_a, denominate))
q_lsl = mod2pi(beta - atan2(cos_b - cos_a, denominate))
return (
t_lsl,
p_lsl,
q_lsl,
)