def steer(self,x_from_node,x_toward,cost_limit=True): A = self.A B = self.B Q = self.Q R = self.R x_from = x_from_node['state'] assert len(x_from) == self.n+1 T = x_toward[self.n] - x_from[self.n] #how much time to do the steering assert T-int(T) == 0 #discrete time T=int(T) if T<=0: return (x_from,np.zeros(shape=(0,self.m))) #stay here desired = np.matrix(x_toward[0:self.n].reshape(self.n,1)) Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf,desired) Qhf = np.zeros(shape=(self.n+1,self.n+1)) Qhf[0:self.n,0:self.n] = Qf Qhf[0:self.n,[self.n]] = qf Qhf[[self.n],0:self.n] = qf.T Qhf[self.n,self.n] = np.dot(desired.T,np.dot(Qf,desired)) (Ah,Bh,Qh,Rh,pk) = AQR( A=A, B=B, c=np.zeros(shape=(self.n,1)), Q=Q, R=R, q=np.zeros(shape=(self.n,1)), r=np.zeros(shape=(self.m,1)), ctdt='dt') #pk should be zeros. Fs, Ps = dtfh_lqr(A=Ah,B=Bh,Q=Qh,R=R,N=T,Q_terminal=Qhf) #print Fs xs = np.zeros(shape=(T+1,self.n+1)) us = np.zeros(shape=(T,self.m)) xs[0] = x_from cost = 0 for i in range(T): us[i] = -1 * (np.dot(Fs[i,:,0:self.n],xs[i,0:self.n]) + Fs[i,:,self.n]) xs[i+1,0:self.n] = np.dot(A,xs[i,0:self.n].T) + np.dot(B,us[i].T) xs[i+1,self.n] = xs[i,self.n] + 1 cost += self.cost(xs[i].T,us[i].reshape(1,self.m)) if cost_limit and cost > self.max_steer_cost: break if i == T-1: x_actual = xs[-1] return (x_actual, us) else: return (xs[i], us[:i])
def distance(self, from_node, to_point): #to_point is an array and from_point is a node A, B, c = self.get_ABc(from_node['state'][0:self.n], self.u0) Q, R, q, r, d = self.get_QRqrd(from_node['state'][0:self.n], self.u0) x_from = from_node['state'] x_toward = to_point assert len(x_toward) == self.n + 1 T = x_toward[self.n] - x_from[ self.n] #how much time to do the steering assert T - int(T) == 0 #discrete time T = int(T) if T < 0: return np.inf elif T == 0: return 0 if self.same_state(x_from, x_toward) else np.inf desired = np.matrix(x_toward[0:self.n].reshape(self.n, 1)) #we want the final bowl to be centered at desired: #(x-x_d)^T * Qf * (x-x_d) #xT*Qf*x -x_dT * Qf * x - xT *Qf *x_d * x_dT * Qf * x_d Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf, desired) Qhf = np.zeros(shape=(self.n + 1, self.n + 1)) Qhf[0:self.n, 0:self.n] = Qf Qhf[0:self.n, [self.n]] = qf Qhf[[self.n], 0:self.n] = qf.T Qhf[self.n, self.n] = np.dot(desired.T, np.dot(Qf, desired)) (Ah, Bh, Qh, Rh, pk) = AQR(A=A, B=B, Q=Q, R=R, c=c, q=q, r=r, d=d, ctdt='dt') T = T + 1 Fs, Ps = dtfh_lqr(A=Ah, B=Bh, Q=Qh, R=R, N=T, Q_terminal=Qhf) x_from_homo = np.zeros(self.n + 1) x_from_homo[0:self.n] = x_from[0:self.n] x_from_homo[self.n] = 1 #assert False return np.dot(x_from_homo.T, np.dot(Ps[0], x_from_homo))
def distance(self,from_node,to_point): #to_point is an array and from_point is a node A,B,c = self.get_ABc(from_node['state'][0:self.n],self.u0) Q,R,q,r,d = self.get_QRqrd(from_node['state'][0:self.n],self.u0) x_from = from_node['state'] x_toward = to_point assert len(x_toward)==self.n+1 T = x_toward[self.n] - x_from[self.n] #how much time to do the steering assert T-int(T) == 0 #discrete time T=int(T) if T<0: return np.inf elif T==0: return 0 if self.same_state(x_from,x_toward) else np.inf desired = np.matrix(x_toward[0:self.n].reshape(self.n,1)) #we want the final bowl to be centered at desired: #(x-x_d)^T * Qf * (x-x_d) #xT*Qf*x -x_dT * Qf * x - xT *Qf *x_d * x_dT * Qf * x_d Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf,desired) Qhf = np.zeros(shape=(self.n+1,self.n+1)) Qhf[0:self.n,0:self.n] = Qf Qhf[0:self.n,[self.n]] = qf Qhf[[self.n],0:self.n] = qf.T Qhf[self.n,self.n] = np.dot(desired.T,np.dot(Qf,desired)) (Ah,Bh,Qh,Rh,pk) = AQR( A=A, B=B, Q=Q, R=R, c=c, q=q, r=r, d=d, ctdt='dt') T = T+1 Fs, Ps = dtfh_lqr(A=Ah,B=Bh,Q=Qh,R=R,N=T,Q_terminal=Qhf) x_from_homo = np.zeros(self.n+1) x_from_homo[0:self.n] = x_from[0:self.n] x_from_homo[self.n] = 1 #assert False return np.dot(x_from_homo.T,np.dot(Ps[0],x_from_homo))
desired = np.matrix([[0], [1]]) q = -np.dot(Q,desired) r=np.matrix([0]).T (Ah,Bh,Qh,Rh,pk) = AQR(A=A,B=B,c=c,Q=Q,R=R,q=q,r=r,ctdt='dt') T = 50 x0 = np.array([0e0,0]) DUAL = False if not DUAL: gain_matrices,cost_to_go_matrices = dtfh_lqr(Ah,Bh,Qh,Rh,N=T-1, Q_terminal=Qh*1e6) else: gain_matrices,cost_to_go_matrices = dtfh_lqr_dual(Ah,Bh,Qh,Rh,N=T-1, Q_terminal_inv=Qh*0e0) dp_xs,dp_us = simulate_lti_fb_dt(A=Ah,B=Bh,x0=np.concatenate([x0,[1]]), gain_schedule=-gain_matrices,T=T) qp_solution,(QP_P,QP_q,QP_A,QP_B),qp_xs,qp_us = LQR_QP(Ah,Bh,Qh,Rh,T=T,x0=np.concatenate([x0,[1]]), #xT=np.concatenate([[10,0],[1]]) #xT=np.concatenate([[1,0],[1]]) xT=np.concatenate([desired.flat,[1]]) ) #qp_solution,(QP_P,QP_q,QP_A,QP_B) = LQR_QP(Ah,Bh,Qh,Rh,T=T,x0=np.concatenate([x0,[1]]))
def steer(self, x_from_node, x_toward, cost_limit=True): A = self.A B = self.B Q = self.Q R = self.R x_from = x_from_node['state'] assert len(x_from) == self.n + 1 T = x_toward[self.n] - x_from[ self.n] #how much time to do the steering assert T - int(T) == 0 #discrete time T = int(T) if T <= 0: return (x_from, np.zeros(shape=(0, self.m))) #stay here desired = np.matrix(x_toward[0:self.n].reshape(self.n, 1)) Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf, desired) Qhf = np.zeros(shape=(self.n + 1, self.n + 1)) Qhf[0:self.n, 0:self.n] = Qf Qhf[0:self.n, [self.n]] = qf Qhf[[self.n], 0:self.n] = qf.T Qhf[self.n, self.n] = np.dot(desired.T, np.dot(Qf, desired)) (Ah, Bh, Qh, Rh, pk) = AQR(A=A, B=B, c=np.zeros(shape=(self.n, 1)), Q=Q, R=R, q=np.zeros(shape=(self.n, 1)), r=np.zeros(shape=(self.m, 1)), ctdt='dt') #pk should be zeros. Fs, Ps = dtfh_lqr(A=Ah, B=Bh, Q=Qh, R=R, N=T, Q_terminal=Qhf) #print Fs xs = np.zeros(shape=(T + 1, self.n + 1)) us = np.zeros(shape=(T, self.m)) xs[0] = x_from cost = 0 for i in range(T): us[i] = -1 * (np.dot(Fs[i, :, 0:self.n], xs[i, 0:self.n]) + Fs[i, :, self.n]) xs[i + 1, 0:self.n] = np.dot(A, xs[i, 0:self.n].T) + np.dot(B, us[i].T) xs[i + 1, self.n] = xs[i, self.n] + 1 cost += self.cost(xs[i].T, us[i].reshape(1, self.m)) if cost_limit and cost > self.max_steer_cost: break if i == T - 1: x_actual = xs[-1] return (x_actual, us) else: return (xs[i], us[:i])
import numpy as np import matplotlib.pyplot as plt A = np.matrix([[1.2,0], [0,1.4]]) B = np.matrix([1,100]).T Q = np.matrix([[1,0], [0,1]]) R = np.eye(1) * 1 T = 40 gain_schedule,cost_to_go_schedule = dtfh_lqr(A,B,Q,R,T-1) x0 = np.array([2,1]) xsol, usol = simulate_lti_fb_dt(A,B,x0,-gain_schedule,T) plt.figure(None) plt.plot(xsol[0],xsol[1]) plt.plot(xsol[0],xsol[1],'o') fig = plt.figure(None) fig.add_subplot(3,1,1).plot(xsol[0]) fig.add_subplot(3,1,2).plot(xsol[1]) fig.add_subplot(3,1,3).plot(usol[0,:])
B=tmp[0:n,n:n+nb] #only penalize error (deviation from desired) at the final time step. Q = np.matrix([[0,0], [0,0]]) #to demonstate that there are, in general, multiple minima in the cost-to-go function, must assign unequal weights #to the state variables. Q_terminal = np.matrix([[0,0], [0,1]]) R = np.eye(1)*1e0 #Fs are gain matrices, Ps are cost-to-go matrices (Fs, Ps) = lqr_tools.dtfh_lqr(A,B,Q,R,Q_terminal = Q_terminal,N=200) #the set point desired = np.matrix([[0], [1]]) #starting from initial = np.matrix([ [0], [0]]) error = initial-desired #compute series of cost-to-go values starting from . the following just computes (error^T * P_i * error) for all i """ unvectorized: b = [] for P in Ps: b.append( np.dot(desired.T,np.dot(P,desired))[0,0] )
tmp = np.vstack((tmp, ztmp)) tmp = expm(tmp * Ts) A = tmp[0:n, 0:n] B = tmp[0:n, n:n + nb] #only penalize error (deviation from desired) at the final time step. Q = np.matrix([[0, 0], [0, 0]]) #to demonstate that there are, in general, multiple minima in the cost-to-go function, must assign unequal weights #to the state variables. Q_terminal = np.matrix([[0, 0], [0, 1]]) R = np.eye(1) * 1e0 #Fs are gain matrices, Ps are cost-to-go matrices (Fs, Ps) = lqr_tools.dtfh_lqr(A, B, Q, R, Q_terminal=Q_terminal, N=200) #the set point desired = np.matrix([[0], [1]]) #starting from initial = np.matrix([[0], [0]]) error = initial - desired #compute series of cost-to-go values starting from . the following just computes (error^T * P_i * error) for all i """ unvectorized: b = [] for P in Ps: b.append( np.dot(desired.T,np.dot(P,desired))[0,0] ) b = np.array(b) """
def steer(self, x_from_node, x_toward, cost_limit=True): A, B, c = self.get_ABc(x_from_node['state'][0:self.n], self.u0) Q, R, q, r, d = self.get_QRqrd(x_from_node['state'][0:self.n], self.u0) x_from = x_from_node['state'] assert len(x_from) == self.n + 1 T = x_toward[self.n] - x_from[ self.n] #how much time to do the steering assert T - int(T) == 0 #discrete time T = int(T) if T <= 0: return (x_from, np.zeros(shape=(0, self.m))) #stay here desired = np.matrix(x_toward[0:self.n].reshape(self.n, 1)) Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf, desired) Qhf = np.zeros(shape=(self.n + 1, self.n + 1)) Qhf[0:self.n, 0:self.n] = Qf Qhf[0:self.n, [self.n]] = qf Qhf[[self.n], 0:self.n] = qf.T Qhf[self.n, self.n] = np.dot(desired.T, np.dot(Qf, desired)) (Ah, Bh, Qh, Rh, pk) = AQR(A=A, B=B, c=c, Q=Q, R=R, q=q, r=r, d=d, ctdt='dt') Fs, Ps = dtfh_lqr(A=Ah, B=Bh, Q=Qh, R=R, N=T, Q_terminal=Qhf) #print Fs xs = np.zeros(shape=(T + 1, self.n + 1)) us = np.zeros(shape=(T, self.m)) xs[0] = x_from cost = 0 for i in range(T): us[i] = -1 * (np.dot(Fs[i, :, 0:self.n], xs[i, 0:self.n]) + Fs[i, :, self.n]) + pk #FIXME #xs[i+1,0:self.n] = np.dot(A,xs[i,0:self.n].T) + np.dot(B,us[i].T) + c #pretend the system is linear #xs[i+1,self.n] = xs[i,self.n] + 1 xs[i + 1] = self.run_forward(xs[i], us[[i]]) cost += self.cost(xs[i].T, us[i].reshape(1, self.m)) if cost_limit and cost > self.max_steer_cost: break if i < T - 1: us = us[:i] x_final = xs[i + 1] else: x_final = xs[T] return (x_final, us)
import scipy import scipy.signal from lqr_tools import dtfh_lqr, simulate_lti_fb_dt import numpy as np import matplotlib.pyplot as plt A = np.matrix([[1.2, 0], [0, 1.4]]) B = np.matrix([1, 100]).T Q = np.matrix([[1, 0], [0, 1]]) R = np.eye(1) * 1 T = 40 gain_schedule, cost_to_go_schedule = dtfh_lqr(A, B, Q, R, T - 1) x0 = np.array([2, 1]) xsol, usol = simulate_lti_fb_dt(A, B, x0, -gain_schedule, T) plt.figure(None) plt.plot(xsol[0], xsol[1]) plt.plot(xsol[0], xsol[1], 'o') fig = plt.figure(None) fig.add_subplot(3, 1, 1).plot(xsol[0]) fig.add_subplot(3, 1, 2).plot(xsol[1]) fig.add_subplot(3, 1, 3).plot(usol[0, :])
def steer(self,x_from_node,x_toward,cost_limit=True): A,B,c = self.get_ABc(x_from_node['state'][0:self.n],self.u0) Q,R,q,r,d = self.get_QRqrd(x_from_node['state'][0:self.n],self.u0) x_from = x_from_node['state'] assert len(x_from) == self.n+1 T = x_toward[self.n] - x_from[self.n] #how much time to do the steering assert T-int(T) == 0 #discrete time T=int(T) if T<=0: return (x_from,np.zeros(shape=(0,self.m))) #stay here desired = np.matrix(x_toward[0:self.n].reshape(self.n,1)) Qf = np.eye(self.n) * 1e8 qf = -np.dot(Qf,desired) Qhf = np.zeros(shape=(self.n+1,self.n+1)) Qhf[0:self.n,0:self.n] = Qf Qhf[0:self.n,[self.n]] = qf Qhf[[self.n],0:self.n] = qf.T Qhf[self.n,self.n] = np.dot(desired.T,np.dot(Qf,desired)) (Ah,Bh,Qh,Rh,pk) = AQR( A=A, B=B, c=c, Q=Q, R=R, q=q, r=r, d=d, ctdt='dt') Fs, Ps = dtfh_lqr(A=Ah,B=Bh,Q=Qh,R=R,N=T,Q_terminal=Qhf) #print Fs xs = np.zeros(shape=(T+1,self.n+1)) us = np.zeros(shape=(T,self.m)) xs[0] = x_from cost = 0 for i in range(T): us[i] = -1 * (np.dot(Fs[i,:,0:self.n],xs[i,0:self.n]) + Fs[i,:,self.n]) + pk #FIXME #xs[i+1,0:self.n] = np.dot(A,xs[i,0:self.n].T) + np.dot(B,us[i].T) + c #pretend the system is linear #xs[i+1,self.n] = xs[i,self.n] + 1 xs[i+1] = self.run_forward(xs[i], us[[i]] ) cost += self.cost(xs[i].T,us[i].reshape(1,self.m)) if cost_limit and cost > self.max_steer_cost: break if i < T-1: us = us[:i] x_final = xs[i+1] else: x_final = xs[T] return (x_final, us)