def test_setitem_quat(Qs):
Ps = Qs[:]
# setitem from quaternion
for j in range(len(Ps)):
Ps[j] = np.quaternion(1.3, 2.4, 3.5, 4.7)
for k in range(j):
assert Ps[k] == np.quaternion(1.3, 2.4, 3.5, 4.7)
for k in range(j + 1, len(Ps)):
assert Ps[k] == Qs[k]
# setitem from np.array, list, or tuple
for seq_type in [np.array, list, tuple]:
Ps = Qs[:]
with pytest.raises(TypeError):
Ps[0] = seq_type(())
with pytest.raises(TypeError):
Ps[0] = seq_type((1.3,))
with pytest.raises(TypeError):
Ps[0] = seq_type((1.3, 2.4,))
with pytest.raises(TypeError):
Ps[0] = seq_type((1.3, 2.4, 3.5))
with pytest.raises(TypeError):
Ps[0] = seq_type((1.3, 2.4, 3.5, 4.7, 5.9))
with pytest.raises(TypeError):
Ps[0] = seq_type((1.3, 2.4, 3.5, 4.7, 5.9, np.nan))
for j in range(len(Ps)):
Ps[j] = seq_type((1.3, 2.4, 3.5, 4.7))
for k in range(j):
assert Ps[k] == np.quaternion(1.3, 2.4, 3.5, 4.7)
for k in range(j + 1, len(Ps)):
assert Ps[k] == Qs[k]
with pytest.raises(TypeError):
Ps[0] = 's'
with pytest.raises(TypeError):
Ps[0] = 's'
def test_from_spherical_coords():
np.random.seed(1843)
random_angles = [[np.random.uniform(-np.pi, np.pi), np.random.uniform(-np.pi, np.pi)]
for i in range(5000)]
for vartheta, varphi in random_angles:
assert abs((np.quaternion(0, 0, 0, varphi / 2.).exp() * np.quaternion(0, 0, vartheta / 2., 0).exp())
- quaternion.from_spherical_coords(vartheta, varphi)) < 1.e-15
def Rs():
ones = [0, -1., 1.]
rs = [np.quaternion(w, x, y, z).normalized() for w in ones for x in ones for y in ones for z in ones][1:]
np.random.seed(1842)
rs = rs + [r.normalized() for r in [np.quaternion(np.random.uniform(-1, 1), np.random.uniform(-1, 1),
np.random.uniform(-1, 1), np.random.uniform(-1, 1)) for i in range(20)]]
return np.array(rs)
def test_Wigner_D_element_underflow(Rs, ell_max):
# NOTE: This is a delicate test, which depends on the result underflowing exactly when expected.
# In particular, it should underflow to 0.0 when |mp+m|>32, but should never undeflow to 0.0
# when |mp+m|<32. So it's not the end of the world if this test fails, but it does mean that
# the underflow properties have changed, so it might be worth a look.
assert sf.ell_max >= 15 # Test can't work if this has been set lower
eps = 1.e-10
ell_max = max(sf.ell_max, ell_max)
# Test |Ra|=1e-10
LMpM = np.array(
[[ell, mp, m] for ell in range(ell_max + 1) for mp in range(-ell, ell + 1) for m in range(-ell, ell + 1) if
abs(mp + m) > 32])
R = np.quaternion(eps, 1, 0, 0).normalized()
assert np.all(sf.Wigner_D_element(R, LMpM) == 0j)
LMpM = np.array(
[[ell, mp, m] for ell in range(ell_max + 1) for mp in range(-ell, ell + 1) for m in range(-ell, ell + 1) if
abs(mp + m) < 32])
R = np.quaternion(eps, 1, 0, 0).normalized()
assert np.all(sf.Wigner_D_element(R, LMpM) != 0j)
# Test |Rb|=1e-10
LMpM = np.array(
[[ell, mp, m] for ell in range(ell_max + 1) for mp in range(-ell, ell + 1) for m in range(-ell, ell + 1) if
abs(m - mp) > 32])
R = np.quaternion(1, eps, 0, 0).normalized()
assert np.all(sf.Wigner_D_element(R, LMpM) == 0j)
LMpM = np.array(
[[ell, mp, m] for ell in range(ell_max + 1) for mp in range(-ell, ell + 1) for m in range(-ell, ell + 1) if
abs(m - mp) < 32])
R = np.quaternion(1, eps, 0, 0).normalized()
assert np.all(sf.Wigner_D_element(R, LMpM) != 0j)
def integrate_gyro(self, initwindow=0.5):
# convert gyro to rad/sec
gyrorad = self.gyro * np.pi/180.0
qorient = np.zeros_like(self.t, dtype=np.quaternion)
gvec = np.zeros_like(self.gyro)
dt = 1.0 / self.sampfreq
isfirst = self.t <= self.t[0] + initwindow
qorient[0] = self.orientation_from_accel(np.mean(self.acc[isfirst, :], axis=0))
for i, gyro1 in enumerate(gyrorad[1:, :], start=1):
qprev = qorient[i-1]
# quaternion angular change from the gyro
qdotgyro = 0.5 * (qprev * np.quaternion(0, *gyro1))
qorient[i] = qprev + qdotgyro * dt
g1 = qorient[i].conj() * np.quaternion(0, 0, 0, -1) * qorient[i]
gvec[i, :] = g1.components[1:]
self.qorient = qorient
self.orient_sensor = quaternion.as_euler_angles(qorient)
self.gvec = gvec
self.accdyn_sensor = self.acc - gvec
def test_rotation_matrix_of_composition_is_dot_product_of_rotation_matrices(self):
q1 = np.quaternion(np.pi, 5.12, -np.pi, 2.56).normalized() # random
q2 = np.quaternion(-np.pi, 0, 1.1, -2.2).normalized() # random
m_of_composition = Quaternion.to_rotation_matrix(q1 * q2)
m_from_q1 = Quaternion.to_rotation_matrix(q1)
m_from_q2 = Quaternion.to_rotation_matrix(q2)
m_from_dot = m_from_q1.dot(m_from_q2)
np.testing.assert_allclose(m_of_composition, m_from_dot)
def test_Wigner_D_element_overflow(Rs, ell_max):
assert sf.ell_max >= 15 # Test can't work if this has been set lower
ell_max = max(sf.ell_max, ell_max)
LMpM = sf.LMpM_range(0, ell_max)
# Test |Ra|=1e-10
R = np.quaternion(1.e-10, 1, 0, 0).normalized()
assert np.all(np.isfinite(sf.Wigner_D_element(R, LMpM)))
# Test |Rb|=1e-10
R = np.quaternion(1, 1.e-10, 0, 0).normalized()
assert np.all(np.isfinite(sf.Wigner_D_element(R, LMpM)))
def test_multiply_by_unit_quaternion(self):
id = np.quaternion(1, 0, 0, 0)
q1 = np.quaternion(1, 2, 3, 4) # random
self.assertEqual(id*q1, q1)
self.assertEqual(q1*id, q1)
q2 = np.quaternion(5, 6, 1000, 2000) # random
self.assertEqual(id*q2, q2)
self.assertEqual(q2*id, q2)
def test_from_euler_angles():
np.random.seed(1843)
random_angles = [[np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi)]
for i in range(5000)]
for alpha, beta, gamma in random_angles:
assert abs((np.quaternion(0, 0, 0, alpha / 2.).exp()
* np.quaternion(0, 0, beta / 2., 0).exp()
* np.quaternion(0, 0, 0, gamma / 2.).exp()
)
- quaternion.from_euler_angles(alpha, beta, gamma)) < 1.e-15
def test_quaternion_parity_conjugates(Qs):
for q in Qs[Qs_finite]:
assert q.x_parity_conjugate() == np.quaternion(q.w, q.x, -q.y, -q.z)
assert q.y_parity_conjugate() == np.quaternion(q.w, -q.x, q.y, -q.z)
assert q.z_parity_conjugate() == np.quaternion(q.w, -q.x, -q.y, q.z)
assert q.parity_conjugate() == np.quaternion(q.w, q.x, q.y, q.z)
assert np.array_equal(np.x_parity_conjugate(Qs[Qs_finite]),
np.array([q.x_parity_conjugate() for q in Qs[Qs_finite]]))
assert np.array_equal(np.y_parity_conjugate(Qs[Qs_finite]),
np.array([q.y_parity_conjugate() for q in Qs[Qs_finite]]))
assert np.array_equal(np.z_parity_conjugate(Qs[Qs_finite]),
np.array([q.z_parity_conjugate() for q in Qs[Qs_finite]]))
assert np.array_equal(np.parity_conjugate(Qs[Qs_finite]), np.array([q.parity_conjugate() for q in Qs[Qs_finite]]))
def _get_orientation_madgwick(self, initwindow=0.5, beta=2.86):
gyrorad = self.gyro * np.pi/180.0
betarad = beta * np.pi/180.0
qorient = np.zeros_like(self.t, dtype=np.quaternion)
qgyro = np.zeros_like(self.t, dtype=np.quaternion)
gvec = np.zeros_like(self.gyro)
dt = 1.0 / self.sampfreq
isfirst = self.t <= self.t[0] + initwindow
qorient[0] = self.orientation_from_accel(np.mean(self.acc[isfirst, :], axis=0))
for i, gyro1 in enumerate(gyrorad[1:, :], start=1):
qprev = qorient[i-1]
acc1 = self.acc[i, :]
acc1 = -acc1 / np.linalg.norm(acc1)
# quaternion angular change from the gryo
qdotgyro = 0.5 * (qprev * np.quaternion(0, *gyro1))
qgyro[i] = qprev + qdotgyro * dt
if beta > 0:
# gradient descent algorithm corrective step
qp = qprev.components
F = np.array([2*(qp[1]*qp[3] - qp[0]*qp[2]) - acc1[0],
2*(qp[0]*qp[1] + qp[2]*qp[3]) - acc1[1],
2*(0.5 - qp[1]**2 - qp[2]**2) - acc1[2]])
J = np.array([[-2*qp[2], 2*qp[3], -2*qp[0], 2*qp[1]],
[2*qp[1], 2*qp[0], 2*qp[3], 2*qp[2]],
[0, -4*qp[1], -4*qp[2], 0]])
step = np.dot(J.T, F)
step = step / np.linalg.norm(step)
step = np.quaternion(*step)
qdot = qdotgyro - betarad * step
else:
qdot = qdotgyro
qorient[i] = qprev + qdot * dt
# get the gravity vector
# gravity is -Z
gvec = [(q.conj() * np.quaternion(0, 0, 0, -1) * q).components[1:] for q in qorient]
self.qorient = qorient
self.orient_sensor = quaternion.as_euler_angles(qorient)
self.gvec = gvec
self.accdyn_sensor = self.acc - gvec
def test_quaternion_getset(Qs):
# get parts a and b
for q in Qs[Qs_nonnan]:
assert q.a == q.w + 1j * q.z
assert q.b == q.y + 1j * q.x
# Check multiplication law for parts a and b
part_mul_precision = 1.e-14
for p in Qs[Qs_finite]:
for q in Qs[Qs_finite]:
assert abs((p * q).a - (p.a * q.a - p.b.conjugate() * q.b)) < part_mul_precision
assert abs((p * q).b - (p.b * q.a + p.a.conjugate() * q.b)) < part_mul_precision
# get components/vec
for q in Qs[Qs_nonnan]:
assert np.array_equal(q.components, np.array([q.w, q.x, q.y, q.z]))
assert np.array_equal(q.vec, np.array([q.x, q.y, q.z]))
# set components/vec from np.array, list, tuple
for q in Qs[Qs_nonnan]:
for seq_type in [np.array, list, tuple]:
p = np.quaternion(*q.components)
r = np.quaternion(*q.components)
p.components = seq_type((-5.5, 6.6, -7.7, 8.8))
r.vec = seq_type((6.6, -7.7, 8.8))
assert np.array_equal(p.components, np.array([-5.5, 6.6, -7.7, 8.8]))
assert np.array_equal(r.components, np.array([q.w, 6.6, -7.7, 8.8]))
# TypeError when setting components with the wrong type or size of thing
for q in Qs:
for seq_type in [np.array, list, tuple]:
p = np.quaternion(*q.components)
r = np.quaternion(*q.components)
with pytest.raises(TypeError):
p.components = '1.1, 2.2, 3.3, 4.4'
with pytest.raises(TypeError):
p.components = seq_type([])
with pytest.raises(TypeError):
p.components = seq_type((-5.5,))
with pytest.raises(TypeError):
p.components = seq_type((-5.5, 6.6,))
with pytest.raises(TypeError):
p.components = seq_type((-5.5, 6.6, -7.7,))
with pytest.raises(TypeError):
p.components = seq_type((-5.5, 6.6, -7.7, 8.8, -9.9))
with pytest.raises(TypeError):
r.vec = '2.2, 3.3, 4.4'
with pytest.raises(TypeError):
r.vec = seq_type([])
with pytest.raises(TypeError):
r.vec = seq_type((-5.5,))
with pytest.raises(TypeError):
r.vec = seq_type((-5.5, 6.6))
with pytest.raises(TypeError):
r.vec = seq_type((-5.5, 6.6, -7.7, 8.8))
def __mul__(self, other):
if isinstance(other, Vector):
new_vector = np.quaternion(0.0, other[0], other[1], other[2])
new_vector = self.quaternion * new_vector * np.conjugate(
self.quaternion)
new_vector = Vector(new_vector.x, new_vector.y, new_vector.z)
return new_vector
def test_initial_setup(self):
accel_item_first = (0.0, np.asarray([0.0, 0.0, -9.81]).reshape((3, 1)))
gyro_item_second = (0.0, np.asarray([0.0, 0.0, 0.0]).reshape((3, 1)))
initial_accel_mean = np.zeros((3, 1))
state = State(accel_item_first, gyro_item_second, initial_accel_mean)
state.augment()
q_C_G = state.get_rotation_from_global_to_camera_pose(0)
self.assertEqual(q_C_G, np.quaternion(1.0, 0.0, 0.0, 0.0))
def mean_rotor_in_chordal_metric(R, t=None):
"""Return rotor that is closest to all R in the least-squares sense
This can be done (quasi-)analytically because of the simplicity of
the chordal metric function. The only approximation is the simple
2nd-order discrete formula for the definite integral of the input
rotor function.
Note that the `t` argument is optional. If it is present, the
times are used to weight the corresponding integral. If it is not
present, a simple sum is used instead (which may be slightly
faster).
"""
if not t:
return np.quaternion(*(np.sum(as_float_array(R)))).normalized()
mean = np.empty((4,), dtype=float)
definite_integral(as_float_array(R), t, mean)
return np.quaternion(*mean).normalized()
def test_simple_move_in_x_direction(self):
accel_item_first = (0.0, np.asarray([1.0, 0.0, -9.81]).reshape((3, 1)))
gyro_item_second = (0.0, np.asarray([0.0, 0.0, 0.0]).reshape((3, 1)))
initial_accel_mean = np.zeros((3, 1))
state = State(accel_item_first, gyro_item_second, initial_accel_mean)
accel_item_second = (1.0, np.asarray([1.0, 0.0, -9.81]).reshape((3, 1)))
gyro_item_second = (1.0, np.asarray([0.0, 0.0, 0.0]).reshape((3, 1)))
state = State.propagate(state, accel_item_second, gyro_item_second)
self.assertEqual(state.q_B_G, np.quaternion(1.0, 0.0, 0.0, 0.0))
np.testing.assert_allclose(state.p_B_G, np.asarray([0.5, 0.0, 0.0]).reshape((3, 1)))
np.testing.assert_allclose(state.v_B_G, np.asarray([1.0, 0.0, 0.0]).reshape((3, 1)))
def ImuDef(data):
global q_k, wk
q_k = np.quaternion(data.orientation.w,
data.orientation.x,
data.orientation.y,
data.orientation.z)
wk = np.array([data.angular_velocity.x,
data.angular_velocity.y,
data.angular_velocity.z])
SLAM.Q[5:7,5:7] = np.reshape(data.orientation_covariance, (3,3))[:2,:2]
SLAM.Q[8,8] = np.reshape(data.angular_velocity_covariance, (3,3))[2,2]
def test_transformation_using_sandwich_and_rotation_matrix_are_same(self):
q = np.quaternion(-1, -8, 4, 6).normalized() # random
x = np.asarray([1.0, 2.0, 3.0]).reshape((3, 1)) # random
x = x / x.sum()
x_quat = Quaternion.vector_to_quaternion_form(x)
x_next_quat_form = q * x_quat * q.conjugate()
x_next_using_quaternions = Quaternion.vector_from_quaternion_form(x_next_quat_form)
r = Quaternion.to_rotation_matrix(q)
x_next_using_matrices = r.dot(x)
np.testing.assert_allclose(x_next_using_quaternions, x_next_using_matrices)
def __init__(self):
self._lock = threading.Lock()
self._position = np.array([0, 0, 0], dtype='float32').T
self._orientation = np.quaternion(1, 0, 0, 0)
self._velocity = np.array([0, 0, 0], dtype='float32').T
self._rotational_velocity = np.array([0, 0, 0], dtype='float32')
self._acceleration = np.array([0, 0, 0], dtype='float32').T
self._rotational_acceleration = np.array([0, 0, 0], dtype='float32')
self._props = Props(2.0, 3.0, 2.0, 3.0)
self._running = True
self._clock = pygame.time.Clock()
self._dt = 0.2
def orientation_from_accel(self, acc):
"""Get a quaternion orientation from an accelerometer reading, assuming that the accelerometer correctly
measures the gravitational acceleration"""
acc1 = -acc / np.linalg.norm(acc)
ax, ay, az = acc1
AXZ = ax * np.sqrt(1 - az)
AXY = np.sqrt(ax**2 + ay**2)
q0 = np.quaternion(0, AXZ / (np.sqrt(2)*AXY), ay*AXZ / (np.sqrt(2) * ax * AXY), ax*AXY / (np.sqrt(2)*AXZ))
return q0
def __init__(self, axis, angle):
if isinstance(angle, np.ndarray):
angle = angle[0]
elif isinstance(angle, int):
angle = float(angle)
if not isinstance(angle, float):
raise Exception('angle must be a float')
self.axis = axis.unit()
self.angle = f.convert_to_radians(angle)
_ = self.axis * np.sin(self.angle/2.0)
self.quaternion = np.quaternion(np.cos(self.angle/2.0), _[0], _[1],
_[2])
def rotate(vector, quaternion):
"""Rotates a vector using a quaternion.
Args:
vector: The vector which is to be rotated.
quaternion: The quaternion which describes the rotation.
Returns:
The rotated vector.
"""
vector = np.quaternion(0, vector[0], vector[1], vector[2])
new_vector = quaternion * vector * np.conjugate(quaternion)
new_vector = np.array(new_vector.imag)
return new_vector
def test_SWSH_spin_behavior(Rs, special_angles, ell_max):
# We expect that the SWSHs behave according to
# sYlm( R * exp(gamma*z/2) ) = sYlm(R) * exp(-1j*s*gamma)
# See http://moble.github.io/spherical_functions/SWSHs.html#fn:2
# for a more detailed explanation
# print("")
for i, R in enumerate(Rs):
# print("\t{0} of {1}: R = {2}".format(i, len(Rs), R))
for gamma in special_angles:
for ell in range(ell_max + 1):
for s in range(-ell, ell + 1):
LM = np.array([[ell, m] for m in range(-ell, ell + 1)])
Rgamma = R * np.quaternion(math.cos(gamma / 2.), 0, 0, math.sin(gamma / 2.))
sYlm1 = sf.SWSH(Rgamma, s, LM)
sYlm2 = sf.SWSH(R, s, LM) * cmath.exp(-1j * s * gamma)
# print(R, gamma, ell, s, np.max(np.abs(sYlm1-sYlm2)))
assert np.allclose(sYlm1, sYlm2, atol=ell ** 6 * precision_SWSH, rtol=ell ** 6 * precision_SWSH)
def rotation_to_quaternion(vector, angle):
"""Converts a rotation in terms of an angle and unit vector into a
quaternion.
Args:
axis: The vector in the direction of the axis of rotation.
angle The angle through which the rotation is made. It has to
be passed in degrees.
Returns:
A quaternion that performs the rotation.
"""
axis = unit_vector(vector)
angle = convert_to_radians(angle)
_ = axis * np.sin(angle/2)
q = np.quaternion(np.cos(angle/2), _[0], _[1], _[2])
return q
def _get_filenames_and_classes(dataset_dir):
trajectory_abs = [] #abosolute camera pose
with open(dataset_dir + '/vicon0/sampled_relative.csv') as csvfile:
count = 0
spamreader = csv.reader(csvfile, delimiter=',', quotechar='|')
for row in spamreader:
if count == 0:
count = 1
continue
trajectory_abs.append(row)
print('Total data: ' + str(len(trajectory_abs)))
# Calculate relative pose
trajectory_relative = []
for i in range(len(trajectory_abs) - 1):
#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m],q_RS_w [],q_RS_x [],q_RS_y [],q_RS_z []
timestamp = trajectory_abs[i + 1][0]
X, Y, Z = np.array(
trajectory_abs[i + 1][1:4]).astype(float) - np.array(
trajectory_abs[i][1:4]).astype(float)
ww0, wx0, wy0, wz0 = np.array(trajectory_abs[i][4:]).astype(float)
ww1, wx1, wy1, wz1 = np.array(trajectory_abs[i + 1][4:]).astype(float)
q0 = np.quaternion(ww0, wx0, wy0, wz0)
q1 = np.quaternion(ww1, wx1, wy1, wz1)
relative_rot = quaternion.as_float_array(q1 * q0.inverse())
relative_pose = [
timestamp, X, Y, Z, relative_rot[0], relative_rot[1],
relative_rot[2], relative_rot[3]
]
se3R6 = xyzQuaternion2se3_([relative_pose[1],\
relative_pose[2],\
relative_pose[3],\
relative_pose[4],\
relative_pose[5],\
relative_pose[6],\
relative_pose[7]])
trajectory_relative.append(se3R6)
#print(i)
with open(dataset_dir + '/vicon0/sampled_relative_R6.csv', 'w+') as f:
tmpStr = ",".join(trajectory_abs[0])
f.write(tmpStr + '\n')
for i in range(len(trajectory_relative)):
r1 = float_to_str(trajectory_relative[i][0])
r2 = float_to_str(trajectory_relative[i][1])
r3 = float_to_str(trajectory_relative[i][2])
r4 = float_to_str(trajectory_relative[i][3])
r5 = float_to_str(trajectory_relative[i][4])
r6 = float_to_str(trajectory_relative[i][5])
tmpStr = str(
trajectory_relative[i][0]
) + ',' + r1 + ',' + r2 + ',' + r3 + ',' + r4 + ',' + r5 + ',' + r6
f.write(tmpStr + '\n')
return
ipython = get_ipython()
except AttributeError:
print("This script must be run with `ipython`")
raise
import sys
import numpy as np
import random
import quaternion
import numbapro as nb
from numbapro import *
import spherical_functions as sf
ru = lambda: random.uniform(-1, 1)
q = np.quaternion(ru(), ru(), ru(), ru())
q = q / q.abs()
ells = np.array(range(16 + 1) + [24, 32])
ells = ells[ells <= sf.ell_max]
evals = np.empty_like(ells, dtype=int)
nanoseconds = np.empty_like(ells, dtype=float)
for i, ell_max in enumerate(ells):
indices = np.array(
[[ell, mp, m] for ell in range(ell_max + 1) for mp in range(-ell, ell + 1) for m in range(-ell, ell + 1)],
dtype=int)
elements = np.zeros((indices.shape[0],), dtype=complex)
result = ipython.magic("timeit -o sf._Wigner_D_matrices(q.a, q.b, 0, ell_max, elements)")
evals[i] = len(indices)
nanoseconds[i] = 1e9 * result.best / evals[i]
print("With ell_max={0}, and {1} evaluations, each D component averages {2:.0f} ns".format(ell_max, evals[i],
def quat_prod(p, q):
pq = np.quaternion()
pq.w = p.w * q.w - (p.vec).dot(q.vec)
pq.vec = p.w * q.vec + q.w * p.vec + np.cross(p.vec, q.vec)
return pq
#heading = 280.0 - heading
heading = 165 - yaw #165.0 - yaw
if heading < 0: heading += 360
heading_keep_rad = math.radians(heading)
heading_quantized = round(heading/90) * 90 #-90
heading_rad = math.radians(heading_quantized)
file_heading.write('{:.3f}\n'.format(heading_keep_rad))
if visualise and (time.time() - last_sent_heading_data_time > 0.1):
last_sent_heading_data_time = time.time()
MESSAGE = '{:.3f}\n'.format(heading)
print(MESSAGE)
sock.sendto(bytes(MESSAGE, "utf-8"), (UDP_IP, UDP_PORT))
# local_coordinate_system:
lcs = data["fusionQPose"]
q_lcs = np.quaternion(lcs[0], lcs[1], lcs[2], lcs[3])
# accel quaternion:
acc_lcs = data["accel"]
q_acc = np.quaternion(acc_lcs[0], acc_lcs[1], acc_lcs[2])
# global_coordinate_system:
q_gcs = q_lcs * q_acc * q_lcs.inverse()
acc_gcs = q_gcs.vec
acc_x, acc_y, acc_z = (i * g for i in acc_gcs)
acc_z = (1 - alpha) * acc_z + alpha * sample_old
#start of new possible step
if (acc_z > sample_old) and (acc_z > threshold) and (threshold > sample_old):
start_of_step = time.time()
#check if we have a step
def identity(cls):
return cls(quaternion.one, np.quaternion(0., 0., 0., 0.))
def __init__(self, session_time, x, y, z, w):
super().__init__(session_time)
self.quaternion = np.quaternion(w, x, y, z)
class SystemModel(ABC):
(
OPENGL_FRAME,
SPACECRAFT_FRAME,
ASTEROID_FRAME,
OPENCV_FRAME,
) = range(4)
# from sc cam frame (axis: +x, up: +z) to opengl (axis -z, up: +y)
sc2gl_q = np.quaternion(0.5, 0.5, -0.5, -0.5)
# from opencv cam frame (axis: +z, up: -y) to opengl (axis -z, up: +y)
cv2gl_q = np.quaternion(0, 1, 0, 0)
def __init__(self, asteroid, camera, limits, *args, **kwargs):
super(SystemModel, self).__init__()
self.asteroid = asteroid
self.cam = camera
# mission limits
(
self.min_distance, # min_distance in km
self.min_med_distance, # min_med_distance in km
self.max_med_distance, # max_med_distance in km
self.max_distance, # max_distance in km
self.min_elong, # min_elong in deg
self.min_time # min time instant as astropy.time.Time
) = limits
assert self.min_altitude > 0, \
'min distance %.2fkm too small, possible collision as asteroid max_radius=%.0fm'%(self.min_distance, self.asteroid.max_radius)
self.mission_id = None # overridden by particular missions
self.view_width = VIEW_WIDTH
# spacecraft position relative to asteroid, z towards spacecraft,
# x towards right when looking out from s/c camera, y up
self.x_off = Parameter(-4, 4, estimate=False)
self.y_off = Parameter(-4, 4, estimate=False)
# whole view: 1.65km/tan(2.5deg) = 38km
# can span ~30px: 1.65km/tan(2.5deg * 30/1024) = 1290km
self.z_off = Parameter(-self.max_distance,
-self.min_distance,
def_val=-self.min_med_distance,
is_gl_z=True) # was 120, 220
# spacecraft orientation relative to stars
self.x_rot = Parameter(-90, 90, estimate=False) # axis latitude
self.y_rot = Parameter(-180, 180, estimate=False) # axis longitude
self.z_rot = Parameter(-180, 180, estimate=False) # rotation
# asteroid zero orientation relative to stars
self.ast_x_rot = Parameter(-90, 90, estimate=False) # axis latitude
self.ast_y_rot = Parameter(-180, 180, estimate=False) # axis longitude
self.ast_z_rot = Parameter(-180, 180, estimate=False) # rotation
self.asteroid_rotation_from_model()
# time in seconds since 1970-01-01 00:00:00
self.time = Parameter(self.min_time.unix,
self.min_time.unix +
self.asteroid.rotation_period,
estimate=False)
# override any default params
for n, v in kwargs.items():
setattr(self, n, v)
# set default values to params
for n, p in self.get_params():
p.value = p.def_val
@property
def min_altitude(self):
""" in km """
return self.min_distance - self.asteroid.max_radius / 1000
@property
def view_height(self):
return int(self.cam.height * self.view_width / self.cam.width)
def get_params(self, all=False):
return ((n, getattr(self, n)) for n in sorted(self.__dict__)
if isinstance(getattr(self, n), Parameter) and (
all or getattr(self, n).estimate))
def param_change_events(self, enabled):
for n, p in self.get_params(all=True):
p.fire_change_events = enabled
@property
def spacecraft_pos(self):
return self.x_off.value, self.y_off.value, self.z_off.value
@spacecraft_pos.setter
def spacecraft_pos(self, pos):
self.z_off.value = pos[2]
half_range = abs(pos[2] / 170 * 4)
self.x_off.range = (pos[0] - half_range, pos[0] + half_range)
self.x_off.value = pos[0]
self.y_off.range = (pos[1] - half_range, pos[1] + half_range)
self.y_off.value = pos[1]
@property
def spacecraft_rot(self):
return self.x_rot.value, self.y_rot.value, self.z_rot.value
@spacecraft_rot.setter
def spacecraft_rot(self, r):
self.x_rot.value, self.y_rot.value, self.z_rot.value = r
@property
def asteroid_axis(self):
return self.ast_x_rot.value, self.ast_y_rot.value, self.ast_z_rot.value
@asteroid_axis.setter
def asteroid_axis(self, r):
self.ast_x_rot.value, self.ast_y_rot.value, self.ast_z_rot.value = r
self.update_asteroid_model()
@property
def spacecraft_dist(self):
return math.sqrt(sum(x**2 for x in self.spacecraft_pos))
@property
def spacecraft_altitude(self):
sc_ast_v = tools.normalize_v(np.array(self.spacecraft_pos))
ast_vx = self.sc_asteroid_vertices()
min_distance = np.min(sc_ast_v.dot(ast_vx.T))
return min_distance
@property
def real_spacecraft_altitude(self):
sc_ast_v = tools.normalize_v(np.array(self.real_spacecraft_pos))
ast_vx = self.sc_asteroid_vertices(real=True)
min_distance = np.min(sc_ast_v.dot(ast_vx.T))
return min_distance
def asteroid_rotation_from_model(self):
self.ast_x_rot.value = math.degrees(self.asteroid.axis_latitude)
self.ast_y_rot.value = math.degrees(self.asteroid.axis_longitude)
self.ast_z_rot.value = (math.degrees(self.asteroid.rotation_pm) +
180) % 360 - 180
def update_asteroid_model(self):
self.asteroid.axis_latitude = math.radians(self.ast_x_rot.value)
self.asteroid.axis_longitude = math.radians(self.ast_y_rot.value)
self.asteroid.rotation_pm = math.radians(self.ast_z_rot.value)
def pixel_extent(self, distance=None):
distance = abs(self.z_off) if distance is None else distance
return self.cam.width * math.atan(
self.asteroid.mean_radius / 1000 / distance) * 2 / math.radians(
self.cam.x_fov)
@property
def real_spacecraft_pos(self):
return self.x_off.real_value, self.y_off.real_value, self.z_off.real_value
@real_spacecraft_pos.setter
def real_spacecraft_pos(self, rv):
self.x_off.real_value, self.y_off.real_value, self.z_off.real_value = rv
@property
def real_spacecraft_rot(self):
return self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value
@real_spacecraft_rot.setter
def real_spacecraft_rot(self, rv):
self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value = rv
@property
def real_asteroid_axis(self):
return self.ast_x_rot.real_value, self.ast_y_rot.real_value, self.ast_z_rot.real_value
@real_asteroid_axis.setter
def real_asteroid_axis(self, rv):
self.ast_x_rot.real_value, self.ast_y_rot.real_value, self.ast_z_rot.real_value = rv
@property
def spacecraft_q(self):
return tools.ypr_to_q(*list(
map(math.radians, (self.x_rot.value, self.y_rot.value,
self.z_rot.value))))
@spacecraft_q.setter
def spacecraft_q(self, new_q):
self.x_rot.value, self.y_rot.value, self.z_rot.value = \
list(map(math.degrees, tools.q_to_ypr(new_q)))
@property
def real_spacecraft_q(self):
return tools.ypr_to_q(*list(
map(math.radians, (self.x_rot.real_value, self.y_rot.real_value,
self.z_rot.real_value))))
@real_spacecraft_q.setter
def real_spacecraft_q(self, new_q):
self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value = \
list(map(math.degrees, tools.q_to_ypr(new_q)))
@property
def asteroid_q(self):
return self.asteroid.rotation_q(self.time.value)
@asteroid_q.setter
def asteroid_q(self, new_q):
ast = self.asteroid
sc2ast_q = SystemModel.frm_conv_q(SystemModel.SPACECRAFT_FRAME,
SystemModel.ASTEROID_FRAME,
ast=ast)
ast.axis_latitude, ast.axis_longitude, new_theta = tools.q_to_ypr(
new_q * sc2ast_q)
old_theta = ast.rotation_theta(self.time.value)
ast.rotation_pm = tools.wrap_rads(ast.rotation_pm + new_theta -
old_theta)
self.asteroid_rotation_from_model()
@property
def real_asteroid_q(self):
org_ast_axis = self.asteroid_axis
self.asteroid_axis = self.real_asteroid_axis
q = self.asteroid.rotation_q(self.time.real_value)
self.asteroid_axis = org_ast_axis
return q
@real_asteroid_q.setter
def real_asteroid_q(self, new_q):
org_ast_axis = self.asteroid_axis
self.asteroid_axis = self.real_asteroid_axis
self.asteroid_q = new_q
self.asteroid_axis = org_ast_axis
def gl_sc_asteroid_rel_q(self, discretize_tol=False):
""" rotation of asteroid relative to spacecraft in opengl coords """
assert not discretize_tol, 'discretize_tol deprecated at gl_sc_asteroid_rel_q function'
self.update_asteroid_model()
sc_ast_rel_q = SystemModel.sc2gl_q.conj() * self.sc_asteroid_rel_q()
if discretize_tol:
qq, _ = tools.discretize_q(sc_ast_rel_q, discretize_tol)
err_q = sc_ast_rel_q * qq.conj()
sc_ast_rel_q = qq
if not BATCH_MODE and DEBUG:
print('asteroid x-axis: %s' %
tools.q_times_v(sc_ast_rel_q, np.array([1, 0, 0])))
return sc_ast_rel_q, err_q if discretize_tol else False
def sc_asteroid_rel_q(self, time=None):
""" rotation of asteroid relative to spacecraft in spacecraft coords """
ast_q = self.asteroid.rotation_q(time or self.time.value)
sc_q = self.spacecraft_q
return sc_q.conj() * ast_q
def real_sc_asteroid_rel_q(self):
org_sc_rot = self.spacecraft_rot
org_ast_axis = self.asteroid_axis
self.spacecraft_rot = self.real_spacecraft_rot
self.asteroid_axis = self.real_asteroid_axis
q_tot = self.sc_asteroid_rel_q(time=self.time.real_value)
self.spacecraft_rot = org_sc_rot
self.asteroid_axis = org_ast_axis
return q_tot
def rotate_spacecraft(self, q):
new_q = self.spacecraft_q * q
self.x_rot.value, self.y_rot.value, self.z_rot.value = \
list(map(math.degrees, tools.q_to_ypr(new_q)))
def rotate_asteroid(self, q):
""" rotate asteroid in spacecraft frame """
# global rotation q on asteroid in sc frame, followed by local rotation to asteroid frame
new_q = q * self.asteroid.rotation_q(self.time.value)
self.asteroid_q = new_q
def reset_to_real_vals(self):
for n, p in self.get_params(True):
assert p.real_value is not None, 'real value missing for %s' % n
p.value = p.real_value
def swap_values_with_real_vals(self):
for n, p in self.get_params(True):
assert p.real_value is not None, 'real value missing for %s' % n
assert p.value is not None, 'current value missing %s' % n
tmp = p.value
p.value = p.real_value
p.real_value = tmp
def calc_shift_err(self):
est_vertices = self.sc_asteroid_vertices()
self.swap_values_with_real_vals()
target_vertices = self.sc_asteroid_vertices()
self.swap_values_with_real_vals()
return tools.sc_asteroid_max_shift_error(est_vertices, target_vertices)
def sc_asteroid_vertices(self, real=False):
""" asteroid vertices rotated and translated to spacecraft frame """
if self.asteroid.real_shape_model is None:
return None
sc_ast_q = self.real_sc_asteroid_rel_q(
) if real else self.sc_asteroid_rel_q()
sc_pos = self.real_spacecraft_pos if real else self.spacecraft_pos
return tools.q_times_mx(
sc_ast_q, np.array(
self.asteroid.real_shape_model.vertices)) + sc_pos
def gl_light_rel_dir(self, err_q=False, discretize_tol=False):
""" direction of light relative to spacecraft in opengl coords """
assert not discretize_tol, 'discretize_tol deprecated at gl_light_rel_dir function'
light_v, err_angle = self.light_rel_dir(err_q=False,
discretize_tol=False)
err_q = (err_q or np.quaternion(1, 0, 0, 0))
light_gl_v = tools.q_times_v(err_q.conj() * SystemModel.sc2gl_q.conj(),
light_v)
# new way to discretize light, consistent with real fdb inplementation
if discretize_tol:
dlv, _ = tools.discretize_v(
light_gl_v,
discretize_tol,
lat_range=(-math.pi / 2, math.radians(90 - self.min_elong)))
err_angle = tools.angle_between_v(light_gl_v, dlv)
light_gl_v = dlv
return light_gl_v, err_angle
def light_rel_dir(self, err_q=False, discretize_tol=False):
""" direction of light relative to spacecraft in s/c coords """
assert not discretize_tol, 'discretize_tol deprecated at light_rel_dir function'
light_v = tools.normalize_v(self.asteroid.position(self.time.value))
sc_q = self.spacecraft_q
err_q = (err_q or np.quaternion(1, 0, 0, 0))
# old, better way to discretize light, based on asteroid rotation axis, now not in use
if discretize_tol:
ast_q = self.asteroid_q
light_ast_v = tools.q_times_v(ast_q.conj(), light_v)
dlv, _ = tools.discretize_v(light_ast_v, discretize_tol)
err_angle = tools.angle_between_v(light_ast_v, dlv)
light_v = tools.q_times_v(ast_q, dlv)
return tools.q_times_v(err_q.conj() * sc_q.conj(), light_v),\
err_angle if discretize_tol else False
def solar_elongation(self, real=False):
ast_v = self.asteroid.position(
self.time.real_value if real else self.time.value)
sc_q = self.real_spacecraft_q if real else self.spacecraft_q
elong, direc = tools.solar_elongation(ast_v, sc_q)
if not BATCH_MODE and DEBUG:
print('elong: %.3f | dir: %.3f' %
(math.degrees(elong), math.degrees(direc)))
return elong, direc
def rel_rot_err(self):
return tools.angle_between_q(self.sc_asteroid_rel_q(),
self.real_sc_asteroid_rel_q())
def lat_pos_err(self):
real_pos = self.real_spacecraft_pos
err = np.subtract(self.spacecraft_pos, real_pos)
return math.sqrt(err[0]**2 + err[1]**2) / abs(real_pos[2])
def dist_pos_err(self):
real_d = self.real_spacecraft_pos[2]
return abs(self.spacecraft_pos[2] - real_d) / abs(real_d)
def calc_visibility(self, pos=None):
if pos is None:
pos = self.spacecraft_pos
if isinstance(pos, np.ndarray):
pos = pos.reshape((-1, 3))
return_array = True
else:
pos = np.array([pos], shape=(1, 3))
return_array = False
rad = self.asteroid.mean_radius * 0.001
xt = np.abs(pos[:, 2]) * math.tan(math.radians(self.cam.x_fov) / 2)
yt = np.abs(pos[:, 2]) * math.tan(math.radians(self.cam.y_fov) / 2)
# xm = np.clip((xt - (abs(pos[0])-rad))/rad/2, 0, 1)
# ym = np.clip((yt - (abs(pos[1])-rad))/rad/2, 0, 1)
xm = 1 - np.minimum(1, (np.maximum(0, pos[:, 0] + rad - xt) +
np.maximum(0, rad - pos[:, 0] - xt)) / rad / 2)
ym = 1 - np.minimum(1, (np.maximum(0, pos[:, 1] + rad - yt) +
np.maximum(0, rad - pos[:, 1] - yt)) / rad / 2)
visib = xm * ym * 100
return visib if return_array else visib[0]
def export_state(self, filename):
""" saves state in an easy to access format """
qn = ('w', 'x', 'y', 'z')
vn = ('x', 'y', 'z')
lines = [['type'] + ['ast_q' + i
for i in qn] + ['sc_q' + i for i in qn] +
['ast_sc_v' + i for i in vn] + ['sun_ast_v' + i for i in vn]]
for t in ('initial', 'real'):
# if settings.USE_ICRS, all in solar system barycentric equatorial frame
ast_q = self.asteroid.rotation_q(self.time.value)
sc_q = self.spacecraft_q
ast_sc_v = tools.q_times_v(sc_q, self.spacecraft_pos)
sun_ast_v = self.asteroid.position(self.time.value)
lines.append((t, ) + tuple(
'%f' % f
for f in (tuple(ast_q.components) + tuple(sc_q.components) +
tuple(ast_sc_v) + tuple(sun_ast_v))))
self.swap_values_with_real_vals()
with open(filename, 'w') as f:
f.write('\n'.join(['\t'.join(l) for l in lines]))
def save_state(self, filename, printout=False):
config = configparser.ConfigParser()
filename = filename + ('.lbl' if len(filename) < 5
or filename[-4:] != '.lbl' else '')
config.read(filename)
if not config.has_section('main'):
config.add_section('main')
if not config.has_section('real'):
config.add_section('real')
for n, p in self.get_params(all=True):
config.set('main', n, str(p.value))
if p.real_value is not None:
config.set('real', n, str(p.real_value))
if self.asteroid.real_position is not None:
config.set('real', 'sun_asteroid_pos',
str(self.asteroid.real_position))
if not printout:
with open(filename, 'w') as f:
config.write(f)
else:
config.write(sys.stdout)
def load_state(self, filename, sc_ast_vertices=False):
if not os.path.isfile(filename):
raise FileNotFoundError(filename)
config = configparser.ConfigParser()
filename = filename + ('.lbl' if len(filename) < 5
or filename[-4:] != '.lbl' else '')
config.read(filename)
for n, p in self.get_params(all=True):
v = float(config.get('main', n))
if n == 'time':
rp = self.asteroid.rotation_period
p.range = (v - rp / 2, v + rp / 2)
p.value = v
rv = config.get('real', n, fallback=None)
if rv is not None:
p.real_value = float(rv)
rv = config.get('real', 'sun_asteroid_pos', fallback=None)
if rv is not None:
self.asteroid.real_position = np.fromstring(rv[1:-1],
dtype=np.float,
sep=' ')
assert np.isclose(self.time.value, float(config.get('main', 'time'))), \
'Failed to set time value: %s vs %s'%(self.time.value, float(config.get('main', 'time')))
self.update_asteroid_model()
if sc_ast_vertices:
# get real relative position of asteroid model vertices
self.asteroid.real_sc_ast_vertices = self.sc_asteroid_vertices(
real=True)
@staticmethod
def frm_conv_q(fsrc, fdst, ast=None):
fqm = {
SystemModel.OPENGL_FRAME:
np.quaternion(1, 0, 0, 0),
SystemModel.OPENCV_FRAME:
SystemModel.cv2gl_q,
SystemModel.SPACECRAFT_FRAME:
SystemModel.sc2gl_q,
SystemModel.ASTEROID_FRAME:
None if ast is None else ast.ast2sc_q * SystemModel.sc2gl_q,
}
return fqm[fsrc] * fqm[fdst].conj()
def __repr__(self):
return ('system state:\n\t%s\n' + '\nsolar elongation: %s\n' +
'\nasteroid rotation: %.2f\n') % (
'\n\t'.join('%s = %s' % (n, p)
for n, p in self.get_params(all=True)),
tuple(map(math.degrees, self.solar_elongation())),
math.degrees(self.asteroid.rotation_theta(
self.time.value)),
)
dm_body_stream['right_knee'].append(
-AngleBetweenBodyPos(r_hip_pos, r_knee_pos, r_ankle_pos))
dm_body_stream['left_knee'].append(
-AngleBetweenBodyPos(l_hip_pos, l_knee_pos, l_ankle_pos))
dm_body_stream['right_elbow'].append(
AngleBetweenBodyPos(r_shoulder_pos, r_elbow_pos, r_wrist_pos) -
np.pi / 2)
dm_body_stream['left_elbow'].append(
AngleBetweenBodyPos(l_shoulder_pos, l_elbow_pos, l_wrist_pos) -
np.pi / 2)
return dm_body_stream
unit_quat = np.quaternion(1, 0, 0, 0)
def AngleBetweenQuat(quat1, quat2):
knee_rot = (quat1.inverse() * unit_quat) * quat1
ankle_rot = (quat2.inverse() * unit_quat) * quat2
knee_rot_vec = np.array([knee_rot.w, knee_rot.x, knee_rot.y, knee_rot.z])
ankle_rot_vec = np.array(
[ankle_rot.w, ankle_rot.x, ankle_rot.y, ankle_rot.z])
angle = np.arccos(
knee_rot_vec.dot(ankle_rot_vec) /
(np.linalg.norm(knee_rot_vec) * np.linalg.norm(ankle_rot_vec)))
return angle
def AngleBetweenBodyPos(pos0, pos1, pos2):
def __init__(self,
noise_scale=1,
position_noise: Union[None, Dict, Sequence] = None,
velocity_noise: Union[None, Dict, Sequence] = None,
rotation_noise: Union[None, Dict, Sequence] = None,
angular_velocity_noise: Union[None, Dict, Sequence] = None,
propeller_speed_noise: Union[None, Dict, Sequence] = None):
"""
Constructs a new noise state map with the given parameters.
Note:
The gaussian noises can be specified in a number of ways
1. `std`
2. `(mean, std)`
3. `dict(mean=your_mean, std=your_std)`
The mean and standard deviation can be either scalar or match the dimension as the state part they are
applied to.
:param noise_scale: Global scale multiplied to all noises. Default is 1.
:param position_noise: The noise to apply to the position.
:param velocity_noise: The noise to apply to the velocity.
:param rotation_noise: The noise to apply to the rotation.
:param angular_velocity_noise: The noise to apply to the angular velocity.
:param propeller_speed_noise: The noise to apply to the propeller speed.
"""
if position_noise is not None:
position_mean, position_std = _parse_noise_spec(position_noise)
self.noised_position = lambda state: state.position + \
np.random.randn(3) * position_std * noise_scale + position_mean
else:
self.noised_position = lambda state: state.position
if velocity_noise is not None:
velocity_mean, velocity_std = _parse_noise_spec(velocity_noise)
self.noised_velolicty = lambda state: state.velocity + \
np.random.randn(3) * velocity_std * noise_scale + velocity_mean
else:
self.noised_velolicty = lambda state: state.velocity
if angular_velocity_noise is not None:
angular_velocity_mean, angular_velocity_std = _parse_noise_spec(
angular_velocity_noise)
self.noised_angular_velocity = lambda state: state.angular_velocity + \
np.random.randn(3) * angular_velocity_std * noise_scale + \
angular_velocity_mean
else:
self.noised_angular_velocity = lambda state: state.angular_velocity
if propeller_speed_noise is not None:
propeller_speed_mean, propeller_speed_std = _parse_noise_spec(
propeller_speed_noise)
self.noised_propeller_speed = lambda state: state.propeller_speed + \
np.random.randn(4) * propeller_speed_std * noise_scale + \
propeller_speed_mean
else:
self.noised_propeller_speed = lambda state: state.propeller_speed
if rotation_noise is not None:
rotation_mean, rotation_std = _parse_noise_spec(rotation_noise)
self.noised_rotation = lambda state: np.quaternion(
*(state.rotation.components + np.random.randn(4) * rotation_std
* noise_scale + rotation_mean))
else:
self.noised_rotation = lambda state: state.rotation
def update(self, gyro, accel, mag):
"""
Perform one update step with data from a AHRS sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
:param magnetometer: A three-element array containing the magnetometer data. Can be any unit since a normalized value is used.
:return:
"""
qnp = self.quaternion
q = quaternion.as_float_array(self.quaternion)
gyroscope = gyro.flatten()
accelerometer = accel.flatten()
magnetometer = mag.flatten()
if norm(magnetometer) > 200.:
raise ValueError("magnetometer value a bit off")
if norm(accelerometer) > 30.:
raise ValueError("accelerometer value a bit off")
# Normalise accelerometer measurement
if norm(accelerometer) is 0 or norm(accelerometer) is np.nan:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Normalise magnetometer measurement
if norm(magnetometer) is 0 or norm(magnetometer) is np.nan:
warnings.warn("magnetometer is zero")
return
magnetometer /= norm(magnetometer)
# magnetometer reading in Earth's frame quaternion
# Sources of interference fixed in the sensor frame,
# termed hard iron biases, can be removed through calibration
h = qnp * np.quaternion(0, *magnetometer) * qnp.conj()
# the following is based on assumption that magnetic field is in the direction of the north
# (no East/West component), but does have a vertical component (inclination). This is not to
# manipulate the measurement, but to obtain the inclination of the field as the reference.
# See section III.D
b = np.array([0, norm(h.imag[0:2]), 0, h.z])
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - magnetometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (qnp * np.quaternion(0, *gyroscope)) * 0.5 - np.quaternion(
*(self.beta * step))
# Integrate to yield quaternion
qnp += qdot * self.samplePeriod
self.quaternion = qnp.normalized() # normalise quaternion
def even_permutations(a, b, c, d):
vertices = []
vertices.append(np.quaternion(a, b, c, d))
vertices.append(np.quaternion(a, c, d, b))
vertices.append(np.quaternion(a, d, b, c))
vertices.append(np.quaternion(b, a, d, c))
vertices.append(np.quaternion(b, d, c, a))
vertices.append(np.quaternion(b, c, a, d))
vertices.append(np.quaternion(c, d, a, b))
vertices.append(np.quaternion(c, a, b, d))
vertices.append(np.quaternion(c, b, d, a))
vertices.append(np.quaternion(d, c, b, a))
vertices.append(np.quaternion(d, b, a, c))
vertices.append(np.quaternion(d, a, c, b))
return vertices
def from_q_to_6d(q):
q = np.quaternion(q[0], q[1], q[2], q[3])
mat = quaternion.as_rotation_matrix(q) # 3x3
rep6d = mat[:, 0:2].transpose().reshape(-1, 6) # 6
return rep6d
def get_quaternion(self): """Returns the quaternion (q0,q1,q2,q3) """ return np.quaternion(self.q0,self.q1,self.q2,self.q3).normalized()
SLAM.update(meas)
header = Header()
header.seq = i
i += 1
header.stamp = rospy.Time.now()
GMM = GaussianMixtureModel()
for mean, var, weight in zip(SLAM.map.means_, SLAM.map.covars_, SLAM.map.weights_):
gaussian = GaussianModel()
for m in mean:
gaussian.mean.append(m)
for v1 in var:
row = CovarianceRow()
for v2 in v1:
row.rows.append(v2)
gaussian.covariance.append(row)
GMM.gaussians.append(gaussian)
GMM.weights.append(weight)
map_pub.publish(GMM)
pose = PoseStamped()
pose.header = header
pose.pose.position.x = SLAM.particles[:,0].mean()
pose.pose.position.y = SLAM.particles[:,1].mean()
pose.pose.position.z = SLAM.particles[:,2].mean()
q = np.quaternion(0, SLAM.particles[:,5].mean(), SLAM.particles[:,6].mean(), SLAM.particles[:,7].mean())
q = np.exp(q/2)
pose.pose.orientation.w = q.real
pose.pose.orientation.x = q.vec[0]
pose.pose.orientation.y = q.vec[1]
pose.pose.orientation.z = q.vec[2]
pose_pub.publish(pose)
rate.sleep()
def identity():
return np.quaternion(1, 0, 0, 0)
def VecToQuat(vec):
return np.quaternion(vec[0], vec[1], vec[2], vec[3])
def test_pyrobot_noisy_actions(noise_multiplier, robot, controller):
np.random.seed(0)
scene_graph = hsim.SceneGraph()
agent_config = habitat_sim.AgentConfiguration()
agent_config.action_space = dict(
noisy_move_backward=habitat_sim.ActionSpec(
"pyrobot_noisy_move_backward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=0.25,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_move_forward=habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=0.25,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_turn_left=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(
amount=10.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_turn_right=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(
amount=10.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
move_backward=habitat_sim.ActionSpec(
"move_backward", habitat_sim.ActuationSpec(amount=0.25)),
move_forward=habitat_sim.ActionSpec(
"move_forward", habitat_sim.ActuationSpec(amount=0.25)),
turn_left=habitat_sim.ActionSpec(
"turn_left", habitat_sim.ActuationSpec(amount=10.0)),
turn_right=habitat_sim.ActionSpec(
"turn_right", habitat_sim.ActuationSpec(amount=10.0)),
)
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(),
agent_config)
for base_action in set(
act.replace("noisy_", "") for act in agent_config.action_space):
state = agent.state
state.rotation = np.quaternion(1, 0, 0, 0)
agent.state = state
agent.act(base_action)
base_state = agent.state
base_translation_delta = _delta_translation(state, base_state)
base_rotation_delta = _delta_rotation(state, base_state)
delta_translations = []
delta_rotations = []
for _ in range(300):
agent.state = state
agent.act(f"noisy_{base_action}")
noisy_state = agent.state
delta_translations.append(base_translation_delta -
_delta_translation(state, noisy_state))
delta_rotations.append(base_rotation_delta -
_delta_rotation(state, noisy_state))
delta_translations = np.stack(delta_translations)
delta_rotations = np.stack(delta_rotations)
if "move" in base_action:
noise_model = pyrobot_noise_models[robot][controller].linear_motion
else:
noise_model = pyrobot_noise_models[robot][
controller].rotational_motion
assert (np.linalg.norm(noise_model.linear.mean * noise_multiplier -
np.abs(delta_translations.mean(0))) < 5e-2)
assert (np.linalg.norm(noise_model.rotation.mean * noise_multiplier -
np.abs(delta_rotations.mean(0))) < 5e-2)
assert (np.linalg.norm(noise_model.linear.cov * noise_multiplier -
np.diag(delta_translations.std(0)**2)) < 5e-2)
assert (np.linalg.norm(noise_model.rotation.cov * noise_multiplier -
(delta_rotations.std(0)**2)) < 5e-2)
import numpy as np
from numpy.random import random
from numpy import sin
from numpy import cos
from numpy import pi
import quaternion
def mySlerp(t, q0, qf):
teta = np.linalg.norm(quaternion.as_rotation_vector((q0.conjugate() * qf)))
t0 = 0
tf = 10
dt = (t - t0) / (tf - t0)
quat_slerp = (sin(teta / 2 *
(1 - dt)) * q0 + sin(teta / 2 * dt) * qf) / sin(teta / 2)
return quat_slerp
q0 = np.quaternion(0, 0, 0, 0)
qf = np.quaternion(pi / 2, 0, 0, 0)
q = mySlerp(10, q0, qf)
print(q)
for line in allLines:
if flag == 0:
line = line.split('\n')[0]
items = line.split(' ')
IMAGE_ID = int(items[0])
QW = float(items[1])
QX = float(items[2])
QY = float(items[3])
QZ = float(items[4])
TX = float(items[5])
TY = float(items[6])
TZ = float(items[7])
CAMERA_ID = [int(items[8])]
NAME = items[9]
rotationMat = qt.as_rotation_matrix(np.quaternion(QW, QX, QY, QZ))
translationMat = np.asarray([TX, TY, TZ]).reshape(3, 1)
transformationMat = np.hstack((rotationMat, translationMat))
image[NAME] = transformationMat
flag = 1
else:
allthings = line.split(' ')
points = []
index = 0
while index < (len(allthings) - 2):
px = float(allthings[index])
py = float(allthings[index + 1])
cons = int(allthings[index + 2])
points.append([px, py, cons])
index = index + 3
__all__ = ['quaternion', 'from_spherical_coords', 'from_euler_angles',
'rotor_intrinsic_distance', 'rotor_chordal_distance',
'rotation_intrinsic_distance', 'rotation_chordal_distance',
'slerp', 'squad_evaluate',
'zero', 'one', 'x', 'y', 'z',
'as_float_array', 'as_quat_array', 'as_spinor_array',
'squad', 'derivative', 'definite_integral', 'indefinite_integral']
if 'quaternion' in np.__dict__:
raise RuntimeError('The NumPy package already has a quaternion type')
np.quaternion = quaternion
np.typeDict['quaternion'] = np.dtype(quaternion)
zero = np.quaternion(0, 0, 0, 0)
one = np.quaternion(1, 0, 0, 0)
x = np.quaternion(0, 1, 0, 0)
y = np.quaternion(0, 0, 1, 0)
z = np.quaternion(0, 0, 0, 1)
def as_float_array(a):
"""View the quaternion array as an array of floats
This function is fast (of order 1 microsecond) because no data is
copied; the returned quantity is just a "view" of the original.
The output view has one more dimension (of size 4) than the input
array, but is otherwise the same shape.
def evaluate(projectdir, filename_cad_appearance, filename_annotations):
appearances_cad = JSONHelper.read(filename_cad_appearance)
benchmark_per_scan = collections.defaultdict(lambda : collections.defaultdict(lambda : 0)) # <-- benchmark_per_scan
benchmark_per_class = collections.defaultdict(lambda : collections.defaultdict(lambda : 0)) # <-- benchmark_per_class
if opt.dataset == "scannet":
catid2catname = get_top8_classes_scannet()
groundtruth = {}
cad2info = {}
idscan2trs = {}
testscenes = [os.path.basename(f).split(".")[0] for f in glob.glob(projectdir + "/*.csv")]
testscenes_gt = []
for r in JSONHelper.read(filename_annotations):
id_scan = r["id_scan"]
# NOTE: remove this
if id_scan not in testscenes:
continue
# <-
testscenes_gt.append(id_scan)
idscan2trs[id_scan] = r["trs"]
for model in r["aligned_models"]:
id_cad = model["id_cad"]
catid_cad = model["catid_cad"]
catname_cad = catid2catname[catid_cad]
model["n_total"] = len(r["aligned_models"])
groundtruth.setdefault((id_scan, catid_cad),[]).append(model)
cad2info[(catid_cad, id_cad)] = {"sym" : model["sym"], "catname" : catname_cad}
benchmark_per_class[catname_cad]["n_total"] += 1
benchmark_per_scan[id_scan]["n_total"] += 1
projectname = os.path.basename(os.path.normpath(projectdir))
# Iterate through your alignments
counter = 0
for file0 in glob.glob(projectdir + "/*.csv"):
alignments = CSVHelper.read(file0)
id_scan = os.path.basename(file0.rsplit(".", 1)[0])
if id_scan not in testscenes_gt:
continue
benchmark_per_scan[id_scan]["seen"] = 1
appearance_counter = {}
for alignment in alignments: # <- multiple alignments of same object in scene
# -> read from .csv file
catid_cad = alignment[0]
id_cad = alignment[1]
cadkey = catid_cad + "_" + id_cad
#import pdb; pdb.set_trace()
if cadkey in appearances_cad[id_scan]:
n_appearances_allowed = appearances_cad[id_scan][cadkey] # maximum number of appearances allowed
else:
n_appearances_allowed = 0
appearance_counter.setdefault(cadkey, 0)
if appearance_counter[cadkey] >= n_appearances_allowed:
continue
appearance_counter[cadkey] += 1
catname_cad = cad2info[(catid_cad, id_cad)]["catname"]
sym = cad2info[(catid_cad, id_cad)]["sym"]
t = np.asarray(alignment[2:5], dtype=np.float64)
q0 = np.asarray(alignment[5:9], dtype=np.float64)
q = np.quaternion(q0[0], q0[1], q0[2], q0[3])
s = np.asarray(alignment[9:12], dtype=np.float64)
# <-
key = (id_scan, catid_cad) # <-- key to query the correct groundtruth models
for idx, model_gt in enumerate(groundtruth[key]):
is_same_class = model_gt["catid_cad"] == catid_cad # <-- is always true (because the way the 'groundtruth' was created
if is_same_class: # <-- proceed only if candidate-model and gt-model are in same class
Mscan = SE3.compose_mat4(idscan2trs[id_scan]["translation"], idscan2trs[id_scan]["rotation"], idscan2trs[id_scan]["scale"])
Mcad = SE3.compose_mat4(model_gt["trs"]["translation"], model_gt["trs"]["rotation"], model_gt["trs"]["scale"], -np.array(model_gt["center"]))
t_gt, q_gt, s_gt = SE3.decompose_mat4(np.dot(np.linalg.inv(Mscan), Mcad))
error_translation = np.linalg.norm(t - t_gt, ord=2)
error_scale = 100.0*np.abs(np.mean(s/s_gt) - 1)
# --> resolve symmetry
if sym == "__SYM_ROTATE_UP_2":
m = 2
tmp = [calc_rotation_diff(q, q_gt*quaternion.from_rotation_vector([0, (i*2.0/m)*np.pi, 0])) for i in range(m)]
error_rotation = np.min(tmp)
elif sym == "__SYM_ROTATE_UP_4":
m = 4
tmp = [calc_rotation_diff(q, q_gt*quaternion.from_rotation_vector([0, (i*2.0/m)*np.pi, 0])) for i in range(m)]
error_rotation = np.min(tmp)
elif sym == "__SYM_ROTATE_UP_INF":
m = 36
tmp = [calc_rotation_diff(q, q_gt*quaternion.from_rotation_vector([0, (i*2.0/m)*np.pi, 0])) for i in range(m)]
error_rotation = np.min(tmp)
else:
error_rotation = calc_rotation_diff(q, q_gt)
# -> define Thresholds
threshold_translation = 0.2 # <-- in meter
threshold_rotation = 20 # <-- in deg
threshold_scale = 20 # <-- in %
# <-
is_valid_transformation = error_translation <= threshold_translation and error_rotation <= threshold_rotation and error_scale <= threshold_scale
counter += 1
if is_valid_transformation:
benchmark_per_scan[id_scan]["n_good"] += 1
benchmark_per_class[catname_cad]["n_good"] += 1
del groundtruth[key][idx]
break
print("***********")
benchmark_per_scan = sorted(benchmark_per_scan.items(), key=lambda x: x[1]["n_good"], reverse=True)
total_accuracy = {"n_good" : 0, "n_total" : 0, "n_scans" : 0}
for k, v in benchmark_per_scan:
if "seen" in v:
total_accuracy["n_good"] += v["n_good"]
total_accuracy["n_total"] += v["n_total"]
total_accuracy["n_scans"] += 1
print("id-scan: {:>20s} \t n-cads-positive: {:>4d} \t n-cads-total: {:>4d} \t accuracy: {:>4.4f}".format(k, v["n_good"], v["n_total"], float(v["n_good"])/v["n_total"]))
instance_mean_accuracy = float(total_accuracy["n_good"])/total_accuracy["n_total"]
print("instance-mean-accuracy: {:>4.4f} \t n-cads-positive: {:>4d} \t n-cads-total: {:>4d} \t n-total-scans: {:>4d}".format(instance_mean_accuracy, total_accuracy["n_good"], total_accuracy["n_total"], total_accuracy["n_scans"]))
print("*********** PER CLASS **************************")
accuracy_per_class = {}
for k,v in benchmark_per_class.items():
accuracy_per_class[k] = float(v["n_good"])/v["n_total"]
print("category-name: {:>20s} \t n-cads-positive: {:>4d} \t n-cads-total: {:>4d} \t accuracy: {:>4.4f}".format(k, v["n_good"], v["n_total"], float(v["n_good"])/v["n_total"]))
class_mean_accuracy = np.mean([ v for k,v in accuracy_per_class.items()])
print("class-mean-accuracy: {:>4.4f}".format(class_mean_accuracy))
return instance_mean_accuracy, class_mean_accuracy
time.sleep(0.2)
continue
_line = line_to_array(_line)
_data_list.append((_line[sig_start: (sig_start + n_signals)]))
if len(_data_list) > win_size:
print("+++++++++++++++++++++++++++++++++++ WINDOW FULL")
_win_data = np.array(_data_list[0:win_size])
ts = _win_data[:, 0]
acc = _win_data[:, 2:5]
#print(acc)
gyro = _win_data[:, 5:8]
mag = _win_data[:, 8:11]
norm_acceleration = np.zeros((len(acc), 1))
for i in range(gyro.shape[0]):
""" GYROSCOPE"""
q_gyro = np.quaternion(gyro[i, 0], gyro[i, 1], gyro[i, 2])
q1 = q_gyro.components[0]
q2 = q_gyro.components[1]
q3 = q_gyro.components[2]
q4 = q_gyro.components[3]
# Auxiliary variables to avoid repeated arithmetic
_2q1 = 2 * q1
_2q2 = 2 * q2
_2q3 = 2 * q3
_2q4 = 2 * q4
_2q1q3 = 2 * q1 * q3
_2q3q4 = 2 * q3 * q4
q1q1 = q1 * q1
q1q2 = q1 * q2
q1q3 = q1 * q3
q1q4 = q1 * q4
import numpy as np import rotation as r x = np.quaternion(1,2,-3,4) print help(np.quaternion) print help(r)
def wxyz_class_to_np_quaternion(xyz_class):
return np.quaternion(xyz_class.w, xyz_class.x, xyz_class.y, xyz_class.z)
class Stars:
# from VizieR catalogs:
SOURCE_HIPPARCHOS = 'H' # I/239/hip_main
SOURCE_PASTEL = 'P' # B/pastel/pastel
SOURCE_WU = 'W' # J/A+A/525/A71/table2
SOURCE_GAIA1 = 'G' # J/MNRAS/471/770/table2
STARDB_TYC = os.path.join(DATA_DIR, 'deep_space_objects_tyc.sqlite')
STARDB_HIP = os.path.join(DATA_DIR, 'deep_space_objects_hip.sqlite')
STARDB = STARDB_HIP
MAG_CUTOFF = 10
MAG_V_LAM0 = 545e-9
SUN_MAG_V = -26.74
SUN_MAG_B = 0.6222 + SUN_MAG_V
# from sc cam frame (axis: +x, up: +z) to equatorial frame (axis: +y, up: +z)
sc2ec_q = np.quaternion(1, 0, 0, 1).normalized().conj()
@staticmethod
def black_body_radiation(Teff, lam):
return Stars.black_body_radiation_fn(Teff)(lam)
@staticmethod
def black_body_radiation_fn(Teff):
def phi(lam):
# planck's law of black body radiation [W/m3/sr]
h = 6.626e-34 # planck constant (m2kg/s)
c = 3e8 # speed of light
k = 1.380649e-23 # Boltzmann constant
r = 2 * h * c**2 / lam**5 / (np.exp(h * c / lam / k / Teff) - 1)
return r
return phi
@staticmethod
def synthetic_radiation(Teff, fe_h, log_g, lam, mag_v=None):
return Stars.synthetic_radiation_fn(Teff, fe_h, log_g,
mag_v=mag_v)(lam)
@staticmethod
@lru_cache(maxsize=1000)
def synthetic_radiation_fn(Teff,
fe_h,
log_g,
mag_v=None,
model='k93models',
lam_min=0,
lam_max=np.inf,
return_sp=False):
return Stars.uncached_synthetic_radiation_fn(Teff, fe_h, log_g, mag_v,
model, lam_min, lam_max,
return_sp)
@staticmethod
def uncached_synthetic_radiation_fn(Teff,
fe_h,
log_g,
mag_v=None,
model='k93models',
lam_min=0,
lam_max=np.inf,
return_sp=False):
sp = None
orig_log_g = log_g
if isinstance(model, tuple):
# give in meters, W/m3
sp = S.spectrum.ArraySourceSpectrum(
np.array(model[0]) * 1e10,
np.array(model[1]) * 1e-4 * 1e-10 / 1e-7, 'angstrom', 'flam')
else:
first_try = True
if Teff < 3500:
print(
'could not init spectral model with given t_eff=%s, using t_eff=3500K instead'
% Teff)
Teff = 3500
for i in range(15):
try:
sp = S.Icat(model, Teff, fe_h,
log_g) # 'ck04models' or 'k93models'
break
except:
first_try = False
log_g = log_g + (0.2 if Teff > 6000 else -0.2)
assert sp is not None, 'could not init spectral model with given params: t_eff=%s, log_g=%s, fe_h=%s' % (
Teff, orig_log_g, fe_h)
if not first_try:
print(
'could not init spectral model with given params (t_eff=%s, log_g=%s, fe_h=%s), changed log_g to %s'
% (Teff, orig_log_g, fe_h, log_g))
if mag_v is not None:
sp = sp.renorm(mag_v, 'vegamag', S.ObsBandpass('johnson,v'))
if return_sp:
return sp
# for performance reasons (caching)
from scipy.interpolate import interp1d
I = np.logical_and(sp.wave >= lam_min * 1e10,
sp.wave <= lam_max * 1e10)
sample_fn = interp1d(sp.wave[I],
sp.flux[I],
kind='linear',
assume_sorted=True)
def phi(lam):
r = sample_fn(
lam * 1e10) # wavelength in Å, result in "flam" (erg/s/cm2/Å)
return r * 1e-7 / 1e-4 / 1e-10 # result in W/m3
return phi
@staticmethod
def magnitude_to_spectral_flux_density(mag):
# spectral flux density for standard magnitude for V-band (at 545nm)
# from "Model atmospheres broad-band colors, bolometric corrections and temperature calibrations for O - M stars"
# Bessel M.S. et al, Astronomy and Astrophysics, 1998, table A2
# Also at http://web.ipac.caltech.edu/staff/fmasci/home/astro_refs/magsystems.pdf
# 363.1e-11 erg/cm2/s/Å (erg=1e-7J, 1cm2=1e-4m2, Å=1e-10m)
phi0 = 363.1e-11 * 1e-7 / 1e-4 / 1e-10 # W/m3
return np.power(10., -0.4 * mag) * phi0
@staticmethod
def tycho_to_johnson(mag_bt, mag_vt):
v = mag_vt - 0.09 * (mag_bt - mag_vt)
b = 0.85 * (mag_bt - mag_vt) + v
return b, v
@staticmethod
def effective_temp(b_v, metal=0, log_g=0):
""" magnitudes in johnson system """
# calculate star effective temperatures, from:
# - http://help.agi.com/stk/index.htm#stk/starConstruction.htm
# - Sekiguchi, M. and Fukugita, M., 2000. A Study of the B−V Color-Temperature Relation. The Astronomical Journal, 120(2), p.1072.
# - metallicity (Fe/H) and log surface gravity can be set to zero without big impact
c0 = 3.939654
c1 = -0.395361
c2 = 0.2082113
c3 = -0.0604097
f1 = 0.027153
f2 = 0.005036
g1 = 0.007367
h1 = -0.01069
return 10**(c0 + c1 * (b_v) + c2 * (b_v)**2 + c3 * (b_v)**3 +
f1 * metal + f2 * metal**2 + g1 * log_g + h1 *
(b_v) * log_g)
@staticmethod
def flux_density(cam_q,
cam,
mask=None,
mag_cutoff=MAG_CUTOFF,
array=False,
undistorted=False,
order_by=None):
"""
plots stars based on Tycho-2 database, gives out photon count per unit area given exposure time in seconds,
cam_q is a quaternion in ICRS coord frame, x_fov and y_fov in degrees
"""
# calculate query conditions for star ra and dec
cam_dec, cam_ra, _ = tools.q_to_ypr(
cam_q) # camera boresight in ICRS coords
d = np.linalg.norm((cam.x_fov, cam.y_fov)) / 2
min_dec, max_dec = math.degrees(cam_dec) - d, math.degrees(cam_dec) + d
dec_cond = '(dec BETWEEN %s AND %s)' % (min_dec, max_dec)
# goes over the pole to the other side of the sphere, easy solution => ignore limit on ra
skip_ra_cond = min_dec < -90 or max_dec > 90
if skip_ra_cond:
ra_cond = '1'
else:
min_ra, max_ra = math.degrees(cam_ra) - d, math.degrees(cam_ra) + d
if min_ra < 0:
ra_cond = '(ra < %s OR ra > %s)' % (max_ra,
(min_ra + 360) % 360)
elif max_ra > 360:
ra_cond = '(ra > %s OR ra < %s)' % (min_ra, max_ra % 360)
else:
ra_cond = '(ra BETWEEN %s AND %s)' % (min_ra, max_ra)
conn = sqlite3.connect(Stars.STARDB)
cursor = conn.cursor()
# the magnitudes for tycho id xxxx-xxxxx-2 entries are bad as they are most likely taken from hip catalog that bundles all .*-(\d)
results = cursor.execute(
"""
SELECT x, y, z, mag_v""" +
(", mag_b, t_eff, fe_h, log_g, dec, ra, id" if array else "") + """
FROM deep_sky_objects
WHERE """ + ("tycho like '%-1' AND " if Stars.STARDB ==
Stars.STARDB_TYC else "") + "mag_v < " +
str(mag_cutoff) + " AND " + dec_cond + " AND " + ra_cond +
((" ORDER BY %s ASC" % order_by) if order_by is not None else ''))
stars = np.array(results.fetchall())
conn.close()
flux_density = ([], None) if array else np.zeros(
(cam.height, cam.width), dtype=np.float32)
if len(stars) == 0:
return flux_density
stars[:, 0:3] = tools.q_times_mx(
SystemModel.sc2gl_q.conj() * cam_q.conj(), stars[:, 0:3])
stars_ixy_ = cam.calc_img_R(stars[:, 0:3], undistorted=undistorted)
stars_ixy = np.round(stars_ixy_.astype(np.float)).astype(np.int)
I = np.logical_and.reduce(
(np.all(stars_ixy >= 0, axis=1), stars_ixy[:, 0] <= cam.width - 1,
stars_ixy[:, 1] <= cam.height - 1))
if array:
cols = ('ix', 'iy', 'x', 'y', 'z', 'mag_v', 'mag_b', 't_eff',
'fe_h', 'log_g', 'dec', 'ra', 'id')
return (np.hstack((stars_ixy_[I, :], stars[I, :])),
dict(zip(cols, range(len(cols)))))
stars_ixy = stars_ixy[I, :]
flux_density_per_star = Stars.magnitude_to_spectral_flux_density(
stars[I, 3])
for i, f in enumerate(flux_density_per_star):
flux_density[stars_ixy[i, 1], stars_ixy[i, 0]] += f
if mask is not None:
flux_density[np.logical_not(mask)] = 0
if True:
# assume every star is like our sun, convert to total flux density [W/m2]
solar_constant = 1360.8
# sun magnitude from http://mips.as.arizona.edu/~cnaw/sun.html
sun_flux_density = Stars.magnitude_to_spectral_flux_density(
Stars.SUN_MAG_V)
flux_density = flux_density * (solar_constant / sun_flux_density)
return flux_density
@staticmethod
def get_property_by_id(id, field=None):
res = Stars._query_cursor.execute(
f"select {field} from deep_sky_objects where id = {int(id)}"
).fetchone()[0]
return res
@staticmethod
def get_catalog_id(id, field=None):
try:
is_arr = False
id = int(id)
except:
is_arr = True
if Stars._query_conn is None:
Stars._conn = sqlite3.connect(Stars.STARDB)
Stars._query_cursor = Stars._conn.cursor()
field = field or ("tycho"
if Stars.STARDB == Stars.STARDB_TYC else "hip")
if is_arr:
res = Stars._query_cursor.execute(
"select id, %s from deep_sky_objects where id IN (%s)" %
(field, ','.join(str(i) for i in id))).fetchall()
return {r[0]: str(r[1]) for r in res}
else:
res = Stars._query_cursor.execute(
"select %s from deep_sky_objects where id = %s" %
(field, id)).fetchone()[0]
return str(res)
_query_conn, _query_cursor = None, None
@staticmethod
def _create_stardb(fname):
conn = sqlite3.connect(fname)
cursor = conn.cursor()
cursor.execute("DROP TABLE IF EXISTS deep_sky_objects")
cursor.execute("""
CREATE TABLE deep_sky_objects (
id INTEGER PRIMARY KEY ASC NOT NULL,
hip INT,
hd INT DEFAULT NULL,
simbad CHAR(20) DEFAULT NULL,
ra REAL NOT NULL, /* src[0] */
dec REAL NOT NULL, /* src[0] */
x REAL NOT NULL,
y REAL NOT NULL,
z REAL NOT NULL,
mag_v REAL NOT NULL, /* src[1] */
mag_b REAL DEFAULT NULL, /* src[2] */
t_eff REAL DEFAULT NULL, /* src[3] */
log_g REAL DEFAULT NULL, /* src[4] */
fe_h REAL DEFAULT NULL, /* src[5] */
src CHAR(6) DEFAULT 'HHHPPP'
)""")
cursor.execute("DROP INDEX IF EXISTS ra_idx")
cursor.execute("CREATE INDEX ra_idx ON deep_sky_objects (ra)")
cursor.execute("DROP INDEX IF EXISTS dec_idx")
cursor.execute("CREATE INDEX dec_idx ON deep_sky_objects (dec)")
cursor.execute("DROP INDEX IF EXISTS mag_idx")
cursor.execute("CREATE INDEX mag_idx ON deep_sky_objects (mag_v)")
cursor.execute("DROP INDEX IF EXISTS hd")
cursor.execute("CREATE INDEX hd ON deep_sky_objects (hd)")
cursor.execute("DROP INDEX IF EXISTS simbad")
cursor.execute("CREATE INDEX simbad ON deep_sky_objects (simbad)")
cursor.execute("DROP INDEX IF EXISTS hip")
cursor.execute("CREATE UNIQUE INDEX hip ON deep_sky_objects (hip)")
conn.commit()
@staticmethod
def import_stars_hip():
# I/239/hip_main
Stars._create_stardb(Stars.STARDB_HIP)
conn = sqlite3.connect(Stars.STARDB_HIP)
cursor = conn.cursor()
from astroquery.vizier import Vizier
Vizier.ROW_LIMIT = -1
cols = ["HIP", "HD", "_RA.icrs", "_DE.icrs", "Vmag", "B-V"]
r = Vizier(catalog="I/239/hip_main", columns=cols,
row_limit=-1).query_constraints()[0]
for i, row in enumerate(r):
hip, hd, ra, dec, mag_v, b_v = [row[f] for f in cols]
if np.any(list(map(np.ma.is_masked, (ra, dec, mag_v)))):
continue
hd = 'null' if np.ma.is_masked(hd) else hd
mag_b = 'null' if np.ma.is_masked(b_v) or np.isnan(
b_v) else b_v + mag_v
x, y, z = tools.spherical2cartesian(math.radians(dec),
math.radians(ra), 1)
cursor.execute("""
INSERT INTO deep_sky_objects (hip, hd, ra, dec, x, y, z, mag_v, mag_b)
VALUES (%s, %s, %f, %f, %f, %f, %f, %f, %s)""" %
(hip, hd, ra, dec, x, y, z, mag_v, mag_b))
if i % 100 == 0:
conn.commit()
tools.show_progress(len(r), i)
conn.commit()
conn.close()
@staticmethod
def import_stars_tyc():
assert False, 'not supported anymore'
Stars._create_stardb(Stars.STARDB_TYC, 12)
conn = sqlite3.connect(Stars.STARDB_TYC)
cursor = conn.cursor()
# Tycho-2 catalogue, from http://archive.eso.org/ASTROM/TYC-2/data/
for file in ('catalog.dat', 'suppl_1.dat'):
with open(os.path.join(DATA_DIR, file), 'r') as fh:
line = fh.readline()
while line:
c = line
line = fh.readline()
# mean position, ICRS, at epoch 2000.0
# proper motion milliarcsecond/year
# apparent magnitude
if file == 'catalog.dat':
# main catalog
epoch = 2000.0
tycho, ra, dec, pmra, pmdec, mag_bt, mag_vt = c[
0:12], c[15:27], c[28:40], c[41:48], c[49:56], c[
110:116], c[123:129]
mag_b, mag_v = Stars.tycho_to_johnson(
float(mag_bt), float(mag_vt))
else:
# supplement-1 has the brightest stars, from hipparcos and tycho-1
epoch = 1991.25
tycho, ra, dec, pmra, pmdec, mag_bt, mag_vt, flag, hip = \
c[0:12], c[15:27], c[28:40], c[41:48], c[49:56], c[83:89], c[96:102], c[81:82], c[115:120]
if flag in ('H', 'V', 'B'):
if len(hip.strip()) > 0:
mag_b, mag_v = Stars.get_hip_mag_bv(hip)
else:
continue
else:
mag_b, mag_v = Stars.tycho_to_johnson(
float(mag_bt), float(mag_vt))
tycho = tycho.replace(' ', '-')
if np.all(list(map(tools.numeric, (ra, dec)))):
ra, dec = list(map(float, (ra, dec)))
if -10 < mag_v < Stars.MAG_CUTOFF:
curr_epoch = datetime.now().year + \
(datetime.now().timestamp()
- datetime.strptime(str(datetime.now().year),'%Y').timestamp()
)/365.25/24/3600
years = curr_epoch - epoch
# TODO: (1) adjust to current epoch using proper motion and years since epoch
x, y, z = tools.spherical2cartesian(
math.radians(dec), math.radians(ra), 1)
cursor.execute(
"INSERT INTO deep_sky_objects (tycho,ra,dec,x,y,z,mag_b,mag_v) VALUES (?,?,?,?,?,?,?,?)",
(tycho,
(ra + 360) % 360, dec, x, y, z, mag_b, mag_v))
conn.commit()
conn.close()
@staticmethod
def add_simbad_col():
conn = sqlite3.connect(Stars.STARDB)
cursor_r = conn.cursor()
cursor_w = conn.cursor()
# cursor_w.execute("alter table deep_sky_objects add column simbad char(20) default null")
# conn.commit()
N_tot = cursor_r.execute(
"SELECT max(id) FROM deep_sky_objects WHERE 1").fetchone()[0]
skip = 0
result = cursor_r.execute(
"select id, hip from deep_sky_objects where id >= %d" % skip)
import time
from astroquery.simbad import Simbad
Simbad.add_votable_fields('typed_id')
while 1:
rows = result.fetchmany(1000)
if rows is None or len(rows) == 0:
break
tools.show_progress(N_tot, rows[0][0] - 1)
s = Simbad.query_objects(['HIP %d' % int(row[1]) for row in rows])
time.sleep(2)
values = []
if s is not None:
s.add_index('TYPED_ID')
for row in rows:
sr = get(s, ('HIP %d' % int(row[1])).encode('utf-8'))
if sr is not None:
k = sr['MAIN_ID'].decode('utf-8')
values.append("(%d, '%s', 0,0,0,0,0,0)" %
(row[0], k.replace("'", "''")))
if len(values) > 0:
cursor_w.execute("""
INSERT INTO deep_sky_objects (id, simbad, ra, dec, x, y, z, mag_v) VALUES """
+ ','.join(values) + """
ON CONFLICT(id) DO UPDATE SET simbad = excluded.simbad""")
conn.commit()
conn.close()
@staticmethod
def query_t_eff():
from astroquery.vizier import Vizier
v = Vizier(catalog="B/pastel/pastel",
columns=["ID", "Teff", "logg", "[Fe/H]"],
row_limit=-1)
v2 = Vizier(catalog="J/A+A/525/A71/table2",
columns=["Name", "Teff", "log(g)", "[Fe/H]"],
row_limit=-1)
v3 = Vizier(catalog="J/MNRAS/471/770/table2",
columns=["HIP", "Teff", "log(g)"],
row_limit=-1)
conn = sqlite3.connect(Stars.STARDB)
cursor_r = conn.cursor()
cursor_w = conn.cursor()
cond = "(t_eff is null OR log_g is null OR 1)"
N_tot = cursor_r.execute("""
SELECT max(id) FROM deep_sky_objects
WHERE %s
""" % cond).fetchone()[0]
skip = 37601
f_id, f_hip, f_hd, f_sim, f_ra, f_dec, f_t, f_g, f_m, f_src = range(10)
results = cursor_r.execute(
"""
SELECT id, hip, hd, simbad, ra, dec, t_eff, log_g, fe_h, src
FROM deep_sky_objects
WHERE %s AND id >= ?
ORDER BY id ASC
""" % cond, (skip, ))
r = v.query_constraints()[0]
r.add_index('ID')
N = 40
while True:
rows = results.fetchmany(N)
if rows is None or len(rows) == 0:
break
tools.show_progress(N_tot, rows[0][f_id] - 1)
ids = {
row[f_id]: [i, row[f_src][:3] + '___']
for i, row in enumerate(rows)
}
insert = {}
for i, row in enumerate(rows):
k = 'HIP %6d' % int(row[f_hip])
if get(r, k) is None and row[f_hd]:
k = 'HD %6d' % int(row[f_hd])
if get(r, k) is None and row[f_sim]:
k = row[f_sim]
if get(r, k) is None and row[f_sim]:
k = row[f_sim] + ' A'
dr = get(r, k)
if dr is not None:
t_eff, log_g, fe_h = median(dr,
('Teff', 'logg', '__Fe_H_'),
null='null')
src = row[f_src][0:3] + ''.join(
[('_' if v == 'null' else Stars.SOURCE_PASTEL)
for v in (t_eff, log_g, fe_h)])
insert[row[f_id]] = [t_eff, log_g, fe_h, src]
if '_' not in src[3:5]:
ids.pop(row[f_id])
else:
ids[row[f_id]][1] = src
if len(ids) > 0:
# try using other catalog
r = v2.query_constraints(
Name='=,' + ','.join([('HD%06d' % int(rows[i][f_hd]))
for i, s in ids.values()
if rows[i][f_hd] is not None]))
time.sleep(2)
if len(r) > 0:
r = r[0]
r.add_index('Name')
for id, (i, src) in ids.copy().items():
dr = get(r, 'HD%06d' %
int(rows[i][f_hd])) if rows[i][f_hd] else None
if dr is not None:
t_eff, log_g, fe_h = median(
dr, ('Teff', 'log_g_', '__Fe_H_'), null='null')
src = src[0:3] + ''.join(
[('_' if v == 'null' else Stars.SOURCE_WU)
for v in (t_eff, log_g, fe_h)])
insert[id] = [t_eff, log_g, fe_h, src]
if '_' not in src[3:5]:
ids.pop(rows[i][f_id])
else:
ids[rows[i][f_id]][1] = src
if len(ids) > 0:
# try using other catalog
r = v3.query_constraints(
HIP='=,' +
','.join([str(rows[i][f_hip])
for i, s in ids.values()]))[0]
r.add_index('HIP')
for id, (i, src) in ids.copy().items():
dr = get(r, int(rows[i][f_hip]))
if dr is not None:
t_eff, log_g = median(dr, ('Teff', 'log_g_'),
null='null')
src = src[0:3] + ''.join(
[('_' if v == 'null' else Stars.SOURCE_GAIA1)
for v in (t_eff, log_g)]) + src[5]
insert[id] = [
t_eff, log_g,
insert[id][2] if id in insert else 'null', src
]
# if '_' not in src[3:5]:
# ids.pop(rows[i][f_id])
# else:
# ids[rows[i][f_id]][1] = src
if len(insert) > 0:
values = [
"(%d, %s, %s, %s, '%s', 0,0,0,0,0,0)" %
(id, t_eff, log_g, fe_h, src)
for id, (t_eff, log_g, fe_h, src) in insert.items()
]
cursor_w.execute("""
INSERT INTO deep_sky_objects (id, t_eff, log_g, fe_h, src, ra, dec, x, y, z, mag_v) VALUES """
+ ','.join(values) + """
ON CONFLICT(id) DO UPDATE SET
t_eff = excluded.t_eff,
log_g = excluded.log_g,
fe_h = excluded.fe_h,
src = excluded.src
""")
conn.commit()
conn.close()
@staticmethod
def query_v_mag():
from astroquery.vizier import Vizier
from tqdm import tqdm
v = Vizier(catalog="B/pastel/pastel",
columns=["ID", "Vmag"],
row_limit=-1)
conn = sqlite3.connect(Stars.STARDB)
cursor_r = conn.cursor()
cursor_w = conn.cursor()
cond = f"(substr(src,2,1) = '{Stars.SOURCE_HIPPARCHOS}')"
N_tot = cursor_r.execute(
f"SELECT count(*) FROM deep_sky_objects WHERE {cond}").fetchone(
)[0]
f_id, f_hip, f_hd, f_sim, f_mag_v, f_src = range(6)
results = cursor_r.execute("""
SELECT id, hip, hd, simbad, mag_v, src
FROM deep_sky_objects
WHERE %s
ORDER BY mag_v ASC
""" % cond)
r = v.query_constraints()[0]
r.add_index('ID')
N = 40
pbar = tqdm(total=N_tot)
while True:
rows = results.fetchmany(N)
if rows is None or len(rows) == 0:
break
ids = {row[f_id]: [i, row[f_src]] for i, row in enumerate(rows)}
insert = {}
for i, row in enumerate(rows):
k = 'HIP %6d' % int(row[f_hip])
if get(r, k) is None and row[f_hd]:
k = 'HD %6d' % int(row[f_hd])
if get(r, k) is None and row[f_sim]:
k = row[f_sim]
if get(r, k) is None and row[f_sim]:
k = row[f_sim] + ' A'
dr = get(r, k)
if dr is not None:
v_mag, *_ = median(dr, ('Vmag', ), null='null')
if v_mag != 'null':
src = row[f_src]
src = src[:1] + Stars.SOURCE_PASTEL + src[2:]
insert[row[f_id]] = [v_mag, src]
ids.pop(row[f_id])
if len(insert) > 0:
values = [
f"({id}, 0, 0, 0, '{src}', 0, 0, 0, 0, 0, {v_mag})"
for id, (v_mag, src) in insert.items()
]
cursor_w.execute(
"INSERT INTO deep_sky_objects (id, t_eff, log_g, fe_h, src, ra, dec, x, y, z, mag_v) "
"VALUES " + ','.join(values) + " "
"ON CONFLICT(id) DO UPDATE SET "
" mag_v = excluded.mag_v, "
" src = excluded.src")
conn.commit()
pbar.set_postfix(
{'v_mag': np.max([float(row[f_mag_v]) for row in rows])})
pbar.update(len(rows))
conn.close()
@staticmethod
def correct_supplement_data():
conn = sqlite3.connect(Stars.STARDB)
cursor = conn.cursor()
def insert_mags(hips):
res = Stars.get_hip_mag_bv([h[0] for h in hips.values()])
insert = [
"('%s', %f, %f, %f, %f, %f, %f, %f)" %
(t, h[1], h[2], h[3], h[4], h[5], res[h[0]][0], res[h[0]][1])
for t, h in hips.items()
if h[0] in res and -10 < res[h[0]][1] < Stars.MAG_CUTOFF
]
if len(insert) > 0:
cursor.execute("""
INSERT INTO deep_sky_objects (tycho, ra, dec, x, y, z, mag_b, mag_v) VALUES
""" + ','.join(insert) + """
ON CONFLICT(tycho) DO UPDATE SET mag_b = excluded.mag_b, mag_v = excluded.mag_v """
)
conn.commit()
file = 'suppl_1.dat'
N = 30
rx = re.compile(r'0*(\d+)')
with open(os.path.join(DATA_DIR, file), 'r') as fh:
hips = {}
line = fh.readline()
while line:
c = line
line = fh.readline()
tycho, ra, dec, mag_bt, mag_vt, flag, hip = c[0:12], c[
15:27], c[28:40], c[83:89], c[96:102], c[81:82], c[115:123]
tycho = tycho.replace(' ', '-')
hip = rx.findall(hip)[0] if len(hip.strip()) > 0 else False
if flag in ('H', 'V', 'B') and hip:
ra, dec = float(ra), float(dec)
x, y, z = tools.spherical2cartesian(
math.radians(dec), math.radians(ra), 1)
hips[tycho] = (hip, ra, dec, x, y, z)
if len(hips) >= N:
insert_mags(hips)
hips.clear()
else:
continue
if len(hips) > 0:
insert_mags(hips)
@staticmethod
def get_hip_mag_bv(hip, v=None):
from astroquery.vizier import Vizier
Vizier.ROW_LIMIT = -1
hips = [hip] if isinstance(hip, str) else hip
v = Vizier(columns=["HIP", "Vmag", "B-V"],
catalog="I/239/hip_main",
row_limit=-1)
r = v.query_constraints(HIP='=,' + ','.join(hips))
results = {}
if len(r):
r = r[0]
r.add_index('HIP')
for h in hips:
try:
if not np.ma.is_masked(
r.loc[int(h)]['Vmag']) and not np.ma.is_masked(
r.loc[int(h)]['B-V']):
mag_v, b_v = float(r.loc[int(h)]['Vmag']), float(
r.loc[int(h)]['B-V'])
results[h] = (mag_v + b_v, mag_v)
except:
continue
return results.get(hip,
(None, None)) if isinstance(hip, str) else results
@staticmethod
def override_betelgeuse():
conn = sqlite3.connect(Stars.STARDB)
cursor = conn.cursor()
# from "The Advanced Spectral Library (ASTRAL): Reference Spectra for Evolved M Stars",
# The Astrophysical Journal, 2018, https://iopscience.iop.org/article/10.3847/1538-4357/aaf164/pdf
#t_eff = 3650 # based on query_t_eff was 3562
#mag_v = 0.42 # based on tycho2 suppl2 was 0.58
# from CTOA observations on 2018-12-07 and 18-12-22, accessed through https://www.aavso.org database
mag_v = 0.8680
mag_b = 2.6745 # based on tycho2 suppl2 was 2.3498
t_eff = None # Stars.effective_temp(mag_b - mag_v, metal=0.006, log_g=-0.26) gives 3565K vs 3538K without log_g & metal
cursor.execute(
"UPDATE deep_sky_objects SET t_eff=?, mag_v=?, mag_b=? where tycho='0129-01873-1'",
(t_eff, mag_v, mag_b))
conn.commit()
conn.close()
def update_q(self):
self.q = np.quaternion(0.0, self.x, self.y, self.z)
def test_constants():
assert quaternion.one == np.quaternion(1.0, 0.0, 0.0, 0.0)
assert quaternion.x == np.quaternion(0.0, 1.0, 0.0, 0.0)
assert quaternion.y == np.quaternion(0.0, 0.0, 1.0, 0.0)
assert quaternion.z == np.quaternion(0.0, 0.0, 0.0, 1.0)
def quat_from_magnum(quat: mn.Quaternion) -> np.quaternion:
a = np.quaternion(1, 0, 0, 0)
a.real = quat.scalar
a.imag = quat.vector
return a
except KeyError as e:
print(e)
try:
self.low_pass.fc = params['fc']
except KeyError as e:
print(e)
if __name__ == '__main__':
### Parameters
HZ = 240.0
OD_params_dict = {
'dt': 1.0 / HZ,
'K': 6.0,
'fc': 8.0,
'q0': np.quaternion(1.0, 0.0, 0.0, 0.0)
}
dt = OD_params_dict['dt']
q = OD_params_dict['q0'].copy()
### Initialize Orientation Dynamics
od = OrientationDynamics(**OD_params_dict)
qg = np.quaternion(*list(np.random.rand(4))) # np.quaternion(1,0,0,0) #
od.set_goal(qg)
### Simulate
t = 0.0
tf = 2.0
e_array = []
t_array = []
w_array = []
while (t < tf):
q, w = od.step(q, dt)
def test_reach_both_sides_box(torque_mode=False):
flag_box = False
box_tf = TransformListener()
baxter_ctrlr = MinJerkController()
left_task_complete = False
right_task_complete = False
box_length = 0.410 #m
box_breadth = 0.305
box_height = 0.107
#for better show off ;)
baxter_ctrlr.set_neutral()
while not rospy.is_shutdown():
try:
tfmn_time = box_tf.getLatestCommonTime('base', 'box')
flag_box = True
except tf.Exception:
print "Some exception occured while getting the transformation!!!"
if flag_box:
flag_box = False
box_pos, box_ori = box_tf.lookupTransform('base', 'box', tfmn_time)
box_pos = np.array([box_pos[0], box_pos[1], box_pos[2]])
box_ori = np.quaternion(box_ori[3], box_ori[0], box_ori[1],
box_ori[2])
left_pos, left_ori = baxter_ctrlr.left_arm.get_ee_pose()
right_pos, right_ori = baxter_ctrlr.right_arm.get_ee_pose()
left_goal_pos = box_pos + np.dot(
quaternion.as_rotation_matrix(box_ori),
np.array([box_length / 2., -box_height / 2., 0.]))
right_goal_pos = box_pos + np.dot(
quaternion.as_rotation_matrix(box_ori),
np.array([-box_length / 2., -box_height / 2., 0.]))
error_left = np.linalg.norm(left_pos - left_goal_pos)
error_right = np.linalg.norm(right_pos - right_goal_pos)
if torque_mode:
#minimum jerk trajectory for left arm
baxter_ctrlr.configure(left_pos, left_ori, left_goal_pos,
box_ori)
min_jerk_traj_left = baxter_ctrlr.get_min_jerk_trajectory()
#minimum jerk trajectory for right arm
baxter_ctrlr.configure(right_pos, right_ori, right_goal_pos,
box_ori)
min_jerk_traj_right = baxter_ctrlr.get_min_jerk_trajectory()
for t in range(baxter_ctrlr.timesteps):
#compute the left joint torque command
left_cmd = baxter_ctrlr.osc_torque_cmd(
goal_pos=min_jerk_traj_left['pos_traj'][t, :],
goal_ori=min_jerk_traj_left['ori_traj'][t],
limb_idx=0,
orientation_ctrl=True)
#compute the right joint torque command if the error is above some threshold
right_cmd = baxter_ctrlr.osc_torque_cmd(
goal_pos=min_jerk_traj_right['pos_traj'][t, :],
goal_ori=min_jerk_traj_right['ori_traj'][t],
limb_idx=1,
orientation_ctrl=True)
baxter_ctrlr.left_arm.exec_torque_cmd(left_cmd)
baxter_ctrlr.right_arm.exec_torque_cmd(right_cmd)
time.sleep(0.1)
break
else:
#compute the left joint position command if the error is above some threshold
left_cmd = baxter_ctrlr.osc_position_cmd(
goal_pos=left_goal_pos,
goal_ori=box_ori,
limb_idx=0,
orientation_ctrl=False)
#compute the right joint position command if the error is above some threshold
right_cmd = baxter_ctrlr.osc_position_cmd(
goal_pos=right_goal_pos,
goal_ori=box_ori,
limb_idx=1,
orientation_ctrl=False)
baxter_ctrlr.left_arm.move_to_joint_position(left_cmd)
baxter_ctrlr.right_arm.move_to_joint_position(right_cmd)
print "error_right \t", error_right
print "error_left \t", error_left
if (error_right < baxter_ctrlr.osc_pos_threshold) and (
error_left < baxter_ctrlr.osc_pos_threshold):
#break the loop if within certain threshold
break
def test_quaternion_power(Qs):
import math
qpower_precision = 4*eps
# Test equivalence between scalar and real-quaternion exponentiation
for b in [0, 0.0, 1, 1.0, 2, 2.0, 5.6]:
for e in [0, 0.0, 1, 1.0, 2, 2.0, 4.5]:
be = np.quaternion(b**e, 0, 0, 0)
assert allclose(be, np.quaternion(b, 0, 0, 0)**np.quaternion(e, 0, 0, 0), rtol=qpower_precision)
assert allclose(be, b**np.quaternion(e, 0, 0, 0), rtol=qpower_precision)
assert allclose(be, np.quaternion(b, 0, 0, 0)**e, rtol=qpower_precision)
for q in [-3*quaternion.one, -2*quaternion.one, -quaternion.one, quaternion.zero, quaternion.one, 3*quaternion.one]:
for s in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
for t in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
assert allclose((s*t)**q, (s**q)*(t**q), rtol=2*qpower_precision)
# Test basic integer-exponent and additive-exponent properties
for q in Qs[Qs_finitenonzero]:
assert allclose(q ** 0, np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(q ** 0.0, np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(q ** np.quaternion(0, 0, 0, 0), np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(((q ** 0.5) * (q ** 0.5)), q, rtol=qpower_precision)
assert allclose(q ** 1.0, q, rtol=qpower_precision)
assert allclose(q ** 1, q, rtol=qpower_precision)
assert allclose(q ** np.quaternion(1, 0, 0, 0), q, rtol=qpower_precision)
assert allclose(q ** 2.0, q * q, rtol=qpower_precision)
assert allclose(q ** 2, q * q, rtol=qpower_precision)
assert allclose(q ** np.quaternion(2, 0, 0, 0), q * q, rtol=qpower_precision)
assert allclose(q ** 3, q * q * q, rtol=qpower_precision)
assert allclose(q ** -1, q.inverse(), rtol=qpower_precision)
assert allclose(q ** -1.0, q.inverse(), rtol=qpower_precision)
for s in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
for t in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
assert allclose(q**(s+t), (q**s)*(q**t), rtol=2*qpower_precision)
assert allclose(q**(s-t), (q**s)/(q**t), rtol=2*qpower_precision)
# Check that exp(q) is the same as e**q
for q in Qs[Qs_finitenonzero]:
assert allclose(q.exp(), math.e**q, rtol=qpower_precision)
for s in [0, 0., 1.0, 1, 1.2, 2.3, 3]:
for t in [0, 0., 1.0, 1, 1.2, 2.3, 3]:
assert allclose((s*t)**q, (s**q)*(t**q), rtol=2*qpower_precision)
for s in [1.0, 1, 1.2, 2.3, 3]:
assert allclose(s**q, (q*math.log(s)).exp(), rtol=qpower_precision)
qinverse_precision = 2*eps
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** -1.0) * q, Qs[q_1], rtol=qinverse_precision)
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** -1) * q, Qs[q_1], rtol=qinverse_precision)
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** Qs[q_1]), q, rtol=qpower_precision)
strict_assert(False) # Try more edge cases
for q in [quaternion.x, quaternion.y, quaternion.z]:
assert allclose(quaternion.quaternion(math.exp(-math.pi/2), 0, 0, 0),
q**q, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, 0, math.sin(math.pi/2)),
quaternion.x**quaternion.y, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, -math.sin(math.pi/2), 0),
quaternion.x**quaternion.z, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, 0, -math.sin(math.pi/2)),
quaternion.y**quaternion.x, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), math.sin(math.pi/2), 0, 0),
quaternion.y**quaternion.z, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, math.sin(math.pi/2), 0),
quaternion.z**quaternion.x, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), -math.sin(math.pi/2), 0, 0),
quaternion.z**quaternion.y, rtol=qpower_precision)
def test_coop_position_control():
limb_idx_l = 0 #0 is left and 1 is right
limb_idx_r = 1 #0 is left and 1 is right
baxter_ctrlr = MinJerkController()
arm_l = baxter_ctrlr.get_arm_handle(limb_idx_l)
arm_r = baxter_ctrlr.get_arm_handle(limb_idx_r)
baxter_ctrlr.set_neutral()
baxter_ctrlr.untuck_arms()
start_pos, start_ori = arm_l.get_ee_pose()
goal_pos = start_pos + np.array([0., 0.8, 0.])
angle = 90.0
axis = np.array([1., 0., 0.])
axis = np.sin(0.5 * angle * np.pi / 180.) * axis / np.linalg.norm(axis)
goal_ori = np.quaternion(np.cos(0.5 * angle * np.pi / 180.), axis[0],
axis[1], axis[2], axis[3])
baxter_ctrlr.configure(start_pos, start_ori, goal_pos, goal_ori)
left_pos, left_ori = arm_l.get_ee_pose()
right_pos, right_ori = arm_r.get_ee_pose()
pos_rel = right_pos - left_pos
vel_rel = np.array([0., 0., 0.])
omg_rel = np.array([0., 0., 0.])
jnt_l = arm_l.angles()
jnt_r = arm_r.angles()
min_jerk_traj = baxter_ctrlr.get_min_jerk_trajectory()
print "Starting co-operative position controller"
for t in range(baxter_ctrlr.timesteps):
jac_master_rel = baxter_ctrlr.relative_jac(pos_rel)
jac_tmp = np.dot(jac_master_rel, jac_master_rel.T)
jac_star = np.dot(
jac_master_rel.T,
(np.linalg.inv(jac_tmp + np.eye(jac_tmp.shape[0]) * 0.01)))
pos_tmp = np.hstack([
min_jerk_traj['vel_traj'][t, :], min_jerk_traj['omg_traj'][t, :],
vel_rel, omg_rel
])
jnt_vel = np.dot(jac_star, pos_tmp)
if np.any(np.isnan(jnt_vel)):
jnt_vel_l = np.zeros(7)
jnt_vel_r = np.zeros(7)
else:
jnt_vel_l = jnt_vel[:7]
jnt_vel_r = jnt_vel[7:]
jnt_l += jnt_vel_l * baxter_ctrlr.dt
jnt_r += jnt_vel_r * baxter_ctrlr.dt
arm_l.exec_position_cmd(jnt_l)
arm_r.exec_position_cmd(jnt_r)
time.sleep(0.1)
final_pos, final_ori = arm_l.get_ee_pose()
print "ERROR in position \t", np.linalg.norm(final_pos - goal_pos)
print "ERROR in orientation \t", np.linalg.norm(final_ori - goal_ori)
def __quaternion_by_axis__(self, theta, v):
t = theta
return np.quaternion(cos(t), v[0]*sin(t), v[1]*sin(t), v[2]*sin(t))
def test_coop_torque_control():
limb_idx_l = 0 #0 is left and 1 is right
limb_idx_r = 1 #0 is left and 1 is right
baxter_ctrlr = MinJerkController()
arm_l = baxter_ctrlr.get_arm_handle(limb_idx_l)
arm_r = baxter_ctrlr.get_arm_handle(limb_idx_r)
#baxter_ctrlr.set_neutral()
baxter_ctrlr.untuck_arms()
start_pos, start_ori = arm_l.get_ee_pose()
goal_pos = start_pos + np.array([0., -0.0, 0.9])
angle = 45.0
axis = np.array([1., 0., 0.])
axis = np.sin(0.5 * angle * np.pi / 180.) * axis / np.linalg.norm(axis)
goal_ori = np.quaternion(np.cos(0.5 * angle * np.pi / 180.), axis[0],
axis[1], axis[2])
baxter_ctrlr.configure(start_pos, start_ori, goal_pos, goal_ori)
left_pos, left_ori = arm_l.get_ee_pose()
right_pos, right_ori = arm_r.get_ee_pose()
pos_rel = right_pos - left_pos
vel_rel = np.array([0., 0., 0.])
omg_rel = np.array([0., 0., 0.])
jnt_l = arm_l.angles()
jnt_r = arm_r.angles()
min_jerk_traj = baxter_ctrlr.get_min_jerk_trajectory()
print "Starting co-operative torque controller"
# rate = rospy.Rate(10)
mags_r = []
mags_l = []
for t in range(baxter_ctrlr.timesteps):
state_l = arm_l._state
state_r = arm_r._state
state_l['jnt_start'] = jnt_l
state_r['jnt_start'] = jnt_r
jac_ee_l = state_l['jacobian']
jac_ee_r = state_r['jacobian']
left_pos = state_l['ee_point']
left_ori = state_l['ee_ori']
right_pos = state_r['ee_point']
right_ori = state_r['ee_ori']
jac_master_rel = baxter_ctrlr.relative_jac(pos_rel)
jac_tmp = np.dot(jac_master_rel, jac_master_rel.T)
jac_star = np.dot(
jac_master_rel.T,
(np.linalg.inv(jac_tmp + np.eye(jac_tmp.shape[0]) * 0.001)))
pos_tmp = np.hstack([
min_jerk_traj['vel_traj'][t, :], min_jerk_traj['omg_traj'][t, :],
vel_rel, omg_rel
])
jnt_vel = np.dot(jac_star, pos_tmp)
ee_req_vel_l = np.dot(jac_ee_l, jnt_vel[:7])
ee_req_vel_r = np.dot(jac_ee_r, jnt_vel[7:])
cmd_l = baxter_ctrlr.osc_torque_cmd_2(arm_data=state_l,
goal_pos=ee_req_vel_l[0:3] *
baxter_ctrlr.dt,
goal_ori=None,
orientation_ctrl=False)
cmd_r = baxter_ctrlr.osc_torque_cmd_2(arm_data=state_r,
goal_pos=ee_req_vel_r[0:3] *
baxter_ctrlr.dt,
goal_ori=None,
orientation_ctrl=False)
#cmd = [0., 0.0, 0.0, 0.0, 0.0, 0.0, 0.10]
arm_l.exec_torque_cmd(cmd_l)
#print "cmd \n", cmd
#print "magnitude \t", np.linalg.norm(cmd)
arm_r.exec_torque_cmd(cmd_r)
mags_r.append(np.linalg.norm(cmd_r))
mags_l.append(np.linalg.norm(cmd_l))
time.sleep(0.1)
jnt_r = state_r['position']
jnt_l = state_l['position']
import matplotlib.pyplot as plt
plt.figure()
plt.plot(mags_l)
plt.figure()
plt.plot(mags_r)
#plt.show()
arm_l.exec_position_cmd2(np.zeros(7))
arm_r.exec_position_cmd2(np.zeros(7))
final_pos, final_ori = arm_l.get_ee_pose()
print "ERROR in position \t", np.linalg.norm(final_pos - goal_pos)
print "ERROR in orientation \t", np.linalg.norm(final_ori - goal_ori)
def KinectToDMControl(body_stream):
dm_body_stream = {}
dm_body_stream['left_knee'] = []
dm_body_stream['right_knee'] = []
dm_body_stream['right_elbow'] = []
dm_body_stream['left_elbow'] = []
dm_body_stream['right_shoulder1'] = []
dm_body_stream['right_shoulder2'] = []
dm_body_stream['left_shoulder1'] = []
dm_body_stream['left_shoulder2'] = []
dm_body_stream['left_hip_x'] = []
dm_body_stream['left_hip_y'] = []
dm_body_stream['left_hip_z'] = []
dm_body_stream['right_hip_x'] = []
dm_body_stream['right_hip_y'] = []
dm_body_stream['right_hip_z'] = []
for i in range(len(body_stream[0])):
r_hip_pos = np.array(body_stream[BodyPart.HIP_R.value][i][0:3])
r_knee_pos = np.array(body_stream[BodyPart.KNEE_R.value][i][0:3])
r_ankle_pos = np.array(body_stream[BodyPart.ANKLE_R.value][i][0:3])
l_hip_pos = np.array(body_stream[BodyPart.HIP_L.value][i][0:3])
l_knee_pos = np.array(body_stream[BodyPart.KNEE_L.value][i][0:3])
l_ankle_pos = np.array(body_stream[BodyPart.ANKLE_L.value][i][0:3])
r_shoulder_pos = np.array(
body_stream[BodyPart.SHOULDER_R.value][i][0:3])
r_elbow_pos = np.array(body_stream[BodyPart.ELBOW_R.value][i][0:3])
r_wrist_pos = np.array(body_stream[BodyPart.WRIST_R.value][i][0:3])
l_shoulder_pos = np.array(
body_stream[BodyPart.SHOULDER_L.value][i][0:3])
l_elbow_pos = np.array(body_stream[BodyPart.ELBOW_L.value][i][0:3])
l_wrist_pos = np.array(body_stream[BodyPart.WRIST_L.value][i][0:3])
###### This is NOT correct, but a heuristic ######
dm_body_stream['right_shoulder1'].append(
AngleToAxes(r_shoulder_pos, r_elbow_pos, [2, 1, 1], np.arccos) -
np.pi / 2)
dm_body_stream['right_shoulder2'].append(
AngleToAxes(r_shoulder_pos, r_elbow_pos, [0, -1, 1], np.arccos) -
np.pi / 2)
dm_body_stream['left_shoulder1'].append(
AngleToAxes(l_shoulder_pos, l_elbow_pos, [2, -1, 1], np.arccos) -
np.pi / 2)
dm_body_stream['left_shoulder2'].append(
AngleToAxes(l_shoulder_pos, l_elbow_pos, [0, 1, 1], np.arccos) -
np.pi / 2)
######
knee_quat = VecToQuat(body_stream[BodyPart.KNEE_R.value][i][3:7])
knee_eul = QuatToEuler(knee_quat *
np.quaternion(0, -0.707, -0, -0.707))
dm_body_stream['right_hip_x'].append(-knee_eul[2])
dm_body_stream['right_hip_y'].append(-knee_eul[0])
dm_body_stream['right_hip_z'].append(knee_eul[1])
######
knee_quat = VecToQuat(body_stream[BodyPart.KNEE_L.value][i][3:7])
knee_eul = QuatToEuler(knee_quat * np.quaternion(0, 0.707, -0, -0.707))
dm_body_stream['left_hip_x'].append(knee_eul[2])
dm_body_stream['left_hip_y'].append(-knee_eul[0])
dm_body_stream['left_hip_z'].append(-knee_eul[1])
######
dm_body_stream['right_knee'].append(
-AngleBetweenBodyPos(r_hip_pos, r_knee_pos, r_ankle_pos))
dm_body_stream['left_knee'].append(
-AngleBetweenBodyPos(l_hip_pos, l_knee_pos, l_ankle_pos))
dm_body_stream['right_elbow'].append(
AngleBetweenBodyPos(r_shoulder_pos, r_elbow_pos, r_wrist_pos) -
np.pi / 2)
dm_body_stream['left_elbow'].append(
AngleBetweenBodyPos(l_shoulder_pos, l_elbow_pos, l_wrist_pos) -
np.pi / 2)
return dm_body_stream
def process_transformation_kwargs(ell_max, **kwargs):
# Build the supertranslation and spacetime_translation arrays
supertranslation = np.zeros((4, ),
dtype=complex) # For now; may be resized below
ell_max_supertranslation = 1 # For now; may be increased below
if "supertranslation" in kwargs:
supertranslation = np.array(kwargs.pop("supertranslation"),
dtype=complex)
if supertranslation.dtype != "complex" and supertranslation.size > 0:
# I don't actually think this can ever happen...
raise TypeError(
"\nInput argument `supertranslation` should be a complex array with size>0.\n"
"Got a {} array of shape {}.".format(supertranslation.dtype,
supertranslation.shape))
# Make sure the array has size at least 4, by padding with zeros
if supertranslation.size <= 4:
supertranslation = np.lib.pad(supertranslation,
(0, 4 - supertranslation.size),
"constant",
constant_values=(0.0, ))
# Check that the shape is a possible array of scalar modes with complete (ell,m) data
ell_max_supertranslation = int(np.sqrt(len(supertranslation))) - 1
if (ell_max_supertranslation + 1)**2 != len(supertranslation):
raise ValueError(
"\nInput supertranslation parameter must contain modes from ell=0 up to some ell_max, "
"including\nall relevant m modes in standard order (see `spherical_functions` "
"documentation for details).\nThus, it must be an array with length given by a "
"perfect square; its length is {}".format(
len(supertranslation)))
# Check that the resulting supertranslation will be real
for ell in range(ell_max_supertranslation + 1):
for m in range(ell + 1):
i_pos = sf.LM_index(ell, m, 0)
i_neg = sf.LM_index(ell, -m, 0)
a = supertranslation[i_pos]
b = supertranslation[i_neg]
if abs(a - (-1.0)**m * b.conjugate()) > 3e-16 + 1e-15 * abs(b):
raise ValueError(
f"\nsupertranslation[{i_pos}]={a} # (ell,m)=({ell},{m})\n"
+
"supertranslation[{}]={} # (ell,m)=({},{})\n".format(
i_neg, b, ell, -m) +
"Will result in an imaginary supertranslation.")
spacetime_translation = np.zeros((4, ), dtype=float)
spacetime_translation[0] = sf.constant_from_ell_0_mode(
supertranslation[0]).real
spacetime_translation[1:4] = -sf.vector_from_ell_1_modes(
supertranslation[1:4]).real
if "spacetime_translation" in kwargs:
st_trans = np.array(kwargs.pop("spacetime_translation"), dtype=float)
if st_trans.shape != (4, ) or st_trans.dtype != "float":
raise TypeError(
"\nInput argument `spacetime_translation` should be a float array of shape (4,).\n"
"Got a {} array of shape {}.".format(st_trans.dtype,
st_trans.shape))
spacetime_translation = st_trans[:]
supertranslation[0] = sf.constant_as_ell_0_mode(
spacetime_translation[0])
supertranslation[1:4] = sf.vector_as_ell_1_modes(
-spacetime_translation[1:4])
if "space_translation" in kwargs:
s_trans = np.array(kwargs.pop("space_translation"), dtype=float)
if s_trans.shape != (3, ) or s_trans.dtype != "float":
raise TypeError(
"\nInput argument `space_translation` should be an array of floats of shape (3,).\n"
"Got a {} array of shape {}.".format(s_trans.dtype,
s_trans.shape))
spacetime_translation[1:4] = s_trans[:]
supertranslation[1:4] = sf.vector_as_ell_1_modes(
-spacetime_translation[1:4])
if "time_translation" in kwargs:
t_trans = kwargs.pop("time_translation")
if not isinstance(t_trans, float):
raise TypeError(
"\nInput argument `time_translation` should be a single float.\n"
"Got {}.".format(t_trans))
spacetime_translation[0] = t_trans
supertranslation[0] = sf.constant_as_ell_0_mode(
spacetime_translation[0])
# Decide on the number of points to use in each direction. A nontrivial supertranslation will introduce
# power in higher modes, so for best accuracy, we need to account for that. But we'll make it a firm
# requirement to have enough points to capture the original waveform, at least
w_ell_max = ell_max
ell_max = w_ell_max + ell_max_supertranslation
n_theta = kwargs.pop("n_theta", 2 * ell_max + 1)
n_phi = kwargs.pop("n_phi", 2 * ell_max + 1)
if n_theta < 2 * ell_max + 1 and abs(supertranslation[1:]).max() > 0.0:
warning = (
f"n_theta={n_theta} is small; because of the supertranslation, " +
f"it will lose accuracy for anything less than 2*ell+1={ell_max}")
warnings.warn(warning)
if n_theta < 2 * w_ell_max + 1:
raise ValueError(f"n_theta={n_theta} is too small; " +
"must be at least 2*ell+1={}".format(2 * w_ell_max +
1))
if n_phi < 2 * ell_max + 1 and abs(supertranslation[1:]).max() > 0.0:
warning = (
f"n_phi={n_phi} is small; because of the supertranslation, " +
f"it will lose accuracy for anything less than 2*ell+1={ell_max}")
warnings.warn(warning)
if n_phi < 2 * w_ell_max + 1:
raise ValueError(f"n_phi={n_phi} is too small; " +
"must be at least 2*ell+1={}".format(2 * w_ell_max +
1))
# Get the rotor for the frame rotation
frame_rotation = np.quaternion(
*np.array(kwargs.pop("frame_rotation", [1, 0, 0, 0]), dtype=float))
if frame_rotation.abs() < 3e-16:
raise ValueError(
f"frame_rotation={frame_rotation} should be a unit quaternion")
frame_rotation = frame_rotation.normalized()
# Get the boost velocity vector
boost_velocity = np.array(kwargs.pop("boost_velocity", [0.0] * 3),
dtype=float)
beta = np.linalg.norm(boost_velocity)
if boost_velocity.shape != (3, ) or beta >= 1.0:
raise ValueError(
"Input boost_velocity=`{}` should be a 3-vector with "
"magnitude strictly less than 1.0.".format(boost_velocity))
gamma = 1 / math.sqrt(1 - beta**2)
varphi = math.atanh(beta)
# These are the angles in the transformed system at which we need to know the function values
thetaprm_j_phiprm_k = np.array([[
[thetaprm_j, phiprm_k]
for phiprm_k in np.linspace(0.0, 2 * np.pi, num=n_phi, endpoint=False)
] for thetaprm_j in np.linspace(0.0, np.pi, num=n_theta, endpoint=True)])
# Construct the function that modifies our rotor grid to account for the boost
if beta > 3e-14: # Tolerance for beta; any smaller and numerical errors will have greater effect
vhat = boost_velocity / beta
def Bprm_j_k(thetaprm, phiprm):
"""Construct rotor taking r' to r
I derived this result in a different way, but I've also found it described in Penrose-Rindler Vol. 1,
around Eq. (1.3.5). Note, however, that their discussion is for the past celestial sphere,
so there's a sign difference.
"""
# Note: It doesn't matter which we use -- r' or r; all we need is the direction of the bivector
# spanned by v and r', which is the same as the direction of the bivector spanned by v and r,
# since either will be normalized, and one cross product is zero iff the other is zero.
rprm = np.array([
math.cos(phiprm) * math.sin(thetaprm),
math.sin(phiprm) * math.sin(thetaprm),
math.cos(thetaprm)
])
Thetaprm = math.acos(np.dot(vhat, rprm))
Theta = 2 * math.atan(math.exp(-varphi) * math.tan(Thetaprm / 2.0))
rprm_cross_vhat = np.quaternion(0.0, *np.cross(rprm, vhat))
if rprm_cross_vhat.abs() > 1e-200:
return (rprm_cross_vhat.normalized() * (Thetaprm - Theta) /
2).exp()
else:
return quaternion.one
else:
def Bprm_j_k(thetaprm, phiprm):
return quaternion.one
# Set up rotors that we can use to evaluate the SWSHs in the original frame
R_j_k = np.empty(thetaprm_j_phiprm_k.shape[:2], dtype=np.quaternion)
for j in range(thetaprm_j_phiprm_k.shape[0]):
for k in range(thetaprm_j_phiprm_k.shape[1]):
thetaprm_j, phiprm_k = thetaprm_j_phiprm_k[j, k]
R_j_k[j,
k] = (Bprm_j_k(thetaprm_j, phiprm_k) * frame_rotation *
quaternion.from_spherical_coords(thetaprm_j, phiprm_k))
return (
supertranslation,
ell_max_supertranslation,
ell_max,
n_theta,
n_phi,
boost_velocity,
beta,
gamma,
varphi,
R_j_k,
Bprm_j_k,
thetaprm_j_phiprm_k,
kwargs,
)
