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])