def plot1():
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(-60, 110)
ax.set_ylim(-60, 110)
# Initialize classes
params = Parameters()
target = Target(25, 25, 0)
agents = []
for i in range(params.nveh):
agents.append(Agent(0, 25 * i, 0, params.monSpeed, 0, params, ax=ax))
# Give the target a commanded speed
target.send_cmd(3, 0)
# Get first plan
for i, agent in enumerate(agents):
agent.detect_target(target.get_state())
agent.compute_flight_traj(tf=params.tflight + i * params.tmon)
# Plot initial states
pts = target.get_state()
trgtPlot = ax.plot(pts[0], pts[1], 'r*', markersize=10, label='Target')
for i, agent in enumerate(agents):
agent.plot_arrow()
# Plot the inner and outer radii
cir1 = Circle(target.get_state()[:2],
ls=':',
fill=False,
ec='r',
label='Outer Radius',
radius=params.outerR)
cir2 = Circle(target.get_state()[:2],
ls=':',
fill=False,
ec='r',
label='Outer Radius',
radius=params.innerR)
cir3 = Circle(target.get_state()[:2],
lw=None,
fc='gray',
alpha=0.5,
label='No Fly Zone',
radius=params.noflyR)
ax.add_artist(cir1)
ax.add_artist(cir2)
ax.add_artist(cir3)
# Draw legend and clean up Agent class
ax.legend([trgtPlot[0]] + [agent._arrow for agent in agents],
['Target', 'Agent 1', 'Agent 2', 'Agent 3'])
Agent.agentIdx = 0
Agent.trajList = []
Agent.timeList = []
plt.title('$t = 0$')
return
def plot2():
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(-60, 110)
ax.set_ylim(-60, 110)
# Initialize classes
params = Parameters()
target = Target(25, 25, 0)
agents = []
for i in range(params.nveh):
agents.append(Agent(0, 25*i, 0, params.monSpeed, 0, params, ax=ax))
# Give the target a commanded speed
target.send_cmd(0, 0)
# Get first plan
for i, agent in enumerate(agents):
agent.detect_target(target.get_state())
agent.compute_flight_traj(tf=params.tflight + i*params.tmon)
# Plot initial states
pts = target.get_state()
trgtPlot = ax.plot(pts[0], pts[1], 'r*', markersize=10, label='Target')
for i, agent in enumerate(agents):
agent.plot_arrow()
# Run the simulation
for t in np.arange(0, params.tflight + params.nveh*params.tmon + 0.1, 0.1):
# Update states
target.update(t)
for agent in agents:
agent.update(t)
# Detect target
if t % params.detPer < 1e-6:
for agent in agents:
agent.detect_target(target.get_state())
# Update plots
pts = target.get_state()
trgtPlot[0].set_data(pts[0], pts[1])
for i, agent in enumerate(agents):
agent.plot_arrow()
fig.canvas.draw()
plt.pause(0.01)
# Plot the inner and outer radii
cir1 = Circle(target.get_state()[:2], ls=':', fill=False, ec='r',
label='Outer Radius', radius=params.outerR)
cir2 = Circle(target.get_state()[:2], ls=':', fill=False, ec='r',
label='Inner Radius', radius=params.innerR)
cir3 = Circle(target.get_state()[:2], lw=None, fc='gray', alpha=0.5,
label='No Fly Zone', radius=params.noflyR)
ax.add_artist(cir1)
ax.add_artist(cir2)
ax.add_artist(cir3)
ax.legend([trgtPlot[0]] + [agent._arrow for agent in agents],
['Target',
'Agent 1',
'Agent 2',
'Agent 3'])
Agent.agentIdx = 0
Agent.trajList = []
Agent.timeList = []
plt.title(f'$t = {t}$')
return
def main(ax, SIM_TF):
# Initialize plots
# plt.close('all')
# fig, ax = plt.subplots(1, 1)
ax.set_aspect('equal')
ax.set_xlim(-15, 110)
ax.set_ylim(-15, 110)
# Initialize classes
params = Parameters()
target = Target(25, 25, 0)
agents = []
for i in range(params.nveh):
agents.append(Agent(0, 25 * i, 0, params.monSpeed, 0, params, ax=ax))
# Give the target a commanded speed
target.send_cmd(0, 0)
# Get first plan
for i, agent in enumerate(agents):
agent.detect_target(target.get_state())
agent.compute_flight_traj(tf=params.tflight + i * params.tmon)
# Plot initial states
pts = target.get_state()
trgtPlot = ax.plot(pts[0], pts[1], 'r*', markersize=10, label='Target')
agentPlots = []
for i, agent in enumerate(agents):
pts = agent.get_state()
agentPlots.append(
ax.plot(pts[0], pts[1], f'{Agent.colors[i]}X', label=f'Agent {i}'))
ax.legend()
# Run the simulation
t0 = 0.0
t = 0.0
t_trgt = 0.0
agentIdx = 0
while t < SIM_TF:
# Update states
target.update(t)
for agent in agents:
agent.update(t)
# if t - t_trgt >= TARGET_PERIOD:
# t_trgt = t
# agent.detect_target(target.get_state())
# Detect target
if t % params.detPer < 1e-6:
for agent in agents:
agent.detect_target(target.get_state())
# Update plots
pts = target.get_state()
trgtPlot[0].set_data(pts[0], pts[1])
for i, agent in enumerate(agents):
pts = agent.get_state()
agentPlots[i][0].set_data(pts[0], pts[1])
plt.pause(0.01)
# t = TIME_MULT*time.time() - t0
t += 0.1
# print(t)
if t > 35:
target.send_cmd(3, 0)
if t > 45:
target.send_cmd(3, np.pi / 8)
if t > 47:
target.send_cmd(3, 0)
Agent.agentIdx = 0
Agent.trajList = []
Agent.timeList = []
return
ax.set_xlim(-50, 100)
ax.set_ylim(-60, 100)
# Initialize classes
params = Parameters()
target = Target(25, 25, 0)
agents = []
for i in range(params.nveh):
agents.append(Agent(0, 15 * i, 0, 3, 0, params, ax=ax))
# Give the target a commanded speed
target.send_cmd(0.3, 0)
# Get first plan
for i, agent in enumerate(agents):
agent.detect_target(target.get_state())
agent.compute_flight_traj(dt=(i + 1) * params.tflight)
# agent.compute_mon_traj()
idxs = list(range(params.nveh))
idxs.remove(i)
for j in idxs:
cpts_f = agent.flight_traj.cpts
times_f = np.array([agent.flight_traj.t0, agent.flight_traj.tf])
agents[j].send_traj(cpts_f, times_f)
# cpts_m = agent.mon_traj.cpts
# times_m = np.array([agent.mon_traj.t0, agent.mon_traj.tf])
# agents[j].send_traj(cpts_m, times_m)
# Plot initial states
pts = target.get_state()
trgtPlot = ax.plot(pts[0], pts[1], 'r*', label='Target')
