def test_quaternion_generation():
q = Quaternion()
identity = (1, 0, 0, 0)
assert all(i == j for i, j in zip(q, identity))
q_id = Quaternion.identity()
assert all(i == j for i, j in zip(q_id, identity))
def __init__(self, *args, **kwargs):
if len(args) == len(kwargs) == 0:
args = np.concatenate([P0().data, Q0().data], axis=1)
if len(args) == 2:
args = np.concatenate([args[0].data, args[1].data], axis=1)
super().__init__(*args, **kwargs)
self.p = Point(self.data[:, :3])
self.q = Quaternion(self.data[:, 3:])
def test_quaternion_pow():
_ = Quaternion.identity()
i = Quaternion(0, 1, 0, 0)
j = Quaternion(0, 0, 1, 0)
k = Quaternion(0, 0, 0, 1)
i2 = i**2
j2 = j**2
k2 = k**2
assert i2 == j2 == k2
def test_quaternion_add():
q1 = Quaternion(1, 2, 3, 4)
q2 = Quaternion(1, 2, 3, 4)
q3 = Quaternion(2, 4, 6, 8)
assert q1 + q2 == q3
q1 += q2
assert q1 == q3
with pytest.raises(TypeError):
q1 + 45
def maneuverRPY(rhdg: float, state: State,
enu2ned: Transformation) -> [float, float, float, Point]:
quat = state.att
# given runway heading rhdg, calculate roll angle as angle between
# rhdg/earthz plane and body x/y plane
#hv = Point(cos(rdhg), sin(rhdg), 0)
# velocity in contest frame?
vel3d = state.vel
quat.transform_point(vel3d)
rhdg = mPlane(rhdg, vel3d)
# this hzplane requires maneuvers to lie in a vertical plane
hzplane = Point(-sin(rhdg), cos(rhdg), 0)
bx = quat.transform_point(Point(1, 0, 0))
# the wind correction angle (WCA) relative to flight path is the
# angle between body frame x and hzplane
# This should be independent of roll and pitch: roll does not affect the direction
# of bx and pitch is a rotation about hzplane, which does not change the angle
wca_axis = cross_product(bx, hzplane)
wca = (np.pi / 2) - np.arctan2(vector_norm(wca_axis),
dot_product(bx, hzplane))
if abs(wca) > 12 * np.pi / 180:
print('large wca: {:.1f} degrees'.format(wca * 180 / np.pi))
# to back out wca, rotate about cross(bx, hzplane)
wca_axis = normalize_vector(wca_axis)
# this will be inv(quatc) if correct
r2hzp = Quaternion.from_axis_angle(wca_axis * -wca)
# this is the attitude with body x rotated into maneuver plane
fq = (r2hzp * quat).norm()
# calculate Euler pitch in maneuver plane
rpy = fq.to_euler()
pitch = rpy.y
# back out rhdg and pitch
ryaw = Quaternion.from_axis_angle(Point(0, 0, 1) * -rhdg)
rpitch = Quaternion.from_axis_angle(Point(0, 1, 0) * -pitch)
# remaining rotation should be roll relative to maneuver plane
rollq = (rpitch * ryaw * fq).norm()
axisr = Quaternion.to_axis_angle(rollq)
thetar = abs(axisr)
axisr /= thetar
direction = dot_product(axisr, Point(1, 0, 0))
# roll = np.sign(direction) * wrapPi(thetar)
# hack to invert roll
roll = np.sign(direction) * wrapPi(thetar + np.pi)
return [roll, pitch, wca, wca_axis]
def test_quaternion_sub():
q1 = Quaternion(1, 2, 3, 4)
q2 = Quaternion(1, 1, 0, 0)
q3 = Quaternion(0, 1, 3, 4)
assert q1 - q2 == q3
q1 -= q2
assert q1 == q3
with pytest.raises(TypeError):
q1 - 45
def test_quaternion_inverse():
q1 = Quaternion(1, 2, 3, 4)
inv = q1.inverse()
assert q1 * inv == Quaternion.identity()
with pytest.raises(ValueError):
zero_q = Quaternion(0, 0, 0, 0)
zero_q.inverse()
def normRotation(self, tmpSkel=None, refPtIdx=None):
'''tmpSkel: normalize every palm pose to the tmpSkel pose (template skeleton)
refPtIdx: indexes of joints on the palm
'''
if tmpSkel is None:
tmpSkel = self.frmList[0].norm_skel
if refPtIdx is None:
refPtIdx = self.refPtIdx
refIdx = []
for idx in refPtIdx:
refIdx += [idx * 3, idx * 3 + 1, idx * 3 + 2]
keep_list = set(range(3*self.skel_num)).\
difference(set(refIdx+range(self.centerPtIdx, self.centerPtIdx+3)))
keep_list = list(keep_list)
temp = tmpSkel[refIdx].copy()
temp.shape = (-1, 3)
for frm in self.frmList:
model = frm.norm_skel[refIdx]
model.shape = (-1, 3)
R = np.zeros((3, 3), np.float32)
for vt, vm in zip(temp, model):
R = R + np.dot(vm.reshape(3, 1), vt.reshape(1, 3))
U, s, V = svd(R, full_matrices=True)
R = np.dot(V.transpose(), U.transpose())
frm.quad = Quaternion(R)
frm.norm_skel.shape = (-1, 3)
frm.norm_skel = np.dot(R, frm.norm_skel.transpose())
frm.norm_skel = frm.norm_skel.flatten('F')
def build(p: Point, q: Quaternion):
if len(p) == len(q):
return Transformation(np.concatenate([p.data, q.data], axis=1))
elif len(p) == 1 and len(q) > 1:
return Transformation.build(p.tile(len(q)), q)
elif len(p) > 1 and len(q) >= 1:
return Transformation.build(q.tile(len(p)))
else:
raise ValueError("incompatible lengths")
def test_quaternion_normalize():
q1 = Quaternion(1, 2, 3, 4)
normalized = q1.normalize()
assert normalized.is_unit_quaternion()
with pytest.raises(ValueError):
zero_q = Quaternion(0, 0, 0, 0)
zero_q.normalize()
def test_quaternion_mul_basis():
_ = Quaternion.identity()
i = Quaternion(0, 1, 0, 0)
j = Quaternion(0, 0, 1, 0)
k = Quaternion(0, 0, 0, 1)
# test identity multiplication
assert _ * i == i
assert i * _ == i
assert j * _ == j
assert _ * j == j
assert _ * k == k
assert k * _ == k
i2 = i * i
j2 = j * j
k2 = k * k
ijk = i * j * k
assert i2 == j2 == k2 == ijk == -_
def test_rotate():
q = Quaternion.from_euler(Point(0, 0, np.pi / 2))
c = Coord.from_nothing(10)
rc = c.rotate(q)
assert rc.origin == P0()
np.testing.assert_almost_equal(rc.x_axis.data, PY(1, 10).data)
np.testing.assert_almost_equal(rc.y_axis.data, PX(-1, 10).data)
np.testing.assert_almost_equal(rc.z_axis.data, PZ(1, 10).data)
def test_from_coords():
c1 = Coord.from_xy(P0(), PX(), PY())
c2 = Coord.from_xy(P0(), PY(), PZ())
trans_to = Transformation.from_coords(c1, c2)
trans_from = Transformation.from_coords(c2, c1)
ps = Point(np.random.random((100, 3)))
np.testing.assert_array_almost_equal(
ps.data,
trans_from.translate(trans_to.translate(ps)).data)
qs = Quaternion.from_euler(ps)
np.testing.assert_array_almost_equal(
qs, trans_from.rotate(trans_to.rotate(qs)))
def transform(self,
transformantion: Transformation = Transformation(
Point(0.75, 0, 0),
Quaternion.from_euler(Point(np.pi, 0, -np.pi / 2)))):
return OBJ(transformantion.point(self.vertices), self.faces)
def from_coords(coord_a, coord_b):
q1 = Quaternion.from_rotation_matrix(
coord_b.rotation_matrix()).inverse()
q2 = Quaternion.from_rotation_matrix(coord_a.rotation_matrix())
return Transformation.build(coord_b.origin - coord_a.origin, -q1 * q2)
def inverse_rotation_matrix(self):
return Quaternion.from_rotation_matrix(self.rotation_matrix()).inverse().to_rotation_matrix()
def test_quaternion_neg():
q1 = Quaternion(1, 2, 3, 4)
assert -q1 == Quaternion(-1, -2, -3, -4)
def test_quaternion_div_real():
q1 = Quaternion(1, 2, 3, 4)
real = 4
q2 = Quaternion(*(i / real for i in q1))
assert q1 / real == q2
def test_quaternion_div():
q1 = Quaternion(1, 2, 3, 4)
assert q1 / q1 == Quaternion.identity()
class Transformation(Base):
cols = ["x", "y", "z", "rw", "rx", "ry", "rz"]
def __init__(self, *args, **kwargs):
if len(args) == len(kwargs) == 0:
args = np.concatenate([P0().data, Q0().data], axis=1)
if len(args) == 2:
args = np.concatenate([args[0].data, args[1].data], axis=1)
super().__init__(*args, **kwargs)
self.p = Point(self.data[:, :3])
self.q = Quaternion(self.data[:, 3:])
def offset(self, p: Point):
return Transformation(self.p + p, self.q)
def __getattr__(self, name):
if name in list("xyz"):
return getattr(self.translation, name)
if len(name) == 2 and name[0] == "r":
if name[1] in list("wxyz"):
return getattr(self.rotation, name[1])
raise AttributeError(name)
@staticmethod
def build(p: Point, q: Quaternion):
if len(p) == len(q):
return Transformation(np.concatenate([p.data, q.data], axis=1))
elif len(p) == 1 and len(q) > 1:
return Transformation.build(p.tile(len(q)), q)
elif len(p) > 1 and len(q) >= 1:
return Transformation.build(q.tile(len(p)))
else:
raise ValueError("incompatible lengths")
@staticmethod
def zero(count):
return Transformation.build(P0(count), Q0(count))
@property
def translation(self) -> Point:
return self.p
@property
def rotation(self) -> Quaternion:
return self.q
@staticmethod
def from_coords(coord_a, coord_b):
q1 = Quaternion.from_rotation_matrix(
coord_b.rotation_matrix()).inverse()
q2 = Quaternion.from_rotation_matrix(coord_a.rotation_matrix())
return Transformation.build(coord_b.origin - coord_a.origin, -q1 * q2)
def apply(self, oin: Union[Point, Quaternion]):
if isinstance(oin, Point):
return self.point(oin)
elif isinstance(oin, Quaternion):
return self.rotate(oin)
elif isinstance(oin, self.__class__):
return Transformation(self.apply(oin.p), self.apply(oin.q))
def rotate(self, oin: Union[Point, Quaternion]):
if isinstance(oin, Point):
return self.q.transform_point(oin)
elif isinstance(oin, Quaternion):
return self.q * oin
def translate(self, point: Point):
return point + self.p
def point(self, point: Point):
return self.translate(self.rotate(point))
def coord(self, coord):
return coord.translate(self.p).rotate(self.q.to_rotation_matrix())
def to_matrix(self):
outarr = np.identity(4).reshape(1, 4, 4)
outarr[:, :3, :3] = self.rotation.to_rotation_matrix()
outarr[:, 3, :3] = self.translation.data
return outarr
def test_quaternion_mul_real():
q1 = Quaternion(1, 2, 3, 4)
real = 4
q2 = Quaternion(*(i * real for i in q1))
assert q1 * real == q2
