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]))