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 train(self, rotations, ts, tau): q = rotations # Sanity-check input if len(q) != len(ts): raise RuntimeError("len(p) != len(ts)") # Initial- and goal positions self.q0 = Quaternion(q[0]) self.gq = Quaternion(q[-1]) # Differential time vector dt = np.gradient(ts)[:,np.newaxis] # Scaling factor # ***** Change to be the diagonal of the vector elements of 2*log(g*q.conj) error = 2*Quaternion.log(self.gq*self.q0.conjugate) self.Dp = np.diag(error.vector) Dp_inv = np.linalg.inv(self.Dp) # Desired velocities and accelerations # ***** Change to be the values of 2*log(g*q.conj) or some form of velocity and acceleration of the training data. d_q = [Quaternion([0,0,0,0])] dd_q = [Quaternion([0,0,0,0])] for i in range(len(q)-1): d_q.append( 2*Quaternion.log(q[i+1]*q[i].conjugate) / dt[i] ) for i in range(len(d_q)-1): dd_q.append( (d_q[i+1] - d_q[i]) / dt[i] ) dd_q.append(Quaternion([0, 0, 0, 0])) # Integrate canonical system x = self.cs.rollout(ts, tau) # Set up system of equations to solve for weights def features(xj): psi = np.exp(-self.h * (xj - self.c)**2) return xj * psi / psi.sum() def forcing(j): vel = 2*Quaternion.log(self.gq * q[j].conjugate) return Dp_inv.dot(tau**2 * dd_q[j].vector - self.alpha * (self.beta * (vel.vector) - tau * d_q[j].vector)) A = np.stack(features(xj) for xj in x) f = np.stack(forcing(j) for j in range(len(ts))) # Least squares solution for Aw = f (for each column of f) self.w = np.linalg.lstsq(A, f, rcond=None)[0].T # Cache variables for later inspection self.train_q = q self.train_d_q = d_q self.train_dd_q = dd_q
def L2dist(data, start, stop): breakn = 1 # This is the sliding window size d = 0 ssize = len(data) - breakn finaldistances = np.zeros( (ssize, ssize)) # ((len(samples._data)-2)* (len(samples._data)-2)) i = 0 for m in range(start, stop): # start limit=1 stop limit ssize+1 if m % 100 == 0: print('dist ', m) for n in range(1, ssize + 1): d = 0.0 for j in range(0, breakn): quatern_fault[:, i] = (odeint(samples.KinematicModel, quatern_fault[:, i - 1], [(data[m - 1 + j, 0]), (data[m + j, 0])], args=((data[m - 1 + j, 4:7]), )))[1, :] my_quaternion = Quaternion(quatern_fault[:, i]) spinnor = Quaternion.log(my_quaternion).elements[1:4] c1 = np.concatenate(((data[m - 1 + j, 4:7]), (spinnor[m - 1 + j, :]))) c2 = np.concatenate(((data[n - 1 + j, 4:7]), (spinnor[n - 1 + j, :]))) d += np.sum([(a - b)**2 for a, b in zip(c1, c2)]) finaldistances[m - 1][n - 1] = math.sqrt(d) i += 1 distanceMatrix = symmetrize((finaldistances)) return distanceMatrix
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 calcspinnors(data): breakn = 10 ssize = len(data) - breakn qua = np.zeros((4, breakn, ssize)) spinnor = np.zeros((3, breakn, ssize)) qua[:, 0, 0] = [1.0, 0.0, 0.0, 0.0] for i in range(1, ssize + 1): # if i % 100 == 0: # print('calculatingspin ', i) for j in range(0, breakn): qua[:, j, i - 1] = (odeint(samples.KinematicModel, qua[:, 0, 0], [(data[i - 1 + j, 0]), (data[i + j, 0])], args=((data[i - 1 + j, 1:4]), )))[1, :] my_quaternion = Quaternion(qua[:, j, i - 1]) spinnor[:, j, i - 1] = Quaternion.log(my_quaternion).elements[1:4] return spinnor
def log_Quaternion(quaternion): """Estimate the logarithm of a Quaternion""" log_quaternion = Quaternion.log(quaternion) return log_quaternion
def test_log(self): from math import log q = Quaternion(axis=[1,0,0], angle=pi) log_q = Quaternion.log(q) self.assertEqual(log_q, Quaternion(scalar=0, vector=[pi/2,0,0]))
def forcing(j): vel = 2*Quaternion.log(self.gq * q[j].conjugate) return Dp_inv.dot(tau**2 * dd_q[j].vector - self.alpha * (self.beta * (vel.vector) - tau * d_q[j].vector))