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)
