Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = Quaternion(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(q)
assert q_test.dcm.close(dcm)
q_test = Quaternion(q)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# # construct q from a euler angles
q_test = Quaternion(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(euler)
assert q_test.dcm.close(dcm)
q_test = Quaternion(euler)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# # construct q from dcm (Matrix3 instance)
q_test = Quaternion(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(dcm)
assert q_test.dcm.close(dcm)
q_test = Quaternion(dcm)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
def _helper_test_constructor(self, q, euler, dcm):
"""
Helper function for constructor test
Calls constructor for the quaternion from q euler and dcm and checks
if the resulting converions are equivalent to the arguments.
The test for the euler angles is weak as the solution is not unique
:param q: quaternion 4x1, [w, x, y, z]
:param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
:param q: Matrix3, needs to be equivalent to q
"""
# construct q from a Quaternion
quaternion_instance = Quaternion(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
def test_conversion(self):
"""
Tests forward and backward conversions
"""
for q in self.quaternions:
# quaternion -> euler -> quaternion
q0 = q
e = Quaternion(q.q).euler
q1 = Quaternion(e)
assert q0.close(q1)
# quaternion -> dcm (Matrix3) -> quaternion
q0 = q
dcm = Quaternion(q.q).dcm
q1 = Quaternion(dcm)
assert q0.close(q1)
Calls constructor for the quaternion from q euler and dcm and checks
if the resulting converions are equivalent to the arguments.
The test for the euler angles is weak as the solution is not unique
:param q: quaternion 4x1, [w, x, y, z]
:param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
:param q: Matrix3, needs to be equivalent to q
"""
# construct q from a Quaternion
quaternion_instance = Quaternion(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
target_roll_degrees = 0
if abs(r - target_roll_degrees) < tolerance:
state = state_done
else:
raise ValueError("Unknown state %s" % str(state))
m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT']
self.progress("%s Roll: %f desired=%f set=%f" %
(state, r, m_nav.nav_roll, target_roll_degrees))
time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = 0b10000001 ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion([math.radians(target_roll_degrees),
0,
0])
roll_rate_radians = 0.5
pitch_rate_radians = 0
yaw_rate_radians = 0
thrust = 1.0
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system,
target_component,
type_mask,
q,
roll_rate_radians,
pitch_rate_radians,
yaw_rate_radians,
thrust)
except Exception as e:
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = Quaternion(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(q)
assert q_test.dcm.close(dcm)
q_test = Quaternion(q)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# # construct q from a euler angles
q_test = Quaternion(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(euler)
assert q_test.dcm.close(dcm)
q_test = Quaternion(euler)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = Quaternion(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(q)
assert q_test.dcm.close(dcm)
q_test = Quaternion(q)
q_euler = Quaternion(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# # construct q from a euler angles
q_test = Quaternion(euler)
np.testing.assert_almost_equal(q_test.q, q)
vel = (pos_ned - last_pos) * (1.0/dt)
last_pos = pos_ned
last_frame_num = frame_num
filtered_vel = self.vel_filter.apply(vel)
if self.vicon_settings.vision_rate > 0:
dt = now - last_msg_time
if dt < 1.0 / self.vicon_settings.vision_rate:
continue
last_msg_time = now
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw += math.radians(self.vicon_settings.yaw_offset)
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
self.pos = pos_ned
self.att = [math.degrees(roll), math.degrees(pitch), math.degrees(yaw)]
self.frame_count += 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
time_us = int(now * 1.0e6)
self.sphere.material.set_color(1.2 * self.base_color)
self.sphere.material.specular_exponent = 4
self.sphere.material.alpha = 1.0
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)
self.sphere.draw(self.program)
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
self.mag.draw(self.program)
self.sphere.material.set_color(self.base_color)
self.sphere.material.alpha = .4
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
faces = [i for i in range(80) if self.visible[i]]
self.sphere.draw(self.program, faces=faces, camera=self.camera)
self.last_render_quaternion = quaternion.Quaternion(
self.common_model_transform.quaternion)
def SetAttitude(self, roll, pitch, yaw, timestamp):
if not self.renderer:
return
dt = 0xFFFFFFFF & (timestamp - self.attitude_timestamp)
dt *= 0.001
self.attitude_timestamp = timestamp
desired_quaternion = quaternion.Quaternion((roll, pitch, yaw))
desired_quaternion.normalize()
error = desired_quaternion / self.renderer.common_model_transform.quaternion
error.normalize()
self.gyro = quaternion_to_axis_angle(error) * (1.0 / dt)