def test_explog_quaternion(self):
""" test if q = exp(log(quat)) """
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
quat_trajectory = lqr_solver.com_ori.copy()
for i in range(quat_trajectory.shape[0]):
qnew = lqr_solver.exp_quaternion(
.5 * lqr_solver.log_quaternion(quat_trajectory[i]))
np.testing.assert_array_almost_equal(quat_trajectory[i], qnew)
def test_quaternion_to_rotation(self):
"""check if rotation matrix from quaternion satisfies all group
conditions"""
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
for t in range(lqr_solver.N + 1):
rotation = lqr_solver.quaternion_to_rotation(lqr_solver.com_ori[t])
np.testing.assert_array_almost_equal(rotation.dot(rotation.T),
np.eye(3))
np.testing.assert_almost_equal(np.linalg.det(rotation), 1.)
def test_compute_gains(self):
""" This test is disabled as it is too slow """
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
lqr_solver.compute_gains()
feedback = lqr_solver.kfb.copy()
time = lqr_solver.dt * np.arange(feedback.shape[0])
norms = []
for t in range(feedback.shape[0]):
norms += [np.linalg.norm(feedback[t])]
def test_quaternion_difference(self):
''' check if computed quaternion differences equal scaled angular
velocities '''
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
quat_trajectory = lqr_solver.com_ori.copy()
for i in range(10):
w = np.random.rand(3)
q2 = lqr_solver.integrate_quaternion(quat_trajectory[i], w)
diff = lqr_solver.quaternion_difference(quat_trajectory[i], q2)
# integrate quaternion accounts for dt, hence need to divide by it
np.testing.assert_almost_equal(w, diff)
def test_initialization(self):
""" check dimensions upon initialization """
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
assert (lqr_solver.com_pos.shape[1] + lqr_solver.com_vel.shape[1] +
lqr_solver.com_ori.shape[1] +
lqr_solver.com_ang_vel.shape[1] == lqr_solver.n)
assert lqr_solver.com_pos.shape[0] == lqr_solver.N + 1
assert lqr_solver.com_vel.shape[0] == lqr_solver.N + 1
assert lqr_solver.com_ori.shape[0] == lqr_solver.N + 1
assert lqr_solver.com_ang_vel.shape[0] == lqr_solver.N + 1
assert lqr_solver.cent_force.shape[0] == lqr_solver.N + 1
assert lqr_solver.cent_moments.shape[0] == lqr_solver.N + 1
assert lqr_solver.cent_force.shape[1] + \
lqr_solver.cent_moments.shape[1] == lqr_solver.m
def test_cost_along_trajectory(self):
'''tests computation of cost residuals along a reference trajectory '''
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
c = 0.0
# here we compute cost of planner trajectory compared to integrated
# trajectory
for t in range(lqr_solver.N):
if t == lqr_solver.N - 1:
c += lqr_solver.cost(t, lqr_solver.xp[t],
np.zeros(lqr_solver.m))
else:
c += lqr_solver.cost(t, lqr_solver.xp[t], lqr_solver.u0[t])
print 'cost along trajectory = ', c
def test_quaternion_integration(self):
""" test if integrating quaternion with angular
velocity matches obtained quaternion trajectory """
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
quat_trajectory = np.zeros((lqr_solver.N + 1, 4))
quat_trajectory[0] = lqr_solver.com_ori[0].copy()
for t in range(lqr_solver.N):
quat_trajectory[t + 1] = lqr_solver.integrate_quaternion(
quat_trajectory[t], lqr_solver.dt * lqr_solver.com_ang_vel[t])
# time_array = lqr_solver.dt * np.arange(lqr_solver.N)
# names = ['x', 'y', 'z', 'w']
# for i in range(4):
# plt.figure('quat_'+names[i])
# plt.plot(time_array, lqr_solver.com_ori[:, i], label='solver')
# plt.plot(time_array, quat_trajectory[:, i], label='integrated')
# plt.grid()
# plt.legend()
# plt.title('quat_'+names[i])
# plt.show()
np.testing.assert_array_almost_equal(quat_trajectory,
lqr_solver.com_ori,
decimal=4)
def test_cost_derivatives(self):
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
cx, cu, cxx, cuu, cxu = lqr_solver.cost_derivatives(
0, lqr_solver.x0[0], lqr_solver.u0[0])
def test_dynamics_derivatives(self):
lqr_solver = lqr_gain_manifold.CentroidalLqr(self.data_dir)
fx, fu = lqr_solver.dynamics_derivatives(0, lqr_solver.x0[0],
lqr_solver.u0[0])