def test_some_shit(): a_milk_rotation = [-0.00014899999999999999, -0.046429999999999999, -0.017059000000000001] q = ms3d_euler__to__cal3d_quat(*a_milk_rotation) print "w,x,y,z",q q2 = quaternion.fromEuler(*a_milk_rotation) print "x,y,z,w", q2.internal milk_as_angles = map(quaternion.radians_to_angle, a_milk_rotation) q3 = ms3d_euler__to__cal3d_quat(*milk_as_angles) print "x,y,z,w",q3 q_forquat = [q[1], q[2], q[3], q[0]] q3_forquat = [q3[1], q3[2], q3[3], q3[0]] q4 = quaternion.Quaternion(q_forquat) q5 = quaternion.Quaternion(q3_forquat) print q4.internal print "q2", q2.AXYZ() print "q4", q4.AXYZ() print "q5", q5.AXYZ()
def setUp(self): import quaternion self.i = quaternion.Quaternion(0,1,0,0) self.j = quaternion.Quaternion(0,0,1,0) self.k = quaternion.Quaternion(0,0,0,1)
def mat2quat(mat, rot_type='proper'): """ Convert Rotation Matrices to Quaternions Parameters ---------- mat: numpy array or a list of (3 x 3) rotation matrix rot_type: string ('proper' or 'improper') ``improper`` if there is a possibility of having improper matrices in the input, ``proper`` otherwise. \v Default: ``proper`` Returns ---------- quaternion_rep: numpy array (5 x 1) See Also -------- quat2mat, axang2quat """ import quaternion as quat ax_ang = vrrotmat2vec(mat, rot_type) q0 = np.cos(ax_ang[3, :]/2) q1 = ax_ang[0, :]*np.sin(ax_ang[3, :]/2) q2 = ax_ang[1, :]*np.sin(ax_ang[3, :]/2) q3 = ax_ang[2, :]*np.sin(ax_ang[3, :]/2) qtype = ax_ang[4, :] return quat.Quaternion(q0, q1, q2, q3, qtype)
def rand_quat(n): """ Create a random quaternion Parameters ----------- n: Integer number of random quaternions to be returned Returns ----------- returns *n* random quaternions """ q0 = np.zeros((n, )) q1 = np.zeros((n, )) q2 = np.zeros((n, )) q3 = np.zeros((n, )) for ct1 in range(n): x1, x2 = pick_rand2() x3, x4 = pick_rand2() q0[ct1] = x1 q1[ct1] = x2 q2[ct1] = x3 * (np.sqrt((1 - x1**2 - x2**2) / (x3**2 + x4**2))) q3[ct1] = x4 * (np.sqrt((1 - x1**2 - x2**2) / (x3**2 + x4**2))) print q0**2 + q1**2 + q2**2 + q3**2 return quat.Quaternion(q0, q1, q2, q3)
def main(): while True: ximu.processNewChar( ord(s.read()) % 256) # char to unsigned char conversion with modulus if ximu.isBattAndThermGetReady(): BattAndTemp = ximu.getBattAndTherm() print "battery = ", BattAndTemp.battery print "tempereature = ", BattAndTemp.thermometer if ximu.isInertialAndMagGetReady(): InertialAndMag = ximu.getInertialAndMag() print "gyrX = ", InertialAndMag.gyrX print "gyrY = ", InertialAndMag.gyrY print "gyrZ = ", InertialAndMag.gyrZ print "accX = ", InertialAndMag.accX print "accY = ", InertialAndMag.accY print "accZ = ", InertialAndMag.accZ print "magX = ", InertialAndMag.magX print "magY = ", InertialAndMag.magY print "magZ = ", InertialAndMag.magZ if ximu.isQuaternionGetReady(): Quaternions = ximu.getQuaternion() Quaternion = quaternion.Quaternion(Quaternions.w, Quaternions.x, Quaternions.y, Quaternions.z) EulerAngles = Quaternion.getEulerAngles() print "roll = ", EulerAngles.roll print "pitch = ", EulerAngles.pitch print "yaw = ", EulerAngles.yaw
def reorder(self, newOrder): # WARNING: self discards revolution information -bhouston q = quaternion.Quaternion() q.setFromEuler(self) return self.setFromQuaternion(q, newOrder)
def preprocessDeviceMotionG(data): dataG = data['timestamp'].to_frame() for index, row in enumerate(data.itertuples()): q = quaternion.Quaternion(row.attitudeW, row.attitudeX, row.attitudeY, row.attitudeZ) g = utils.rotation3D(q, row.gravityX, row.gravityY, row.gravityZ) q = utils.generateUnitQuaternion(np.pi / 2, 1, 0, 0) g = utils.rotation3D(q, g[0], g[1], g[2]) for axisNum, axis in enumerate(['x', 'y', 'z']): dataG.loc[index, axis] = g[axisNum] return dataG
def rotate_using_quaternion(self, angle, eigenaxis): """Rotates a vector about arbitrary eigenaxis using quaternion. This is an alternative formulation for rotate_about_eigenaxis. Interface is the same as rotate_about_eigenaxis.""" q = quat.Quaternion(eigenaxis, 0.0) ##Need to negate here because set_values performs a negative rotation Quaternion definition updated. #q.set_values(eigenaxis, -angle) #quaternion now represents the rotation return (make_celestial_vector(q.cnvrt(self)))
def _getTransform(qEffector): q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix() #compute rotation matrix local -> world p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for k in range(0, 3): for l in range(0, 3): rm[k, l] = rot[k, l] for m in range(0, 3): rm[m, 3] = qEffector[m] rm[3, 3] = 1 return rm
def OPMaker(row, v_tG, v_left, v_face): v_acc = vector.Vector(row[1:4]) new_accx = v_acc.dot_product(v_tG) new_accy = v_acc.dot_product(v_face) new_accz = v_acc.dot_product(v_left) v_acc_new = vector.Vector([new_accx,new_accy,new_accz]) list_acc_new = list(v_acc_new.coordinates) rx = row[4] * (1 / target_rate) ry = row[5] * (1 / target_rate) rz = row[6] * (1 / target_rate) cos = math.cos(math.pi * (rx/360)) sin = math.sin(math.pi * (rx/360)) qx = quaternion.Quaternion(cos, sin, 0, 0) cos = math.cos(math.pi * (ry/360)) sin = math.sin(math.pi * (ry/360)) qy = quaternion.Quaternion(cos, 0, sin, 0) cos = math.cos(math.pi * (rz/360)) sin = math.sin(math.pi * (rz/360)) qz = quaternion.Quaternion(cos, 0, 0, sin) qf = (qx*qz*qy).normalize() deg_half = math.acos(qf.s) sin_deg_half = math.sin(deg_half) ux = qf.x / sin_deg_half uy = qf.y / sin_deg_half uz = qf.z / sin_deg_half v_axis = vector.Vector([ux, uy, uz]) new_axisx = v_axis.dot_product(v_tG) new_axisy = v_axis.dot_product(v_face) new_axisz = v_axis.dot_product(v_left) v_axis_new = vector.Vector([new_axisx, new_axisy, new_axisz]).direction() list_axis_new = list(v_axis_new.coordinates) if list_axis_new[0] < 0: for m in range(len(list_axis_new)): list_axis_new[m] *= -1 deg_half *= -1 return list_acc_new + list_axis_new + [deg_half]
def __init__(self, position=None, forward=None, up=None): if position is None: self.position = quat.Quaternion(0, 0, 0, 3e11) # High above the plane else: self.position = position if forward is None: self.forward = quat.Quaternion(0, 0, 0, -1) else: self.forward = forward if up is None: self.up = quat.Quaternion(0, 0, 1, 0) else: self.up = up # Refactor the crap below. self.x_min = 0 self.y_min = 0 self.x_max, self.y_max = WINDOW_SIZE self.x_center = self.x_max // 2 self.y_center = self.y_max // 2 self.z = self.x_center # Try this for size. self.minimum_distance = 10000 # 10 km
def test_quaternion(type1, case): """ Tests the quaternion class Parameters ----------- type1: allowed values are 'Init' and 'Slice' * string case: allowed values are 0 and 5 * integer Returns -------- None Notes ------ * If type1 == 'Init' and case == 0 An empty quaternion array is created. * If type1 == 'Init' and case == 5 A quaternion array of size (5 x 10) is created. * If type1 == 'Slice' and case == 5 A quaternion array of size (5 x 10) is created and a new quaternion array of size (5 x 3) is created by slicing this array. See Also --------- * test_indiv_mehtods * rand_quat """ if type1 == 'Init': if case == 0: q1 = quat.Quaternion() test_indiv_methods(q1) if case == 5: ctn = 10 q1 = rand_quat(ctn) test_indiv_methods(q1) if type1 == 'Slice': if case == 5: ctn = 10 q1 = rand_quat(ctn) q2 = q1[:, 0:3] test_indiv_methods(q1) test_indiv_methods(q2) return
def join(self, joinables): quaternionPackage = joinables[self.quaternionName] vectorPackage = joinables[self.vectorName] quaternion = quat.Quaternion(quaternionPackage.value[0:3], quaternionPackage.value[3]) vector = quaternion.rotateVector(vectorPackage.value) name = self.__class__.__name__ value = pk_imu.Vector3D.fromContainer(vector) timestamp = int( (quaternionPackage.timestamp + vectorPackage.timestamp) / 2) return pk.Package.make(name, value, timestamp)
def axang2quat(ax_ang): """ Create a quaternion corresponding to the rotation specified by an axis and an angle Parameters ---------- ax_ang: numpy array or a list of (4 x 1) Returns ---------- quaternion_rep: numpy array (5 x 1) """ import quaternion as quat if ax_ang.ndim == 1: if np.size(ax_ang) == 5: ax_ang = np.reshape(ax_ang, (5, 1)) msz = 1 else: raise Exception('Wrong Input Type') elif ax_ang.ndim == 2: if np.shape(ax_ang)[0] == 5: msz = np.shape(ax_ang)[1] elif np.shape(ax_ang)[1] == 5: ax_ang = ax_ang.transpose() msz = np.shape(ax_ang)[1] else: raise Exception('Wrong Input Type') else: raise Exception('Wrong Input Type') direction = ax_ang[0:3, :] angle = ax_ang[3, :] d = np.array(direction, dtype=np.float64) d /= np.linalg.norm(d, axis=0) x = d[0, :] y = d[1, :] z = d[2, :] q0 = np.cos(angle/2) s = np.sin(angle/2) q1 = x*s q2 = y*s q3 = z*s qtype = ax_ang[4, :] return quat.Quaternion(q0, q1, q2, q3, qtype)
def star_to_screen_space(self, position): """Translate given position to screen space.""" relat_pos = quat.Quaternion(0, position.x, position.y, position.z) forward_length = relat_pos * self.forward if forward_length <= 0.0: return -1, -1 vertical_length = relat_pos * self.up vertical_tangent = vertical_length / forward_length lateral_length = relat_pos * (self.forward @ self.up) lateral_tangent = lateral_length / forward_length x = self.x_center + self.z * lateral_tangent y = self.y_center - self.z * vertical_tangent return int(x), int(y)
def world_to_screen_space(self, position): """Translate given position to screen space.""" relat_pos = quat.Quaternion(0, position.x - self.position.x, position.y - self.position.y, position.z - self.position.z) distance = abs(relat_pos) forward_length = relat_pos * self.forward if forward_length < self.minimum_distance: return -1, -1, 1 vertical_length = relat_pos * self.up vertical_tangent = vertical_length / forward_length lateral_length = relat_pos * (self.forward @ self.up) lateral_tangent = lateral_length / forward_length x = self.x_center + self.z * lateral_tangent y = self.y_center - self.z * vertical_tangent return int(x), int(y), -int(distance)
def printComPosition(nbConfigs): for i in range(0, nbConfigs): q = fullBody.shootRandomConfig() q[0:7] = zeroConf fullBody.setCurrentConfig(q) #setConfiguration matching sample com = fullBody.getCenterOfMass() for x in range(0, 3): q[x] = -com[x] fullBody.setCurrentConfig(q) #~ print ("final com" + str(com)) #~ print ("final com" + str(fullBody.getCenterOfMass())) if (fullBody.isConfigValid(q)[0]): global success success += 1 for j in range(0, len(effectors)): effectorName = effectors[j] limbId = limbIds[j] qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix( ) #compute rotation matrix world -> local p = qEffector[ 0:3] #(0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for k in range(0, 3): for l in range(0, 3): rm[k, l] = rot[k, l] for m in range(0, 3): rm[m, 3] = qEffector[m] rm[3, 3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0, 0, 0, 1]) points[j].append(p) #~ print (points[j]) else: global fails fails += 1 #~ print fullBody.isConfigValid(q)[1] for j in range(0, len(limbIds)): f1 = open('./' + str(limbIds[j]) + '_com.erom', 'w+') for p in points[j]: f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n") f1.close()
def printRootPosition(limbId, effectorName, nbSamples): f1 = open('../data/roms/com' + limbId + '.erom', 'w+') for i in range(0, nbSamples - 1): q = fullBody.getSample(limbId, i) fullBody.setCurrentConfig(q) #setConfiguration matching sample qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix() #compute rotation matrix world -> local p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for i in range(0, 3): for j in range(0, 3): rm[i, j] = rot[i, j] for i in range(0, 3): rm[i, 3] = qEffector[i] rm[3, 3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0, 0, 0, 1]) f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n") f1.close()
def printComPosition(nbConfigs): num_invalid = 0 for i in range(0, nbConfigs): q = fullBody.shootRandomConfig() q[0:7] = zeroConf fullBody.setCurrentConfig(q) # setConfiguration matching sample com = fullBody.getCenterOfMass() for x in range(0, 3): q[x] = -com[x] fullBody.setCurrentConfig(q) # print ("final com" + str(com)) # print ("final com" + str(fullBody.getCenterOfMass())) if fullBody.isConfigValid(q)[0]: for j in range(0, len(effectors)): effectorName = effectors[j] # limbId = limbIds[j] qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix( ) # compute rotation matrix world -> local p = qEffector[ 0:3] # (0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for k in range(0, 3): for j in range(0, 3): rm[k, j] = rot[k, j] for m in range(0, 3): rm[m, 3] = qEffector[m] rm[3, 3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0, 0, 0, 1]) points[j].append(p) # print (points[j]) else: num_invalid += 1 for j in range(0, len(limbIds)): f1 = open("./" + str(limbIds[j]) + "_com.erom", "w+") for p in points[j]: f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n") f1.close() print("%invalid ", (float)(num_invalid) / (float)(nbConfigs) * 100, "%")
# -*- coding: utf-8 -*- import numpy as np import matplotlib.pyplot as plt from scipy import interpolate import pandas as pd import quaternion from mpl_toolkits.mplot3d import Axes3D # 機体のスペックシートを読み込む spec = pd.read_csv("spec.csv") # 機体の推力シートを読む thrust = np.loadtxt("I205.txt") qua = quaternion.Quaternion() T = 60. N = 100 dt = T / float(N) t = np.empty(N) t[0] = 0. b = 1. / 6. count = -1 # 機体の初期重量[kg] m0 = float(spec['VALUE'][0]) # 機体の燃焼後重量[kg] mb = float(spec['VALUE'][1]) # 先頭から見た初期重心の位置[m] rg = float(spec['VALUE'][2])
def setUp(self): import quaternion self.q = quaternion.Quaternion(1,1,1,1)
#!/usr/bin/env python import quaternion as q q1 = q.Quaternion((1., 0., 0., 0.)) q2 = q.fromEuler(90., 0., 0.) print(q1 * q2).getAngleAxis()
def misorient_fz(misquats, cryst_ptgrp, tol=1e-12): """ The function takes as input the misorientations and the corresponding crystallographic point group. It converts them using symmetry operations and returns the disorientations Parameters ---------- misquats: Quaternion class Quaternion misorientations cryst_ptgrp: string Crystallogrphic point group in Schoenflies notation tol: float Tolerance for the disorientation to belong in the fundamental zone Returns ------- disquats: quaternion class Disorientations for the given misorientations """ file_dir = os.path.dirname(os.path.realpath(__file__)) fil2 = file_dir + '/pkl_files/symm_quats_' + cryst_ptgrp + '.pkl' pkl_fil2 = open(fil2, 'rb') symm_quat = pickle.load(pkl_fil2) if misquats.ndim == 1: misquats = np.reshape(misquats, (5, 1)) disquats = quat.Quaternion(np.zeros(np.shape(misquats))) disquats[:] = np.NaN msz = quat.get_size(misquats) symm_sz = quat.get_size(symm_quat) for ct1 in range(msz): tq1 = misquats[:, ct1] tcheck = 0 for j in range(symm_sz): tsymm_q1 = quat.mtimes(tq1, symm_quat[:, j]) for k in range(symm_sz): tsymm_q2 = quat.mtimes(symm_quat[:, k], tsymm_q1) tqn1 = quat.Quaternion(np.copy(tsymm_q2)) tqn2 = quat.antipodal(tsymm_q2) tqn3 = quat.inverse(tsymm_q2) tqn4 = quat.inverse(quat.antipodal(tsymm_q2)) if check_cond(tqn1, cryst_ptgrp, tol): tcheck = 1 disquats[:, ct1] = tqn1 break elif check_cond(tqn2, cryst_ptgrp, tol): tcheck = 1 disquats[:, ct1] = tqn2 break elif check_cond(tqn3, cryst_ptgrp, tol): tcheck = 1 disquats[:, ct1] = tqn3 break elif check_cond(tqn4, cryst_ptgrp, tol): tcheck = 1 disquats[:, ct1] = tqn4 break if tcheck == 1: break if tcheck == 0: raise Exception('FZ Quaternion not found') return disquats
def __init__(self): self.nodes = [] self.edges = [] self.faces = [] self.quaternion = quat.Quaternion()
lArmy = 0.024 fullBody.addLimb(larmId, larm, lHand, lArmOffset, lArmNormal, lArmx, lArmy, nbSamples, 0.01) #make sure this is 0 q_0 = fullBody.getCurrentConfig() q_0[0:7] = [0, 0, 0, 1, 0, 0, 0] fullBody.setCurrentConfig(q_0) import numpy as np effectorName = rfoot limbId = rLegId q = fullBody.getSample(limbId, 1) fullBody.setCurrentConfig(q) #setConfiguration matching sample qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix() #compute rotation matrix world -> local p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for i in range(0, 3): for j in range(0, 3): rm[i, j] = rot[i, j] for i in range(0, 3): rm[i, 3] = qEffector[i] rm[3, 3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0, 0, 0, 1]) def printRootPosition(limbId, effectorName, nbSamples): f1 = open('../data/roms/com' + limbId + '.erom', 'w+')
def test_rotateListVector(self): self.quaternion = quaternion.Quaternion((0, 0, 0), 1)
def setUp(self): import quaternion self.q = quaternion.Quaternion(1,1,1,1) self.n = quaternion.Quaternion(0.5,0.5,0.5,0.5)
def generate_symm_quats(cryst_ptgrp, tol=1e-10): """ Give crystallographic point group, this function generates all the symmetry operations (as quaternions) that belong to the point group using 'generators' Parameters ----------------- cryst_ptgrp: string Crystallogrphic point group in Schoenflies notation tol: float The tolerance used to check if two matrices are the same Returns ------------ symm_quat: quaternion array Size: n x 5 \v Symmetry operations as matrices for the corresponding point group """ prop_grps = ['C1', 'C2', 'C3', 'C4', 'C6', 'D2', 'D3', 'D4', 'D6', 'T', 'O'] laue_grps = ['Ci', 'C2h', 'C3i', 'C4h', 'C6h', 'D2h', 'D3d', 'D4h', 'D6h', 'D8h', 'Th', 'Oh'] noncentro_grps = ['Cs', 'S4', 'S6', 'C2v', 'C3v', 'C4v', 'C6v', 'D2d', 'D3h', 'Td'] # if cryst_ptgrp in prop_grps: # rot_type = 'proper' # elif cryst_ptgrp in laue_grps: # rot_type = 'improper' # else: # raise Exception('Wrong Input for crystal point group') if cryst_ptgrp in prop_grps: if cryst_ptgrp == 'C1': n = 1 gsz = 1 order_ptgrp = n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C2': n = 2 gsz = 2 order_ptgrp = n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C3': n = 3 gsz = 2 order_ptgrp = n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C4': n = 4 gsz = 2 order_ptgrp = n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C6': n = 6 gsz = 2 order_ptgrp = n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D2': n = 2 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D3': n = 3 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D4': n = 4 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D6': n = 6 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'T': gsz = 3 order_ptgrp = 12 # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/2, 1])) generators[:, 2] = trans.axang2quat( np.array([1, 1, 1, 2*np.pi/3, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'O': gsz = 3 order_ptgrp = 24 # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, np.pi/2, 1])) generators[:, 2] = trans.axang2quat( np.array([1, 0, 0, np.pi/2, 1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators if cryst_ptgrp in laue_grps: if cryst_ptgrp == 'Ci': n = 1 gsz = 2 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C2h': n = 2 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C3i': n = 3 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C4h': n = 4 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'C6h': n = 6 gsz = 3 order_ptgrp = 2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D2h': n = 2 gsz = 4 order_ptgrp = 2*2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D3d': n = 3 gsz = 4 order_ptgrp = 2*2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D4h': n = 4 gsz = 4 order_ptgrp = 2*2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D6h': n = 6 gsz = 4 order_ptgrp = 2*2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'D8h': n = 8 gsz = 4 order_ptgrp = 2*2*n # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/n, 1])) generators[:, 2] = trans.axang2quat(np.array([1, 0, 0, np.pi, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'Td': gsz = 4 order_ptgrp = 2*12 # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, 2*np.pi/2, 1])) generators[:, 2] = trans.axang2quat( np.array([1, 1, 1, 2*np.pi/3, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators elif cryst_ptgrp == 'Oh': gsz = 4 order_ptgrp = 2*24 # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) generators[:, 1] = trans.axang2quat( np.array([0, 0, 1, np.pi/2, 1])) generators[:, 2] = trans.axang2quat( np.array([1, 0, 0, np.pi/2, 1])) generators[:, 3] = trans.axang2quat(np.array([0, 0, 1, 0, -1])) # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators if cryst_ptgrp in noncentro_grps: if cryst_ptgrp == 'Cs': n = 1 gsz = 2 order_ptgrp = 2*n # Generators # Generators generators = quat.Quaternion(np.zeros((5, gsz))) generators[:, 0] = trans.axang2quat(np.array([0, 0, 1, 0, 1])) t_q1 = trans.axang2quat(np.array([0, 0, 1, 0, -1])) t_q2 = trans.axang2quat(np.array([0, 0, 1, np.pi, 1])) t_mat = quat.mtimes(t_q1, t_q2) generators[:, 1] = t_mat # Symmetry Operations symm_quat = quat.Quaternion(np.zeros([5, order_ptgrp])) symm_quat[:, :gsz] = generators count1 = 1 numops = gsz-1 while numops < order_ptgrp-1: initsize = numops for ct1 in np.arange(count1, initsize+1): t1 = symm_quat[:, ct1] t2 = symm_quat[:, count1] tM1 = quat.mtimes(t1, t2) tcheck = 0 ct2 = np.arange(0, numops+1) if np.any(quat.eq(tM1, symm_quat[:, ct2], tol)): tcheck = 1 if tcheck == 0: symm_quat[:, numops+1] = tM1 numops += 1 if numops == initsize: count1 += 1 symm_quat = quat.antipodal(symm_quat) # print quat.display(symm_quat) return symm_quat
def idquaternion(): return quat.Quaternion(1, 0, 0, 0, 1)
def talker(str,filename): ximu = ximureceiver.XimuReceiver() pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz s=serial.Serial(str, 115200) hello_str = "roll:0,pitch:0,yaw:0" linVel = np.zeros((3)) linPos = np.zeros((3)) acc = np.zeros((3)) mag = np.zeros((3)) gyr = np.zeros((3)) if filename != "ok": fr = open(filename,"w") while not rospy.is_shutdown(): #while True: ximu.processNewChar(ord(s.read())%256) if ximu.isInertialAndMagGetReady(): InertialAndMag = ximu.getInertialAndMag() acc[0] = InertialAndMag.accX acc[1] = InertialAndMag.accY acc[2] = InertialAndMag.accZ mag[0] = InertialAndMag.magX mag[1] = InertialAndMag.magY mag[2] = InertialAndMag.magZ gyr[0] = InertialAndMag.gyrX gyr[1] = InertialAndMag.gyrY gyr[2] = InertialAndMag.gyrZ if ximu.isQuaternionGetReady(): Quaternions = ximu.getQuaternion() Quaternion = quaternion.Quaternion(Quaternions.w, Quaternions.x, Quaternions.y, Quaternions.z) Quater = np.array([Quaternions.w, Quaternions.x, Quaternions.y, Quaternions.z]) EulerAngles = Quaternion.getEulerAngles() #print "roll = ", EulerAngles.roll #print "pitch = ", EulerAngles.pitch #print "yaw = ", EulerAngles.yaw hello_str = "roll:%s,pitch:%s,yaw:%s" %(EulerAngles.roll,EulerAngles.pitch,EulerAngles.yaw) write_str = "%s,%s,%s\r\n" %(EulerAngles.roll,EulerAngles.pitch,EulerAngles.yaw) ## R = quatern2rotMat(Quater) tcAcc = np.dot(R, acc) linAcc = tcAcc - np.array([0,0,0.919]) print "a:", linAcc[0]," b:", linAcc[1]," c:", linAcc[2] linAcc = linAcc * 9.81 linVel = linVel + linAcc/256.0 tmp = math.pow(linAcc[0],2) + math.pow(linAcc[1],2) +math.pow(linAcc[2],2) print tmp if tmp<2: linVel = np.zeros((3)) linAcc = np.zeros((3)) linPos = linPos + linVel/256.0 hello_str = "%s,%s,%s" %(linPos[0],linPos[1],linPos[2]) rospy.loginfo(hello_str) pub.publish(hello_str) if filename != "ok": fr.write(write_str)