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 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 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 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 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 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 __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 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 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 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 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 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")
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 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 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 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 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 __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 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 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 __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, 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_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 _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_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 asQuaternion(self):
"""
@returns: a quaternion representing the same rotation
@rtype: L{Scientific.Geometry.Quaternion.Quaternion}
"""
from Quaternion import Quaternion
axis, angle = self.axisAndAngle()
sin_angle_2 = N.sin(0.5 * angle)
cos_angle_2 = N.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 receiveRigidBodyFrame():
i=0
global positions, orientations, recordingTimes, originalQuart, totalRowList, originalPosition
firstPosSet = False
while True:
i=i+1
try:
csvFile = open("/home/pi/t265/coordinateData.csv", "r")
content = csvFile.readline()
row = content.split(",")
if(not(firstPosSet)):
originalPosition[0][0] = float(row[8])
originalPosition[0][1] = float(row[6])
originalPosition[0][2] = float(row[7])
firstPosSet = True
#Calculates the delta in current position from initial position
#Mapping of coordinate systems is done here
positions[0][0] = float(row[8]) - originalPosition[0][0] #z
positions[0][1] = float(row[6]) - originalPosition[0][1]#x
positions[0][2] = float(row[7]) - originalPosition[0][2] #y
#The quaternion set here is the difference between the current quaternion and the initial quaternion
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
#Keeps track of recording times so that velocity between two corresponding positions can be calculated
recordingTimes[0] = recordingTimes[1]
recordingTimes[1] = int(row[1]) #This gives time in microseconds
#Used to sync the threads
event.set()
except:
continue
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)
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
