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)
