def step(self, state, model=False):
r_c, z_c, dz_c, r_c_old, p, r, q, dq, u_r, vel_q, x_2, y_2 = state
#Inputs
z_r = np.array([self.function.f(*pt) for pt in r]) #Mimick measurements on robots
hessian = self.function.hessian_f(r_c[0], r_c[1]) #True value
if model:
z_c_k, dz_c_k, p_k = kalmanFilter(z_c, dz_c, r, z_r, r_c, r_c_old, p, hessian)
z_c_p, dz_c_p, p_p = model.predict(z_c, dz_c, r, z_r, r_c, r_c_old, p, hessian)
# var1.push(z_c_k, z_c_p)
# var2.push(dz_c_k[0], dz_c_p[0])
# var3.push(dz_c_k[1], dz_c_p[1])
z_c = z_c_k
dz_c = dz_c_k
p = p_k
else:
z_c, dz_c, p = kalmanFilter(z_c, dz_c, r, z_r, r_c, r_c_old, p, hessian)
# z_c = f(r_c[0], r_c[1])
# dz_c = dz_f(r_c[0], r_c[1])
r_c_old = r_c
r_c, x_2, y_2 = formationCenter(r_c, z_c, dz_c, hessian, x_2, y_2,
self.function.mu_f, self.function.z_desired)
r_c_p, x_2_p, y_2_p = formationCenter(r_c, z_c_p, dz_c_p, hessian, x_2, y_2,
self.function.mu_f, self.function.z_desired)
var1.push(r_c[0], r_c_p[0])
var2.push(r_c[1], r_c_p[1])
var3.push(x_2[0], x_2_p[0])
var4.push(x_2[1], x_2_p[1])
r, q, dq, u_r, vel_q = formationControl(r_c, r, q, dq, u_r, vel_q)
state = [r_c, z_c, dz_c, r_c_old, p, r, q, dq, u_r, vel_q, x_2, y_2]
data = [r_c, z_c, dz_c, hessian, r, z_r, p]
return state, data
def step(self, t, model=False):
r_c, z_c, dz_c, r_c_old, p, r, q, dq, u_r, vel_q, x_2, y_2, z_r = self.state
#Inputs
z_r = np.array([self.function.f(*pt)
for pt in r]) #Mimick measurements on robots
hessian = self.function.hessian_f(r_c[0], r_c[1]) #True value
z_c = self.function.f(*r_c) # Measure field value at center
# dz_c = self.function.dz_f(*r_c) # Measure gradient value at the center
if model:
# z_c_k, dz_c_k, p_k, data = kalmanFilter(z_c, dz_c, r, z_r, r_c, r_c_old, p, hessian)
z_c_p, dz_c_p, p_p, data = self.kalmanFilter(
z_c, dz_c, r, z_r, r_c, r_c_old, p, hessian, model)
z_c = z_c_p
dz_c = dz_c_p
p = p_p
else:
z_c, dz_c, p, data = self.kalmanFilter(z_c, dz_c, r, z_r, r_c,
r_c_old, p, hessian)
# z_c = f(r_c[0], r_c[1])
# dz_c = dz_f(r_c[0], r_c[1])
r_c_old = r_c
# We dont need to calculate force. Velocity is sufficient.
# If we dont do this, fc only tracks gradient, not at desired field value, but where it was left to start with.
r_c, x_2, y_2 = formationCenter(r_c, z_c, dz_c, hessian, x_2, y_2,
self.function.mu_f,
self.function.z_desired)
r, q, dq, u_r, vel_q = formationControl(r_c, r, dz_c, q, dq, u_r,
vel_q, t)
self.old_state = self.state
self.state = [
r_c, z_c, dz_c, r_c_old, p, r, q, dq, u_r, vel_q, x_2, y_2, z_r
]
# data = [r_c, z_c, dz_c, hessian, r, z_r, p]
return data
def stepFunction(self, function, plot=False, collect=False, test=False):
p = params.Params()
r_c, r_c_old, x_2, y_2, r = p.r_c, p.r_c, 0, 0, p.r
r_c_plot = [r_c]
r_plot = [r]
q, dq, u_r, vel_q = p.q, p.dq, p.u_r, p.vel_q
trainData, model = False, False
if collect:
trainData = []
if test:
model_orig = load_model("hessian.h5")
scaler = pkl.load(open("hessian_scaler.p", "rb"))
# re-define the batch size
batch_size = 1
# re-define model
model = Sequential()
model.add(LSTM(22, batch_input_shape=(batch_size, 1, 21), stateful=True))
model.add(Dense(4))
# copy weights
weights = model_orig.get_weights()
model.set_weights(weights)
# compile model
model.compile(loss='mean_squared_error', optimizer='adam')
print(model.summary())
hessian = function.hessian_f(r_c[0], r_c[1])
z_c = function.f(r_c[0], r_c[1])
dz_c = function.dz_f(r_c[0], r_c[1])
y_2 = dz_c / norm(dz_c)
x_2 = p.rotateRight @ y_2
for i in range(10000):
# Decoupled: Ideal test.
z_c = function.f(r_c[0], r_c[1])
dz_c = function.dz_f(r_c[0], r_c[1])
realHessian = []
predictedHessians = []
if collect:
hessian = function.hessian_f(r_c[0], r_c[1])
if test:
realHessian.append(np.linalg.det(function.hessian_f(r_c[0], r_c[1])))
predictedHessians.append(np.linalg.det(hessian))
z_r = np.array([function.f(*pt) for pt in r])
# hessian = [[0, 0], [0, 0]]
r_c, x_2, y_2 = formationCenter(r_c, z_c, dz_c, hessian, x_2, y_2, function.mu_f, function.z_desired)
r, q, dq, u_r, vel_q = formationControl(r_c, r, dz_c, q, dq, u_r, vel_q, i)
state = np.concatenate([r_c, [z_c], dz_c, *r, z_r, *hessian])
if collect:
trainData.append(state)
if test:
state = scaler.transform(state.reshape(1,-1))
hessian = model.predict(np.reshape(state, (state.shape[0], 1, state.shape[1])),
batch_size=1)[0].reshape(2,2)
# pltVar.push(hessian[0][0])
if plot:
r_c_plot.append(r_c)
if i % 150 == 0:
r_plot.append(r)
if plot:
x = np.linspace(-10, 10, 200)
y = np.linspace(-10, 10, 200)
xx, yy = np.meshgrid(x, y)
z = function.f(xx, yy)
plt.contour(x, y, z, [function.z_desired])
plt.plot(*zip(*r_c_plot), 'b')
for r in r_plot:
plt.plot([r[0, 0], r[1, 0]], [r[0, 1], r[1, 1]], 'yo-')
plt.plot([r[2, 0], r[3, 0]], [r[2, 1], r[3, 1]], 'go-')
plt.title(function.name)
# plt.show()
plt.savefig("{}_hessianEstimation.pdf".format(function.name), bbox_inches='tight')
plt.close()
# pltVar.plot()
if collect:
trainData = pd.DataFrame(trainData)
print("Finished stepping through function: {}\n".format(function.name))
if test:
print("Hessian error is: {}".format(math.sqrt(mean_squared_error(realHessian, predictedHessians))))
return trainData
# c.T @ .. @ c = 3x4 @ 4x4 @ 4x3 = 3x3
p = np.linalg.inv(
np.linalg.inv(p_e) +
c.T @ np.linalg.inv(d @ u @ d.T + R) @ c) # 3x3
z_c = s[0]
dz_c = s[1:]
# return z_c, dz, p
# End of Kalman filter
p_det.append(np.linalg.det(p))
# z_c = f(r_c[0], r_c[1])
# dz_c = dz_f(r_c[0], r_c[1])
# print(dz_c)
r_c_old = r_c
r_c, x_2, y_2 = formationCenter(r_c, z_c, dz_c, hessian, x_2, y_2,
function.mu_f, function.z_desired)
r, q, dq, u_r, vel_q = formationControl(r_c, r, q, dq, u_r, vel_q)
hessian_state = np.concatenate(
[r_c, [z_c], dz_c, *r, z_r, *hessian])
hessian_state = scaler_hessian.transform(
hessian_state.reshape(1, -1))
hessian = model_hessian.predict(np.reshape(
hessian_state,
(hessian_state.shape[0], 1, hessian_state.shape[1])),
batch_size=1)[0].reshape(2, 2)
r_c_plot.append(r_c)
if i % 150 == 0:
r_plot.append(r)