def stoch_descent(X, y, alpha, w):
"""
Stochastic gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs, logs_stoch
logs = []
logs_stoch = []
random.seed(0)
idx = list(range(len(X)))
for epoch in range(1000):
random.shuffle(idx)
w_old = w
for i in idx:
loss = y[i] - vector.dot(X[i], w)
gradient = vector.mul(loss, X[i])
w = vector.add(w, vector.mul(alpha, gradient))
logs_stoch += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
logs += (w, alpha, sse(X, y, w))
print("Epoch", epoch)
return w
def cc_int(p1, r1, p2, r2):
"""
Intersect circle (p1,r1) circle (p2,r2)
where p1 and p2 are 2-vectors and r1 and r2 are scalars
Returns a list of zero, one or two solution points.
"""
d = vector.norm(p2-p1)
if not tol_gt(d, 0):
return []
u = ((r1*r1 - r2*r2)/d + d)/2
if tol_lt(r1*r1, u*u):
return []
elif r1*r1 < u*u:
v = 0.0
else:
v = math.sqrt(r1*r1 - u*u)
s = (p2-p1) * u / d
if tol_eq(vector.norm(s),0):
p3a = p1+vector.vector([p2[1]-p1[1],p1[0]-p2[0]])*r1/d
if tol_eq(r1/d,0):
return [p3a]
else:
p3b = p1+vector.vector([p1[1]-p2[1],p2[0]-p1[0]])*r1/d
return [p3a,p3b]
else:
p3a = p1 + s + vector.vector([s[1], -s[0]]) * v / vector.norm(s)
if tol_eq(v / vector.norm(s),0):
return [p3a]
else:
p3b = p1 + s + vector.vector([-s[1], s[0]]) * v / vector.norm(s)
return [p3a,p3b]
def make_hcs_3d (a, b, c, righthanded=True):
"""build a 3D homogeneous coordiate system from three points. The origin is point a. The x-axis is
b-a, the y axis is c-a, or as close as possible after orthogonormalisation."""
# create orthonormal basis
u = normalised(b-a)
v = normalised(c-a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu,0.0) and tol_eq(nv,0.0):
# all points equal, no rotation
u = vector.vector([1.0,0.0,0.0])
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0.0):
# determine u perpendicular from v
u,dummy = perp_3d(v)
elif tol_eq(nv, 0.0):
# determine v perpendicular from u
dummy,v = perp_3d(u)
# ensure that u and v are different
if tol_eq(vector.norm(u-v),0.0):
dummy,v = perp_3d(u)
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
# flip basis if lefthanded desired
if righthanded==False:
w = -w
# create matix with basis vectors + translation as columns
hcs = Mat([
[u[0],v[0], w[0], a[0]],
[u[1],v[1], w[1], a[1]],
[u[2],v[2], w[2], a[2]],
[0.0, 0.0, 0.0, 1.0] ])
return hcs
def angle_3p(p1, p2, p3):
"""Returns the angle, in radians, rotating vector p2p1 to vector p2p3.
arg keywords:
p1 - a vector
p2 - a vector
p3 - a vector
returns: a number
In 2D, the angle is a signed angle, range [-pi,pi], corresponding
to a clockwise rotation. If p1-p2-p3 is clockwise, then angle > 0.
In 3D, the angle is unsigned, range [0,pi]
"""
d21 = vector.norm(p2-p1)
d23 = vector.norm(p3-p2)
if tol_eq(d21,0) or tol_eq(d23,0):
# degenerate, indeterminate angle
return None
v21 = (p1-p2) / d21
v23 = (p3-p2) / d23
t = vector.dot(v21,v23) # / (d21 * d23)
if t > 1.0: # check for floating point error
t = 1.0
elif t < -1.0:
t = -1.0
angle = math.acos(t)
if len(p1) == 2: # 2D case
if is_counterclockwise(p1,p2,p3):
angle = -angle
return angle
def make_hcs_2d (a, b):
"""build a 2D homogeneus coordiate system from two points"""
u = b-a
if tol_eq(vector.norm(u), 0.0):
u = vector.vector([0.0,0.0])
else:
u = u / vector.norm(u)
v = vector.vector([-u[1], u[0]])
hcs = Mat([ [u[0],v[0],a[0]] , [u[1],v[1],a[1]] , [0.0, 0.0, 1.0] ] )
return hcs
def test_perp_3d():
for i in range(10):
u = normalised(vector.randvec(3))
v,w = perp_3d(u)
print u, vector.norm(u)
print v, vector.norm(v)
print w, vector.norm(w)
print tol_eq(vector.dot(u,v),0.0)
print tol_eq(vector.dot(v,w),0.0)
print tol_eq(vector.dot(w,u),0.0)
def distance_point_line(p,l1,l2):
"""distance from point p to line l1-l2"""
# v,w is p, l2 relative to l1
v = p-l1
w = l2-l1
# x = projection v on w
lw = vector.norm(w)
if tol_eq(lw,0):
x = 0*w
else:
x = w * vector.dot(v,w) / lw
# result is distance x,v
return vector.norm(x-v)
def biCGsolve(self, x0, b, tol=1.0e-10, nmax=1000):
"""
Solve self*x = b and return x using the bi-conjugate gradient method
"""
try:
if not vector.isVector(b):
raise TypeError, self.__class__, " in solve "
else:
if self.size()[0] != len(b) or self.size()[1] != len(b):
print("**Incompatible sizes in solve")
print("**size()=", self.size()[0], self.size()[1])
print("**len=", len(b))
else:
kvec = diag(self) # preconditionner
n = len(b)
x = x0 # initial guess
r = b - dot(self, x)
rbar = r
w = r / kvec
wbar = rbar / kvec
p = vector.zeros(n)
pbar = vector.zeros(n)
beta = 0.0
rho = vector.dot(rbar, w)
err = vector.norm(dot(self, x) - b)
k = 0
print(" bi-conjugate gradient convergence (log error)")
while abs(err) > tol and k < nmax:
p = w + beta * p
pbar = wbar + beta * pbar
z = dot(self, p)
alpha = rho / vector.dot(pbar, z)
r = r - alpha * z
rbar = rbar - alpha * dot(pbar, self)
w = r / kvec
wbar = rbar / kvec
rhoold = rho
rho = vector.dot(rbar, w)
x = x + alpha * p
beta = rho / rhoold
err = vector.norm(dot(self, x) - b)
print(k, " %5.1f " % math.log10(err))
k = k + 1
return x
except:
print("ERROR ", self.__class__, "::biCGsolve")
def biCGsolve(self,x0, b, tol=1.0e-10, nmax = 1000): """ Solve self*x = b and return x using the bi-conjugate gradient method """ try: if not vector.isVector(b): raise TypeError, self.__class__,' in solve ' else: if self.size()[0] != len(b) or self.size()[1] != len(b): print '**Incompatible sizes in solve' print '**size()=', self.size()[0], self.size()[1] print '**len=', len(b) else: kvec = diag(self) # preconditionner n = len(b) x = x0 # initial guess r = b - dot(self, x) rbar = r w = r/kvec; wbar = rbar/kvec; p = vector.zeros(n); pbar = vector.zeros(n); beta = 0.0; rho = vector.dot(rbar, w); err = vector.norm(dot(self,x) - b); k = 0 print " bi-conjugate gradient convergence (log error)" while abs(err) > tol and k < nmax: p = w + beta*p; pbar = wbar + beta*pbar; z = dot(self, p); alpha = rho/vector.dot(pbar, z); r = r - alpha*z; rbar = rbar - alpha* dot(pbar, self); w = r/kvec; wbar = rbar/kvec; rhoold = rho; rho = vector.dot(rbar, w); x = x + alpha*p; beta = rho/rhoold; err = vector.norm(dot(self, x) - b); print k,' %5.1f ' % math.log10(err) k = k+1 return x except: print 'ERROR ',self.__class__,'::biCGsolve'
def distance_2p(p1, p2):
"""Returns the euclidean distance between two points
arg keywords:
p1 - a vector
p2 - a vector
returns: a number
"""
return vector.norm(p2 - p1)
def circleArcAroundPivotClosestPieces(pivot, start, end, smoothingFactor):
pivotToStart = vector.translate(vector.minus(pivot))(start)
pivotToEnd = vector.translate(vector.minus(pivot))(end)
smallerNorm = min(vector.norm(pivotToStart),vector.norm(pivotToEnd))
def findActualEndPoint(vec):
unitVec = vector.unit(vec)
return vector.translate(pivot)(vector.scale(vector.unit(vec),smallerNorm*smoothingFactor))
circularPart = circleArcAroundPivot(pivot,findActualEndPoint(pivotToStart),findActualEndPoint(pivotToEnd))
#FIX!!
if smallerNorm == vector.norm(pivotToStart):
output = circularPart
if smallerNorm != vector.norm(pivotToEnd):
output.attachPartial(1, cutSegment(pivot, end, smallerNorm * smoothingFactor / vector.norm(pivotToEnd), smoothingFactor))
else:
output = cutSegment(start, pivot, 1 - smoothingFactor, 1 - smallerNorm * smoothingFactor / vector.norm(pivotToStart))
output.attachPartial(1, circularPart)
return output
def make_hcs_2d_scaled (a, b):
"""build a 2D homogeneus coordiate system from two points, but scale with distance between input point"""
u = b-a
if tol_eq(vector.norm(u), 0.0):
u = vector.vector([1.0,0.0])
#else:
# u = u / vector.norm(u)
v = vector.vector([-u[1], u[0]])
hcs = Mat([ [u[0],v[0],a[0]] , [u[1],v[1],a[1]] , [0.0, 0.0, 1.0] ] )
return hcs
def sss_int(p1, r1, p2, r2, p3, r3):
"""Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively.
Returns a list of zero, one or two solution points.
"""
solutions = []
# intersect circles in plane
cp1 = vector.vector([0.0,0.0])
cp2 = vector.vector([vector.norm(p2-p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# determine normal of plane though p1, p2, p3
n = vector.cross(p2-p1, p3-p1)
if not tol_eq(vector.norm(n),0.0):
n = n / vector.norm(n)
else:
# traingle p1, p2, p3 is degenerate
# check for 2d solutions
if len(cpxs) == 0:
return []
# project cpxs back to 3d and check radius r3
cp4 = cpxs[0]
u = normalised(p2-p1)
v,w = perp_3d(u)
p4 = p1 + cp4[0] * u + cp4[1] * v
if tol_eq(vector.norm(p4-p3), r3):
return [p4]
else:
return []
# px, rx, nx is circle
px = p1 + (p2-p1) * cpxs[0][0] / vector.norm(p2-p1)
rx = abs(cpxs[0][1])
nx = p2-p1
nx = nx / vector.norm(nx)
# py is projection of p3 on px,nx
dy3 = vector.dot(p3-px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
# ry is radius of circle in py
if tol_eq(r3,0.0):
ry = 0.0
else:
ry = math.sin(math.acos(min(1.0,abs(dy3/r3))))*r3
# determine intersection of circle px, rx and circle py, ry, projected relative to line py-px
cpx = vector.vector([0.0,0.0])
cpy = vector.vector([vector.norm(py-px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py-px) * cp4[0] / vector.norm(py-px) + n * cp4[1]
solutions.append(p4)
return solutions
def CGsolve(self, x0, b, tol=1.0e-10, nmax=1000, verbose=1):
"""
Solve self*x = b and return x using the conjugate gradient method
"""
if not vector.isVector(b):
raise TypeError, self.__class__, " in solve "
else:
if self.size()[0] != len(b) or self.size()[1] != len(b):
print("**Incompatible sizes in solve")
print("**size()=", self.size()[0], self.size()[1])
print("**len=", len(b))
else:
kvec = diag(self) # preconditionner
n = len(b)
x = x0 # initial guess
r = b - dot(self, x)
try:
w = r / kvec
except:
print("***singular kvec")
p = vector.zeros(n)
beta = 0.0
rho = vector.dot(r, w)
err = vector.norm(dot(self, x) - b)
k = 0
if verbose:
print(" conjugate gradient convergence (log error)")
while abs(err) > tol and k < nmax:
p = w + beta * p
z = dot(self, p)
alpha = rho / vector.dot(p, z)
r = r - alpha * z
w = r / kvec
rhoold = rho
rho = vector.dot(r, w)
x = x + alpha * p
beta = rho / rhoold
err = vector.norm(dot(self, x) - b)
if verbose:
print(k, " %5.1f " % math.log10(err))
k = k + 1
return x
def circleArc(center, start, end):
radToStart = vector.translate(vector.minus(center))(start)
radToEnd = vector.translate(vector.minus(center))(end)
radiusSquared = vector.norm(radToStart)*vector.norm(radToEnd)
radius = math.sqrt(radiusSquared)
innerProductValue = vector.innerproduct(radToStart, radToEnd)
angle = math.acos(innerProductValue/radiusSquared)
firstUnitRadial = vector.unit(radToStart)
secondUnitRadial = vector.unit(vector.translate(vector.scale(radToStart,-innerProductValue/radiusSquared))(radToEnd))
def CN(t):
point = vector.translate(center)(vector.translate(vector.scale(firstUnitRadial,radius*math.cos(t/radius)))(vector.scale(secondUnitRadial,radius*math.sin(t/radius))))
tangent = vector.translate(vector.scale(firstUnitRadial,-math.sin(t/radius)))(vector.scale(secondUnitRadial,math.cos(t/radius)))
output = [point,tangent]
#output.append([math.cos(t/radius),math.sin(t/radius),0])
return output
return TwistingAxis(angle*radius,CN)
def test_cc_int():
"""Generates two random circles, computes the intersection,
and verifies that the number of intersections and the
positions of the intersections are correct.
Returns True or False"""
# gen two random circles p1,r2 and p2, r2
p1 = vector.randvec(2, 0.0, 10.0,1.0)
r1 = random.uniform(0, 10.0)
p2 = vector.randvec(2, 0.0, 10.0,1.0)
r2 = random.uniform(0, 10.0)
# 33% change that r2=abs(r1-|p1-p2|)
if random.random() < 0.33:
r2 = abs(r1-vector.norm(p1-p2))
# do interesection
diag_print("problem:"+str(p1)+","+str(r1)+","+str(p2)+","+str(r2),"test_cc_int")
sols = cc_int(p1, r1, p2, r2)
diag_print("solutions:"+str(map(str, sols)),"test_cc_int")
# test number of intersections
if len(sols) == 0:
if not tol_gt(vector.norm(p2-p1),r1 + r2) and not tol_lt(vector.norm(p2-p1),abs(r1 - r2)) and not tol_eq(vector.norm(p1-p2),0):
diag_print("number of solutions 0 is wrong","test_cc_int")
return False
elif len(sols) == 1:
if not tol_eq(vector.norm(p2-p1),r1 + r2) and not tol_eq(vector.norm(p2-p1),abs(r1-r2)):
diag_print("number of solutions 1 is wrong","test_cc_int")
return False
elif len(sols) == 2:
if not tol_lt(vector.norm(p2-p1),r1 + r2) and not tol_gt(vector.norm(p2-p1),abs(r1 - r2)):
diag_prin("number of solutions 2 is wrong")
return False
else:
diag_print("number of solutions > 2 is wrong","test_cc_int")
return False
# test intersection coords
for p3 in sols:
if not tol_eq(vector.norm(p3-p1), r1):
diag_print("solution not on circle 1","test_cc_int")
return False
if not tol_eq(vector.norm(p3-p2), r2):
diag_print("solution not on circle 2","test_cc_int")
return False
diag_print("OK","test_cc_int")
return True
def make_hcs_3d_scaled (a, b, c):
"""build a 3D homogeneus coordiate system from three points, and derive scale from distance between points"""
# create orthonormal basis
u = normalised(b-a)
v = normalised(c-a)
nu = vector.norm(u)
nv = vector.norm(v)
if tol_eq(nu,0) and tol_eq(nv,0):
# all points equal, no rotation
u = vector.vector([1.0,0.0,0.0])
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0):
# determine u perpendicular from v
u,dummy = perp_3d(v)[0]
elif tol_eq(nv, 0):
# determine v perpendicular from u
dummy,v = perp_3d(u)[0]
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
# scale again
if not tol_eq(vector.norm(b-a),0.0):
u = u / vector.norm(b-a)
if not tol_eq(vector.norm(c-a),0.0):
v = v / vector.norm(c-a)
# note: w is not scaled
# create matix with basis vectors + translation as columns
hcs = Mat([
[u[0],v[0], w[0], a[0]],
[u[1],v[1], w[1], a[1]],
[u[2],v[2], w[2], a[2]],
[0.0, 0.0, 0.0, 1.0] ])
return hcs
def show_debug_info(self, itnum):
conv_vect = array([vector.norm(task.f()) for task in self.tasks])
conv_str = ["%10.8f" % x for x in conv_vect]
print " %4d: %s" % (itnum, ' '.join(conv_str))
for task in self.tasks:
if type(task) is COMTask:
com = self.robot.compute_com(self.robot.rave.GetDOFValues())
pymanoid_sage.rave.display_box(
self.robot.env, com, box_id="COM", color='g',
thickness=0.01)
pymanoid_sage.rave.display_box(
self.robot.env, task.target, box_id='Target', color='b',
thickness=0.01)
def batch_descent(X, y, alpha, w):
"""
Batch gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs
logs = []
alpha /= len(X)
for epoch in range(1, 1000):
loss = vector.sub(y, vector.mul_mat_vec(X, w))
gradient = vector.mul_mat_vec(vector.transpose(X), loss)
w_old = w
w = vector.add(w, vector.mul(alpha, gradient))
logs += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
print("Epoch", epoch)
return w
def test_rr_int():
"""test random ray-ray intersection. returns True iff succesful"""
# generate tree points A,B,C an two rays AC, BC.
# then calculate the intersection of the two rays
# and check that it equals C
p_a = vector.randvec(2, 0.0, 10.0,1.0)
p_b = vector.randvec(2, 0.0, 10.0,1.0)
p_c = vector.randvec(2, 0.0, 10.0,1.0)
# print p_a, p_b, p_c
if tol_eq(vector.norm(p_c - p_a),0) or tol_eq(vector.norm(p_c - p_b),0):
return True # ignore this case
v_ac = (p_c - p_a) / vector.norm(p_c - p_a)
v_bc = (p_c - p_b) / vector.norm(p_c - p_b)
s = rr_int(p_a, v_ac, p_b, v_bc)
if tol_eq(math.fabs(vector.dot(v_ac, v_bc)),1.0):
return len(s) == 0
else:
if len(s) > 0:
p_s = s[0]
return tol_eq(p_s[0],p_c[0]) and tol_eq(p_s[1],p_c[1])
else:
return False
def segment(start, end):
startToEnd = vector.translate(vector.minus(start))(end)
distance = vector.norm(startToEnd)
try:
tng = vector.unit(startToEnd)
def CN(t):
output = [vector.translate(start)(vector.scale(tng,t)),tng]
#output.append(normal)
return output
except:
def CN(t):
output = [start, [0,0,0]]
return TwistingAxis(distance, CN)
def f_new_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0] * n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z) / vector.norm(y), -1), 1)
return n
r1 = map(lambda y, z: fac * beta[1] * (beta[2] - dip(y, z)), m, g)
return r0 + r1
def f_new_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0] * a + beta[1] * b, initial[:3],
u, v)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[2] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
def deg(y, z):
n = min(max(vector.dot(y, z) / vector.norm(y), -1), 1)
return math.degrees(math.asin(n))
r1 = map(lambda y, z: fac * beta[2] * (beta[3] - deg(y, z)), m, g)
return r0 + r1
def f_new_sphere3(beta, x):
b = numpy.matrix(lmap(lambda a, b: a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y: beta[3] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z) / vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z: fac * beta[3] * (beta[4] - dip(y, z)), m, g)
return r0 + r1
def _call(v):
if len(hv) < 1:
v_reduced = (0, 0, 0)
else:
# Compute the direction of average acceleration
a_hat = vector.norm(vector.vector_mean(hv))
# Remove the component parallel to average accleration
v_parallel = vector.vector_multiply(vector.dot(v, a_hat), a_hat)
v_reduced = vector.vector_subtract(v, v_parallel)
# Update the histopy elements with the new vector
hv.append(v)
if len(hv) > history:
hv.pop(0)
return v_reduced
def boundarycompute(self):
try:
return self.boundary
except:
if self[0].str2[6:9] != 'GLY':
atoms = self.findatoms([" N "," C "," CA "," O "," OT1", " CB "])
else:
atoms = self.findatoms([" N "," C "," CA "," O "," OT1", " HA2"])
atoms.remove(0)
posterm = vector.Nlocation(atoms[1].coords(),atoms[2].coords(),atoms[3].coords())
axis = vector.translate(vector.minus(atoms[0].coords()))(posterm)
N1 = vector.unit(axis)
length = vector.norm(axis)
N2 = vector.unit(vector.translate(vector.minus(atoms[2].coords()))(atoms[4].coords()))
self.boundary = [atoms[0].coords(), N1, N2, length]
return self.boundary
def ComputeDeviation(points, fit):
m, d = 0, 0
for p in points:
v = vector.sub(p[:3], fit[:3])
m += (1 - vector.dot(v, v) / fit[3]**2)**2
if len(fit) > 4:
n = vector.dot(v, p[3:]) / vector.norm(v)
if abs(n) <= 1:
ang = math.degrees(math.asin(n))
d += (fit[4] - ang)**2
else:
d += 1e111
m /= len(points)
d /= len(points)
return [m**.5, d**.5]
def make_hcs_3d_scaled(a, b, c):
"""build a 3D homogeneus coordiate system from three vectors"""
# create orthnormal basis
u = b - a
u = u / vector.norm(u)
v = c - a
v = v / vector.norm(v)
w = vector.cross(u, v)
v = vector.cross(w, u)
# scale
u = u / vector.norm(u) / vector.norm(b - a)
v = v / vector.norm(v) / vector.norm(c - a)
hcs = Mat([[u[0], v[0], w[0], a[0]], [u[1], v[1], w[1], a[1]],
[u[2], v[2], w[2], a[2]], [0.0, 0.0, 0.0, 1.0]])
return hcs
def boundarycompute(self):
try:
return self.boundary
except:
if self[0].str2[6:9] != 'GLY':
atoms = self.findatoms(
[" N ", " C ", " CA ", " O ", " OT1", " CB "])
else:
atoms = self.findatoms(
[" N ", " C ", " CA ", " O ", " OT1", " HA2"])
atoms.remove(0)
posterm = vector.Nlocation(atoms[1].coords(), atoms[2].coords(),
atoms[3].coords())
axis = vector.translate(vector.minus(atoms[0].coords()))(posterm)
N1 = vector.unit(axis)
length = vector.norm(axis)
N2 = vector.unit(
vector.translate(vector.minus(atoms[2].coords()))(
atoms[4].coords()))
self.boundary = [atoms[0].coords(), N1, N2, length]
return self.boundary
def smoothedPieceWiseLinearFullFunctionality(points, mapFromSpaceToTime, Wnew, smoothingFactor = 0.33):
if len(points) < 2:
print("Error: only " + str(len(points)) + " points supplied to smoothedPieceWiseLinear")
return
currentPoints = copy.deepcopy(points)
numPoints = len(currentPoints)
attachElements = [cutSegment(points[0],points[1],0,smoothingFactor)]
for i in range(0,numPoints - 2):
lastToPivot = vector.translate(vector.minus(points[i]))(points[i+1])
pivotToNext = vector.translate(vector.minus(points[i+1]))(points[i+2])
angle = vector.innerproduct(lastToPivot, pivotToNext)/(vector.norm(lastToPivot)*vector.norm(pivotToNext))
collinear = (abs(angle) < 0.001)
if collinear:
attachElements.append(cutSegment(points[i],points[i+1],smoothingFactor,1))
attachElements.append(cutSegment(points[i+1],points[i+2],1,smoothingFactor))
else:
attachElements.append(cutSegment(points[i],points[i+1],smoothingFactor,1-smoothingFactor))
attachElements.append(circleArcAroundPivotClosestPieces(points[i+1],points[i],points[i+2],smoothingFactor))
attachElements.append(cutSegment(points[-2],points[-1],smoothingFactor,1))
def iteratedAttach(start, end):
if start < end:
midpoint = (start + end)/2
iteratedAttach(start, midpoint)
iteratedAttach(midpoint + 1, end)
attachElements[start].attachPartial(1, attachElements[midpoint + 1])
iteratedAttach(0, len(attachElements)-1)
Wmap = attachElements[0]
produceN2 = lambda x : vector.unit(vector.orthogonalComponent(vector.translate(vector.minus(Wnew.ev(mapFromSpaceToTime(x[0]))[0]))(x[0]),x[1]))
appendN2 = lambda x: [x[0],x[1],produceN2(x)]
Wmap.CN = compose(appendN2,Wmap.CN)
return [Wmap, Wnew, lambda t: mapFromSpaceToTime(Wmap.ev(t)[0])]
def poll(self):
msgs = self.client.receive()
for name in msgs:
value = msgs[name]
if name == 'imu.accel.calibration':
self.s.AccelCalValid = True
b, t = value[0][:3], value[0][3]
self.s.AccelCalMin = b[0] - t, b[1] - t, b[2] - t
self.s.AccelCalMax = b[0] + t, b[1] + t, b[2] + t
elif name == 'imu.compass.calibration':
self.compass_calibration_updated = True
self.s.CompassCalEllipsoidValid = True
self.s.CompassCalEllipsoidOffset = tuple(value[0][:3])
#rtimu.resetFusion()
elif name == 'imu.rate':
self.rate = value
print('imu rate set to rate', value)
if not self.lastdata:
return
gyro, compass = self.lastdata
self.lastdata = False
# see if gyro is out of range, sometimes the sensors read
# very high gyro readings and the sensors need to be reset by software
# this is probably a bug in the underlying driver with fifo misalignment
d = .05 / self.rate # filter constant
for i in range(3): # filter gyro vector
self.avggyro[i] = (1 - d) * self.avggyro[i] + d * gyro[i]
if vector.norm(self.avggyro) > .8: # 55 degrees/s
print('too high standing gyro bias, resetting sensors', gyro,
self.avggyro)
self.init()
# detects the problem even faster:
if any(map(lambda x: abs(x) > 1000, compass)):
print('compass out of range, resetting', compass)
self.init()
def check_diff(self, m=10, reltol=0.05):
"""Check differential constraints (e.g., dq = qd * dt).
m -- number of random time instants to test
reltol -- tolerance in relative error
"""
tset = numpy.random.random(m) * self.duration
q, qd, qdd, dt = self.q, self.qd, self.qdd, 1e-5
for t in tset:
q_dev = q(t + dt) - q(t) - dt * qd(t)
qd_dev = qd(t + dt) - qd(t) - dt * qdd(t)
if norm(q_dev) > reltol * dt * norm(qd(t)):
raise TrajectoryError("bad velocity", self, t)
if norm(qd_dev) > reltol * dt * norm(qdd(t)):
kron = norm(qd_dev), reltol * dt * norm(qdd(t))
msg = "bad acceleration (%e > %e)" % kron
raise TrajectoryError(msg, self, t)
def fit_batch(X, y, alpha, w, epochs=500, epsilon=1.0e-5):
"""
Batch gradient descent
:param X:
:param y:
:param alpha:
:param w:
:param epochs:
:param epsilon:
:return:
"""
global logs
logs = []
alpha /= len(X)
for epoch in range(epochs):
y_hat = predict(X, w)
loss = vector.sub(y, y_hat)
gradient = vector.mul_mat_vec(vector.transpose(X), loss)
w = vector.add(w, vector.mul(alpha, gradient))
logs += (w, alpha, sse(X, y, w))
if vector.norm(gradient) < epsilon:
break
print("Epoch", epoch)
return w
def test_sss_int():
p1 = vector.randvec(3, 0.0, 10.0,1.0)
p2 = vector.randvec(3, 0.0, 10.0,1.0)
p3 = vector.randvec(3, 0.0, 10.0,1.0)
p4 = vector.randvec(3, 0.0, 10.0,1.0)
#p1 = vector.vector([0.0,0.0,0.0])
#p2 = vector.vector([1.0,0.0,0.0])
#p3 = vector.vector([0.0,1.0,0.0])
#p4 = vector.vector([1.0,1.0,1.0])
d14 = vector.norm(p4-p1)
d24 = vector.norm(p4-p2)
d34 = vector.norm(p4-p3)
sols = sss_int(p1,d14,p2,d24,p3,d34)
sat = True
for sol in sols:
# print sol
d1s = vector.norm(sol-p1)
d2s = vector.norm(sol-p2)
d3s = vector.norm(sol-p3)
sat = sat and tol_eq(d1s,d14)
sat = sat and tol_eq(d2s,d24)
sat = sat and tol_eq(d3s,d34)
# print sat
return sat
def test_sss_int():
p1 = vector.randvec(3, 0.0, 10.0, 1.0)
p2 = vector.randvec(3, 0.0, 10.0, 1.0)
p3 = vector.randvec(3, 0.0, 10.0, 1.0)
p4 = vector.randvec(3, 0.0, 10.0, 1.0)
#p1 = vector.vector([0.0,0.0,0.0])
#p2 = vector.vector([1.0,0.0,0.0])
#p3 = vector.vector([0.0,1.0,0.0])
#p4 = vector.vector([1.0,1.0,1.0])
d14 = vector.norm(p4 - p1)
d24 = vector.norm(p4 - p2)
d34 = vector.norm(p4 - p3)
sols = sss_int(p1, d14, p2, d24, p3, d34)
sat = True
for sol in sols:
# print sol
d1s = vector.norm(sol - p1)
d2s = vector.norm(sol - p2)
d3s = vector.norm(sol - p3)
sat = sat and tol_eq(d1s, d14)
sat = sat and tol_eq(d2s, d24)
sat = sat and tol_eq(d3s, d34)
# print sat
return sat
def ExtraFit():
ellipsoid_fit = False
'''
if len(points) >= 10:
def f_ellipsoid3(beta, x):
return (x[0]-beta[0])**2 + (beta[4]*(x[1]-beta[1]))**2 + (beta[5]*(x[2]-beta[2]))**2 - beta[3]**2
ellipsoid_fit = FitLeastSq(sphere_fit + [1, 1], f_ellipsoid3, zpoints)
print('ellipsoid_fit', ellipsoid_fit)
'''
# return [sphere_fit, 1, ellipsoid_fit]
def f_rotellipsoid3(beta, x):
return x[0]**2 + (beta[1]*x[1] + beta[3]*x[0])**2 + (beta[2]*x[2] + beta[4]*x[0] + beta[5]*x[1])**2 - beta[0]**2
a = x[0]-beta[0]
b = x[1]-beta[1]
c = x[2]-beta[2]
return (a)**2 + (beta[4]*b + beta[6]*a)**2 + (beta[5]*c + beta[7]*a + beta[8]*b)**2 - beta[3]**2
def f_ellipsoid3_cr(beta, x, cr):
a = x[0]-beta[0]
b = x[1]-beta[1]
c = x[2]-beta[2]
return (a)**2 + (beta[4]*b + cr[0]*a)**2 + (beta[5]*c + cr[1]*a + cr[2]*b)**2 - beta[3]**2
# if the ellipsoid fit is sane
if abs(ellipsoid_fit[4]-1) < .2 and abs(ellipsoid_fit[5]-1) < .2:
cpoints = lmap(lambda a, b : a - b, zpoints[:3], ellipsoid_fit[:3])
rotellipsoid_fit = FitLeastSq(ellipsoid_fit[3:] + [0, 0, 0], f_rotellipsoid3, cpoints)
#print('rotellipsoid_fit', rotellipsoid_fit)
ellipsoid_fit2 = FitLeastSq(ellipsoid_fit[:3] + rotellipsoid_fit[:3], f_ellipsoid3_cr, (zpoints, rotellipsoid_fit[3:]))
#print('ellipsoid_fit2', ellipsoid_fit2)
cpoints = lmap(lambda a, b : a - b, zpoints[:3], ellipsoid_fit2[:3])
rotellipsoid_fit2 = FitLeastSq(ellipsoid_fit[3:] + [0, 0, 0], f_rotellipsoid3, cpoints)
print('rotellipsoid_fit2', rotellipsoid_fit2)
else:
ellipsoid_fit = False
def f_uppermatrixfit(beta, x):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
r = numpy.matrix([beta[3:6], [0]+list(beta[6:8]), [0, 0]+[beta[8]]])
print('b', beta)
m = r * b
m = list(numpy.array(m.transpose()))
r0 = lmap(lambda y : 1 - vector.dot(y, y), m)
return r0
def f_matrixfit(beta, x, efit):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], efit[:3]))
r = numpy.matrix([[1, beta[0], beta[1]],
[beta[2], efit[4], beta[3]],
[beta[4], beta[5], efit[5]]])
m = r * b
m = list(numpy.array(m.transpose()))
r0 = lmap(lambda y : efit[3]**2 - vector.dot(y, y), m)
#return r0
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
r1 = lmap(lambda y, z : beta[6] - vector.dot(y, z), m, g)
return r0+r1
def f_matrix2fit(beta, x, efit):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
r = numpy.matrix([[1, efit[0], efit[1]],
[efit[2], beta[4], efit[3]],
[efit[4], efit[5], beta[5]]])
m = r * b
m = list(numpy.array(m.transpose()))
r0 = lmap(lambda y : beta[3]**2 - vector.dot(y, y), m)
#return r0
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
r1 = lmap(lambda y, z : beta[6] - vector.dot(y, z), m, g)
return r0+r1
if False:
matrix_fit = FitLeastSq([0, 0, 0, 0, 0, 0, 0], f_matrixfit, (zpoints, ellipsoid_fit))
#print('matrix_fit', matrix_fit)
matrix2_fit = FitLeastSq(ellipsoid_fit + [matrix_fit[6]], f_matrix2fit, (zpoints, matrix_fit))
#print('matrix2_fit', matrix2_fit)
matrix_fit2 = FitLeastSq(matrix_fit, f_matrixfit, (zpoints, matrix2_fit))
print('matrix_fit2', matrix_fit2)
matrix2_fit2 = FitLeastSq(matrix2_fit[:6] + [matrix_fit2[6]], f_matrix2fit, (zpoints, matrix_fit2))
print('matrix2_fit2', matrix2_fit2)
def rot(v, beta):
sin, cos = math.sin, math.cos
v = vector.normalize(v)
# q = angvec2quat(beta[0], [0, 1, 0])
# return rotvecquat(v, q)
v1 = [v[0]*cos(beta[0]) + v[2]*sin(beta[0]),
v[1],
v[2]*cos(beta[0]) - v[0]*sin(beta[0])]
v2 = [v1[0],
v1[1]*cos(beta[1]) + v1[2]*sin(beta[1]),
v1[2]*cos(beta[1]) - v1[1]*sin(beta[1])]
v3 = [v2[0]*cos(beta[2]) + v2[1]*sin(beta[2]),
v2[1]*cos(beta[2]) - v2[0]*sin(beta[1]),
v2[2]]
return v3
def f_quat(beta, x, sphere_fit):
sphere_fit = numpy.array(sphere_fit)
n = [x[0]-sphere_fit[0], x[1]-sphere_fit[1], x[2]-sphere_fit[2]]
q = [1 - vector.norm(beta[:3])] + list(beta[:3])
q = angvec2quat(vector.norm(beta[:3]), beta[:3])
m = lmap(lambda v : rotvecquat(vector.normalize(v), q), list(zip(n[0], n[1], n[2])))
# m = lmap(lambda v : rot(v, beta), list(zip(n[0], n[1], n[2])))
m = numpy.array(list(zip(*m)))
d = m[0]*x[3] + m[1]*x[4] + m[2]*x[5]
return beta[3] - d
quat_fit = FitLeastSq([0, 0, 0, 0], f_quat, (zpoints, sphere_fit))
# q = [1 - vector.norm(quat_fit[:3])] + list(quat_fit[:3])
q = angvec2quat(vector.norm(quat_fit[:3]), quat_fit[:3])
print('quat fit', q, math.degrees(angle(q)), math.degrees(math.asin(quat_fit[3])))
def f_rot(beta, x, sphere_fit):
sphere_fit = numpy.array(sphere_fit)
n = [x[0]-sphere_fit[0], x[1]-sphere_fit[1], x[2]-sphere_fit[2]]
m = lmap(lambda v : rot(v, beta), zip(n[0], n[1], n[2]))
m = numpy.array(list(zip(*m)))
d = m[0]*x[3] + m[1]*x[4] + m[2]*x[5]
return beta[3] - d
rot_fit = FitLeastSq([0, 0, 0, 0], f_rot, (zpoints, sphere_fit))
print('rot fit', rot_fit, math.degrees(rot_fit[0]), math.degrees(rot_fit[1]), math.degrees(rot_fit[2]), math.degrees(math.asin(min(1, max(-1, rot_fit[3])))))
def normalised(v):
n = vector.norm(v)
if tol_eq(n, 0.0):
return v
else:
return v / n
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
def converged(self):
conv_norms = (vector.norm(task.f()) for task in self.tasks)
return max(conv_norms) < CONV_THRES
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
return r0
def FitPointsCompass(debug, points, current, norm):
# ensure current and norm are float
current = lmap(float, current)
norm = lmap(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = lmap(lambda x : x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
debug('0d fit, insufficient data %.1f %.1f < 9' % (point_dev, point_max_dev))
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
plane_fit, plane_dev, plane_max_dev = plane
# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = lmap(min, p[:3], minc)
maxc = lmap(max, p[:3], maxc)
guess = lmap(lambda a, b : (a+b)/2, minc, maxc)
diff = lmap(lambda a, b : b-a, minc, maxc)
guess.append((diff[0]+diff[1]+diff[2])/3)
#debug('initial guess', guess)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
#debug('initial 1d fit', initial)
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] < 0:
print('FitLeastSq sphere1d failed!!!! ', len(points))
return False
sphere1d_fit = lmap(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
debug('sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit))
'''
def f_new_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints, debug, 2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
new_sphere1d_fit = current
else:
new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
#print('new sphere1 fit', new_sphere1d_fit)
if line_max_dev < 2:
debug('line fit found, insufficient data %.1f %.1f' % (line_dev, line_max_dev))
return False
# 2d sphere fit across normal vector
u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
#debug('initial 2d fit', initial)
'''
def f_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] < 0:
print('FitLeastSq sphere2d failed!!!! ', len(points))
new_sphere2d_fit = initial
else:
sphere2d_fit = lmap(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
debug('sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit))
'''
def f_new_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[2], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2, zpoints, debug, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0 or abs(new_sphere2d_fit[3]) >= 1:
debug('FitLeastSq sphere2 failed!!!! ', len(points), new_sphere2d_fit)
return False
new_sphere2d_fit = lmap(lambda x, a, b: x + new_sphere2d_fit[0]*a + new_sphere2d_fit[1]*b, initial[:3], u, v) + [new_sphere2d_fit[2], math.degrees(math.asin(new_sphere2d_fit[3]))]
new_sphere2d_fit = [new_sphere2d_fit, ComputeDeviation(points, new_sphere2d_fit), 2]
if plane_max_dev < 1.2:
ang = math.degrees(math.asin(vector.norm(vector.cross(plane_fit[1], norm))))
debug('plane fit found, 2D fit only', ang, plane_fit, plane_dev, plane_max_dev)
if ang > 30:
debug('angle of plane not aligned to normal: no 2d fit')
new_sphere2d_fit = False
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print('FitLeastSq sphere failed!!!! ', len(points))
return False
debug('sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit))
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, debug, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0 or abs(new_sphere3d_fit[4]) >= 1:
debug('FitLeastSq sphere3 failed!!!! ', len(points))
return False
new_sphere3d_fit[4] = math.degrees(math.asin(new_sphere3d_fit[4]))
new_sphere3d_fit = [new_sphere3d_fit, ComputeDeviation(points, new_sphere3d_fit), 3]
#debug('new sphere3 fit', new_sphere3d_fit)
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def isMoving(self):
previous = vector.vector_mean(self.positions[:self.NCONTOURS])
now = vector.vector_mean(self.positions[self.NCONTOURS:])
dist = vector.norm(vector.vector_sub(previous, now))
print 'dist: ', dist
return dist > self.MOVING_DIST_THRESHOLD
def test_cl_int():
"""Generates random circle and line, computes the intersection,
and verifies that the number of intersections and the
positions of the intersections are correct.
Returns True or False"""
# 3 random points
p1 = vector.randvec(2, 0.0, 10.0,1.0)
p2 = vector.randvec(2, 0.0, 10.0,1.0)
p3 = vector.randvec(2, 0.0, 10.0,1.0)
# prevent div by zero / no line direction
if tol_eq(vector.norm(p1-p2),0):
p2 = p1 + p3 * 0.1
# line (o,v): origin p1, direction p1-p2
o = p1
v = (p2 - p1) / vector.norm(p2 - p1)
# cirlce (c, r): centered in p3, radius p3-p2 + rx
c = p3
r0 = vector.norm(p3-p2)
# cases rx = 0, rx > 0, rx < 0
case = random.choice([1,2,3])
if case==1:
r = r0 #should have one intersection (unles r0 = 0)
elif case==2:
r = random.random() * r0 # should have no ints (unless r0=0)
elif case==3:
r = r0 + random.random() * r0 # should have 2 ints (unless r0=0)
# do interesection
diag_print("problem:"+str(c)+","+str(r)+","+str(o)+","+str(v),"test_cl_int")
sols = cl_int(c,r,o,v)
diag_print("solutions:"+str(map(str, sols)),"test_cl_int")
# distance from point on line closest to circle center
l = vector.dot(c-o, v) / vector.norm(v)
p = o + v * l / vector.norm(v)
d = vector.norm(p-c)
diag_print("distance center to line="+str(d),"test_cl_int")
# test number of intersections
if len(sols) == 0:
if not tol_gt(d, r):
diag_print("wrong number of solutions: 0", "test_cl_int")
return False
elif len(sols) == 1:
if not tol_eq(d, r):
diag_print("wrong number of solutions: 1", "test_cl_int")
return False
elif len(sols) == 2:
if not tol_lt(d, r):
diag_print("wrong number of solutions: 2", "test_cl_int")
return False
else:
diag_print("wrong number of solutions: >2", "test_cl_int")
# test coordinates of intersection
for s in sols:
# s on line (o,v)
if not is_colinear(s, o, o+v):
diag_print("solution not on line", "test_cl_int")
return False
# s on circle c, r
if not tol_eq(vector.norm(s-c), r):
diag_print("solution not on circle", "test_cl_int")
return False
return True
def isMoving(self):
previous = vector.vector_mean( self.positions[:self.NCONTOURS])
now = vector.vector_mean( self.positions[self.NCONTOURS:])
dist = vector.norm(vector.vector_sub(previous, now))
print 'dist: ', dist
return dist > self.MOVING_DIST_THRESHOLD
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print 'IMURead failed!'
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print 'vector n', data['accel']
return False
self.last_imuread = time.time()
self.loopfreq.strobe()
if not self.FirstTimeStamp:
self.FirstTimeStamp = data['timestamp']
data['timestamp'] -= self.FirstTimeStamp
#data['accel_comp'] = quaternion.rotvecquat(vector.sub(data['accel'], down), self.alignmentQ.value)
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
self.server.TimeStamp('imu', data['timestamp'])
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
compass, accel, down = False, False, False
if not self.accel_calibration.locked.value:
accel = list(data['accel'])
if not self.compass_calibration.locked.value:
down = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(origfusionQPose))
compass = list(data['compass']) + down
if accel or compass:
self.auto_cal.AddPoint((accel, compass, down))
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
if self.heading_off.value != self.last_heading_off:
self.update_alignment(self.alignmentQ.value)
self.last_heading_off = self.heading_off.value
result = self.auto_cal.UpdatedCalibration()
if result:
if result[0] == 'accel' and not self.accel_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.accel_calibration.sigmapoints.set(result[2])
if result[1]:
self.accel_calibration.age.reset()
self.accel_calibration.set(result[1])
elif result[
0] == 'compass' and not self.compass_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.compass_calibration.sigmapoints.set(result[2])
if result[1]:
self.compass_calibration.age.reset()
self.compass_calibration.set(result[1])
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def __init__(self, camera_azimuth, camera_altitude, camera_distance,
camera_fov):
self.camera_azimuth = camera_azimuth
self.camera_altitude = camera_altitude
self.camera_distance = camera_distance
self.camera_fov = camera_fov
near = -30
far = -60
fov = math.radians(camera_fov)
ratio = 1.333
lookat = numpy.array([0, 0, 0])
#
# Camera
#
camera = vector.norm(numpy.array([0, 0, 1])) * camera_distance
alti = math.radians(-camera_altitude)
c = math.cos(alti)
s = math.sin(alti)
cameraRotateAroundX = numpy.array([
[1, 0, 0, 0],
[0, c, -s, 0],
[0, s, c, 0],
[0, 0, 0, 1],
])
azim = math.radians(camera_azimuth)
c = math.cos(azim)
s = math.sin(azim)
cameraRotateAroundY = numpy.array([
[c, 0, s, 0],
[0, 1, 0, 0],
[-s, 0, c, 0],
[0, 0, 0, 1],
])
self.cameraRotate = numpy.dot(cameraRotateAroundY, cameraRotateAroundX)
self.camera = vector.affineTransform(camera, self.cameraRotate)
#print "Camera", camera
#print "Lookat", lookat
#
# Perspective transform
#
self.width = width = -2 * near * math.tan(fov / 2)
self.height = height = width / ratio
#print "Width", width
#print "Height", height
m11 = 2 * near / width
m22 = 2 * near / height
m33 = -(far + near) / (far - near)
m34 = -2 * far * near / (far - near)
self.perspectiveTransform = numpy.array([
[m11, 0, 0, 0],
[0, m22, 0, 0],
[0, 0, m33, m34],
[0, 0, -1, 0],
])
#print perspectiveTransform
#
# Camera look transform
#
up = numpy.array([0, 1, 0])
n = vector.norm(lookat - self.camera)
u = vector.norm(numpy.cross(up, n))
v = numpy.cross(n, u)
ux, uy, uz = u
vx, vy, vz = v
nx, ny, nz = n
self.cameraLookTransform = numpy.array([
[ux, uy, uz, 0],
[vx, vy, vz, 0],
[nx, ny, nz, 0],
[0, 0, 0, 1],
])
#print cameraLookTransform
#
# Camera translation transform
#
tx, ty, tz = -self.camera
self.cameraLocationTransform = numpy.array([
[1, 0, 0, tx],
[0, 1, 0, ty],
[0, 0, 1, tz],
[0, 0, 0, 1],
])
#print cameraLocationTransform
#
# Final transform
#
self.transform = numpy.dot(
numpy.dot(self.perspectiveTransform, self.cameraLookTransform),
self.cameraLocationTransform)
def test_cl_int():
"""Generates random circle and line, computes the intersection,
and verifies that the number of intersections and the
positions of the intersections are correct.
Returns True or False"""
# 3 random points
p1 = vector.randvec(2, 0.0, 10.0, 1.0)
p2 = vector.randvec(2, 0.0, 10.0, 1.0)
p3 = vector.randvec(2, 0.0, 10.0, 1.0)
# prevent div by zero / no line direction
if tol_eq(vector.norm(p1 - p2), 0):
p2 = p1 + p3 * 0.1
# line (o,v): origin p1, direction p1-p2
o = p1
v = (p2 - p1) / vector.norm(p2 - p1)
# cirlce (c, r): centered in p3, radius p3-p2 + rx
c = p3
r0 = vector.norm(p3 - p2)
# cases rx = 0, rx > 0, rx < 0
case = random.choice([1, 2, 3])
if case == 1:
r = r0 #should have one intersection (unles r0 = 0)
elif case == 2:
r = random.random() * r0 # should have no ints (unless r0=0)
elif case == 3:
r = r0 + random.random() * r0 # should have 2 ints (unless r0=0)
# do interesection
diag_print(
"problem:" + str(c) + "," + str(r) + "," + str(o) + "," + str(v),
"test_cl_int")
sols = cl_int(c, r, o, v)
diag_print("solutions:" + str(map(str, sols)), "test_cl_int")
# distance from point on line closest to circle center
l = vector.dot(c - o, v) / vector.norm(v)
p = o + v * l / vector.norm(v)
d = vector.norm(p - c)
diag_print("distance center to line=" + str(d), "test_cl_int")
# test number of intersections
if len(sols) == 0:
if not tol_gt(d, r):
diag_print("wrong number of solutions: 0", "test_cl_int")
return False
elif len(sols) == 1:
if not tol_eq(d, r):
diag_print("wrong number of solutions: 1", "test_cl_int")
return False
elif len(sols) == 2:
if not tol_lt(d, r):
diag_print("wrong number of solutions: 2", "test_cl_int")
return False
else:
diag_print("wrong number of solutions: >2", "test_cl_int")
# test coordinates of intersection
for s in sols:
# s on line (o,v)
if not is_colinear(s, o, o + v):
diag_print("solution not on line", "test_cl_int")
return False
# s on circle c, r
if not tol_eq(vector.norm(s - c), r):
diag_print("solution not on circle", "test_cl_int")
return False
return True
def imu_process(pipe, cal_pipe, accel_cal, compass_cal, gyrobias, period):
if not RTIMU:
while True:
time.sleep(10)
#print 'imu on', os.getpid()
if os.system('sudo chrt -pf 99 %d 2>&1 > /dev/null' % os.getpid()):
print('warning, failed to make imu process realtime')
#os.system("sudo renice -10 %d" % os.getpid())
SETTINGS_FILE = "RTIMULib"
s = RTIMU.Settings(SETTINGS_FILE)
s.FusionType = 1
s.CompassCalValid = False
s.CompassCalEllipsoidOffset = tuple(compass_cal[:3])
s.CompassCalEllipsoidValid = True
s.MPU925xAccelFsr = 0 # +- 2g
s.MPU925xGyroFsr = 0 # +- 250 deg/s
# compass noise by rate 10=.043, 20=.033, 40=.024, 80=.017, 100=.015
rate = 100
s.MPU925xGyroAccelSampleRate = rate
s.MPU925xCompassSampleRate = rate
s.AccelCalValid = True
if accel_cal:
s.AccelCalMin = tuple(map(lambda x: x - accel_cal[3], accel_cal[:3]))
s.AccelCalMax = tuple(map(lambda x: x + accel_cal[3], accel_cal[:3]))
else:
s.AccelCalMin = (-1, -1, -1)
s.AccelCalMax = (1, 1, 1)
s.GyroBiasValid = True
if gyrobias:
s.GyroBias = tuple(map(math.radians, gyrobias))
else:
s.GyroBias = (0, 0, 0)
s.KalmanRk, s.KalmanQ = .002, .001
# s.KalmanRk, s.KalmanQ = .0005, .001
while True:
print("Using settings file " + SETTINGS_FILE + ".ini")
s.IMUType = 0 # always autodetect imu
rtimu = RTIMU.RTIMU(s)
if rtimu.IMUName() == 'Null IMU':
print('no IMU detected... try again')
time.sleep(1)
continue
print("IMU Name: " + rtimu.IMUName())
if not rtimu.IMUInit():
print("ERROR: IMU Init Failed, no inertial data available")
time.sleep(1)
continue
# this is a good time to set any fusion parameters
rtimu.setSlerpPower(.01)
rtimu.setGyroEnable(True)
rtimu.setAccelEnable(True)
rtimu.setCompassEnable(True)
poll_interval = rtimu.IMUGetPollInterval()
time.sleep(.1)
cal_poller = select.poll()
cal_poller.register(cal_pipe, select.POLLIN)
avggyro = [0, 0, 0]
compass_calibration_updated = False
while True:
t0 = time.time()
if not rtimu.IMURead():
print('failed to read IMU!!!!!!!!!!!!!!')
pipe.send(False)
break # reinitialize imu
data = rtimu.getIMUData()
data['accel.residuals'] = list(rtimu.getAccelResiduals())
data['gyrobias'] = s.GyroBias
#data['timestamp'] = t0 # imu timestamp is perfectly accurate
if compass_calibration_updated:
data['compass_calibration_updated'] = True
compass_calibration_updated = False
pipe.send(data, False)
# see if gyro is out of range, sometimes the sensors read
# very high gyro readings and the sensors need to be reset by software
# this is probably a bug in the underlying driver with fifo misalignment
d = .05 * period # filter constant
for i in range(3): # filter gyro vector
avggyro[i] = (1 - d) * avggyro[i] + d * data['gyro'][i]
if vector.norm(avggyro) > .8: # 55 degrees/s
print('too high standing gyro bias, resetting sensors',
data['gyro'], avggyro)
break
# detects the problem even faster:
if any(map(lambda x: abs(x) > 1000, data['compass'])):
print('compass out of range, resetting', data['compass'])
break
if cal_poller.poll(0):
r = cal_pipe.recv()
#print('[imu process] new cal', new_cal)
if r[0] == 'accel':
s.AccelCalValid = True
b, t = r[1][0][:3], r[1][0][3]
s.AccelCalMin = b[0] - t, b[1] - t, b[2] - t
s.AccelCalMax = b[0] + t, b[1] + t, b[2] + t
elif r[0] == 'compass':
compass_calibration_updated = True
s.CompassCalEllipsoidValid = True
s.CompassCalEllipsoidOffset = tuple(r[1][0][:3])
#rtimu.resetFusion()
dt = time.time() - t0
t = period - dt
if t > 0 and t < period:
time.sleep(t)
else:
print('imu process failed to keep time', t)
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print('IMURead failed!')
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
t = time.time()
self.timestamp.set(t - self.starttime)
self.last_imuread = t
self.loopfreq.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
cal_data = {}
if not self.accel_calibration.locked.value:
cal_data['accel'] = list(data['accel'])
if not self.compass_calibration.locked.value:
cal_data['compass'] = list(data['compass'])
cal_data['down'] = quaternion.rotvecquat(
[0, 0, 1], quaternion.conjugate(origfusionQPose))
if cal_data:
self.auto_cal.cal_pipe.send(cal_data)
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def deg(y, z):
n = min(max(vector.dot(y, z) / vector.norm(y), -1), 1)
return math.degrees(math.asin(n))
def read(self):
data = self.IMUread()
if not data:
if time.monotonic(
) - self.last_imuread > 1 and self.frequency.value:
print('IMURead failed!')
self.frequency.set(False)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
self.last_imuread = time.monotonic()
self.frequency.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
aligned = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(aligned))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .01 and dt < .2:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose, aligned))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or \
self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
if self.auto_cal.cal_pipe:
#print('warning, cal pipe always sending despite locks')
cal_data = {}
#how to check this here?? if not 'imu.accel.calibration.locked'
cal_data['accel'] = list(data['accel'])
#how to check this here?? if not 'imu.compass.calibration.locked'
cal_data['compass'] = list(data['compass'])
cal_data['fusionQPose'] = list(data['fusionQPose'])
if cal_data:
#print('send', cal_data)
self.auto_cal.cal_pipe.send(cal_data)
return data
def normalised(v):
n = vector.norm(v)
if tol_eq(n, 0.0):
return v
else:
return v / n
