def initialize_open(self, Q, R, P0, kmax):
self.clear()
self.plan_number = 0
p_init = Plan(
self.x_init,
self.start_idx,
self.graph.env,
self.x_init.shape[0],
R,
kmax,
)
# Set variables in p_init
p_init.set_motion(self.graph.planner.A, self.graph.planner.B, Q)
p_init.set_gain(self.graph.planner.gain)
p_init.set_init_est(P0)
self.P_open.add((p_init, self.plan_number))
self.P[0] = set()
self.P[0].add(deepcopy(p_init))
self.G = self.P_open.copy()
