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 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 _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")