def body2local(q, x): """Convert a vector in the body space to the local inertial reference frame.""" q1 = Quaternion(q) x = Quaternion([0, x[0], x[1], x[2]]) return (q1.inverse() * x * q1).q[1:4]
def openPastCameraData():
global positions, orientations, recordingTimes, originalQuart
with open('/home/pi/RealSenseProjects/coordinateData.csv') as csv_file:
csv_reader = csv.reader(csv_file, delimiter=',')
line_count=0
i = 0
for row in csv_reader:
#Mapping of coordinates is done here
positions[0][0] = float(row[8]) #x
positions[0][1] = float(row[6]) #y
positions[0][2] = float(row[7]) #z
if(originalQuart.x == 0 and originalQuart.y == 0 and originalQuart.z ==0 and originalQuart.w == 0):
originalQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
currQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
#Following operation makes the current quaternion the delta between the current orintation vector and the original
transform = originalQuart.inverse().multiplication(currQuart)
orientations[0] = [transform.x, transform.y, transform.z, transform.w]
trackingFlags[0] = True
recordingTimes[0] = recordingTimes[1]
recordingTimes[1] = int(row[1]) #This gives time in microseconds
#Used to sync the threads
i=i+1
time.sleep(0.005)
event.set()
def test_getEulerAngles(self):
q = Quaternion(toVector(0.75, -0.51, pi))
vec = q.getEulerAngles()
phi, theta, psi = toValue(vec)
self.assertAlmostEqual(0.75, phi, delta=0.001)
self.assertAlmostEqual(-0.51, theta, delta=0.001)
self.assertAlmostEqual(pi, psi, delta=0.001)
def _mouse_motion(self, event):
"""Handler for mouse motion"""
if self._button1 or self._button2:
dx = event.x - self._event_xy[0]
dy = event.y - self._event_xy[1]
self._event_xy = (event.x, event.y)
if self._button1: #rotate or move vertex
if self._ind is not None:
poly = self.active_poly
self.update_curve(dx, dy)
self.draw_curves()
return
if self._shift:
ax_LR = self._ax_LR_alt
else:
ax_LR = self._ax_LR
rot1 = Quaternion.from_v_theta(self._ax_UD, self._step_UD * dy)
rot2 = Quaternion.from_v_theta(ax_LR, self._step_LR * dx)
self.rotate(rot1 * rot2)
self.draw_curves()
if self._button2:
if self._shift: #translate
self.translate(dx / self.slower, dy / self.slower)
self.draw_curves()
else: #zoom
factor = 1 - 0.003 * (dx + dy)
xlim = self.get_xlim()
ylim = self.get_ylim()
self.set_xlim(factor * xlim[0], factor * xlim[1])
self.set_ylim(factor * ylim[0], factor * ylim[1])
self.figure.canvas.draw()
def body2local(q, x):
"""Convert a vector in the body space to the local inertial reference
frame."""
q1 = Quaternion(q)
x = Quaternion([0, x[0], x[1], x[2]])
return (q1.inverse() * x * q1).q[1:4]
def Initialze(self, acceleration, magneticField, position):
""" calculates attitutde and heading
sets position to given position vector
"""
self.quaternion = Quaternion(Euler(acceleration, magneticField).values)
self.position.values = position
self.isInitialized = True
def update(self, odomMsg):
newTimeStamp = odomMsg.header.stamp.to_sec()
newPos = np.array([
odomMsg.pose.pose.position.x, odomMsg.pose.pose.position.y,
odomMsg.pose.pose.position.z
])
newOrientation = odomMsg.pose.pose.orientation
if self.lastTimeStamp is not None:
dt = newTimeStamp - self.lastTimeStamp
if dt > 0.0:
if self.pos is not None:
newVel = (newPos - self.pos) / dt
if self.vel is not None:
self.accel = (newVel - self.vel) / dt
self.vel = newVel
if self.orientation is not None:
q0 = Quaternion([
self.orientation.w, self.orientation.x,
self.orientation.y, self.orientation.z
])
q1 = Quaternion([
newOrientation.w, newOrientation.x, newOrientation.y,
newOrientation.z
])
self.angularVel = getAngularVelocity(q1, q0, dt)
self.lastTimeStamp = newTimeStamp
self.pos = newPos
self.orientation = newOrientation
def testAngleAxisToQuaternion(self):
q = Quaternion()
q.identity()
angle = 1.57
axis = Axis3(x = -0.340, y = -0.940, z = 0.000)
q.set_angle_axis(angle, axis)
self.assertEquals(str(q), "Quaternion: 0.7074 + -0.2404i + -0.6647j + 0.0000k")
class Individual:
def __init__(self, ttl_torsions = 0, \
lo_grid = Axis3(), hi_grid = Axis3(), \
rng = None):
self.translation_gene = Axis3()
self.rotation_gene = Quaternion()
self.torsions_gene = []
self.random_translation(lo_grid, hi_grid, rng)
self.random_rotation(rng)
self.random_torsions(ttl_torsions, rng)
def random_translation(self, lo_grid, hi_grid, rng):
lo_x, lo_y, lo_z = lo_grid.xyz
hi_x, hi_y, hi_z = hi_grid.xyz
self.translation_gene.x = lo_x + (rng.zero_to_one() * (hi_x - lo_x))
self.translation_gene.y = lo_y + (rng.zero_to_one() * (hi_y - lo_y))
self.translation_gene.z = lo_z + (rng.zero_to_one() * (hi_z - lo_z))
def random_rotation(self, rng):
self.rotation_gene.uniform(rng)
def random_torsions(self, ttl_torsions, rng):
self.torsions_gene = []
for i in xrange(ttl_torsions):
self.torsions_gene.append(rng.neg_pi_to_pi())
def test_update_without_rotation(self):
q = Quaternion()
q.update(toVector(0.0, 0.0, 0.0), 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertEqual(q0, 1.)
self.assertEqual(q1, 0.)
self.assertEqual(q2, 0.)
self.assertEqual(q3, 0.)
def T2DualQuaternion(cls, T):
# RealPart = Quaternion.T2Quaternion(T)
RealPart = Quaternion.T2Quaternion2(T)
t = T[0:3, 3]
tQ = Quaternion.Pose2Quaternion(t)
ImagePart = 0.5 * Quaternion.QuaternionProduct(tQ, RealPart)
return (RealPart, ImagePart)
def test_update(self):
q = Quaternion()
rotationRate = toVector(-100., 50., 120.)
q.update(rotationRate, 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.68217, delta=0.0001)
self.assertAlmostEqual(q1, -0.44581, delta=0.0001)
self.assertAlmostEqual(q2, 0.22291, delta=0.0001)
self.assertAlmostEqual(q3, 0.53498, delta=0.0001)
def get_Lxyz_global(self): """Return angular momentum in global (nonrotating) reference frame.""" q = Quaternion(self.get_Q()) # Note that local angular momentum doesn't really make sense, but it's part # of our conversion process I_cm = self.f_Icm(self.state_vector, self.t) L_local = I_cm * np.asmatrix(self.get_wxyz()).T L_global = q * Quaternion([0] + list(L_local)) * q.inverse() return L_global[1:4]
def __init__(self):
""" creates Strapdown object which includes the current bearing, velocity and position
bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m
"""
self.quaternion = Quaternion()
self.velocity = Velocity()
self.position = EllipsoidPosition(
) # navigation system position with Position()
self.isInitialized = False
def __init__(self):
""" creates Strapdown object which includes the current bearing, velocity and position
bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m
"""
self.quaternion = Quaternion()
self.velocity = Velocity() # vector (if known)
self.position = Position() # or GNSS.getPos()
# toVector(52.521918/180*pi, 13.413215\180*pi, 100.)
self.isInitialized = False
def testCopy(self):
q = Quaternion(1.2, 2.3, 3.4, 4.5)
q1 = q.copy()
q1.a = 100.002
q1.b = 200.003
q1.c = 300.004
q1.d = 400.005
self.assertEquals(str(q), "Quaternion: 1.2000 + 2.3000i + 3.4000j + 4.5000k")
self.assertEquals(str(q1), "Quaternion: 100.0020 + 200.0030i + 300.0040j + 400.0050k")
def get_Lxyz_global(self):
"""Return angular momentum in global (nonrotating) reference frame."""
q = Quaternion(self.get_Q())
# Note that local angular momentum doesn't really make sense, but it's part
# of our conversion process
I_cm = self.f_Icm(self.state_vector, self.t)
L_local = I_cm * np.asmatrix(self.get_wxyz()).T
L_global = q * Quaternion([0] + list(L_local)) * q.inverse()
return L_global[1:4]
def _updateLocalTransformation(self):
translation, euler_angle_matrix, scale, shear = self._transformation.decompose(
)
self._position = translation
self._scale = scale
self._shear = shear
orientation = Quaternion()
orientation.setByMatrix(euler_angle_matrix)
self._orientation = orientation
def test_update_180(self):
q = Quaternion()
rotationRate = toVector(0., deg2rad(5.), 0.) #rad/s
for _ in range(3600):
q.update(rotationRate, 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.0, delta=0.0001)
self.assertAlmostEqual(q1, 0.0, delta=0.0001)
self.assertAlmostEqual(q2, 1., delta=0.0001)
self.assertAlmostEqual(q3, 0.0, delta=0.0001)
def __init__(self, ttl_torsions = 0, \
lo_grid = Axis3(), hi_grid = Axis3(), \
rng = None):
self.translation_gene = Axis3()
self.rotation_gene = Quaternion()
self.torsions_gene = []
self.random_translation(lo_grid, hi_grid, rng)
self.random_rotation(rng)
self.random_torsions(ttl_torsions, rng)
def test_concatenation(self):
q = Quaternion()
q_increment = Quaternion(toVector(deg2rad(10.), 0., 0.))
#q.values = mvMultiplication(q.values,q_increment.values)
q *= q_increment
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.996, delta=0.001)
self.assertAlmostEqual(q1, 0.087, delta=0.001)
self.assertAlmostEqual(q2, 0., delta=0.001)
self.assertAlmostEqual(q3, 0., delta=0.001)
def test_vecTransformation(self):
q = Quaternion(toVector(0.1, 0.5, 0.2))
vec1 = toVector(0.1, -2., 9.81)
vec2 = q.vecTransformation(vec1)
self.assertAlmostEqual(vec2[0].item(), 5.168, delta=0.001)
self.assertAlmostEqual(vec2[1].item(), -1.982, delta=0.001)
self.assertAlmostEqual(vec2[2].item(), 8.343, delta=0.001)
a, b, c = toValue(vec2)
self.assertAlmostEqual(pythagoras(0.1, -2., 9.81),
pythagoras(a, b, c),
delta=0.001)
def __init__(self,
parent: Optional["SceneNode"] = None,
visible: bool = True,
name: str = "") -> None:
super().__init__() # Call super to make multiple inheritance work.
self._children = [] # type: List[SceneNode]
self._mesh_data = None # type: Optional[MeshData]
# Local transformation (from parent to local)
self._transformation = Matrix() # type: Matrix
# Convenience "components" of the transformation
self._position = Vector() # type: Vector
self._scale = Vector(1.0, 1.0, 1.0) # type: Vector
self._shear = Vector(0.0, 0.0, 0.0) # type: Vector
self._mirror = Vector(1.0, 1.0, 1.0) # type: Vector
self._orientation = Quaternion() # type: Quaternion
# World transformation (from root to local)
self._world_transformation = Matrix() # type: Matrix
# Convenience "components" of the world_transformation
self._derived_position = Vector() # type: Vector
self._derived_orientation = Quaternion() # type: Quaternion
self._derived_scale = Vector() # type: Vector
self._parent = parent # type: Optional[SceneNode]
# Can this SceneNode be modified in any way?
self._enabled = True # type: bool
# Can this SceneNode be selected in any way?
self._selectable = False # type: bool
# Should the AxisAlignedBoundingBox be re-calculated?
self._calculate_aabb = True # type: bool
# The AxisAligned bounding box.
self._aabb = None # type: Optional[AxisAlignedBox]
self._bounding_box_mesh = None # type: Optional[MeshData]
self._visible = visible # type: bool
self._name = name # type: str
self._decorators = [] # type: List[SceneNodeDecorator]
# Store custom settings to be compatible with Savitar SceneNode
self._settings = {} # type: Dict[str, Any]
## Signals
# self.parentChanged.connect(self._onParentChanged)
if parent:
parent.addChild(self)
def force_torque(y, t): # Force is actually zero (at least for this case, where the center of # mass isn't moving). force = [0,0,0] # Torque is constantly applied from q = Quaternion(y[6:10]) q.normalize() F = Quaternion([0,0,0,-1]) F_lcl = (q.inverse() * F * q)[1:4] torque = np.cross([1,0,0], F_lcl) return (force, torque)
def twist(self, angles):
''' apply the torsion angles.
alway do twist before transform anything
'''
if len(angles)!=len(self.torsions):
raise Exception('bad torsion parameter size')
for angle,torsion in zip(angles,self.torsions):
quat=Quaternion()
quat.set_angle_axis(angle,torsion.axis)
torParam=quat.getRot()
for atom in [x for x in self.atoms if x.no in torsion.torList]:
#print torsion.torList
atom.coords = atom.coords - torsion.base
atom.coords.transform(torParam,torsion.base)
def test_getRotationMatrix(self):
phi = 1.
theta = 0.5
psi = -0.5
q = Quaternion(toVector(phi, theta, psi))
mat = q.getRotationMatrix()
mat2 = matrix(
[[
cos(theta) * cos(psi),
-cos(phi) * sin(psi) + sin(phi) * sin(theta) * cos(psi),
sin(phi) * sin(psi) + cos(phi) * sin(theta) * cos(psi)
],
[
cos(theta) * sin(psi),
cos(phi) * cos(psi) + sin(phi) * sin(theta) * sin(psi),
-sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi)
], [-sin(theta),
sin(phi) * cos(theta),
cos(phi) * cos(theta)]])
self.assertAlmostEqual(mat[0, 0].item(),
mat2[0, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[0, 1].item(),
mat2[0, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[0, 2].item(),
mat2[0, 2].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 0].item(),
mat2[1, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 1].item(),
mat2[1, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 2].item(),
mat2[1, 2].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 0].item(),
mat2[2, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 1].item(),
mat2[2, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 2].item(),
mat2[2, 2].item(),
delta=0.001)
class Strapdown(object):
""" class Strapdown generates bearing, velocity and position from 9 degree IMU
"""
def __init__(self):
""" creates Strapdown object which includes the current bearing, velocity and position
bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m
"""
self.quaternion = Quaternion()
self.velocity = Velocity()
self.position = EllipsoidPosition(
) # navigation system position with Position()
self.isInitialized = False
def Initialze(self, acceleration, magneticField, position):
""" calculates attitutde and heading
sets position to given position vector
"""
self.quaternion = Quaternion(Euler(acceleration, magneticField).values)
self.position.values = position
self.isInitialized = True
def getOrientation(self):
return self.quaternion.getEulerAngles(
) # vector of euler angles in rad
def getVelocity(self):
return self.velocity.values
def getPosition(self):
return self.position.values
def test_update(self):
vel = Velocity(toVector(3., 1., 2.))
acc = toVector(0.1, 2., -10.)
vel.update(acc, Quaternion(toVector(0., 0., 0.)), 1.)
self.assertEqual(3.1, vel.values[0])
self.assertEqual(3., vel.values[1])
self.assertEqual(2., vel.values[2])
def CheckNormalized(cls, dualQuaternion):
Q = dualQuaternion[0]
QConj = Quaternion.QuaternionConj(Q)
QImage = dualQuaternion[1]
QImageConj = Quaternion.QuaternionConj(QImage)
RealPart = Quaternion.QuaternionProduct(Q, QConj)
ImagePart = Quaternion.QuaternionProduct(
Q, QImageConj) + Quaternion.QuaternionProduct(QImage, QConj)
if np.linalg.norm(RealPart) == 1 and -1.0e-10 < np.linalg.norm(
ImagePart) < 1.0e-10:
return True
else:
print 'RealPart:\n', RealPart
print 'ImagePart:\n', ImagePart
return False
def test_init_values(self):
q = Quaternion(toVector(1., 0.5, -0.5))
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.795, delta=0.001)
self.assertAlmostEqual(q1, 0.504, delta=0.001)
self.assertAlmostEqual(q2, 0.095, delta=0.001)
self.assertAlmostEqual(q3, -0.325, delta=0.001)
def scoring(self, dock = None):
self.scores = GeneticAlgorithm.Scores()
for individual in self.individuals:
if DEBUG:
individual.translation_gene = Axis3(2.056477, 5.846611, -7.245407)
individual.rotation_gene = Quaternion(0.532211, 0.379383, 0.612442, 0.444674)
torsions_gene_degrees = [-122.13, -179.41, \
-141.59, 177.29, \
-179.46, -9.31, \
132.37, -89.19, \
78.43, 22.22, \
71.37, 59.52]
individual.torsions_gene = []
for torsions_gene_degree in torsions_gene_degrees:
individual.torsions_gene.append((3.1415926535897931 / 180.0) * torsions_gene_degree)
print individual.translation_gene
print individual.rotation_gene
print individual.torsions_gene
if dock.reset_pose(individual.translation_gene, \
individual.rotation_gene, \
individual.torsions_gene):
self.scores.append(dock.calc_energy())
else:
self.scores.append(float("inf"))
return self.scores
def test_init_empty(self):
q = Quaternion()
q0, q1, q2, q3 = toValue(q.values)
self.assertEqual(q0, 1.)
self.assertEqual(q1, 0.)
self.assertEqual(q2, 0.)
self.assertEqual(q3, 0.)
def __init__(self, fig, sections, xkcd, is_3d=False, *args, **kwargs):
super(DrawCurveInteractive, self).__init__(fig,
sections_=sections,
xkcd=False,
is_3d=False,
*args,
**kwargs)
self.current_curve = 0
self.optimization_curve = self.current_curve
self._do_optimization = False
self.pick_tol = 5
self.slow = 15.
self.slower = 20.
self._ind = None
self.active_poly = None
self._start_trans_x = 0.
self._start_trans_y = 0.
self._start_rot = Quaternion.from_v_theta((1, -1, 0), -np.pi / 6)
# define axes for Up/Down motion and Left/Right motion
self._ax_LR = (0, -1, 0) #Left Right
self._ax_LR_alt = (0, 0, 1)
self._step_LR = 0.01
self._ax_UD = (1, 0, 0) #Up Down
self._step_UD = 0.01
self._active = False # true when mouse is over axes
self._button1 = False # true when button 1 is pressed
self._button2 = False # true when button 2 is pressed
self._button3 = False # true when button 2 is pressed
self._event_xy = None # store xy position of mouse event
self._shift = False # shift key pressed
self._current_trans_x = self._start_trans_x
self._current_trans_y = self._start_trans_y
self._current_rot = self._start_rot
# connect some GUI events
self.figure.canvas.mpl_connect('key_press_event', self._key_press)
#self.figure.canvas.mpl_connect('pick_event',
# self.on_pick_which)
self.figure.canvas.mpl_connect('button_press_event', self._mouse_press)
self.figure.canvas.mpl_connect('button_release_event',
self._mouse_release)
self.figure.canvas.mpl_connect('motion_notify_event',
self._mouse_motion)
self.figure.canvas.mpl_connect('key_release_event', self._key_release)
self.zoom = self.zoom_factory(base_scale=3.0)
#self.figure.canvas.mpl_connect('scroll_event',self.zoom_factory())
#self.figure.text(0.05, 0.15, ("Click and Drag to Move\n"
# "Hold shift key to adjust rotation, etc."))
#self.figure.text(0.05, 0.05, ("Right click to zoom\n"
# "shift right click to pan"))
self.figure.text(0.05, 0.05, ("UNO Naval Architecture"))
def asQuaternion(self):
"Returns a quaternion representing the same rotation."
from Quaternion import Quaternion
axis, angle = self.axisAndAngle()
sin_angle_2 = Numeric.sin(0.5*angle)
cos_angle_2 = Numeric.cos(0.5*angle)
return Quaternion(cos_angle_2, sin_angle_2*axis[0],
sin_angle_2*axis[1], sin_angle_2*axis[2])
def testMultiplication(self):
q = Quaternion(0.3627, 0.3898, 0.8427, 0.0798)
q1 = Quaternion(0.5221, -0.6938, 0.0376, -0.4946)
q *= q1
self.assertEquals(str(q), "Quaternion: 0.4676 + -0.4679i + 0.5910j + 0.4616k")
# Magnitude checking after multiplication
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
self.assertLessEqual(1 - q1.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q1.magnitude(), -0.00001)
def testUniform(self):
exp_q = ["Quaternion: -0.5105 + -0.7011i + 0.0180j + 0.4976k",
"Quaternion: 0.1267 + -0.5028i + -0.7474j + -0.4153k",
"Quaternion: 0.1504 + 0.0998i + -0.3025j + 0.9359k",
"Quaternion: 0.6418 + -0.5843i + 0.2151j + -0.4476k",
"Quaternion: 0.9726 + -0.0334i + 0.2176j + -0.0742k",
"Quaternion: 0.0096 + 0.0015i + -0.0778j + 0.9969k",
"Quaternion: -0.4398 + 0.5669i + -0.2332j + -0.6564k",
"Quaternion: 0.2429 + 0.2133i + -0.3556j + 0.8769k",
"Quaternion: -0.0521 + -0.3024i + -0.8972j + 0.3175k"]
exp_angle = [-2.0702,
2.8876,
2.8396,
1.7478,
0.4689,
3.1225,
-2.2309,
2.6510,
-3.0374]
exp_axis = ["Axis3: -0.8153, 0.0209, 0.5787",
"Axis3: -0.5069, -0.7535, -0.4187",
"Axis3: 0.1009, -0.3059, 0.9467",
"Axis3: -0.7619, 0.2806, -0.5837",
"Axis3: -0.1437, 0.9367, -0.3193",
"Axis3: 0.0015, -0.0778, 0.9970",
"Axis3: 0.6312, -0.2597, -0.7309",
"Axis3: 0.2199, -0.3666, 0.9040",
"Axis3: -0.3028, -0.8985, 0.3179"]
rng = LFSR(lfsr = 1070, bit_len = 16)
q = Quaternion()
for i in xrange(9):
q.uniform(rng)
angle, axis = q.get_angle_axis()
self.assertEquals(str(q), exp_q[i])
self.assertLessEqual(angle - exp_angle[i], 0.0001)
self.assertGreaterEqual(angle - exp_angle[i], -0.0001)
self.assertEquals(str(axis), exp_axis[i])
def testNormalize(self):
q = Quaternion(0.4676, -0.4679, 0.5910, 0.4616)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
q = Quaternion(-0.0918, 0.0463, -0.2413, -0.9650)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
q = Quaternion(a = 1000.0002, b = 2.03, c = 0.04, d = 40004.5)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
def toQuaternion(self):
q = Quaternion()
t = self.m[0][0] + self.m[1][1] + self.m[2][2] + 1
if t > 0:
s = 0.5 / math.sqrt(t)
q.w = 0.25 / s
q.x = (self.m[2][1] - self.m[1][2]) * s
q.y = (self.m[0][2] - self.m[2][0]) * s
q.z = (self.m[1][0] - self.m[0][1]) * s
return q
max = self.m[0][0]
iColMax = 0
for iRow in range(0, 3):
for iCol in range(0, 3):
if self.m[iRow][iCol] > max:
max = self.m[iRow][iCol]
iColMax = iCol
if iColMax == 0:
s = 1.0 / (math.sqrt(1.0 + self.m[0][0] - self.m[1][1] - self.m[2][2]) * 2.0)
q.w = ( self.m[1][2] + self.m[2][1] ) * s
q.x = 0.5 * s
q.y = ( self.m[0][1] + self.m[1][0] ) * s
q.z = ( self.m[0][2] + self.m[2][0] ) * s
elif iColMax == 1:
s = 1.0 / ( math.sqrt( 1.0 + self.m[1][1] - self.m[0][0] - self.m[2][2] ) * 2.0 )
q.w = ( self.m[0][2] + self.m[2][0] ) * s
q.x = ( self.m[0][1] + self.m[1][0] ) * s
q.y = 0.5 * s
q.z = ( self.m[1][2] + self.m[2][1] ) * s
else:
s = 1.0 / ( math.sqrt( 1.0 + self.m[2][2] - self.m[0][0] - self.m[1][1] ) * 2.0 )
q.w = ( self.m[0][1] + self.m[1][0] ) * s
q.x = ( self.m[0][2] + self.m[2][0] ) * s
q.y = ( self.m[1][2] + self.m[2][1] ) * s
q.z = 0.5 * s
return q
def get_wxyz_global(self): """Return angular velocity in global (nonrotating) reference frame.""" q = Quaternion(self.get_Q()) w_global = q * Quaternion([0] + list(self.get_wxyz())) * q.inverse() return w_global[1:4]
def testQuaternionToAngleAxis(self):
q = Quaternion(a = 0.707, b = -0.240, c = -0.665, d = 0.000)
angle, axis = q.get_angle_axis()
self.assertLessEqual(angle - 1.5710983268, 0.0000000001)
self.assertGreaterEqual(angle - 1.5710983268, -0.0000000001)
self.assertEquals(str(axis), "Axis3: -0.3395, -0.9406, 0.0000")
def testInit(self):
q = Quaternion(-0.3308, 0.7273, 0.3217, -0.5080)
q.identity()
self.assertEquals(str(q), "Quaternion: 1.0000 + 0.0000i + 0.0000j + 0.0000k")
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
def testConjugate(self):
q = Quaternion(-0.3308, -0.7273, -0.3217, 0.5080)
q.conjugate()
self.assertEquals(str(q), "Quaternion: -0.3308 + 0.7273i + 0.3217j + -0.5080k")
