def test_rotation(self):
"""Test that a quaternion and its converted matrix perform the same rotation."""
quat_rotated = rowan.rotate(input1, vector_inputs)
matrices = rowan.to_matrix(input1)
matrix_rotated = np.einsum("ijk,ki->ij", matrices, vector_inputs.T)
self.assertTrue(np.allclose(matrix_rotated, quat_rotated))
def __init__(self, J, mass, a_min, a_max): self.mass = mass self.J = J self.a_min = a_min self.a_max = a_max # desired state self.p_d = np.array([0,0,0]) self.v_d = np.array([0,0,0]) self.a_d = np.array([0,0,0 + 9.81]) self.R_d = rowan.to_matrix([1, 0, 0, 0]) self.omega_d = np.array([0,0,0]) # Gains self.Kpos_P = np.array([10,10,5]) self.Kpos_D = np.array([5,5,2.5]) self.lambda_a = np.array([20,20,8]) self.K_att = np.array([0.003, 0.003, 0.003]) self.kq = 0 # state self.omega_r_last = None self.q = [] self.qr = [] self.omega = [] self.omegar = []
def testfun(points, rotation):
vertices = tet.vertices + points
shape = ConvexPolyhedron(vertices)
rotated_shape = ConvexPolyhedron(rowan.rotate(rotation, vertices))
mat = rowan.to_matrix(rotation)
rotated_inertia = rotate_order2_tensor(mat, shape.inertia_tensor)
assert np.allclose(rotated_inertia, rotated_shape.inertia_tensor)
def test_to_from_matrix(self):
# The equality is only guaranteed up to a sign
converted = rowan.from_matrix(rowan.to_matrix(input1))
self.assertTrue(
np.all(
np.logical_or(
np.isclose(input1 - converted, 0),
np.isclose(input1 + converted, 0),
)))
def test_to_from_matrix(self):
"""Test conversion from a quaternion to a matrix and back."""
# The equality is only guaranteed up to a sign
converted = rowan.from_matrix(rowan.to_matrix(input1))
self.assertTrue(
np.all(
np.logical_or(
np.isclose(input1 - converted, 0),
np.isclose(input1 + converted, 0),
)))
def test_rotate_inertia(points):
# Use the input as noise rather than the base points to avoid precision and
# degenerate cases provided by hypothesis.
tet = PlatonicFamily()("Tetrahedron")
vertices = tet.vertices + points
rotation = rowan.random.rand()
shape = ConvexPolyhedron(vertices)
rotated_shape = ConvexPolyhedron(rowan.rotate(rotation, vertices))
mat = rowan.to_matrix(rotation)
rotated_inertia = rotate_order2_tensor(mat, shape.inertia_tensor)
assert np.allclose(rotated_inertia, rotated_shape.inertia_tensor)
def test_to_matrix(self):
v = np.copy(zero)
with self.assertRaises(ZeroDivisionError):
rowan.to_matrix(v)
v = 2 * np.ones(4)
with self.assertRaises(ValueError):
rowan.to_matrix(v)
v = np.copy(one)
self.assertTrue(np.all(rowan.to_matrix(v) == np.eye(3)))
v = np.copy(half)
self.assertTrue(
np.allclose(rowan.to_matrix(v),
np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])))
v[3] *= -1
self.assertTrue(
np.allclose(rowan.to_matrix(v),
np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])))
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller # p_tilde = p - self.p_d # v_tilde = v - self.v_d # v_r = self.v_d - self.lambda_p * p_tilde # v_r_dot = self.a_d - self.lambda_p * v_tilde # s_v = v - v_r # f_r = self.mass * v_r_dot \ # - self.Kv * s_v \ # - self.Kp * p_tilde # thrust = np.linalg.norm(f_r) # qr = mkvec( # asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), # atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), # desiredYaw); # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 rpy_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) R_d = rowan.to_matrix(rowan.from_euler(*rpy_d)) print(rpy_d) print(R_d) # attitude controller # rotation error Rtilde = self.R_d.T @ R qtilde_0 = 1/2 * np.sqrt(1 + np.trace(Rtilde)) qtilde_v = 1 / (4 * qtilde_0) * veemap(Rtilde - Rtilde.T) omega_r = Rtilde.T @ self.omega_d - 2 * self.lambda_a * qtilde_v print(omega_r) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r s_omega = omega - omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * s_omega \ - self.kq * qtilde_v # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(rpy_d)) self.q.append(np.degrees(rowan.to_euler(q, 'xyz'))) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 q_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) if self.q_d_last is not None: q_d_dot = (q_d - self.q_d_last) / 0.01 else: q_d_dot = np.zeros(3) self.q_d_last = q_d # attitude controller q = rowan.to_euler(q, 'xyz') omega_r = Zinv(q) @ (q_d_dot +self.lambda_att * (q_d -q)) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * (omega - omega_r) # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(q_d)) self.q.append(np.degrees(q)) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def test_rotation(self):
quat_rotated = rowan.rotate(input1, vector_inputs)
matrices = rowan.to_matrix(input1)
matrix_rotated = np.einsum('ijk,ki->ij', matrices, vector_inputs.T)
self.assertTrue(np.allclose(matrix_rotated, quat_rotated))