def simulate(sys, x_0, T): x, y, u, v, w = {}, {}, {}, {}, {} x[0] = x_0 for t in range(T + 1): print "simulating time:", t zeta_w, zeta_v = 2 * (np.random.random( (sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5) zeta_w = np.random.randint(-1, 1, size=(sys.n, 1)) zeta_v = np.random.randint(-1, 1, size=(sys.m, 1)) v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x y[t] = np.dot(sys.C[t], x[t]) + v[t] # print "y",y[t].shape,y if t == 0: Zono_Y, Zono_U = zonotope(sys.ybar[t], sys.Phi[0]), zonotope( sys.ubar[t], sys.Theta[0]) elif t == T: print "End of simulation" return x, y, u, v, w else: print t, "continue", t == T, T YU = np.vstack([sys.ybar[tau] for tau in range(t + 1)] + [sys.ubar[tau] for tau in range(t)]) Zono_Y,Zono_U=zonotope(YU,np.vstack((\ sys.Phi[t],np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1])))) ))),\ zonotope(sys.ubar[t],sys.Theta[t][sys.m*t:,:]) H = np.vstack([y[tau] for tau in range(t + 1)] + [u[tau] for tau in range(t)]) u[t] = zonotopic_controller(H, Zono_Y, Zono_U) # print "control",t,u[t],u[t].shape x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t] return x, y, u, v, w
def simulate_zonotope(sys,x_0,T,w,v): x,y,u={},{},{} x[0]=x_0 for t in range(T+1): print "simulating time:",t y[t]=np.dot(sys.C[t],x[t])+v[t] if t==T: return x,y,u,x Y=np.vstack([y[tau] for tau in range(t+1)]) Ybar=np.vstack([sys.ybar[tau] for tau in range(t+1)]) if t<T+2: zono_Y=zonotope(Ybar,np.dot(sys.Phi[t],sys.E[t-1].G)) zono_U=zonotope(sys.ubar[t],np.dot(sys.theta[t],sys.E[t-1].G)) u[t]=zonotopic_controller(Y,zono_Y,zono_U) else: Ubar=np.vstack([sys.ubar[tau] for tau in range(t)]) U=np.vstack([u[tau] for tau in range(t)]) YU=np.vstack((Y,U)) YU_bar=np.vstack((Ybar,Ubar)) phi_E=np.dot(sys.Phi[t],sys.E[t-1].G) theta_E=np.dot(sys.Theta[t-1],sys.E[t-2].G) theta_E_0=np.hstack((theta_E,np.zeros((sys.Theta[t-1].shape[0],phi_E.shape[1]-theta_E.shape[1])))) G=np.vstack((phi_E,theta_E_0)) zono_Y=zonotope(YU_bar,G) zono_U=zonotope(sys.ubar[t],np.dot(sys.theta[t],sys.E[t-1].G)) K=np.dot(np.dot(sys.theta[t],sys.E[t-1].G),np.linalg.pinv(G)) u[t]=sys.ubar[t]+np.dot(K,YU-YU_bar) u[t]=zonotopic_controller(YU,zono_Y,zono_U) x[t+1]=np.dot(sys.A[t],x[t])+np.dot(sys.B[t],u[t])+w[t]
def outputfeedback_synthesis_zonotope_solution(sys,u_tilde,theta): T=max(theta.keys())+1 phi,Phi,Theta={},{},{} y_tilde={} Y_tilde,U_tilde={},{} z_bar,u_bar={},{} Z,U={},{} # Z matrices # Initial y_tilde[0]=np.zeros((sys.o,1)) phi[0]=np.eye(sys.o) # Yashasin Phi[0],Theta[0]=phi[0],theta[0] theta[T]=np.zeros((sys.m,sys.o*(T+1))) u_tilde[T]=np.zeros((sys.m,1)) for t in range(T): Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)]) U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)]) y_tilde[t+1]=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t]) phi[t+1]=np.hstack(( np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o) )) Phi[t+1]=triangular_stack(Phi[t],phi[t+1]) Theta[t+1]=triangular_stack(Theta[t],theta[t+1]) Y_tilde[T]=np.vstack([y_tilde[tau] for tau in range(T+1)]) U_tilde[T]=np.vstack([u_tilde[tau] for tau in range(T+1)]) # Performance variables for t in range(T+1): # Z zonotope print("time synthesis",t) z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\ np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\ np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi[t].x])+\ sys.F[t].x Gz1=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["x",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["x",t]])+\ np.dot(sys.D[t],sys.P["x",t])-np.dot(sys.R[t],sys.Q["x",t]) Gz2=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["w",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["w",t]])+\ np.hstack(( np.dot(sys.D[t],sys.P["w",t]),np.zeros((sys.z,sys.n)) ))-\ np.dot(sys.R[t],sys.Q["w",t]) Gz3=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["v",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["v",t]])+\ -np.dot(sys.R[t],sys.Q["v",t]) Gw=spa.block_diag(*[sys.W[t].G for tau in range(0,t+1)]) Gv=spa.block_diag(*[sys.V[t].G for tau in range(0,t+1)]) Gz=np.hstack(( np.dot(Gz1,sys.X0.G), np.dot(Gz2,Gw), np.dot(Gz3,Gv) )) Z[t]=zonotope(z_bar,Gz) for t in range(T): # U zonotope u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x) Gu=np.dot(theta[t],sys.Xi[t].G) U[t]=zonotope(u_bar,Gu) return Z,U
def reduced_order(sys, T): M, N, Z = {}, {}, {} for t in range(T): # 1: intiial state K_1 = np.dot(sys.F["x", t], sys.X0.G) e_1 = np.dot(sys.C[t + 1], sys.E["x", t]) L_1 = np.dot(e_1, sys.X0.G) # 2: process noice K_2 = np.dot(sys.F["w", t], spa.block_diag(*[sys.W[t].G for tau in range(0, t + 1)])) e_2 = np.dot(sys.C[t + 1], sys.E["w", t]) L_2 = np.dot(e_2, spa.block_diag(*[sys.W[t].G for tau in range(0, t + 1)])) # 3: observation noise K_3 = np.dot(sys.F["v", t], spa.block_diag(*[sys.V[t].G for tau in range(0, t + 1)])) L_3 = np.zeros((sys.o, K_3.shape[1])) # Build the L and K matrices: L=M*K L = np.hstack((L_1, L_2, L_3)) K = np.hstack((K_1, K_2, K_3)) M[t] = np.dot(L, np.linalg.pinv(K)) N[t] = np.dot(sys.C[t + 1], sys.E["u", t]) - np.dot( M[t], sys.F["u", t]) Z[t] = zonotope(None, None) Z[t].G = np.hstack((L - np.dot(M[t], K), sys.V[t + 1].G)) Z[t].x=np.dot(e_1-np.dot(M[t],sys.F["x",t]),sys.X0.x)\ +np.dot(e_2-np.dot(M[t],sys.F["w",t]),np.vstack([sys.W[t].x for tau in range(0,t+1)]))\ +np.dot(np.dot(M[t],sys.F["v",t]),np.vstack([sys.V[t].x for tau in range(0,t+1)]))\ +sys.V[t+1].x Z["X0", t] = L_1 - np.dot(M[t], K_1) Z["W", t] = L_2 - np.dot(M[t], K_2) Z["V", t] = np.hstack((L_3 - np.dot(M[t], K_3), sys.V[t + 1].G)) return M, N, Z
def simulate(sys, x_0, T): x, y, u, v, w = {}, {}, {}, {}, {} x[0] = x_0 for t in range(T + 1): print "simulating time:", t zeta_w, zeta_v = 2 * (np.random.random( (sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5) zeta_w = np.ones((sys.n, 1)) * (-1)**np.random.randint(1, 3) zeta_v = np.ones((sys.o, 1)) * (-1)**np.random.randint(1, 3) v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x y[t] = np.dot(sys.C[t], x[t]) + v[t] # print "y",y[t].shape,y if t == 0: Zono_Y, Zono_U = zonotope(sys.ybar[t], sys.phi[0]), zonotope( sys.ubar[t], sys.theta[0]) elif t == T: print "End of simulation" return x, y, u, v, w else: print t, "continue", t == T, T YU = np.vstack([sys.ybar[tau] for tau in range(t + 1)] + [sys.ubar[tau] for tau in range(t)]) Zono_Y,Zono_U=zonotope(YU,np.vstack((\ sys.Phi[t],np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1])))) ))),\ zonotope(sys.ubar[t],sys.theta[t]) H = np.vstack([y[tau] for tau in range(t + 1)] + [u[tau] for tau in range(t)]) # try: Zono_Y = zonotope(np.vstack([sys.ybar[tau] for tau in range(t + 1)]), sys.Phi[t]) H = np.vstack([y[tau] for tau in range(t + 1)]) u[t] = zonotopic_controller(H, Zono_Y, Zono_U) ## except: ## if t>0: ## H=np.vstack([y[tau] for tau in range(t+1)]+[u[tau] for tau in range(t)]) ## K=np.dot(sys.theta[t],np.linalg.pinv(np.vstack((sys.Phi[t], ## np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1])))))))) ## u[t]=sys.ubar[t]+np.dot(K,H-YU) ## elif t==0: ## K=np.dot(sys.theta[0],np.linalg.pinv(sys.phi[0])) ## u[t]=sys.ubar[t]+np.dot(K,y[0]) u[t] = u[t].reshape(sys.m, 1) print "control", t, u[t], u[t].shape # u[t]=np.zeros((sys.m,1)) x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t] return x, y, u, v, w
def test_LQG_LTV(): S=LTV() print S.R n=7 m=1 o=1 T=55 np.random.seed(212) S.X0=zonotope(np.ones((n,1))*0,np.eye(n)*1) for t in range(T): S.A[t]=0.95*np.eye(n)+np.random.normal(size=(n,n))*0.01 S.B[t]=np.random.randint(0,2,size=(n,m)) S.C[t]=np.zeros((o,n)) S.C[t][0,0]=1 S.W[t]=zonotope(np.zeros((n,1)),np.eye(n)*0.01) S.V[t]=zonotope(np.zeros((o,1)),np.eye(o)*0.01) S.Q[t]=np.eye(n)*1 S.R[t]=np.eye(m)*1 S.F=np.eye(n)*1 L,K=LQG_LTV(S,T=50)
def simulate(sys, x_0, T): x, y, u, v, w = {}, {}, {}, {}, {} x[0] = x_0 for t in range(T + 1): print "simulating time:", t zeta_w, zeta_v = 2 * (np.random.random( (sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5) zeta_w = np.ones((sys.n, 1)) * (-1)**np.random.randint(1, 3) zeta_v = np.ones((sys.o, 1)) * (-1)**np.random.randint(1, 3) v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x y[t] = np.dot(sys.C[t], x[t]) + v[t] if t == T: return x, y, u, v, w Y = np.vstack([y[tau] for tau in range(t + 1)]) Ybar = np.vstack([sys.ybar[tau] for tau in range(t + 1)]) zono_Y = zonotope(Ybar, np.dot(sys.Phi[t], sys.E[t - 1].G)) zono_U = zonotope(sys.ubar[t], np.dot(sys.theta[t], sys.E[t - 1].G)) u[t] = zonotopic_controller(Y, zono_Y, zono_U) # u[t]=np.zeros((sys.m,1)) print "control", t, u[t], u[t].shape x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t] return x, y, u, v, w
def error_construction_old(sys, T, q0): sys.ebar = {} for t in range(-1, T + 1): sys.E[t] = zonotope(None, None) sys.E[-1].x = np.zeros((q0, 1)) for t in range(T + 1): sys.ebar[t] = sys.Z[t].x # t=0 sys.E[-1].G = np.eye(q0) sys.ebar[-1] = sys.E[-1].x # Other t for t in range(T + 1): sys.E[t].x = np.vstack([sys.ebar[tau] for tau in range(-1, t + 1)]) sys.E[t].G = triangular_stack(sys.E[t - 1].G, sys.Z[t].G)
def error_construction(sys, T, q0): sys.ebar = {} for t in range(-1, T + 1): sys.E[t] = zonotope(None, None) sys.E[-1].x = np.zeros((q0, 1)) for t in range(T + 1): sys.ebar[t] = sys.Z[t].x # t=0 sys.E[-1].G = np.eye(q0) sys.ebar[-1] = sys.E[-1].x for source in ["X0", "V", "W"]: sys.E[source, -1, "G"] = np.zeros((0, 0)) # Other t for t in range(T + 1): sys.E[t].x = np.vstack([sys.ebar[tau] for tau in range(-1, t + 1)]) for source in ["X0", "V", "W"]: sys.E[source, t, "G"] = triangular_stack(sys.E[source, t - 1, "G"], sys.Z[source, t]) A = np.hstack([sys.E[source, t, "G"] for source in ["X0", "V", "W"]]) sys.E[t].G = triangular_stack(sys.E[-1].G, A) print("All sources complete")
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\ triangular_stack_list,output_feedback_synthesis_lightweight_many_variables from pypolytrajectory.system import LQG_LTV, LTV, LQG import scipy.linalg as spa np.random.seed(0) S = LTV() n = 1000 m = 2 o = 1 z = 1 T = 32 S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1) B = np.random.randint(0, 2, size=(n, m)) B[0, 0] = 0 #B[1,0]=0 A = 0.995 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 * 0.005 C = np.zeros((o, n)) C[0, 0] = 1 #C[1,1]=1 D = np.eye(n)[0:z, :] #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6)
def invariance_time(sys,T,H_X=None,H_U=None,reset_map=[]): """ The idea is to find invariance """ prog=MP.MathematicalProgram() Theta={} theta={} u_tilde,U_tilde={},{} # Initial # Main variables for t in range(T): theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t) u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t) Theta[0]=theta[0] # Aggragates for t in range(T): U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)]) for t in range(T-1): Theta[t+1]=triangular_stack(Theta[t],theta[t+1]) X,U={},{} for t in range(1,T+1): Ptheta=np.dot(sys.P["u",t],Theta[t-1]) a_x=sys.P["x",t]+np.dot(Ptheta,sys.Xi["x",t-1]) a_w=sys.P["w",t] + np.dot(Ptheta,sys.Xi["w",t-1]) a_v=np.dot(Ptheta,sys.Xi["v",t-1]) Gw=spa.block_diag(*[sys.W[tau].G for tau in range(0,t)]) Gv=spa.block_diag(*[sys.V[tau].G for tau in range(0,t)]) Wbar=np.vstack([sys.W[tau].x for tau in range(0,t)]) Vbar=np.vstack([sys.V[tau].x for tau in range(0,t)]) x_bar=np.dot(sys.P["u",t],U_tilde[t-1]) + np.dot(a_x,sys.X0.x) \ + np.dot(a_w,Wbar) + np.dot(a_v,Vbar) G=np.hstack(( np.dot(a_x,sys.X0.G), np.dot(a_w,Gw), np.dot(a_v,Gv) )) X[t]=zonotope(x_bar,G) if H_X!=None: subset(prog,X[t],H_X) if H_U!=None: for t in range(T): u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x) Gu=np.dot(theta[t],sys.Xi[t].G) U[t]=zonotope(u_bar,Gu) subset(prog,U[t],H_U) print("subset for U done") # Terminal Constraint if len(reset_map)!=0: print("A reset map is found. Are you sure?") X[T].x=np.dot(reset_map,X[T].x) X[T].G=np.dot(reset_map,X[T].G) subset(prog,X[T],sys.X0) # Optimization J=0 for t in range(T): print(t,"u cost") J+=sum(u_tilde[t].flatten()**2) # J+=sum(theta[t].flatten()**2) for t in range(1,T): print(t,"x cost") J+=sum(X[t].x.flatten()**2) # J+=sum(X[t].G.flatten()**2) prog.AddQuadraticCost(J) print("Now solving the Quadratic Program") start=time.time() result=gurobi_solver.Solve(prog,None,None) print("time to solve",time.time()-start) if result.is_success(): print("Synthesis Success!","\n"*5) theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)} u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)} X_n={t:zonotope(sym.Evaluate(result.GetSolution(X[t].x)),\ sym.Evaluate(result.GetSolution(X[t].G))) for t in range(1,T+1)} if H_U!=None: U_n={t:zonotope(sym.Evaluate(result.GetSolution(U[t].x)),\ sym.Evaluate(result.GetSolution(U[t].G))) for t in range(T)} else: U_n={} return u_tilde_n,theta_n,X_n,U_n else: print("Synthesis Failed!")
def output_feedback_synthesis(sys,T): prog=MP.MathematicalProgram() # Add Variables phi,theta,Phi,Theta={},{},{},{} y_tilde,u_tilde={},{} Y_tilde,U_tilde={},{} z_bar,u_bar={},{} Z,U={},{} # Z matrices # Initial Condition y_tilde[0]=np.zeros((sys.o,1)) phi[0]=np.eye(sys.o) # Main variables for t in range(T): theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t) u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t) # Now we the dynamics Phi[0],Theta[0]=phi[0],theta[0] theta[T]=np.zeros((sys.m,sys.o*(T+1))) u_tilde[T]=np.zeros((sys.m,1)) for t in range(T): Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)]) U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)]) y_tilde[t+1]=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t]) phi[t+1]=np.hstack(( np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o) )) Phi[t+1]=triangular_stack(Phi[t],phi[t+1]) Theta[t+1]=triangular_stack(Theta[t],theta[t+1]) Y_tilde[T]=np.vstack([y_tilde[tau] for tau in range(T+1)]) U_tilde[T]=np.vstack([u_tilde[tau] for tau in range(T+1)]) # Performance variables for t in range(T+1): # Z zonotope print("time construction",t) z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\ np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\ np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi[t].x])+\ sys.F[t].x Gz1=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["x",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["x",t]])+\ np.dot(sys.D[t],sys.P["x",t])-np.dot(sys.R[t],sys.Q["x",t]) Gz2=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["w",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["w",t]])+\ np.hstack(( np.dot(sys.D[t],sys.P["w",t]),np.zeros((sys.z,sys.n)) ))-\ np.dot(sys.R[t],sys.Q["w",t]) Gz3=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["v",t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["v",t]])+\ -np.dot(sys.R[t],sys.Q["v",t]) Gw=spa.block_diag(*[sys.W[t].G for tau in range(0,t+1)]) Gv=spa.block_diag(*[sys.V[t].G for tau in range(0,t+1)]) Gz=np.hstack(( np.dot(Gz1,sys.X0.G), np.dot(Gz2,Gw), np.dot(Gz3,Gv) )) Z[t]=zonotope(z_bar,Gz) for t in range(T): # U zonotope u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x) Gu=np.dot(theta[t],sys.Xi[t].G) U[t]=zonotope(u_bar,Gu) # Proxy Quadratic cost for t in range(1,T): print(t,"cost") prog.AddQuadraticCost(sum(U[t].x.flatten()**2)) prog.AddQuadraticCost(sum(U[t].G.flatten()**2)) prog.AddQuadraticCost(sum(Z[t].x.flatten()**2)) prog.AddQuadraticCost(sum(Z[t].G.flatten()**2)) print("Now solving the QP") result=gurobi_solver.Solve(prog,None,None) if result.is_success(): print("Synthesis Success!","\n"*5) # print "D=",result.GetSolution(D) theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)} u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)} return u_tilde_n,theta_n else: print("Synthesis Failed!")
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\ triangular_stack_list,output_feedback_synthesis_lightweight_many_variables,invariance_time from pypolytrajectory.system import LQG_LTV, LTV, LQG import scipy.linalg as spa np.random.seed(0) S = LTV() n = 6 m = 2 o = 1 z = 1 T = 22 S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1) B = np.random.randint(0, 2, size=(n, m)) B[0, 0] = 0 #B[1,0]=0 A = 0.3 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 * 0.2 C = np.zeros((o, n)) C[0, 0] = 1 #C[1,1]=1 D = np.eye(n)[0:z, :] #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6) #B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1)
def output_feedback_synthesis_lightweight_many_variables(sys,T,H_Z={},H_U={}): prog=MP.MathematicalProgram() # Add Variables phi,theta,Phi,Theta={},{},{},{} y_tilde,u_tilde={},{} Y_tilde,U_tilde={},{} z_bar,u_bar={},{} Z,U={},{} # Z matrices Gz={} # Main variables for t in range(T+1): theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t) # T_last=50 # for i in range(sys.o*(t+1)): # if sys.o*(t+1)-i>sys.o*T_last: # theta[t][:,i]=0 u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t) y_tilde[t]=prog.NewContinuousVariables(sys.o,1,"y_tilde%d"%t) phi[t]=prog.NewContinuousVariables(sys.o,sys.o*(t+1),"Phi%d"%t) Gz["var",t]=prog.NewContinuousVariables(sys.z,sys.Xi_reduced[t].G.shape[1],"Gz%d"%t) Theta[0]=theta[0] y_tilde[0]=np.zeros((sys.o,1)) phi[0]=np.eye(sys.o) Phi[0]=phi[0] # Aggragates for t in range(T+1): Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)]) U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)]) for t in range(T): # Dynamics s=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t]) prog.AddLinearConstraint(np.equal(y_tilde[t+1],s,dtype='object').flatten()) # prog.AddLinearConstraint((y_tilde[t+1]==s).flatten()) S=np.hstack((np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o))) prog.AddLinearConstraint(np.equal(phi[t+1],S,dtype='object').flatten()) # prog.AddLinearConstraint((phi[t+1]==S).flatten()) Phi[t+1]=triangular_stack(Phi[t],phi[t+1]) Theta[t+1]=triangular_stack(Theta[t],theta[t+1]) # Performance variables for t in range(T+1): # Z zonotope print("time construction",t) z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\ np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\ np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi_reduced[t].x])+\ sys.F[t].x Gz[t]=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi_reduced[t].G])+\ np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi_reduced[t].G]) Z[t]=zonotope(z_bar,np.hstack((Gz[t],sys.F_reduced[t].G))) # Z[t]=zonotope(z_bar,Gz[t]) prog.AddLinearConstraint(np.equal(Gz[t],Gz["var",t],dtype='object').flatten()) for t in range(T): # U zonotope u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi_reduced[t].x) Gu=np.dot(theta[t],sys.Xi_reduced[t].G) U[t]=zonotope(u_bar,Gu) # Constraints for t,value in H_U.items(): print("performing subset for U",t,value) subset(prog,U[t],H_U[t]) for t,value in H_Z.items(): print("performing subset for Z",t,value) subset(prog,Z[t],H_Z[t]) # Proxy Linear Cost prog.AddLinearConstraint(np.equal(u_tilde[T-1],np.zeros((sys.m,1)),dtype='object').flatten()) if True: r={} # r["u-max"]=prog.NewContinuousVariables(1,1,"ru-max") # r["z-max"]=prog.NewContinuousVariables(1,1,"rz-max") # prog.NewContinuousVariables(1,1,"r") # prog.AddLinearCost(r["u-max"][0,0]) # prog.AddLinearCost(r["z-max"][0,0]) for t in range(1,T+1): print(t,"adding cost for z") r["z",t]=prog.NewContinuousVariables(1,1,"r") R=hyperbox(sys.z) ZT=H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,r["z",t]),symbolic=True) subset(prog,Z[t],ZT) # prog.AddLinearConstraint(np.less_equal(r["z",t],r["z-max"],dtype='object').flatten()) prog.AddQuadraticCost(r["z",t][0,0]*r["z",t][0,0]) for t in range(T): print(t,"adding cost for u") r["u",t]=prog.NewContinuousVariables(1,1,"r") R=hyperbox(sys.m) UT=H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,r["u",t]),symbolic=True) subset(prog,U[t],UT) prog.AddQuadraticCost(r["u",t][0,0]*r["u",t][0,0]) # subset(prog,U[t],H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,3))) # prog.AddLinearConstraint(np.less_equal(r["u",t],r["u-max"],dtype='object').flatten()) # Proxy Quadratic cost elif False: J=0 for t in range(T): print(t,"cost") J+=sum(U[t].x.flatten()**2) J+=sum(U[t].G.flatten()**2) J+=sum(Z[t+1].x.flatten()**2) J+=sum(Gz[t+1].flatten()**2) prog.AddQuadraticCost(J) print("Now solving the Linear Program") start=time.time() result=gurobi_solver.Solve(prog,None,None) print("time to solve",time.time()-start) if result.is_success(): print("Synthesis Success!","\n"*5) # print "D=",result.GetSolution(D) theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)} u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)} r_z=np.array([result.GetSolution(r["z",t][0,0]) for t in range(1,T+1)]) r_u=np.array([result.GetSolution(r["u",t][0,0]) for t in range(0,T)]) print("Upper Cost",np.linalg.norm(r_z,ord=2)**2+np.linalg.norm(r_u,ord=2)**2) # print "Upper Cost",np.linalg.norm(r_u,ord=2)**2 # print r_z print(r_u) return(u_tilde_n,theta_n) else: print("Synthesis Failed!")
tau_g = g * np.array([[m_h * l + m * a + m * l, 0], [0, -m * b]]) for t in range(T): S.A[t] = np.eye(n) S.A[t][0, 2], S.A[t][1, 3] = dt, dt S.A[t][2:, 0:2] = np.dot(Minv, tau_g) * dt Bq = np.dot(Minv, np.array([1, 1])) * dt S.B[t] = np.array([0, 0, Bq[0], Bq[1]]).reshape(4, 1) # S.B[t]=np.array([0,0,1,1]).reshape(4,1) # S.B[t]=np.array([[0,0,1,0],[0,0,0,1]]).T # S.B[t]=np.array([0,0,1,1]).reshape(4,1) # S.C[t]=np.array([[1,0,0,0],[0,1,0,0]]).reshape(2,4) S.C[t] = np.array([[1, 1, 0, 0]]).reshape(1, 4) S.D[t] = np.eye(n) S.d[t] = np.zeros((n, 1)) S.W[t] = zonotope(np.ones((n, 1)) * 0, np.ones((n, 1)) * 0) # S.V[t]=zonotope(np.zeros((2,1)),np.ones((2,1))*0.0000) S.V[t] = zonotope(np.zeros((1, 1)), np.eye(1) * 0.00) if True: prog = MP.MathematicalProgram() T = 20 R = np.array([[0, -1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, -1], [0, 0, -1, 0]]) x, u = {}, {} for t in range(T + 1): x[t] = prog.NewContinuousVariables(4, 1, "x%d" % t) u[t] = prog.NewContinuousVariables(1, 1, "u%d" % t) x[0][0, 0] = 0.12 x[0][1, 0] = 0.12 for t in range(T): x_new = np.dot(S.A[0], x[t]) + np.dot(S.B[0], u[t])
from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\ triangular_stack_list,output_feedback_synthesis_lightweight_many_variables from pypolytrajectory.system import LQG_LTV, LTV, LQG import scipy.linalg as spa np.random.seed(0) S = LTV() n = 20 m = 1 o = 1 z = 20 T = 58 S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1) B = np.random.randint(0, 2, size=(n, m)) B[0, 0] = 1 B[1, 0] = 0 A = 0.9 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 * 0.15 C = np.zeros((o, n)) C[0, 0] = 1 #C[1,1]=1 D = np.eye(n)[0:z, :] #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6) #B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1)
import scipy.linalg as spa # pypolycontain from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZax np.random.seed(0) S=LTV() n=10 m=2 o=1 z=2 T=42 x0_bar=np.ones((n,1))*10 x0_bar[0,0]=25 x0_bar[1,0]=-25 S.X0=zonotope(x0_bar,np.eye(n)*1) B=np.random.randint(0,2,size=(n,m))*1 #B[0,0]=0.05 #B[1,1]=0.05 A=0.95*np.eye(n)+np.random.randint(-100,100,size=(n,n))*0.01*0.1 #n_2=8 #A[n-n_2:,n-n_2:]=np.eye(n_2) #A[n-n_2:,:n-n_2]=np.zeros((n_2,n-n_2)) for i in range(n): for j in range(n): if i>j: A[i,j]=0 C=np.zeros((o,n)) C[0,0]=1 #C[1,1]=1 D=np.eye(n)[0:z,:]
import numpy as np from pypolytrajectory.LTV import system, test_controllability from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard from pypolytrajectory.synthesis import synthesis_disturbance_feedback, zonotopic_controller, synthesis import pydrake.systems.controllers as PC S = system() n = 6 m = 1 o = 1 T = 48 #np.random.seed(1) S.X0 = zonotope(np.array(([0, 0, 0, 0, 0, 0])).reshape(6, 1), np.eye(n) * 1) for t in range(T): S.A[t] = np.array([[0.28, 0.25, -0.19, -0.22, 0.03, -0.50], [0.25, -0.47, 0.30, 0.17, -0.11, -0.11], [-0.19, 0.30, 0.46, 0.09, -0.02, -0.08], [-0.22, 0.17, 0.09, 0.60, -0.06, 0.14], [0.03, -0.11, -0.02, -0.06, 0.46, -0.13], [-0.50, -0.11, -0.08, 0.14, -0.13, -0.23]]).reshape(6, 6) S.B[t] = np.array([[1.0159, 0, 0.5988, 1.8641, 0, -1.2155]]).reshape(6, 1) #S.C[t]=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6) S.C[t] = np.array([[1.29, 0.24, 0, 0, 0, 0]]).reshape(1, 6) S.W[t] = zonotope(np.zeros((n, 1)), np.eye(n) * 0.05) S.V[t] = zonotope(np.zeros((o, 1)), np.eye(o) * 0.05) S.U_set = zonotope(np.zeros((m, 1)), np.eye(m) * 1)
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\ triangular_stack_list,output_feedback_synthesis_lightweight_many_variables from pypolytrajectory.system import LQG_LTV, LTV, LQG import scipy.linalg as spa np.random.seed(1000) S = LTV() n = 6 m = 1 o = 1 z = 1 T = 42 S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1) B = np.random.randint(0, 2, size=(n, m)) B[0, 0] = 0 A = 0.0 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 C = np.zeros((o, n)) C[0, 0] = 1 D = np.eye(n)[0:z, :] #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6) #B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1) #C=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6) for t in range(T):
import numpy as np from pypolytrajectory.LTV import system,test_controllability from pypolytrajectory.reduction import reduced_order,order_reduction_error,error_construction,error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut,Girard from pypolytrajectory.synthesis import synthesis_disturbance_feedback,zonotopic_controller,synthesis,zonotopic_controller_soft from pypolytrajectory.system import LQG_LTV,LTV,LQG S=LTV() n=5 m=1 o=1 T=58 S.X0=zonotope(np.ones((n,1))*0,np.eye(n)*1) B=np.random.randint(0,2,size=(n,m)) B[0,0]=1 B[1,0]=0 A=0.0*np.eye(n)+np.random.normal(size=(n,n))*0.6 C=np.zeros((o,n)) C[0,0]=1 #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6) #B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1) #C=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6) for t in range(T):
T=int((K+d_0)/(v*dt))+1 gap_bar,step_var,gap_var=1,0.3,0.1 x0_bar=np.zeros((n,1)) G=np.zeros((n,n)) x0_bar[0,0],x0_bar[1,0]=-d_0,0 G[0,0]=0 # Variation in x G[1,1]=0 # Variation in speed G[2,2]=0 # Variation in height for i in range(K): x0_bar[3+2*i+1]=gap_bar G[3+2*i+1,3+2*i+1]=gap_var for j in range(K-i): G[3+2*i+2*j,3+2*i]=step_var S.X0=zonotope(x0_bar,G) def visualize(ax,x): # Add the patch to the Axes for i in range(K): rect_down = patches.Rectangle((i,x[3+2*i]-10),1,10,linewidth=1,edgecolor='black',facecolor='orange',alpha=0.59) rect_up = patches.Rectangle((i,x[3+2*i]+x[3+2*i+1]),1,10,linewidth=1,edgecolor='black',facecolor='orange',alpha=0.59) ax.set_xlim([-1,K-1]) ax.set_ylim([-3,3]) ax.add_patch(rect_down) ax.add_patch(rect_up) #zeta=2*(np.random.random((n,1))-0.5) #x=np.dot(S.X0.G,zeta)+S.X0.x #visualize(x)
def _error_zonotopes(self,T=0): Sigma={} Psi={} if T==0: T=max(self.A.keys()) # Construction of Xi: self.Xi,self.Xi_reduced,self.F_reduced={},{},{} self.Xi["x",0]=self.C[0] self.Xi["w",0]=np.zeros((self.o,self.n)) self.Xi["v",0]=np.eye(self.o) for t in range(T): # print "t=",t Gw=spa.block_diag(*[self.W[t].G for tau in range(0,t+1)]) Gv=spa.block_diag(*[self.V[t].G for tau in range(0,t+1)]) Wbar=np.vstack([self.W[tau].x for tau in range(0,t+1)]) Vbar=np.vstack([self.V[tau].x for tau in range(0,t+1)]) L1=np.linalg.multi_dot([self.C[t+1],self.P["x",t+1],self.X0.G]) L2=np.linalg.multi_dot([self.C[t+1],self.P["w",t+1],Gw]) L3=np.zeros((self.o,Gv.shape[1])) L4=self.V[t+1].G F1=np.dot(self.Q["x",t],self.X0.G) F2=np.dot(self.Q["w",t],Gw) F3=np.dot(self.Q["v",t],Gv) F4=np.zeros(((t+1)*self.o,self.V[t+1].G.shape[1])) Sigma["e",t]=np.hstack((L1,L2,L3,L4)) Psi["e",t]=np.hstack((F1,F2,F3,F4)) # print Sigma["e",t].shape # print Psi["e",t].shape K_1=np.linalg.multi_dot([self.D[t],self.P["x",t],self.X0.G]) K_2=np.dot(np.hstack((np.dot(self.D[t],self.P["w",t]),np.zeros((self.z,self.n)))),Gw) K_3=np.zeros((self.z,Gv.shape[1])) P_1=np.dot(self.Q["x",t],self.X0.G) P_2=np.dot(self.Q["w",t],Gw) P_3=-np.dot(self.Q["v",t],Gv) Sigma["f",t]=np.hstack((K_1,K_2,K_3)) Psi["f",t]=np.hstack((P_1,P_2,P_3)) # print "f",Sigma["f",t].shape # print "f",Psi["f",t].shape self.M[t]=np.dot(Sigma["e",t],np.linalg.pinv(Psi["e",t])) self.R[t]=np.dot(Sigma["f",t],np.linalg.pinv(Psi["f",t])) self.N[t]=np.dot(self.C[t+1],self.P["u",t+1])-np.dot(self.M[t],self.Q["u",t]) self.S[t]=np.hstack(( np.dot(self.D[t],self.P["u",t]),np.zeros((self.z,self.m)) ))-np.dot(self.R[t],self.Q["u",t]) # print self.M[t].shape # print self.N[t].shape # print self.R[t].shape # print self.S[t].shape ebar=np.linalg.multi_dot([self.C[t+1],self.P["x",t+1],self.X0.x])\ -np.linalg.multi_dot([self.M[t],self.Q["x",t],self.X0.x])\ +np.linalg.multi_dot([self.C[t+1],self.P["w",t+1],Wbar])\ -np.linalg.multi_dot([self.M[t],self.Q["w",t],Wbar])\ +np.linalg.multi_dot([self.M[t],self.Q["v",t],Vbar])\ +self.V[t+1].x eG=Sigma["e",t]-np.dot(self.M[t],Psi["e",t]) self.E[t]=zonotope(ebar,eG) fbar=np.linalg.multi_dot([self.D[t],self.P["x",t],self.X0.x])\ -np.linalg.multi_dot([self.R[t],self.Q["x",t],self.X0.x])\ +np.dot(np.hstack((np.dot(self.D[t],self.P["w",t]),np.zeros((self.z,self.n)))),Wbar)\ -np.linalg.multi_dot([self.R[t],self.Q["w",t],Wbar])\ -np.linalg.multi_dot([self.R[t],self.Q["v",t],Vbar])\ +self.d[t] fG=Sigma["f",t]-np.dot(self.R[t],Psi["f",t]) self.F[t]=zonotope(fbar,fG) F_G_reduced=Girard(fG,fG.shape[0]) self.F_reduced[t]=zonotope(fbar,F_G_reduced) for t in range(T): self.Xi["x",t+1]=triangular_stack(self.Xi["x",t],np.dot(self.C[t+1],self.P["x",t+1])-np.dot(self.M[t],self.Q["x",t])) self.Xi["w",t+1]=triangular_stack(self.Xi["w",t],np.hstack((np.dot(self.C[t+1],self.P["w",t+1])-np.dot(self.M[t],self.Q["w",t]),\ np.zeros((self.o,self.n)) )) ) self.Xi["v",t+1]=triangular_stack(self.Xi["v",t],np.hstack((np.dot(self.M[t],self.Q["v",t]),np.eye(self.o)))) for t in range(T+1): Gw=spa.block_diag(*[self.W[t].G for tau in range(0,t+1)]) Gv=spa.block_diag(*[self.V[t].G for tau in range(0,t+1)]) Wbar=np.vstack([self.W[tau].x for tau in range(0,t+1)]) Vbar=np.vstack([self.V[tau].x for tau in range(0,t+1)]) # print "t=",t,"Xi_shapes",Xi["x",t].shape,Xi["w",t].shape,Xi["v",t].shape # print "bar",Wbar.shape,Vbar.shape Xi_bar=np.dot(self.Xi["x",t],self.X0.x)+np.dot(self.Xi["w",t],Wbar)+np.dot(self.Xi["v",t],Vbar) Xi_G=np.hstack((np.dot(self.Xi["x",t],self.X0.G),np.dot(self.Xi["w",t],Gw),np.dot(self.Xi["v",t],Gv))) self.Xi[t]=zonotope(Xi_bar,Xi_G) Xi_G_reduced=Girard(Xi_G,Xi_G.shape[0]) self.Xi_reduced[t]=zonotope(Xi_bar,Xi_G_reduced)