def draw(self, node):
path = []
while node.parent:
path.append(node.state)
node = node.parent
path.append(self.start_state)
draw(path[::-1], self.array_index, self.algorithm)
def draw(self, node):
path=[]
while node.parent:
path.append(node.state)
node = node.parent
path.append(self.start_state)
draw(self.map, path[::-1], self.map_index)
while t < t_next_plot:
t = t + P.Ts
if t >= 1:
mcl.change = True
vc = 1 + 0.5 * np.cos(2 * np.pi * (0.2) * t)
omegac = -0.2 + 2 * np.cos(2 * np.pi * (0.6) * t)
noise_v = vc + np.random.normal(
0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2))
noise_omega = omegac + np.random.normal(
0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2))
u_noise = [noise_v, noise_omega]
u_truth = [vc, omegac]
dynamics.propagateDynamics(u_noise)
animation.draw(dynamics.states(), Chi=mcl.get_particles())
mcl.run(dynamics.states(), u_truth)
states = np.hstack((states, dynamics.states()))
mu = np.hstack((mu, mcl.get_mu()))
std = np.vstack((std, mcl.get_std()))
# dataPlot.update(t, dynamics.states(), ukf.get_mu(), ukf.get_sig())
# plt.pause(0.0001)
# set_trace()
# Keeps the program from closing until the user presses a button.
# print('Press key to close')
# plt.waitforbuttonpress()
# plt.close()
for v in range(u + 1, n):
if adj[u, v] > 0:
state.line.append([u, v])
def helper(d: int=3) -> Iterator[animation.State]:
last_vertex = None
iteration = 1
print("Running...")
while d >= 2:
print(f"dim {d}")
if last_vertex is None:
vertex = np.random.normal(loc=0, scale=0.01, size=(n, d))
else:
vertex = last_vertex[:, 0:d]
sl = spring_layout(adj, coord=vertex)
for vertex in sl:
state.title = str(iteration)
iteration += 1
last_vertex = vertex
vertex = vertex[:, 0:2]
minxy = vertex.min(initial=+np.inf) - 1
maxxy = vertex.max(initial=-np.inf) + 1
state.vertex = vertex
state.xylim = ((minxy, maxxy), (minxy, maxxy))
yield state
d -= 1
print("done")
d = 2
animation.draw(helper(d), save=True)
next_1_y = atom_list[i].y + atom_list[i].v_y * settings.dt
next_2_x = atom_list[j].x + atom_list[j].v_x * settings.dt
next_2_y = atom_list[j].y + atom_list[j].v_y * settings.dt
d_after = math.sqrt((next_1_x - next_2_x)**2 +
(next_1_y - next_2_y)**2)
if 2 * settings.radius < d <= 2 * settings.radius + settings.d and d > d_after:
change_velocities(i, j)
if i == 0 or j == 0:
red.collisions += 1
if d_after < d <= 2 * settings.radius:
change_velocities(i, j)
if i == 0 or j == 0:
red.collisions += 1
animation.draw(1, atoms.AtomRed())
check()
plot_xdata = [x for x in range(10, settings.max_atoms_number + 1, 5)]
plot_ydata = []
plot2_ydata = []
Mlist = [50, 100]
if settings.atoms_number not in plot_xdata:
plot_xdata.append(settings.atoms_number)
plot_xdata.sort()
if settings.frames not in Mlist:
Mlist.append(settings.frames)
Mlist.sort()
for M in Mlist:
dataPlot = plotData()
ekf = EKF()
t = P.t_start
while t < P.t_end:
t_next_plot = t + P.t_plot
while t < t_next_plot:
t = t + P.Ts
vc = 1 + 0.5 * np.cos(2 * np.pi * (0.2) * t)
omegac = -0.2 + 2 * np.cos(2 * np.pi * (0.6) * t)
noise_v = vc + np.random.normal(
0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2))
noise_omega = omegac + np.random.normal(
0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2))
u = [noise_v, noise_omega]
dynamics.propagateDynamics(u)
animation.draw(dynamics.states())
ekf.run(dynamics.states(), u)
dataPlot.update(t, dynamics.states(), ekf.get_mu(), ekf.get_sig())
plt.pause(0.001)
# Keeps the program from closing until the user presses a button.
# print('Press key to close')
# plt.waitforbuttonpress()
# plt.close()
plt.pause(100)
def draw(self):
if self.success():
draw(self.path, self.start_row, self.start_col, self.rule)
else:
print("No solutions were found.")
