def spatial_filter(self, R1, R2):
R = (R1 + R2)
E, U = la.eig(R)
order = np.flip(np.argsort(E), axis=0)
E = E[order]
U = U[:, order]
D = np.diag(E)
# Whitening transformation matrix
P = dot([np.sqrt(la.inv(D)), U.T])
S1 = dot([P, R1, P.T])
S2 = dot([P, R2, P.T])
E1, U1 = la.eig(S1)
E2, U2 = la.eig(S2)
order = np.flip(np.argsort(E1), axis=0)
E1 = E1[order]
U1 = U1[:, order]
F = dot([U1.T, P]).astype(np.float32)
return F
def updateDIIS(self,F,P):
FPS = dot([F,P,self.S])
SPF = self.adj(FPS)
# error must be in orthonormal basis
error = dot([self.X,FPS-SPF,self.X])
self.fockSet.append(self.F)
self.errorSet.append(error)
numFock = len(self.fockSet)
# limit subspace, hardcoded for now
if numFock > 8:
del self.fockSet[0]
del self.errorSet[0]
numFock -= 1
B = np.zeros((numFock + 1,numFock + 1))
B[-1,:] = B[:,-1] = -1.0
B[-1,-1] = 0.0
# B is symmetric
for i in range(numFock):
for j in range(i+1):
B[i,j] = B[j,i] = \
np.real(np.trace(np.dot(self.adj(self.errorSet[i]),
self.errorSet[j])))
residual = np.zeros(numFock + 1)
residual[-1] = -1.0
weights = np.linalg.solve(B,residual)
# weights is 1 x numFock + 1, but first numFock values
# should sum to one if we are doing DIIS correctly
assert np.isclose(sum(weights[:-1]),1.0)
F = np.zeros((self.nbasis,self.nbasis),dtype='complex')
for i, Fock in enumerate(self.fockSet):
F += weights[i] * Fock
return F
def fk(q, theta):
return dot([
Tz(l1),
Rz(q[0]),
Rz(theta[0]),
Tz(q[1]),
Tz(theta[1]),
Ty(l3),
Ty(q[2]),
Ty(theta[2])
])
def Jth(q, theta):
H = fk(q, theta)
H[0:3, 3] = 0
inv_H = transpose(H)
dH = dot([
Tz(l1),
Rz(q[0]),
dRz(theta[0]),
Tz(q[1]),
Tz(theta[1]),
Ty(l3),
Ty(q[2]),
Ty(theta[2]), inv_H
])
J1 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
dH = dot([
Tz(l1),
Rz(q[0]),
Rz(theta[0]),
Tz(q[1]),
dTz(),
Ty(l3),
Ty(q[2]),
Ty(theta[2]), inv_H
])
J2 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
dH = dot([
Tz(l1),
Rz(q[0]),
Rz(theta[0]),
Tz(q[1]),
Tz(theta[1]),
Ty(l3),
Ty(q[2]),
dTy(), inv_H
])
J3 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
return h([J1, J2, J3])
def JacobianPassiveLeg(tFk, tb, Tt, qa, qp, th):
tFk[0:3, 3] = 0
dt = dot([tb,dot([Tz(qa),Tz(th[0]),dRz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
J1 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
dRz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
J2 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),dRz(qp[2])]),Tt,transpose(tFk)])
J3 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
return h([J1,J2,J3])
def MatMultiply(a):
from numpy.linalg import dot
from functools import reduce
return reduce(lambda x, y: dot(y, x), (aa for aa in a))
def episode(self, tstart, rcd=False):
state = self.env.reset_state()
action = self.boltzmannPolicy(state, self.β)
x = self.indices(action) #self.φ(state, action, ns=False)
for t in range(1, self.max_n_ep + 1):
η = self.η0 * min(1, self.ηstart / (tstart + t))
# print(η, self.β)
self.β = self.βinit #np.power(self.βinit, (tstart + t)/self.βfrac)
self.render(rcd, state)
# Observe reward r_{t+1} and next state s_{t+1}
next_state, reward, done = self.env.step(state, action)
# print(reward)
# Choose the next action a_{t+1}
next_action = self.boltzmannPolicy(next_state, self.β)
# print(next_action)
next_x = self.indices(
next_action) #self.φ(next_state, next_action, ns=False)
V_x = self.V_rd(x)
V_next_x = self.V_rd(next_x)
Dt = np.diag(np.exp(self.d.d(z=self.encode(x)))) # z_t = x_t-1
Dt1 = np.diag(np.exp(self.d.d(z=self.encode(next_x))))
Q_x = self.α + np.log(det(dot([V_x, Dt, V_x.T])))
Q_next_x = self.α + np.log(det(dot([V_next_x, Dt1, V_next_x.T])))
print(x)
if x == []:
print('\n\n\n\nFUCK\n\n\n')
print(Q_x)
TD = reward + self.ρ * Q_next_x - Q_x
# print(V_x)
invV = pinv(V_x)
# print('louis')
# print(invV.shape)
# print('\n\n')
grad_Q = 2 * invV.T
# self.α += η * TD# grad_α = 1
# V update
# print(self.α)
# self.λ *= 0.9999
# print(self.V)
# self.V_wr( x, η * TD * grad_Q )#
self.V_wr(x, η * TD * grad_Q - (V_x - self.ei_s(x)) * η * self.λ)
# V(x) = V(x) + η*TD*grad_Q - A(x)*η*λ,
# where A(x) = V(x) - I(x), (I(x) is the rows of the identity
# matrix indexed by 1’s in x), where λ may be a function of the
# timestep.
d_wrt_phi = self.d.grad(z=self.encode(x))
Q_wrt_d = np.diag(np.dot(invV, V_x))
# print('Q_wrt_d')
# print(Q_wrt_d)
# dVV = np.zeros((self.N, self.N))
# for i, iv in enumerate(x): dVV[iv, iv] = dVxVx[i]
# dVV = np.diagonal(dVxVx)
# print('dVV')
# print(dVV)
# print('='*50)
# print(self.d.phi)
# print('-'*50)
# print(η * TD * np.dot( dVV, grad_d ))
# print('dVV shape '+ str(dVV.shape))
# print('grad_d shape '+ str(grad_d.shape))
self.d.phi += η * TD * np.outer(Q_wrt_d, d_wrt_phi[0, :])
# self.d.phi += η * TD * np.dot( dVV, grad_d )
self.total_reward += reward
if (tstart + t) % self.bin == 0:
self.rec_reward.append(
(t + tstart, self.total_reward / self.bin))
self.total_reward = 0
if done:
self.render(rcd, next_state)
self.found = True
# print('\n\n\n\nCompleted episode, t=',str(t),'\n\n\n')
return t
state = next_state
action = next_action
x = next_x
return self.max_n_ep
def computeDET(action):
x = self.indices(action)
V = self.V_rd(x)
D = np.diag(np.exp(self.d.d(z=action)))
return det(dot([V, D, V.T]))
Ty(q[2]),
dTy(), inv_H
])
J3 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
return h([J1, J2, J3])
experiments = 30
A1, A2 = zeros((3, 3)), zeros(3)
for i in range(experiments):
q = h([uniform(-pi, pi), uniform(0, 1, 2)])
W = uniform(-1000, 1000, 6)
jTheta = Jth(q, theta)
dt = dot([jTheta, inv(kTheta), transpose(jTheta), W]) + normal(loc=0.0,
scale=1e-5)
jTheta = jTheta[0:3, :]
dt = dt[0:3]
W = W[0:3]
A = zeros(jTheta.shape)
for i in range(jTheta.shape[1]):
j = jTheta[:, i]
A[:, i] = outer(j, j).dot(W)
A1 += transpose(A).dot(A)
A2 += transpose(A).dot(dt)
kTheta2 = diag(divide(1, inv(A1).dot(A2)))
W = [-440, -1370, -1635, 0, 0, 0]
def JacobianthLeg(TFk,Tb,Tt,qa,qp,th):
TFk[0:3, 3] = 0
dt = dot([Tb,dot([Tz(qa),dTz(),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J1 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),dTx(),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J2 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),dTy(),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J3 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),dTz(),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J4 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),dRx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J5 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),dRy(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J6 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),dRz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J7 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),dTx(),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J8 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),dTy(),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J9 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),dTz(),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J10 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),dRx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J11 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),dRy(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J12 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),dRz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
J13 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
return h([J1, J2, J3, J4, J5, J6, J7, J8, J9, J10, J11, J12, J13])
def Rx(q): return [[1,0,0,0],[0,cos(q),-sin(q),0],[0,sin(q),cos(q),0],[0,0,0,1]]
def dRx(q):return [[0,0,0,0],[0,-sin(q),-cos(q),0],[0,cos(q),-sin(q),0],[0,0,0,0]]
def Ry(q): return [[cos(q),0,sin(q),0],[0,1,0,0],[-sin(q),0,cos(q),0],[0,0,0,1]]
def dRy(q):return [[-sin(q),0,cos(q),0],[0,0,0,0],[-cos(q),0,-sin(q),0],[0,0,0,0]]
def Rz(q): return [[cos(q),-sin(q),0,0],[sin(q),cos(q),0,0],[0,0,1,0],[0,0,0,1]]
def dRz(q):return [[-sin(q),-cos(q),0,0],[cos(q),-sin(q),0,0],[0,0,0,0],[0,0,0,0]]
def Tx(q): return [[1,0,0,q],[0,1,0,0],[0,0,1,0],[0,0,0,1]]
def dTx(): return [[0,0,0,1],[0,0,0,0],[0,0,0,0],[0,0,0,0]]
def Ty(q): return [[1,0,0,0],[0,1,0,q],[0,0,1,0],[0,0,0,1]]
def dTy(): return [[0,0,0,0],[0,0,0,1],[0,0,0,0],[0,0,0,0]]
def Tz(q): return [[1,0,0,0],[0,1,0,0],[0,0,1,q],[0,0,0,1]]
def dTz(): return [[0,0,0,0],[0,0,0,0],[0,0,0,1],[0,0,0,0]]
th = [zeros(13), zeros(13), zeros(13)]
Tb = [dot([Ty(1),Ry(pi/2),Rz(pi)]), dot([Tz(1),Rx(-pi/2)]),I(4)]
Tt = [transpose(dot([Ry(pi/2),Rz(pi)])), transpose(Rx(-pi/2)), I(4)]
k11 = [[E*A/L,0,0,0,0,0],[0,12*E*Iz/L**3,0,0,0,6*E*Iz/L**2],[0,0,12*E*Iy/L**3,0,-6*E*Iy/L**2,0],[0,0,0,G*Ip/L,0,0],
[0,0,-6*E*Iy/L**2,0,4*E*Iy/L,0],[0,6*E*Iz/L**2,0,0,0,4*E*Iz/L]]
k12 = [[-E*A/L,0,0,0,0,0],[0,-12*E*Iz/L**3,0,0,0,-6*E*Iz/L**2],[0,0,-12*E*Iy/L**3,0,6*E*Iy/L**2,0],[0,0,0,-G*Ip/L,0,0],
[0,0,-6*E*Iy/L**2,0,2*E*Iy/L,0],[0,6*E*Iz/L**2,0,0,0,2*E*Iz/L]]
k22 = [[E*A/L,0,0,0,0,0],[0,12*E*Iz/L**3,0,0,0,-6*E*Iz/L**2],[0,0,12*E*Iy/L**3,0,6*E*Iy/L**2,0],[0,0,0,G*Ip/L,0,0],
[0,0,6*E*Iy/L**2,0,4*E*Iy/L,0],[0,-6*E*Iz/L**2,0,0,0,4*E*Iz/L]]
k21 = transpose(k12)
#K = v([h([k11,k12]),h([transpose(k12), k22])])
Kth = v([h([Ka,zeros(12)]),h([zeros((6,1)),k22,zeros((6,6))]),h([zeros((6,1)),zeros((6,6)),k22])])
def JacobianPassiveLeg(tFk, tb, Tt, qa, qp, th):
tFk[0:3, 3] = 0
dt = dot([tb,dot([Tz(qa),Tz(th[0]),dRz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),