def extend_RRT(self, x_sample, T, eps_point=1, eps_poly=1):
sys = self.system
if self.states != []:
list_of_goals = [x.p for x in self.states]
else:
print "WARNING: no gaol. Goal fixed to system goal. Is this the initizaliztion?"
list_of_goals = [self.system.goal]
(x_nom, u_nom, delta_PWA, mu_nom,
flag) = point_trajectory_sPWA(self.system,
x_sample,
list_of_goals,
T,
eps=eps_point)
if flag == False:
return False
else:
list_of_cells = []
for t in range(T):
mode = tuple([
i for n in sys.list_of_sum_indices
for i in sys.list_of_modes[n] if delta_PWA[t, n, i] == 1
])
list_of_cells.append(sys.cell[mode])
# print t,"mode",mode,x_nom[t].T
# verify(x_nom,u_nom,delta_PWA,list_of_cells)
goal = None
for _goal in list_of_goals:
if mu_nom[_goal] == 1:
goal = _goal
print "goal is", goal, goal.x.T, "G=", goal.G, "x_nom[T]", x_nom[
T].T
# print "mu=",mu_nom
assert goal != None #Something is found!
(x, u, G,
theta) = polytopic_trajectory_given_modes(x_nom[0],
list_of_cells,
goal,
eps=eps_poly,
scale=sys.scale)
self.add_branch(x, u, G, theta, goal)
return True
up_shift=0
right_shift=0
theta_shift=0.25
x_goal=np.array([0,R-p,theta_shift,0.18,0.82,np.pi/2-0.2,0.8,0,0,0]).reshape(10,1).reshape(10,1)
x0=np.array([0,R-p,0,0.18,0.82,np.pi/2-0.3,0.8,0,0,0]).reshape(10,1).reshape(10,1)
sys.goal=zonotope(x_goal.reshape(10,1),100*np.diag([1,1,0,1,1,1,1,0,0,0]))
T=10
x_traj,u_traj,u_lambda,mode=point_trajectory_tishcom(sys,x0,[sys.goal],T,optimize_controls_indices=[0,1,2,3],cost=1)
list_of_linear_cells=trajectory_to_list_of_linear_cells(sys,Eta_1,x_traj,u_traj,u_lambda,mode)
list_of_linear_cells_full=trajectory_to_list_of_linear_cells_full_linearization(mysystem,x_traj,u_traj,u_lambda,mode,0.06,epsilon_min,epsilon_max)
list_of_linear_cells_PWA=PWA_cells_from_state(mysystem,x_traj[2],0.06,epsilon_min,epsilon_max)
list_of_PWA_sets=hybrid_reachable_sets_from_state(mysystem,x_traj[2],0.06,epsilon_min,epsilon_max)
raise 1
x,u,G,theta=polytopic_trajectory_given_modes(x0,list_of_linear_cells,sys.goal,eps=0,order=1,scale=[])
"""
Visualization
"""
import matplotlib.patches as patches
from matplotlib.collections import PatchCollection
import matplotlib.pyplot as plt
def carrot_vertices(x,y,theta,N=50):
x_c,y_c=x-p*np.sin(theta),y+p*np.cos(theta)
v=np.empty((N,2))
for k in range(N):
phi=-np.pi/2+np.pi/(N-1)*k
v[k,0]=x_c+R*np.sin(phi+theta)
v[k,1]=y_c-R*np.cos(phi+theta)
plt.plot([0], [0], 'o')
#plt.plot([-0.05],[0.5],'o')
# Now build a funnel
list_of_cells = []
for t in range(T):
mode = tuple([
i for n in sys.list_of_sum_indices for i in sys.list_of_modes[n]
if delta_PWA[t, n, i] == 1
])
list_of_cells.append(sys.cell[mode])
sys.scale = np.array([0.12, 1])
(x, u, G, theta) = polytopic_trajectory_given_modes(x0,
list_of_cells,
sys.goal,
eps=1,
order=1,
scale=sys.scale)
from PWA_lib.visualization.visualize import add_tube
fig, ax = plt.subplots()
ax.set_xlim([-0.12, 0.12])
ax.set_ylim([-1, 1])
add_tube(ax, x, G, eps=0.0001, list_of_dimensions=[0, 1])
ax.plot([x[t][0, 0] for t in range(T + 1)], [x[t][1, 0] for t in range(T + 1)])
#ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'+')
ax.plot([0.1, 0.1], [-1, 1], 'black')
ax.plot([-0.1, -0.1], [-1, 1], 'black')
ax.plot([0], [0], 'o')
plt.plot([x_n[t][0, 0] for t in range(T + 1)],
[x_n[t][1, 0] for t in range(T + 1)])
print e.T, e.shape
print all(p.h - np.dot(p.H, np.vstack((x, u))) >= -10**-8)
for t in range(T1, T2 - 1):
print t
cell = q[t]
A, B, c, p = cell.A, cell.B, cell.c, cell.p
x, u, x_plus = X_traj[:, t].reshape(10, 1), U_traj[:, t].reshape(
4, 1), X_traj[:, t + 1].reshape(10, 1)
e = x_plus - np.dot(A, x) - np.dot(B, u) - c
q[t].c = q[t].c + e
q[t].p.h = q[t].p.h + 10**-5
print e.T, e.shape
print all(p.h - np.dot(p.H, np.vstack((x, u))) >= -10**-8)
(x, G) = polytopic_trajectory_given_modes(x0, q[T1:T2], goal, eps=1, order=1)
#assert 1==0
#t=100
#x,u=X_traj[:,t].reshape(10,1),U_traj[:,t].reshape(4,1)
#x_plus=X_traj[:,t+1].reshape(10,1)
#q=find_the_dynamics(cell[t],x,u,A0,B0,c0,H0,h0,x_plus,eps=1,delta_t=0.0025)
#(x,G)=polytopic_trajectory_given_modes(x0,list_of_cells,sys.goal,eps=0.05,order=1)
fig, ax = plt.subplots()
add_tube(ax, x, G, eps=0.0001, list_of_dimensions=[0, 1], axis=0)
ax.plot([x[t][0, 0] for t in range(T2 - T1 + 1)],
[x[t][1, 0] for t in range(T2 - T1 + 1)])
pickle.dump(x, open("x_funnel_4.pkl", "w"))
pickle.dump(G, open("G_funnel_4.pkl", "w"))
for i in sys.list_of_modes[n] if delta_PWA[t, n, i] == 1
]) + 10**-5
H, h = canonical_polytope(H, h)
cell = linear_cell(A, B, c, polytope(H, h))
list_of_cells.append(cell)
for t in range(T):
cell = list_of_cells[t]
A, B, c, p = cell.A, cell.B, cell.c, cell.p
print all(
abs(x_n[t + 1] - np.dot(A, x_n[t]) - np.dot(B, u[t]) - c) < 10**-8)
print all(p.h - np.dot(p.H, np.vstack((x_n[t], u[t]))) >= -10**-8)
(x, G) = polytopic_trajectory_given_modes(x0,
list_of_cells,
sys.goal,
eps=0.5,
order=1)
from visualization.visualize import add_tube
fig, ax = plt.subplots()
ax.set_xlim([-0.12, 0.12])
ax.set_ylim([-1, 1])
add_tube(ax, x, G, eps=0.0001, list_of_dimensions=[0, 1])
ax.plot([x[t][0, 0] for t in range(T + 1)], [x[t][1, 0] for t in range(T + 1)])
#ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'+')
ax.plot([0.1, 0.1], [-1, 1], 'black')
ax.plot([-0.1, -0.1], [-1, 1], 'black')
ax.plot([0], [0], 'o')
plt.plot([x_n[t][0, 0] for t in range(T + 1)],
[x_n[t][1, 0] for t in range(T + 1)])