def test_sub(self):
"""substract 2 quaternions"""
self.assertEqual(self._q1 - self._q2,
quaternion.Quaternion(6, -8, -4, -4))
self.assertEqual(self._q2 - self._q1,
quaternion.Quaternion(-6, 8, 4, 4))
self.assertEqual(self._q1 - self._q1, self._null)
self.assertEqual(self._q2 - self._q2, self._null)
def __init__(self, name):
super(Quadrotor, self).__init__()
self._name = name
self._global_position = numpy.zeros((1, 3))[0]
self._local_velocity = numpy.zeros((1, 6))[0] #vx, vy, vz, wx, wy, wz
self._rotation = numpy.zeros((1, 3))[0] #roll, pitch, yaw
self._quaternion = quaternion.Quaternion(0., 0., 0.,
1.) #qx, qy, qz, qw
#self._local_to_global = array([[1., 0.], [0., 1.]])
self._status = 0
self._battery = 0.0
def set_orientation(self, true_quat):
"""Project earth magnetic field into Body Axes"""
magnetometer = (true_quat.conjugated() * quaternion.Quaternion(
self._mag.bx / 1000, -self._mag.by / 1000, -self._mag.bz / 1000,
0.) * true_quat).get_quaternion()[0:3]
self.set_true_value(magnetometer)
def set_orientation(self, true_quat, body_acc=numpy.zeros((1, 3))[0]):
"""Project gravity into Body Axes"""
acceleration = (
true_quat * quaternion.Quaternion(0., 0., self.GRAVITY, 0.) *
true_quat.conjugated()).get_quaternion()[0:3] + body_acc
self.set_true_value(acceleration)
def test_exponentiate(self):
"""exponentiate the quaternion"""
self._unit.exponentiate()
self.assertTrue(self._unit == quaternion.Quaternion(0., 0., 0., numpy.exp(1.)))
def test_exponential(self):
"""test exponential of quaternion"""
self.assertTrue(self._unit.exponential() == quaternion.Quaternion(0., 0., 0., numpy.exp(1.)))
def test_mul(self):
"""multiply 2 quaternions and a float by a quaternions"""
self.assertEqual(self._q1 * self._q2, quaternion.Quaternion(-44., -14., 48., 28.))
self.assertEqual(self._q1 * 2, quaternion.Quaternion(2., -4., 6., 8.))
self.assertEqual(self._q1 * (-0.5), quaternion.Quaternion(-0.5, 1., -1.5, -2.))
def test_add(self):
"""add 2 quaternions"""
self.assertEqual(self._q1 + self._q2,
quaternion.Quaternion(-4., 4., 10., 12.))
def setUp(self):
self._random = quaternion.random_quaternion()
self._q1 = quaternion.Quaternion(1., -2., 3., 4.)
self._q2 = quaternion.Quaternion(-5., 6., 7., 8.)
self._null = quaternion.Quaternion(0., 0., 0., 0.)
self._unit = quaternion.Quaternion(0., 0., 0., 1.)