def make_state_trajectory_state_end(s, x, u, seq, G, theta, T, state_end): flag = re_verification_tree_extension(s, x[T - 1], G[T - 1], seq[T - 1], state_end, factor=0.95) if flag == None: s.failure_tree_extentions.append( (x[T - 1], G[T - 1], seq[T - 1], u[T - 1], theta[T - 1], x[T], G[T], seq[T], state_end)) return else: u[T - 1] = flag[0] theta[T - 1] = flag[1] x_temp = {} ID = s.ID s.ID += 1 for t in range(0, T): x_temp[t] = state(x[t], G[t], seq[t], ID, t, 1) x_temp[t].time_to_go = T - t + state_end.time_to_go # Build Transitons for t in range(0, T - 1): x_temp[t].successor = (x_temp[t + 1], u[t], theta[t]) x_temp[T - 1].successor = (state_end, u[T - 1], theta[T - 1]) for t in range(1, T): x_temp[t].parent.append(x_temp[t - 1]) state_end.parent.append(x_temp[T - 1]) s.X.extend(list(x_temp.values())[::-1])
def trajectory_cost(s, x, u, seq, G, theta, T): J = 0 ID = 1001 state_considered = {} for t in range(T): state_considered[t] = state(x[t], G[t], seq[t], ID, t, 7) for t in range(T - 1): state_considered[t].successor = (state_considered[t + 1], u[t], theta[t]) J += cost_state(s, state_considered[t], s.Q, s.R, s.g) return J
for i in s.modes: s.F[i] = np.array([[1, -1]]).T s.f[i] = np.array([[4, 4]]).T s.Pi = np.array([[1, 0], [0, 1], [-1, 0], [0, -1]]) s.l[0] = np.array([-0.12, -1]).reshape(2, 1) s.u[0] = np.array([0.1, 1]).reshape(2, 1) s.l[1] = np.array([0.1, -1]).reshape(2, 1) s.u[1] = np.array([0.12, 1]).reshape(2, 1) s.l[2] = np.array([-0.12, -1]).reshape(2, 1) s.u[2] = np.array([-0.1, 1]).reshape(2, 1) s.vertices = vertices_cube(2) """ These are polytopes for each mode """ s.mode_polytope = {} for mode in s.modes: p = polytope(s.H[mode], s.h[mode]) p.anchor = anchor_point(p) s.mode_polytope[mode] = p s.weight = {} s.weight[0] = 100 / 12 s.weight[1] = 1 s.goal = state( np.array([0, 0]).reshape(2, 1), np.array([[0., 0], [0, 0.0]]), 0, 0, 0, 10)
s.B[mode][5, 1] = 1 for i in s.modes: s.F[i] = PI(2) s.f[i] = 10 * np.ones((4, 1)) s.Pi = PI(6) """ These are polytopes for each mode """ s.mode_polytope = {} for mode in s.modes: p = polytope(s.H[mode], s.h[mode]) p.anchor = anchor_point(p) s.mode_polytope[mode] = p s.vertices = vertices_cube(6) s.weight = {} s.weight[0] = 1 / xmax s.weight[1] = 1 / ymax s.weight[2] = 1 / theta_eps s.weight[3] = 1 / b_eps s.weight[4] = 1 / rx_nominal s.weight[5] = 1 / ry_max s.vertices = vertices_cube(6) s.goal = state( np.array([10, 0, 0, 0, 1, 0]).reshape(6, 1), np.eye(6) * 0.01, 0, 0, 0, 10)
def regroup_states(s, list_of_states): for y in list_of_states: s.X.remove(y) new_state = state()
s.f[0] = np.array([[umax, umax]]).T s.F[1] = np.array([[1, -1]]).T s.f[1] = np.array([[umax, umax]]).T s.Pi = np.array([[1, 0], [0, 1], [-1, 0], [0, -1]]) s.l[0] = np.array([0, -vmax]).reshape(2, 1) s.u[0] = np.array([y_p, vmax]).reshape(2, 1) s.l[1] = np.array([-dmax, -vmax]).reshape(2, 1) s.u[1] = np.array([0, vmax]).reshape(2, 1) s.vertices = vertices_cube(2) """ These are polytopes for each mode """ s.mode_polytope = {} for mode in s.modes: p = polytope(s.H[mode], s.h[mode]) p.anchor = anchor_point(p) s.mode_polytope[mode] = p s.weight = {} s.weight[0] = 4 s.weight[1] = 1 s.goal = state( np.array([(xmax + y_p) / 2, 0]).reshape(2, 1), np.array([[(xmax - y_p) / 2, 0], [0, 1]]), 0, 0, 0, 10)