本文整理汇总了Python中quaternion.Quaternion类的典型用法代码示例。如果您正苦于以下问题:Python Quaternion类的具体用法?Python Quaternion怎么用?Python Quaternion使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Quaternion类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的Python代码示例。
示例1: test_quaternion_rot_matrix_det_one
def test_quaternion_rot_matrix_det_one(self):
''' Test that the determinant of the rotation matrix is 1.'''
for _ in range(10):
theta = np.random.normal(0., 1., 4)
theta = Quaternion(theta/np.linalg.norm(theta))
R = theta.rotation_matrix()
self.assertAlmostEqual(np.linalg.det(R), 1.0)
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:7,代码来源:quaternion_test.py
示例2: __init__
def __init__(self, src=None, timestamp=None):
if src is not None:
Quaternion.__init__(self, src.w, src.x, src.y, src.z)
else:
Quaternion.__init__(self)
self.timestamp = timestamp
self.valid = True
self.interpolated = False
开发者ID:utk-robotics-2017,项目名称:navX-Python,代码行数:8,代码来源:timestamped_quaternion.py
示例3: generate_quaternion
def generate_quaternion():
q1 = Quaternion.random()
q2 = Quaternion.random()
while True:
for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True):
yield q
#q1, q2 = q2, q1
q1 = q2
q2 = Quaternion.random()
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:9,代码来源:demo.py
示例4: test_q_bar_matrix
def test_q_bar_matrix(self):
a, b, c, d = randomElements()
q = Quaternion(a, b, c, d)
M = np.array([
[a, -b, -c, -d],
[b, a, d, -c],
[c, -d, a, b],
[d, c, -b, a]])
self.assertTrue(np.array_equal(q._q_bar_matrix(), M))
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:9,代码来源:test_quaternion.py
示例5: test_inverse
def test_inverse(self):
q1 = Quaternion(randomElements())
q2 = Quaternion.random()
if q1:
self.assertEqual(q1 * q1.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
else:
with self.assertRaises(ZeroDivisionError):
q1 * q1.inverse()
self.assertEqual(q2 * q2.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:10,代码来源:test_quaternion.py
示例6: fqa
def fqa(msr):
ax = msr['accel.x']
ay = msr['accel.y']
az = msr['accel.z']
norm = math.sqrt(ax*ax + ay*ay + az*az)
# estimation of elevation quaternion (around y axes)
sin_theta_a = (ax / norm)
cos_theta_a = math.sqrt(1 - sin_theta_a ** 2)
sin_half_theta = math.copysign(1, sin_theta_a) * math.sqrt((1 - cos_theta_a) / 2)
cos_half_theta = math.sqrt((1 + cos_theta_a) / 2)
q_e = Quaternion(cos_half_theta, 0, sin_half_theta, 0)
# estimation of roll quaternion (around x axis)
sin_phi = (-ay / norm) / cos_theta_a
cos_phi = (-az / norm) / cos_theta_a
sin_half_phi = half_sin(sin_phi, cos_phi)
cos_half_phi = half_cos(sin_phi, cos_phi)
# TODO singularity avoidance!!!
q_r = Quaternion(cos_half_phi, sin_half_phi, 0, 0)
# estimation of azimuth quaternion (around z axis)
mx = msr['mag.x']
my = msr['mag.y']
mz = msr['mag.z']
norm = math.sqrt(mx*mx + my*my + mz*mz)
qm = Quaternion(0, mx / norm, my / norm, mz / norm)
qm_a = q_e.multiply(q_r).multiply(qm).multiply(q_r.inverse()).multiply(q_e.inverse())
Quaternion.normalize(qm_a)
cos_zeta = qm_a.c
sin_zeta = qm_a.b
sin_half_zeta = half_sin(sin_zeta, cos_zeta)
cos_half_zeta = half_cos(sin_zeta, cos_zeta)
q_a = Quaternion(cos_half_zeta, 0, 0, sin_half_zeta)
q_fqa = q_a.multiply(q_e).multiply(q_r)
return q_fqa
开发者ID:asgalov,项目名称:python,代码行数:33,代码来源:complimentary_filter.py
示例7: update
def update(self, msr):
t = msr['t']
dt = t - self.t
self.t = t
w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
q_fqa = fqa(msr)
dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
self.q = self.q.add(dq.scalar_multiply(dt))
self.q = Quaternion.normalize(self.q)
开发者ID:asgalov,项目名称:python,代码行数:11,代码来源:complimentary_filter.py
示例8: test_matrix_io
def test_matrix_io(self):
v = np.random.uniform(-100, 100, 3)
for i in range(10):
q0 = Quaternion.random()
R = q0.rotation_matrix()
q1 = Quaternion(matrix=R)
np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:12,代码来源:test_quaternion.py
示例9: __init__
def __init__(self, cx, cy, radius):
"""
Initialize instance of ArcBall with no constraint on axis of rotation
"""
self.center_x = cx
self.center_y = cy
self.radius = radius
self.v_down = PVector()
self.v_drag = PVector()
self.q_now = Quaternion()
self.q_down = Quaternion()
self.q_drag = Quaternion()
self.axis_set = [PVector(1.0, 0.0, 0.0), PVector(0.0, 1.0, 0.0), PVector(0.0, 0.0, 1.0)]
self.axis = -1
开发者ID:monkstone,项目名称:processing.py-examples,代码行数:14,代码来源:arcball.py
示例10: get
def get(self, requested_timestamp):
if len(self.history) == 0:
return None
if requested_timestamp in self.history:
match = self.history[requested_timestamp]
return TimestampedQuaternion(match.w, match.x, match.y, match.z, requested_timestamp)
else:
keys = list(self.history.keys())
keys.sort()
if keys[0] > requested_timestamp or keys[-1] < requested_timestamp:
return None
for i, key in enumerate(keys):
if key > requested_timestamp:
previous_timestamp = keys[i - 1]
next_timestamp = key
timestamp_delta = next_timestamp - previous_timestamp
requested_timestamp_offset = requested_timestamp - previous_timestamp
requested_timestamp_ratio = requested_timestamp_offset / timestamp_delta
interpolated_quaternion = Quaternion.slerp(self.history[previous_timestamp],
self.history[next_timestamp],
requested_timestamp_ratio)
match = TimestampedQuaternion(interpolated_quaternion.w,
interpolated_quaternion.x,
interpolated_quaternion.y,
interpolated_quaternion.z, requested_timestamp)
match.setInterpolated(True)
return match
return None
开发者ID:utk-robotics-2017,项目名称:navX-Python,代码行数:33,代码来源:timestamped_quaternion_history.py
示例11: test_differentiation
def test_differentiation(self):
q = Quaternion.random()
omega = np.random.uniform(-1, 1, 3) # Random angular velocity
q_dash = 0.5 * q * Quaternion(vector=omega)
self.assertEqual(q_dash, q.derivative(omega))
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:7,代码来源:test_quaternion.py
示例12: test_quaternion_inverse
def test_quaternion_inverse(self):
'''Test that the quaternion inverse works.'''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
theta_inv = theta.inverse()
identity = theta*theta_inv
self.assertAlmostEqual(identity.s, 1.0)
self.assertAlmostEqual(identity.p[0], 0.0)
self.assertAlmostEqual(identity.p[1], 0.0)
self.assertAlmostEqual(identity.p[2], 0.0)
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:17,代码来源:quaternion_test.py
示例13: test_quaternion_rotation_angle
def test_quaternion_rotation_angle(self):
''' Test generating rotation angle from quaternion. '''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
rotation_angle = theta.rotation_angle()
phi = Quaternion.from_rotation(rotation_angle)
self.assertAlmostEqual(phi.s, theta.s)
self.assertAlmostEqual(phi.p[0], theta.p[0])
self.assertAlmostEqual(phi.p[1], theta.p[1])
self.assertAlmostEqual(phi.p[2], theta.p[2])
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:17,代码来源:quaternion_test.py
示例14: __init__
class ComplimentaryFilter:
def __init__(self, k=0.5):
self.q = Quaternion(1, 0, 0, 0)
self.t = 0
self.k = k
def update(self, msr):
t = msr['t']
dt = t - self.t
self.t = t
w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
q_fqa = fqa(msr)
dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
self.q = self.q.add(dq.scalar_multiply(dt))
self.q = Quaternion.normalize(self.q)
开发者ID:asgalov,项目名称:python,代码行数:17,代码来源:complimentary_filter.py
示例15: test_interpolate
def test_interpolate(self):
q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
q2 = Quaternion(axis=[1, 0, 0], angle=2*pi/3)
num_intermediates = 3
base = pi/6
list1 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=False))
list2 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=True))
self.assertEqual(len(list1), num_intermediates)
self.assertEqual(len(list2), num_intermediates+2)
self.assertEqual(list1[0], list2[1])
self.assertEqual(list1[1], list2[2])
self.assertEqual(list1[2], list2[3])
self.assertEqual(list2[0], q1)
self.assertEqual(list2[1], Quaternion(axis=[1, 0, 0], angle=base))
self.assertEqual(list2[2], Quaternion(axis=[1, 0, 0], angle=2*base))
self.assertEqual(list2[3], Quaternion(axis=[1, 0, 0], angle=3*base))
self.assertEqual(list2[4], q2)
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:18,代码来源:test_quaternion.py
示例16: test_init_copy
def test_init_copy(self):
q1 = Quaternion.random()
q2 = Quaternion(q1)
self.assertIsInstance(q2, Quaternion)
self.assertEqual(q2, q1)
with self.assertRaises(TypeError):
q3 = Quaternion(None)
with self.assertRaises(ValueError):
q4 = Quaternion("String")
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:9,代码来源:test_quaternion.py
示例17: estimate_divergence
def estimate_divergence(self):
'''
Estimate the divergence term with a deterministic
finite difference approach. This is for a single quaternion.
'''
delta = 1e-6
div_term = np.zeros(3)
for k in range(3):
omega = np.zeros(3)
omega[k] = 1.
quaternion_dt = Quaternion.from_rotation(omega*delta/2.)
quaternion_tilde_1 = quaternion_dt*self.orientation[0]
quaternion_dt = Quaternion.from_rotation(-1.*omega*delta/2.)
quaternion_tilde_2 = quaternion_dt*self.orientation[0]
div_term += np.inner((self.mobility([quaternion_tilde_1]) -
self.mobility([quaternion_tilde_2])),
omega/delta)
return div_term
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:19,代码来源:quaternion_integrator.py
示例18: test_conjugate
def test_conjugate(self):
a, b, c, d = randomElements()
q1 = Quaternion(a, b, c, d)
q2 = Quaternion.random()
self.assertEqual(q1.conjugate(), Quaternion(a, -b, -c, -d))
self.assertEqual((q1 * q2).conjugate(), q2.conjugate() * q1.conjugate())
self.assertEqual((q1 + q1.conjugate()) / 2, Quaternion(scalar=q1.scalar()))
self.assertEqual((q1 - q1.conjugate()) / 2, Quaternion(vector=q1.vector()))
开发者ID:timdelbruegger,项目名称:pyquaternion,代码行数:9,代码来源:test_quaternion.py
示例19: test_quaternion_from_rotation
def test_quaternion_from_rotation(self):
''' Test that we correctly create a quaternion from an angle '''
# Generate a random rotation vector
phi = np.random.rand(3)
phi_norm = np.linalg.norm(phi)
theta = Quaternion.from_rotation(phi)
self.assertAlmostEqual(theta.entries[0], np.cos(phi_norm/2.))
self.assertAlmostEqual(theta.entries[1], np.sin(phi_norm/2.)*phi[0]/phi_norm)
self.assertAlmostEqual(theta.entries[2], np.sin(phi_norm/2.)*phi[1]/phi_norm)
self.assertAlmostEqual(theta.entries[3], np.sin(phi_norm/2.)*phi[2]/phi_norm)
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:11,代码来源:quaternion_test.py
示例20: test_quaternion_rotation_matrix
def test_quaternion_rotation_matrix(self):
''' Test that we create the correct rotation matrix for a quaternion. '''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
R = theta.rotation_matrix()
self.assertAlmostEqual(R[0][0], 2.*(theta.s**2 + theta.p[0]**2 - 0.5))
self.assertAlmostEqual(R[0][1], 2.*(theta.p[0]*theta.p[1] -
theta.s*theta.p[2]))
self.assertAlmostEqual(R[1][0], 2.*(theta.p[0]*theta.p[1] +
theta.s*theta.p[2]))
self.assertAlmostEqual(R[1][1], 2.*(theta.s**2 + theta.p[1]**2 - 0.5))
self.assertAlmostEqual(R[2][2], 2.*(theta.s**2 + theta.p[2]**2 - 0.5))
self.assertAlmostEqual(R[2][0], 2.*(theta.p[0]*theta.p[2] - theta.s*theta.p[1]))
开发者ID:stochasticHydroTools,项目名称:RigidMultiblobsWall,代码行数:20,代码来源:quaternion_test.py
注:本文中的quaternion.Quaternion类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论