def quaternion_R(log_quaternion):
"""Converts the logarithm quaternion to a matrix of rotation 3x3"""
q_exp = Quaternion.exp(log_quaternion)
unit = q_exp.normalised
R = unit.rotation_matrix
return R
def tangent(self, prev_iso, next_iso):
t = 0.5 * (next_iso.t - prev_iso.t)
l1 = Quaternion.log((self.q.inverse * prev_iso.q).normalised)
l2 = Quaternion.log((self.q.inverse * next_iso.q).normalised)
e = Quaternion()
e.q = -0.25 * (l1.q + l2.q)
e = self.q * Quaternion.exp(e)
return Isometry(t=t, q=e)
def step(line, t):
floats = list(
map(float,
re.findall(b"[-+]?\d*\.\d+e[+-]\d+|[-+?]?\d*\.\d+|[-+]\d+", line)))
if len(floats) < 4:
print(line)
return ""
q_att = quat(*floats[:4])
a = min(np.pi, 1.0 * np.pi / 180 * t)
qr = quat(0, 0, -a, 0)
q = quat.exp(qr / 2) * q_att
#q = q_att
return ",".join(map(str, list(q))).encode('utf-8')
def step(self, x, dt, tau):
def fp(xj):
psi = np.exp(-self.h * (xj - self.c)**2)
return self.Dp.dot(self.w.dot(psi) / psi.sum() * xj)
f = fp(x)
f_alt = np.array([0, f[0], f[1], f[2]])
#self.eta[0] = 0
self.eta_dot = (self.alpha*(self.beta*2*Quaternion.log( self.gq * self.q.conjugate ) - self.eta)+f_alt)
#self.eta_dot[0] = 0
self.eta += self.eta_dot * dt / tau
self.q = Quaternion.exp((dt/2)*self.eta/tau)*self.q
return self.q, self.eta/tau, self.eta_dot
def test_exp(self):
from math import exp
q = Quaternion(axis=[1,0,0], angle=pi)
exp_q = Quaternion.exp(q)
self.assertEqual(exp_q, exp(0) * Quaternion(scalar=cos(1.0), vector=[sin(1.0), 0,0]))
