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