def i_strategy(x): s = phy_to_thtalpha(x) ds = phy_to_thtd(x) phi_2 = pi / 2 - s[1] psi = -(pi / 2 - LB / 2 + s[1]) d = ds[1] * (sin(phi_2) / sin(LB / 2)) l1 = sqrt(ds[0]**2 + d**2 - 2 * ds[0] * d * cos(s[2] + psi)) cA = (d**2 + l1**2 - ds[0]**2) / (2 * d * l1) sA = sin(s[2] + psi) * (ds[0] / l1) A = atan2(sA, cA) phi_1 = -(pi - (s[2] + psi) - A) return np.array([phi_1, phi_2, psi])
def natural_barrier_state(t, delta, gmm): x = np.zeros(6, ) x[0] = -(vd * t + r) * cos(gmm) x[1] = -(vd * t + r) * sin(gmm) x[2] = -(vd * t + r) * cos(gmm) x[3] = (vd * t + r) * sin(gmm) x[4] = -vi * t * cos(delta) x[5] = -vi * t * sin(delta) angles = phy_to_thtalpha(x) ds = phy_to_thtd(x) if (0 < angles[0] <= pi / 2) and (0 < angles[1] <= pi / 2) and (0 < angles[2] < 2 * pi): return x, angles, ds else: return x, None, ds
def s_strategy(x): s = phy_to_thtalpha(x) ds = phy_to_thtd(x) phi_1 = -(pi / 2 - s[0]) psi_ = s[2] - (s[0] + pi / 2 - LB / 2) d = (ds[0] / sin(LB / 2)) * sin(-phi_1) l2 = sqrt(d**2 + ds[1]**2 - 2 * d * ds[1] * cos(psi_)) s_B = (sin(psi_) / l2) * d c_B = (ds[1]**2 + l2**2 - d**2) / (2 * ds[1] * l2) B = atan2(s_B, c_B) A = pi - B - psi_ psi = -psi_ def get_K(Omega): K = 2 * pi - pi / 2 - A - (pi - Omega) / 2 return K def delta(Omega): K = get_K(Omega) l0 = 2 * r / sin(LB / 2) * sin(Omega / 2) dI = d + Omega / (tan(LB / 2) / r) dD = sqrt(l2**2 + l0**2 - 2 * l0 * l2 * cos(K)) - r err = dI * (Config.VD / Config.VI) - dD # err = (d*(Config.VD/Config.VI) + Omega/(tan(LB/2)/r) + r) - (l2/sin(LB/2 + Omega/2)*sin(K)) # err = (l2/sin(LB/2 + Omega/2)*sin(K)) return err**2 if (s[2] - (s[0] + s[1]) <= pi - LB) and (d * (Config.VD / Config.VI) < l2 - r): print('curved') res = minimize_scalar(delta, bounds=[0.01, pi / 1.5], method='bounded') Omega = res.x # print(Omega) phi_2 = pi - get_K(Omega) - (LB / 2 + Omega / 2) + B else: print('straight') phi_2 = B return np.array([phi_1, phi_2, psi])
def evelope_barrier_state(t, T=1, gmm=LB / 2): assert t >= 0 if t >= T: t1 = T t2 = t - T else: t1 = t t2 = 0 x0 = -r / tan(LB / 2) y0 = -r rd = r / tan(LB / 2) ri = r / sin(LB / 2) xd1 = x0 + rd * cos(A * t1 + LB) - vd * sin(A * t1 + LB) * t2 yd1 = y0 + rd * sin(A * t1 + LB) + vd * cos(A * t1 + LB) * t2 xi = x0 + ri * cos(A * t1 + LB / 2) - vi * sin(A * t1 + LB / 2) * t2 yi = y0 + ri * sin(A * t1 + LB / 2) + vi * cos(A * t1 + LB / 2) * t2 xd2 = 0 yd2 = r + vd * t d = yd2 xd2 = d * sin(2 * gmm - LB) yd2 = d * cos(2 * gmm - LB) x = np.array([xd1, yd1, xd2, yd2, xi, yi]) angles = phy_to_thtalpha(x) ds = phy_to_thtd(x) if (0 < angles[0] <= pi / 2) and (0 < angles[1] <= pi / 2) and (0 < angles[2] < 2 * pi): return x, angles, ds else: return x, None, None
def iwin_SS_straight_traj(tht, delta=0., L=300., nT=50, length=0.5): def get_init(tht, delta, L): P1 = np.array([-L, 0.]) P2 = np.array([L, 0.]) r = L / sin(tht) Y = L / tan(tht) x = r * cos(delta) y = r * sin(delta) + Y P = np.array([x, y]) l1 = np.linalg.norm(P - P1) l2 = np.linalg.norm(P - P2) cpsi = sin(tht) spsi = cos(tht) - l1 / l2 psi = atan2(spsi, cpsi) P1_P = P - P1 P2_P = P - P2 P_P2 = P2 - P phi_1 = atan2(P1_P[1], P1_P[0]) - pi / 2 phi_2 = atan2(P2_P[1], P2_P[0]) + pi / 2 psi = atan2(P_P2[1], P_P2[0]) + psi return P1, P2, P, phi_1, phi_2, psi p1, p2, p, phi_1, phi_2, psi = get_init(tht, delta, L) xs = [np.concatenate((p1, p2, p))] ss = [phy_to_thtalpha(xs[0])] ds = [phy_to_thtd(xs[0])] times = [0] dt_s = L * length / vd / nT dt_l = 0.98 * L / vd for t in range(nT): if t == 0: dt = dt_l else: dt = dt_s p1 += vd * np.array([cos(phi_1), sin(phi_1)]) * dt p2 += vd * np.array([cos(phi_2), sin(phi_2)]) * dt p += vi * np.array([cos(psi), sin(psi)]) * dt x = np.concatenate((p1, p2, p)) angles = phy_to_thtalpha(x) if (0 < angles[0] < pi / 2) and (0 < angles[1] < pi / 2) and (0 < angles[2] < 2 * pi): xs.append(x) ss.append(angles) ds.append(phy_to_thtd(x)) times.append(times[-1] + dt) else: break # print('[%.2f, %.2f], [%.2f, %.2f], [%.2f, %.2f], %.2f'%(p1[0], p1[1], p2[0], p2[1], p[0], p[1], ss[-1][-1])) outid = [] return np.asarray(xs[3:-3]), np.asarray(ss[3:-3]), np.asarray( ds[3:-3]), np.asarray(times[3:-3]) - times[3]
def barrier_state(phi, tau, gmm): """ trajectory under opitmal play, initial locations should be on SS, reference frame: physical 6D frame, y is along ID_2 upon capture ---------------------------------------------------------------------------- Input: phi: phi_1 in [lb, ub] float tau: time spent on the straight line trajectory float lb: lower bound of phi. float lb=LB: at barrier, lb>LB: other SS Output: x: 6D trajectory, order: D1, D2, I, np.array ---------------------------------------------------------------------------- """ assert 0 <= gmm <= pi def consts(phi): s, c = sin(phi / 2), cos(phi / 2) A, B = l / (w**2 - 1), sqrt(1 - w**2 * c**2) return s, c, A, B def xtau(phi): out = np.zeros((6, )) s, c, A, B = consts(phi) E = ellipe(asin(w * c), 1 / w) F = ellipf(asin(w * c), 1 / w) out[0] = -2 * A * (s**2 * B + w * s**3) # x_D1 out[1] = A * (w * E + 3 * w * c + 2 * s * c * B - 2 * w * c**3) # y_D1 out[3] = 2 * l * F / w - 3 * w * E * A - 3 * w * c * A # y_D2 out[4] = A * (B * (2 * w**2 * c**2 - w**2 - 1) - 2 * w**3 * s**3 + 2 * w * (w**2 - 1) * s) # x_I out[5] = A * (w * E + 2 * w**2 * s * c * B - 2 * w**3 * c**3 + w * c * (2 + w**2)) # y_I return out s, c, A, B = consts(phi) xtemp = np.zeros((6, )) xtemp[0] = -l * sin(LB) - 2 * s * c * tau # x_D1 xtemp[1] = l * cos(LB) + (2 * c**2 - 1) * tau # y_D1 xtemp[3] = l + tau # y_D2 xtemp[4] = -(w**2 * s * c + w * c * B) * tau # x_I xtemp[5] = +(w**2 * c**2 - w * s * B) * tau # y_I dx = xtau(phi) - xtau(LB) if dx[3] < 0: dx[3] = 0 dt = dx[3] x = dx + xtemp t = dt + tau d = sqrt(x[2]**2 + x[3]**2) x[2] = d * sin(2 * gmm - LB) x[3] = d * cos(2 * gmm - LB) x = x * vd s = phy_to_thtalpha(x) d = phy_to_thtd(x) return x, s, d, t
def t_strategy(x): s = phy_to_thtalpha(x) return np.array([-pi / 10, pi / 3, -s[2] / 6])