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, )