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)])