def rotate3DQuaternion(cartesianPt, rotType, angle):
axis = (1, 0, 0)
angle = -(3.14159265359 * angle) / 180
q = Quaternion(math.cos(angle / 2.0),
math.sin(angle / 2.0) * axis[0],
math.sin(angle / 2.0) * axis[1],
math.sin(angle / 2.0) * axis[2])
quaternionPt = Quaternion(0, cartesianPt.x, cartesianPt.y, cartesianPt.z)
qstar = quaternionConjugate(q)
quaternionPt = quaternionMult(q, quaternionPt)
quaternionPt = quaternionMult(quaternionPt, qstar)
t = (quaternionPt.x, quaternionPt.y, quaternionPt.z)
return t
def quaternionMult(quat1, quat2):
u = Quaternion(
quat1.s*quat2.s - quat1.x*quat2.x - quat1.y*quat2.y - quat1.z*quat2.z, \
quat2.s*quat1.x + quat1.s*quat2.x + quat1.y*quat2.z - quat1.z*quat2.y, \
quat2.s*quat1.y + quat1.s*quat2.y + quat1.z*quat2.x - quat1.x*quat2.z, \
quat2.s*quat1.z + quat1.s*quat2.z + quat1.x*quat2.y - quat1.y*quat2.x)
return u
def rotate3DQuaternionApprox(cartesianPt, rotType, angle, normalize=False):
# TODO : rotType ka use karke quaternion define karna h
# print(normalize, threshold)
axis = (1, 0, 0)
d = -(3.14159265359 * angle) / 180
q = Quaternion(1, (d / 2.0) * axis[0], (d / 2.0) * axis[1],
(d / 2.0) * axis[2])
if threshold and normalize:
# print("here")
# print(q.s, q.x, q.y, q.z)
q = mod_q(q)
# print(q.s, q.x, q.y, q.z)
quaternionPt = Quaternion(0, cartesianPt.x, cartesianPt.y, cartesianPt.z)
qstar = quaternionConjugate(q)
quaternionPt = quaternionMult(q, quaternionPt)
quaternionPt = quaternionMult(quaternionPt, qstar)
t = (quaternionPt.x, quaternionPt.y, quaternionPt.z)
return t
def quaternionConjugate(q):
return Quaternion(q.s, -1 * q.x, -1 * q.y, -1 * q.z)
def mod_q(q):
r = math.sqrt(q.s * q.s + q.x * q.x + q.y * q.y + q.z * q.z)
return Quaternion(q.s / r, q.x / r, q.y / r, q.z / r)
def parseDotScene(self):
# TODO: check DTD to make sure you get all nodes/attributes
# TODO: Use the userData for sound/physics
for node in self.root:
if node.nodeType == Node.ELEMENT_NODE and node.nodeName == 'node':
attachMe = None
realName = node.attributes['name'].nodeValue
# create new scene node
newNode = self.rootNode.createChildSceneNode(
) # self.prefix + realName)
#position it
pos = self.findNodes(node, 'position')[0].attributes
newNode.position = (float(pos['x'].nodeValue),
float(pos['y'].nodeValue),
float(pos['z'].nodeValue))
# rotate it
try:
rot = self.findNodes(node, 'rotation')[0].attributes
newNode.orientation = Quaternion(
float(rot['qw'].nodeValue), float(rot['qx'].nodeValue),
float(rot['qy'].nodeValue), float(rot['qz'].nodeValue))
# print float(rot['qw'].nodeValue), float(rot['qx'].nodeValue), float(rot['qy'].nodeValue),float(rot['qz'].nodeValue)
except IndexError: # probably doesn't have rotation attribute
rot = self.findNodes(node, 'quaternion')[0].attributes
newNode.orientation = Quaternion(float(rot['w'].nodeValue),
float(rot['x'].nodeValue),
float(rot['y'].nodeValue),
float(rot['z'].nodeValue))
# print float(rot['w'].nodeValue), float(rot['x'].nodeValue), float(rot['y'].nodeValue), float(rot['z'].nodeValue)
# scale it
scale = self.findNodes(node, 'scale')[0].attributes
newNode.scale = (float(scale['x'].nodeValue),
float(scale['y'].nodeValue),
float(scale['z'].nodeValue))
# is it a light?
#try:
# thingy = self.findNodes(node, 'light')[0].attributes
# name = str(thingy['name'].nodeValue)
#attachMe = self.sceneManager.createLight(name)
#ltypes={'point':ogre.Light.LT_POINT,'directional':ogre.Light.LT_DIRECTIONAL,'spot':ogre.Light.LT_SPOTLIGHT,'radPoint':ogre.Light.LT_POINT}
#try:
# attachMe.type = ltypes[thingy['type'].nodeValue]
#except IndexError:
# pass
# lightNode = self.findNodes(node, 'light')[0]
# try:
# tempnode = self.findNodes(lightNode, 'colourSpecular')[0]
# attachMe.specularColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# try:
# tempnode = self.findNodes(lightNode, 'colourDiffuse')[0]
# attachMe.diffuseColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# try:
# tempnode = self.findNodes(lightNode, 'colourDiffuse')[0]
# attachMe.diffuseColour = (float(tempnode.attributes['r'].nodeValue), float(tempnode.attributes['g'].nodeValue), float(tempnode.attributes['b'].nodeValue), 1.0)
# except IndexError:
# pass
# self.lights.append( attachMe )
# print 'added light: "%s"' % name
#except IndexError:
# pass
# is it an entity?
try:
thingy = self.findNodes(node, 'entity')[0].attributes
name = str(thingy['name'].nodeValue)
mesh = str(thingy['meshFile'].nodeValue)
attachMe = self.sceneManager.createEntity(name, mesh)
print 'added entity: "%s" %s' % (name, mesh)
except IndexError:
pass
#except: #ogre.OgreFileNotFoundException: # mesh is missing
# print "Missing Mesh:",mesh
# pass
# is it a camera?
# TODO: there are other attributes I need in here
try:
thingy = self.findNodes(node, 'camera')[0].attributes
name = str(thingy['name'].nodeValue)
fov = float(thingy['fov'].nodeValue)
projectionType = str(thingy['projectionType'].nodeValue)
attachMe = self.sceneManager.createCamera(name)
try:
tempnode = self.findNodes(node, 'clipping')[0]
attachMe.nearClipDistance = float(
tempnode.attributes['near'].nodeValue)
attachMe.farClipDistance = float(
tempnode.attributes['far'].nodeValue)
except IndexError:
pass
##attachMe.setFOVy ( ogre.Radian( fov ) ) #fOVy = fov
self.cameras.append(attachMe)
print 'added camera: "%s" fov: %f type: %s clipping: %f,%f' % (
name, fov, projectionType, attachMe.nearClipDistance,
attachMe.farClipDistance)
except IndexError:
pass
# attach it to the scene
#try:
if attachMe is not None:
newNode.attachObject(attachMe)
imu_val[0, :] = Ax_raw * Acc_scale_factor[0] + Acc_bias[0]
imu_val[1, :] = Ay_raw * Acc_scale_factor[1] + Acc_bias[1]
imu_val[2, :] = Az_raw * Acc_scale_factor[2] + Acc_bias[2]
imu_val[3, :] = (Wx_raw - Wx_bias) * Gyro_scale_factor
imu_val[4, :] = (Wy_raw - Wy_bias) * Gyro_scale_factor
imu_val[5, :] = (Wz_raw - Wz_bias) * Gyro_scale_factor
#initialization
xt = np.matrix([1, 0, 0, 0, 0, 0, 0]).T
Pt = np.diag([0.01, 0.01, 0.01, 0.01, 0.01, 0.01])
Q = np.diag([78.97, 78.97, 78.97, 6.5, 6.5, 6.5])
R = np.diag([0.2, 0.2, 0.2, 3, 3, 3])
#gravity
g = Quaternion(0.0, 0.0, 0.0, 1.0)
# Sigma Point 2n
n = 6
#store Rot output
rot_UKF = np.zeros((3, 3, ts.shape[1]))
#rot_UKF[:,:,0] = np.eye(3)
#iteration
for i in range(ts.shape[1]):
if i == 0:
dt = 0.01
else:
dt = ts[0, i] - ts[0, i - 1]
#Cholesky decomposition
#find orientation based on acc
Acc_rpy = np.zeros([3, imu_val.shape[1]])
for i in range(imu_val.shape[1]):
v = imu_val[0:3, i]
Acc_rpy[0, i] = -np.arctan2(v[1], (v[0]**2 + v[2]**2)**0.5)
Acc_rpy[1, i] = np.arctan2(v[0], (v[1] ** 2 + v[2] ** 2) ** 0.5)
Acc_rpy[2, i] = 0
#find orientation based on gyro
Gyro_rot = np.zeros([3, 3, imu_val.shape[1]])
Gyro_rpy = np.zeros([3, imu_val.shape[1]])
q = Quaternion(1,0,0,0)
for i in range(imu_val.shape[1]):
# Time increment
if i == 0:
dt = 0.01
else:
dt = ts[0, i] - ts[0, i - 1]
#find angles and axis
v = imu_val[3:6, i]
v_norm = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
angle = v_norm*dt
axis = v / v_norm
qt = qu.angle_axis2quat(angle,axis)
q *= qt
R = q.q_rot()
