def test_jacobian(self):
starting_pos = [0.0, 0.0]
starting_theta = 0.0
if self.use_autoencoder:
q = self.encoder.predict(boxes.generate_samples(offsets=[starting_pos], thetas=[starting_theta]))[0]
else:
q = np.array([*starting_pos, starting_theta])
print("Actual x: ", self.decode_q_to_x(q))
cum_error = 0.0
for x_i in range(8):
for q_i in range(3):
# x_i = 0
# q_i = 0
def f_xi_qi(a):
new_q = q.copy()
new_q[q_i] = a
return self.decode_q_to_x(new_q)[x_i]
numeric_d = numeric_derivative(f_xi_qi, q[q_i], dx=1e-6)
actual_d = self.jac_x_wrt_q(q)[x_i][q_i]
error = np.abs(numeric_d - actual_d)
cum_error += error
print("partial of x", x_i, " wrt q", q_i, sep = "")
print("Numeric derivative:", numeric_d)
print("Acutal derivative:", actual_d)
print("Difference:", error)
print()
print("Cumulative error: ", cum_error)
def numeric_jacobian(self, q):
jac = np.zeros((8,3))
for x_i in range(8):
for q_i in range(3):
def f_xi_qi(a):
new_q = q.copy()
new_q[q_i] = a
return self.decode_q_to_x(new_q)[x_i]
jac[x_i][q_i] = numeric_derivative(f_xi_qi, q[q_i], dx=1e-6)
return jac