def test_tau_heave(self):
M = [10, 10, 10, 10, 10, 10]
D = [10, 10, 10, 10, 10, 10]
W = [10]
B = [10]
COG = [0, 0, 0]
COB = [0, 0, 0]
# input
position = [0, 0, 0]
roll, pitch, yaw = [0, 0, 0]
nu = [0, 0, 0, 0, 0, 0]
tau = [0, 0, 10, 0, 0, 0]
# expected
pos_dot_expected = [0, 0, 0]
quat_dot_expected = [0, 0, 0, 0]
linear_acc = [0, 0, 1]
angular_acc = [0, 0, 0]
eta_dot_expected = pos_dot_expected + quat_dot_expected
nu_dot_expected = linear_acc + angular_acc
x_dot_expected = np.array(eta_dot_expected + nu_dot_expected)
orientation = degrees_to_quat_rotation(roll, pitch, yaw).tolist()
eta = position + orientation
state = np.array(eta + nu, dtype=np.float64)
parameters = np.array(M + D + W + B + COG + COB, dtype=np.float64)
thrust = np.array(tau, dtype=np.float64)
x_dot = diagonal_slow_without_g(state, thrust, parameters)
eta_dot = x_dot[0:7]
nu_dot = x_dot[7:13]
for a, b in zip(x_dot, x_dot_expected):
self.assertAlmostEqual(a, b)
def test_restoring_angular_y(self):
M = [10, 10, 10, 10, 10, 10]
D = [10, 10, 10, 10, 10, 10]
W = [10]
B = [10]
COG = [0, 0, 0]
COB = [0, 1, 0]
# input
position = [0, 0, 0]
roll, pitch, yaw = [0, 0, 0]
nu = [0, 0, 0, 0, 0, 0]
tau = [0, 0, 0, 0, 0, 0]
# expected
eta_dot = [0, 0, 0, 0, 0, 0, 0]
linear_acc = [0, 0, 0]
angular_acc = [-1, 0, 0]
x_dot_expected = np.array(eta_dot + linear_acc + angular_acc)
orientation = degrees_to_quat_rotation(roll, pitch, yaw).tolist()
eta = position + orientation
state = np.array(eta + nu, dtype=np.float64)
parameters = np.array(M + D + W + B + COG + COB, dtype=np.float64)
thrust = np.array(tau, dtype=np.float64)
x_dot = diagonal_slow_without_g(state, thrust, parameters)
for a, b in zip(x_dot, x_dot_expected):
self.assertAlmostEqual(a, b)
def test_deg_to_quat_yaw(self):
roll = 0
pitch = 0
yaw = 90
q_expected = np.array([0.707, 0, 0, 0.707])
q = helper.degrees_to_quat_rotation(roll, pitch, yaw)
for a, b in zip(q, q_expected):
self.assertAlmostEqual(a, b, places=3)
def test_nu_transform_x_roll(self):
nu_body = np.array([2, 0, 0])
roll, pitch, yaw = [90, 0, 0]
nu_ned = np.array([2, 0, 0])
orientation = helper.degrees_to_quat_rotation(roll, pitch, yaw)
R = helper.R(orientation)
result = R @ nu_body
for a, b in zip(result, nu_ned):
self.assertAlmostEqual(a, b)
def test_deg_to_quat_and_back(self):
roll = 90
pitch = 20
yaw = 110
q = helper.degrees_to_quat_rotation(roll, pitch, yaw)
degrees_result = helper.quat_to_degrees(*q)
degrees_expected = np.array([roll, pitch, yaw])
for a, b in zip(degrees_result, degrees_expected):
self.assertAlmostEqual(a, b, places=3)
def test_unit_vector_rotation_combined(self):
roll = 90
pitch = 0
yaw = 0
x_body = [0, 1, 1]
x_ned_expected = [0, -1, 1]
q = helper.degrees_to_quat_rotation(roll, pitch, yaw)
R = helper.R(q)
x_ned = R @ x_body
for a, b in zip(x_ned, x_ned_expected):
self.assertAlmostEqual(a, b, places=3)
def test_unit_vector_rotation_x_yaw(self):
roll = 0
pitch = 0
yaw = 90
x_unit = [1, 0, 0]
x_expected = [0, 1, 0]
q = helper.degrees_to_quat_rotation(roll, pitch, yaw)
R = helper.R(q)
x_res = R @ x_unit
for a, b in zip(x_res, x_expected):
self.assertAlmostEqual(a, b, places=3)
def test_x_yaw(self):
omega_body = np.array([0.1, 0, 0])
roll, pitch, yaw = [0, 0, 90]
omega_ned = np.array([0, 0.1, 0])
q = helper.degrees_to_quat_rotation(roll, pitch, yaw)
T = helper.T(q)
q_dot_ned = T @ omega_body
py_quat = Quaternion(*q)
py_quat_dot = Quaternion(*q_dot_ned)
omega_ned_res = (2 * py_quat_dot * py_quat.conjugate).elements[1:]
for a, b in zip(omega_ned, omega_ned_res):
self.assertAlmostEqual(a, b)
def test_non_invertible_M(self):
M = [10, 10, 0, 10, 10, 10]
D = [10, 10, 10, 10, 10, 10]
W = [0]
B = [10]
COG = [0, 0, 0]
COB = [1, 0, 0]
# input
position = [0, 0, 0]
roll, pitch, yaw = [90, 0, 0]
nu = [0, 0, 0, 0, 0, 0]
tau = [0, 0, 0, 0, 0, 0]
orientation = degrees_to_quat_rotation(roll, pitch, yaw).tolist()
eta = position + orientation
state = np.array(eta + nu, dtype=np.float64)
parameters = np.array(M + D + W + B + COG + COB, dtype=np.float64)
thrust = np.array(tau, dtype=np.float64)
x_dot = diagonal_slow_without_g(state, thrust, parameters)
self.assertTrue(np.isnan(x_dot).all())