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