def CayleySelig(B):
Rp = vsr.Mot(1.0, B[0], B[1], B[2], 0.0, 0.0, 0.0, 0.0)
Rn = vsr.Mot(1.0, -B[0], -B[1], -B[2], 0.0, 0.0, 0.0, 0.0)
Rninv = Rn.inv()
eps = vsr.Mot(0,0,0,0,0,0,0,-1)
b = vsr.Mot(0.0, B[5], -B[4], B[3], 0.0, 0.0, 0.0, 0.0)
return Rp * Rninv + eps * Rninv * b * Rninv * 2
def CayleyLi(B):
BB = B * B
Rp = vsr.Mot(1.0, B[0], B[1], B[2], B[3], B[4], B[5], 0.0)
R0 = vsr.Mot(1.0 - BB[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
R4 = vsr.Mot(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, BB[7])
Rn = R0 + R4
Rden = R0 * R0
return Rp * Rp * Rn * Rden.inv()
def test(a,b):
g = (vsr.CGA(vsr.Mot(0,1,0,0,0,0,0,0)).comm(vsr.CGA(a)) * vsr.CGA(b) * 2.0)[0]
h = (a[1]* b[0] - a[0] * b[1]) * 2
print(h)
print(g)
i = (-a[0] * b[0] - a[1] * b[1] ) * 4.0
j = (vsr.CGA(vsr.Mot(0,1,0,0,0,0,0,0)).comm(vsr.CGA(vsr.Mot(0,1,0,0,0,0,0,0)).comm(vsr.CGA(a))) * vsr.CGA(b) * 4.0)[0]
print(i)
print(j)
def solve(self):
L = self.L
Lrr = L[:4, :4]
Lrq = L[:4, 4:]
Lqr = L[4:, :4]
Lqq = L[4:, 4:]
Lp = Lrr - np.dot(Lrq, np.dot(np.linalg.pinv(Lqq), Lqr))
w, v = np.linalg.eig(Lp)
r = v[:, np.argmax(w)]
q = np.dot(-np.dot(np.linalg.pinv(Lqq), Lqr), r)
return vsr.Mot(*np.array([r, q]).ravel())
def main():
m = create_motor()
m = vsr.Mot(1,0,0,0,0,0,0,0)
print(m)
motor = create_motor()
print(motor)
points = create_points(motor)
errs = []
for i in range(100):
m, err, gnorm = update(points, m)
if gnorm < 1e-3:
break
if err < 1e-6:
break
errs.append(err)
print(i)
print(m)
# print(m.rev() * motor)
plt.semilogy(errs)
plt.show()
def oexp(B):
n = np.sqrt(1 + B[0] * B[0] + B[1] * B[1] + B[2] * B[2])
s = B[0] * B[5] - B[1] * B[4] + B[2] * B[3]
m = vsr.Mot(1.0, B[0], B[1], B[2], B[3], B[4], B[5], s) * (1.0 / n)
return m