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)
