def solve():
"""Main function"""
grid = []
res = 0
max_adj = 4
text = slurp_file("p011.txt")
for row in text.split("\n"):
grid.append([int(x) for x in row.split(" ")])
len_grid = len(grid)
# Horizontal
for i in xrange(len_grid):
for j in xrange(len_grid - max_adj):
res = max(res, mul([grid[i][j + x] for x in xrange(max_adj)]))
# Vertical
for i in xrange(len_grid):
for j in xrange(len_grid - max_adj):
res = max(res, mul([grid[j + x][i] for x in xrange(max_adj)]))
# Diag 1
for i in xrange(len_grid - max_adj):
for j in xrange(len_grid - max_adj):
res = max(res, mul([grid[i + x][j + x] for x in xrange(max_adj)]))
# Diag 2
for i in xrange(max_adj - 1, len_grid - max_adj):
for j in xrange(len_grid - max_adj):
res = max(res, mul([grid[i - x][j + x] for x in xrange(max_adj)]))
return res
def gradient_it(m, eps):
cnt = 0
(A, B) = ut.split_at(m)
x0 = [0] * len(B)
r0 = ut.minus(B, ut.subst(A, x0))
p0 = r0
z0 = r0
s0 = r0
for i in range(0, 2 * len(B)):
sub = ut.subst(A, z0)
prev_p = p0
prev_r = r0
At = ut.transpose_matrix(A)
# print(At)
a = ut.scalar_product(p0, r0) / ut.scalar_product(s0, sub)
x0 = ut.plus(x0, ut.mul(a, z0))
r0 = ut.minus(r0, ut.mul(a, sub))
p0 = ut.minus(p0, ut.mul(a, ut.subst(At, s0)))
b = ut.scalar_product(p0, r0) / ut.scalar_product(prev_p, prev_r)
z0 = ut.plus(r0, ut.mul(b, z0))
s0 = ut.plus(p0, ut.mul(b, s0))
if abs(ut.norm(r0) / ut.norm(B)) < eps:
break
cnt += 1
return (x0, cnt)
def side(self, v0, v1, v2, origin, direction):
v0v1 = sub(v1, v0)
v0v2 = sub(v2, v0)
N = cross(v0v1, v0v2)
raydirection = dot(N, direction)
if abs(raydirection) < 0.0001:
return None
d = dot(N, v0)
t = (dot(N, origin) + d) / raydirection
if t < 0:
return None
P = sum(origin, mul(direction, t))
U, V, W = barycentric(v0, v1, v2, P)
if U < 0 or V < 0 or W < 0:
return None
else:
return Intersect(distance = d,
point = P,
normal = norm(N))
def castRay(self, origin, direction):
material, intersect = self.sceneIntersect(origin, direction)
if material is None:
return self.currentColor
lightDir = norm(sub(self.light.position, intersect.point))
lightDistance = length(sub(self.light.position, intersect.point))
offsetNormal = mul(intersect.normal, 1.1)
shadowOrigin = sub(intersect.point, offsetNormal) if dot(lightDir, intersect.normal) < 0 else sum(intersect.point, offsetNormal)
shadowMaterial, shadowIntersect = self.sceneIntersect(shadowOrigin, lightDir)
shadowIntensity = 0
if shadowMaterial and length(sub(shadowIntersect.point, shadowOrigin)) < lightDistance:
shadowIntensity = 0.9
intensity = self.light.intensity * max(0, dot(lightDir, intersect.normal)) * (1 - shadowIntensity)
reflection = reflect(lightDir, intersect.normal)
specularIntensity = self.light.intensity * (
max(0, -dot(reflection, direction)) ** material.spec
)
diffuse = material.diffuse * intensity * material.albedo[0]
specular = Color(255, 255, 255) * specularIntensity * material.albedo[1]
return diffuse + specular
def castRay(self, origin, direction, recursion=0):
material, intersect = self.sceneIntersect(origin, direction)
if material is None or recursion >= MAX_RECURSION_DEPTH:
return self.currentColor
# Si el rayo no golpeo nada o si llego al limite de recursion
lightDir = norm(sub(self.light.position, intersect.point))
lightDistance = length(sub(self.light.position, intersect.point))
offsetNormal = mul(intersect.normal, 1.1)
shadowOrigin = sub(
intersect.point,
offsetNormal) if dot(lightDir, intersect.normal) < 0 else sum(
intersect.point, offsetNormal)
shadowMaterial, shadowIntersect = self.sceneIntersect(
shadowOrigin, lightDir)
shadowIntensity = 0
if shadowMaterial and length(sub(shadowIntersect.point,
shadowOrigin)) < lightDistance:
shadowIntensity = 0.9
intensity = self.light.intensity * max(
0, dot(lightDir, intersect.normal)) * (1 - shadowIntensity)
reflection = reflect(lightDir, intersect.normal)
specularIntensity = self.light.intensity * (max(
0, -dot(reflection, direction))**material.spec)
if material.albedo[2] > 0:
reflectDir = reflect(direction, intersect.normal)
reflectOrigin = sub(intersect.point, offsetNormal) if dot(
reflectDir, intersect.normal) < 0 else sum(
intersect.point, offsetNormal)
reflectedColor = self.castRay(reflectOrigin, reflectDir,
recursion + 1)
else:
reflectedColor = self.currentColor
if material.albedo[3] > 0:
refractDir = refract(direction, intersect.normal,
material.refractionIndex)
refractOrigin = sub(intersect.point, offsetNormal) if dot(
refractDir, intersect.normal) < 0 else sum(
intersect.point, offsetNormal)
refractedColor = self.castRay(refractOrigin, refractDir,
recursion + 1)
else:
refractedColor = self.currentColor
diffuse = material.diffuse * intensity * material.albedo[0]
specular = Color(255, 255,
255) * specularIntensity * material.albedo[1]
reflected = reflectedColor * material.albedo[2]
refracted = refractedColor * material.albedo[3]
return diffuse + specular + reflected + refracted
def gauss(m):
for i in range(len(m) - 1):
# print(m)
max_el = 0
ind = i
for j in range(i, len(m[0]) - 1):
# print(abs(m[j][i]))
if abs(m[j][i]) > max_el:
max_el = abs(m[j][i])
ind = j
# print(max_el)
swap_str(m, ind, i)
for j in range(i + 1, len(m)):
# c = m[j][i] / m[i][i]
if m[i][i] == 0:
continue
# print(m[j][i])
# print(m[i][i])
# print(m[j][i] / m[i][i])
mul_str = mul(m[j][i] / m[i][i], m[i])
m[j] = minus(m[j], mul_str)
for k in range(len(m[j]) - 1):
if m[j][k] < 0.000001:
m[j][k] = 0
# print(m)
x = [0] * len(m)
for i in range(len(m) - 1, -1, -1):
a = 0
for j in range(i + 1, (len(m))):
a = a + m[i][j] * x[j]
b = m[i][len(m[0]) - 1] - a
if m[i][i] != 0:
x[i] = b / m[i][i]
else:
x[i] = 0
# if b == 0:
# x[i] = 0
# else:
# return []
return x
# print(gauss([[2, 5, 1, 4], [3, 1, -2, -5], [1, 1, 1, 6]]))
# print(gauss([[-1, -1, 3], [-0.3333, -1, 2.3333]]))
# print(gauss([[-1, 0.5, -1.5, -1, -0.5], [-3, -1, -2, -3, -1], [-0.25, -0.75, -1, -0.25, -0.25], [-0.25, -0.5, -0.25, -1, -0.25]]))
# print(gauss([[-1, -0.666666, -0.5, -0.4, -0.333334, -6], [-1.33333, -1, -0.8, -0.666668, -0.571428, -16], [-1.5, -1.2, -1, -0.85714, -0.749999, -59.9999], [-1.6, -1.33334, -1.14286, -1, -0.888888, -24], [-1.66667, -1.42857, -1.25, -1.11111, -1, -550]]))
def chinese_remainder_theorem(discs):
M = mul(d[0] for d in discs)
x = 0
for i, (size, initial) in enumerate(discs, start=1):
# Disc #2 has 17 positions; at time=0, it is at position 15.
# => x \equiv (17 - 15 - 2) (mod 17)
M_i = (M / size)
x += (size - initial - i) * M_i * modinv(M_i, size)
return x
def solve():
'''Main function'''
top = 10
nth = 1000000
nth -= 1
nums = range(top)
res = ""
for i in reversed(xrange(top)):
tmp = mul(xrange(1, i+1))
res += str(nums.pop(nth/tmp))
nth %= tmp
return int(res)
def rayIntersect(self, orig, dir):
# t = (( position - origRayo) dot normal) / (dirRayo dot normal)
denom = dot(dir, self.normal)
if abs(denom) > 0.0001:
t = dot(self.normal, sub(self.position, orig)) / denom
if t > 0:
# P = O + tD
hit = sum(orig, mul(dir, t))
return Intersect(distance = t,
point = hit,
normal = self.normal)
return None
def rayIntersect(self, origin, direction):
L = sub(self.center, origin)
tca = dot(L, direction)
l = length(L)
d2 = l ** 2 - tca ** 2
if d2 > self.radius ** 2:
return None
thc = (self.radius ** 2 - d2) ** 0.5
t0 = tca - thc
t1 = tca + thc
if t0 < 0:
t0 = t1
if t0 < 0:
return None
hit = sum(origin, mul(direction, t0))
normal = norm(sub(hit, self.center))
return Intersect(
distance=t0,
point=hit,
normal=normal
)
break
else:
LOW_POINTS.add(p)
part_1 += (val + 1)
print "Part 1:", part_1
# Solve part 2.
def compute_basin_size(board, start):
"""Return the set of all explored points from a given point."""
seen = set()
def _dfs(node):
val = board[node]
seen.add(node)
for n in node.neighbours_4():
if n in board and n not in seen:
if val <= board[n] and board[n] != 9:
_dfs(n)
_dfs(start)
return len(seen)
BASIN_SIZES = {}
for p in LOW_POINTS:
BASIN_SIZES[p] = compute_basin_size(BOARD, p)
print "Part 2:", mul(sorted(BASIN_SIZES.values(), reverse=True)[:3])
def contr(self, x, t, x_ref=0, u_ref=0):
contr = mul(self.Ks[t], x - x_ref) + self.ks[t] + u_ref
return contr
def backups(self, env, T, Cs, Fs, cs, fs):
print "Computing backwards pass"
start_time = timer.time()
n = env.state_dim
d = env.action_dim
C1, C2, C3, C4 = utils.breakup_C(Cs[-1], n)
c1, c2 = utils.breakup_c(cs[-1], n)
K_T = -np.dot(np.linalg.inv(C4), C3)
k_T = -np.dot(np.linalg.inv(C4), c2)
V_T = C1 + mul(C2, K_T) + mul(K_T.T, C3) + mul(K_T.T, C4, K_T)
v_T = c1 + mul(C2, k_T) + mul(K_T.T, c2) + mul(K_T.T, C4, k_T)
Vs, vs, Ks, ks = [None] * T, [None] * T, [None] * T, [None] * T
Vs[-1], vs[-1], Ks[-1], ks[-1] = V_T, v_T, K_T, k_T
for t in range(T - 1)[::-1]:
C_t = Cs[t]
F_t = Fs[t]
c_t = cs[t]
f_t = fs[t]
# IPython.embed()
Q_t = C_t + mul(F_t.T, Vs[t + 1], F_t)
q_t = c_t + mul(F_t.T, Vs[t + 1], f_t) + mul(F_t.T, vs[t + 1])
Q1, Q2, Q3, Q4 = utils.breakup_C(Q_t, n)
q1, q2 = utils.breakup_c(q_t, n)
Ks[t] = -mul(inv(Q4), Q3)
ks[t] = -mul(inv(Q4), q2)
Vs[t] = Q1 + mul(Q2, Ks[t]) + mul(Ks[t].T, Q3) + mul(
Ks[t].T, Q4, Ks[t])
vs[t] = q1 + mul(Q2, ks[t]) + mul(Ks[t].T, q2) + mul(
Ks[t].T, Q4, ks[t])
self.Vs = Vs
self.vs = vs
self.Ks = Ks
self.ks = ks
print "Done computing back pass"
end_time = timer.time()
print "Total time: " + str(end_time - start_time)
return Vs, vs, Ks, ks
def __init__(self,
numberOfInducingPoints, # Number of inducing ponts in sparse GP
batchSize, # Size of mini batch
dimX, # Dimensionality of the latent co-ordinates
dimZ, # Dimensionality of the latent variables
data, # [NxP] matrix of observations
kernelType='ARD',
encoderType_qX='FreeForm2', # 'MLP', 'Kernel'.
encoderType_rX='FreeForm2', # 'MLP', 'Kernel'
Xu_optimise=False,
numberOfEncoderHiddenUnits=10
):
self.numTestSamples = 5000
# set the data
data = np.asarray(data, dtype=precision)
self.N = data.shape[0] # Number of observations
self.P = data.shape[1] # Dimension of each observation
self.M = numberOfInducingPoints
self.B = batchSize
self.R = dimX
self.Q = dimZ
self.H = numberOfEncoderHiddenUnits
self.encoderType_qX = encoderType_qX
self.encoderType_rX = encoderType_rX
self.Xu_optimise = Xu_optimise
self.y = th.shared(data)
self.y.name = 'y'
if kernelType == 'RBF':
self.numberOfKernelParameters = 2
elif kernelType == 'RBFnn':
self.numberOfKernelParameters = 1
elif kernelType == 'ARD':
self.numberOfKernelParameters = self.R + 1
else:
raise RuntimeError('Unrecognised kernel type')
self.lowerBound = -np.inf # Lower bound
self.numberofBatchesPerEpoch = int(np.ceil(np.float32(self.N) / self.B))
numPad = self.numberofBatchesPerEpoch * self.B - self.N
self.batchStream = srng.permutation(n=self.N)
self.padStream = srng.choice(size=(numPad,), a=self.N,
replace=False, p=None, ndim=None, dtype='int32')
self.batchStream.name = 'batchStream'
self.padStream.name = 'padStream'
self.iterator = th.shared(0)
self.iterator.name = 'iterator'
self.allBatches = T.reshape(T.concatenate((self.batchStream, self.padStream)), [self.numberofBatchesPerEpoch, self.B])
self.currentBatch = T.flatten(self.allBatches[self.iterator, :])
self.allBatches.name = 'allBatches'
self.currentBatch.name = 'currentBatch'
self.y_miniBatch = self.y[self.currentBatch, :]
self.y_miniBatch.name = 'y_miniBatch'
self.jitterDefault = np.float64(0.0001)
self.jitterGrowthFactor = np.float64(1.1)
self.jitter = th.shared(np.asarray(self.jitterDefault, dtype='float64'), name='jitter')
kfactory = kernelFactory(kernelType)
# kernel parameters
self.log_theta = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_theta', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff
self.log_omega = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_omega', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff
self.log_gamma = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_gamma', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff
# Random variables
self.xi = srng.normal(size=(self.B, self.R), avg=0.0, std=1.0, ndim=None)
self.alpha = srng.normal(size=(self.M, self.Q), avg=0.0, std=1.0, ndim=None)
self.beta = srng.normal(size=(self.B, self.Q), avg=0.0, std=1.0, ndim=None)
self.xi.name = 'xi'
self.alpha.name = 'alpha'
self.beta.name = 'beta'
self.sample_xi = th.function([], self.xi)
self.sample_alpha = th.function([], self.alpha)
self.sample_beta = th.function([], self.beta)
self.sample_batchStream = th.function([], self.batchStream)
self.sample_padStream = th.function([], self.padStream)
self.getCurrentBatch = th.function([], self.currentBatch, no_default_updates=True)
# Compute parameters of q(X)
if self.encoderType_qX == 'FreeForm1' or self.encoderType_qX == 'FreeForm2':
# Have a normal variational distribution over location of latent co-ordinates
self.phi_full = sharedZeroMatrix(self.N, self.R, 'phi_full')
self.phi = self.phi_full[self.currentBatch, :]
self.phi.name = 'phi'
if encoderType_qX == 'FreeForm1':
self.Phi_full_sqrt = sharedZeroMatrix(self.N, self.N, 'Phi_full_sqrt')
Phi_batch_sqrt = self.Phi_full_sqrt[self.currentBatch][:, self.currentBatch]
Phi_batch_sqrt.name = 'Phi_batch_sqrt'
self.Phi = dot(Phi_batch_sqrt, Phi_batch_sqrt.T, 'Phi')
self.cPhi, _, self.logDetPhi = cholInvLogDet(self.Phi, self.B, 0)
self.qX_vars = [self.Phi_full_sqrt, self.phi_full]
else:
self.Phi_full_logdiag = sharedZeroArray(self.N, 'Phi_full_logdiag')
Phi_batch_logdiag = self.Phi_full_logdiag[self.currentBatch]
Phi_batch_logdiag.name = 'Phi_batch_logdiag'
self.Phi, self.cPhi, _, self.logDetPhi \
= diagCholInvLogDet_fromLogDiag(Phi_batch_logdiag, 'Phi')
self.qX_vars = [self.Phi_full_logdiag, self.phi_full]
elif self.encoderType_qX == 'MLP':
# Auto encode
self.W1_qX = sharedZeroMatrix(self.H, self.P, 'W1_qX')
self.W2_qX = sharedZeroMatrix(self.R, self.H, 'W2_qX')
self.W3_qX = sharedZeroMatrix(1, self.H, 'W3_qX')
self.b1_qX = sharedZeroVector(self.H, 'b1_qX', broadcastable=(False, True))
self.b2_qX = sharedZeroVector(self.R, 'b2_qX', broadcastable=(False, True))
self.b3_qX = sharedZeroVector(1, 'b3_qX', broadcastable=(False, True))
# [HxB] = softplus( [HxP] . [BxP]^T + repmat([Hx1],[1,B]) )
h_qX = softplus(plus(dot(self.W1_qX, self.y_miniBatch.T), self.b1_qX), 'h_qX' )
# [RxB] = sigmoid( [RxH] . [HxB] + repmat([Rx1],[1,B]) )
mu_qX = plus(dot(self.W2_qX, h_qX), self.b2_qX, 'mu_qX')
# [1xB] = 0.5 * ( [1xH] . [HxB] + repmat([1x1],[1,B]) )
log_sigma_qX = mul( 0.5, plus(dot(self.W3_qX, h_qX), self.b3_qX), 'log_sigma_qX')
self.phi = mu_qX.T # [BxR]
self.Phi, self.cPhi, self.iPhi,self.logDetPhi \
= diagCholInvLogDet_fromLogDiag(log_sigma_qX, 'Phi')
self.qX_vars = [self.W1_qX, self.W2_qX, self.W3_qX, self.b1_qX, self.b2_qX, self.b3_qX]
elif self.encoderType_qX == 'Kernel':
# Draw the latent coordinates from a GP with data co-ordinates
self.Phi = kfactory.kernel(self.y_miniBatch, None, self.log_gamma, 'Phi')
self.phi = sharedZeroMatrix(self.B, self.R, 'phi')
(self.cPhi, self.iPhi, self.logDetPhi) = cholInvLogDet(self.Phi, self.B, self.jitter)
self.qX_vars = [self.log_gamma]
else:
raise RuntimeError('Unrecognised encoding for q(X): ' + self.encoderType_qX)
# Variational distribution q(u)
self.kappa = sharedZeroMatrix(self.M, self.Q, 'kappa')
self.Kappa_sqrt = sharedZeroMatrix(self.M, self.M, 'Kappa_sqrt')
self.Kappa = dot(self.Kappa_sqrt, self.Kappa_sqrt.T, 'Kappa')
(self.cKappa, self.iKappa, self.logDetKappa) \
= cholInvLogDet(self.Kappa, self.M, 0)
self.qu_vars = [self.Kappa_sqrt, self.kappa]
# Calculate latent co-ordinates Xf
# [BxR] = [BxR] + [BxB] . [BxR]
self.Xz = plus( self.phi, dot(self.cPhi, self.xi), 'Xf' )
# Inducing points co-ordinates
self.Xu = sharedZeroMatrix(self.M, self.R, 'Xu')
# Kernels
self.Kzz = kfactory.kernel(self.Xz, None, self.log_theta, 'Kff')
self.Kuu = kfactory.kernel(self.Xu, None, self.log_theta, 'Kuu')
self.Kzu = kfactory.kernel(self.Xz, self.Xu, self.log_theta, 'Kfu')
self.cKuu, self.iKuu, self.logDetKuu = cholInvLogDet(self.Kuu, self.M, self.jitter)
# Variational distribution
# A has dims [BxM] = [BxM] . [MxM]
self.A = dot(self.Kzu, self.iKuu, 'A')
# L is the covariance of conditional distribution q(z|u,Xf)
self.C = minus( self.Kzz, dot(self.A, self.Kzu.T), 'C')
self.cC, self.iC, self.logDetC = cholInvLogDet(self.C, self.B, self.jitter)
# Sample u_q from q(u_q) = N(u_q; kappa_q, Kappa ) [MxQ]
self.u = plus(self.kappa, (dot(self.cKappa, self.alpha)), 'u')
# compute mean of z [QxB]
# [BxQ] = [BxM] * [MxQ]
self.mu = dot(self.A, self.u, 'mu')
# Sample f from q(f|u,X) = N( mu_q, C )
# [BxQ] =
self.z = plus(self.mu, (dot(self.cC, self.beta)), 'z')
self.qz_vars = [self.log_theta]
self.iUpsilon = plus(self.iKappa, dot(self.A.T, dot(self.iC, self.A) ), 'iUpsilon')
_, self.Upsilon, self.negLogDetUpsilon = cholInvLogDet(self.iUpsilon, self.M, self.jitter)
if self.encoderType_rX == 'MLP':
self.W1_rX = sharedZeroMatrix(self.H, self.Q+self.P, 'W1_rX')
self.W2_rX = sharedZeroMatrix(self.R, self.H, 'W2_rX')
self.W3_rX = sharedZeroMatrix(self.R, self.H, 'W3_rX')
self.b1_rX = sharedZeroVector(self.H, 'b1_rX', broadcastable=(False, True))
self.b2_rX = sharedZeroVector(self.R, 'b2_rX', broadcastable=(False, True))
self.b3_rX = sharedZeroVector(self.R, 'b3_rX', broadcastable=(False, True))
# [HxB] = softplus( [Hx(Q+P)] . [(Q+P)xB] + repmat([Hx1], [1,B]) )
h_rX = softplus(plus(dot(self.W1_rX, T.concatenate((self.z.T, self.y_miniBatch.T))), self.b1_rX), 'h_rX')
# [RxB] = softplus( [RxH] . [HxB] + repmat([Rx1], [1,B]) )
mu_rX = plus(dot(self.W2_rX, h_rX), self.b2_rX, 'mu_rX')
# [RxB] = 0.5*( [RxH] . [HxB] + repmat([Rx1], [1,B]) )
log_sigma_rX = mul( 0.5, plus(dot(self.W3_rX, h_rX), self.b3_rX), 'log_sigma_rX')
self.tau = mu_rX.T
# Diagonal optimisation of Tau
self.Tau_isDiagonal = True
self.Tau = T.reshape(log_sigma_rX, [self.B * self.R, 1])
self.logDetTau = T.sum(log_sigma_rX)
self.Tau.name = 'Tau'
self.logDetTau.name = 'logDetTau'
self.rX_vars = [self.W1_rX, self.W2_rX, self.W3_rX, self.b1_rX, self.b2_rX, self.b3_rX]
elif self.encoderType_rX == 'Kernel':
self.tau = sharedZeroMatrix(self.B, self.R, 'tau')
# Tau_r [BxB] = kernel( [[BxQ]^T,[BxP]^T].T )
Tau_r = kfactory.kernel(T.concatenate((self.z.T, self.y_miniBatch.T)).T, None, self.log_omega, 'Tau_r')
(cTau_r, iTau_r, logDetTau_r) = cholInvLogDet(Tau_r, self.B, self.jitter)
# self.Tau = slinalg.kron(T.eye(self.R), Tau_r)
self.cTau = slinalg.kron(cTau_r, T.eye(self.R))
self.iTau = slinalg.kron(iTau_r, T.eye(self.R))
self.logDetTau = logDetTau_r * self.R
self.tau.name = 'tau'
# self.Tau.name = 'Tau'
self.cTau.name = 'cTau'
self.iTau.name = 'iTau'
self.logDetTau.name = 'logDetTau'
self.Tau_isDiagonal = False
self.rX_vars = [self.log_omega]
else:
raise RuntimeError('Unrecognised encoding for r(X|z)')
# Gradient variables - should be all the th.shared variables
# We always want to optimise these variables
if self.Xu_optimise:
self.gradientVariables = [self.Xu]
else:
self.gradientVariables = []
self.gradientVariables.extend(self.qu_vars)
self.gradientVariables.extend(self.qz_vars)
self.gradientVariables.extend(self.qX_vars)
self.gradientVariables.extend(self.rX_vars)
self.lowerBounds = []
self.condKappa = myCond()(self.Kappa)
self.condKappa.name = 'condKappa'
self.Kappa_conditionNumber = th.function([], self.condKappa, no_default_updates=True)
self.condKuu = myCond()(self.Kuu)
self.condKuu.name = 'condKuu'
self.Kuu_conditionNumber = th.function([], self.condKuu, no_default_updates=True)
self.condC = myCond()(self.C)
self.condC.name = 'condC'
self.C_conditionNumber = th.function([], self.condC, no_default_updates=True)
self.condUpsilon = myCond()(self.Upsilon)
self.condUpsilon.name = 'condUpsilon'
self.Upsilon_conditionNumber = th.function([], self.condUpsilon, no_default_updates=True)
self.Xz_get_value = th.function([], self.Xz, no_default_updates=True)
def start(clientID,
quads,
targets,
speed,
proxs,
path,
endpos,
leadfoll=False):
"""
Boids model main program
:param clientID: ID of the VRep connection
:param quads: quadrotor handles
:param targets: quadrotor target handles
:param speed: speed of quadrotors
:param proxs: proximity sensor handles
:param path: quadrotor path coordinates
:param endpos: ending position of quadrotors
:param leadfoll: True - leader/followers mode, False - all boids following path (default)
"""
# definition of constants
quadsNum = len(quads) # number of quadrotors
viewRange = 3 # view range of quadrotors
smp = 0.2 # sampling period
kS = [0.30,
2.0] # separation constants [multiplication const, power const]
kC = [0.30, 0.0] # cohesion constants [multiplication const, power const]
kA = [0.00, 0.0] # alignment constants [multiplication const, power const]
kD = [speed,
1.0] # path following constants [multiplication const, power const]
kO = [0.20, 2.0
] # obstacle avoidance constants [multiplication const, power const]
# data stream init
for i in range(quadsNum):
vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quads[i],
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(clientID, proxs[i],
vrep.simx_opmode_streaming)
# variables init
position = [[0 for _ in range(3)]
for _ in range(quadsNum)] # position of quadrotors
velocity = [[0 for _ in range(3)]
for _ in range(quadsNum)] # velocity of quadrotors
closest = [[0 for _ in range(3)]
for _ in range(quadsNum)] # coords of closest obstacle to quads
visibleQuads = [[0 for _ in range(quadsNum)]
for _ in range(quadsNum)] # visible quadrotors
individualTarget = [
0
] * quadsNum # current waypoint index for each quadrotor
# get closest boid to starting point
leader = 0
_, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1,
vrep.simx_opmode_buffer)
dist = ut.norm(ut.sub(path[1], tmp))
for i in range(1, quadsNum):
_, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_buffer)
nrm = ut.norm(ut.sub(path[1], tmp))
if nrm < dist:
dist = nrm
leader = i
# main boids program
t1 = 0
t2 = 0.0
finished = [False] * quadsNum
count = 0
counting = False
file = open('data.txt', 'wt', encoding='utf-8')
while vrep.simxGetConnectionId(clientID) != -1:
time.sleep(smp)
separation = [[0 for _ in range(3)]
for _ in range(quadsNum)] # separation force
cohesion = [[0 for _ in range(3)]
for _ in range(quadsNum)] # cohesion force
alignment = [[0 for _ in range(3)]
for _ in range(quadsNum)] # alignment force
destination = [[0 for _ in range(3)]
for _ in range(quadsNum)] # path following force
avoidance = [[0 for _ in range(3)]
for _ in range(quadsNum)] # obstacle avoidance force
output = [[0 for _ in range(3)]
for _ in range(quadsNum)] # output force
# check if all quads finished
if counting:
if count >= 0:
file.close()
return (t2 - t1) / 1000
count += 1
else:
for i in range(quadsNum):
nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2]))
if nrm < 2 and not finished[i]:
finished[i] = True
print('Quad #' + str(i) + ' finished in ' +
str((vrep.simxGetLastCmdTime(clientID) - t1) /
1000) + 's')
if (leadfoll and finished[leader]) or (
not leadfoll and all(_ for _ in finished)):
counting = True
t2 = vrep.simxGetLastCmdTime(clientID)
if endpos is None:
file.close()
return (t2 - t1) / 1000
# read data from VRep
for i in range(quadsNum):
_, position[i] = vrep.simxGetObjectPosition(
clientID, quads[i], -1, vrep.simx_opmode_buffer)
_, velocity[i], _ = vrep.simxGetObjectVelocity(
clientID, quads[i], vrep.simx_opmode_buffer)
_, res, closest[i], _, _ = vrep.simxReadProximitySensor(
clientID, proxs[i], vrep.simx_opmode_buffer)
if not res:
closest[i] = [0, 0, 0]
closest[i][2] = 0
# write into data file
ct = vrep.simxGetLastCmdTime(clientID)
file.write(str(ct))
for i in range(quadsNum):
file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) +
' ' + str(position[i][2]))
file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) +
' ' + str(velocity[i][2]))
file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1]))
file.write('\n')
# compute visible quadrotors
for i in range(quadsNum):
for j in range(quadsNum):
if i != j:
temp = ut.sub(position[i], position[j])
if ut.norm(temp) < viewRange:
visibleQuads[i][j] = 1
else:
visibleQuads[i][j] = 0
for i in range(quadsNum):
# compute separation force
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.sub(position[i], position[j])
nrm = ut.norm(temp)
if nrm != 0:
temp = ut.mul(temp, kS[0] / (nrm**kS[1]))
separation[i] = ut.add(separation[i], temp)
# compute cohesion and alignment forces
center = [0, 0, 0] # center of the swarm
if sum(visibleQuads[i]) != 0:
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.mul(position[j], 1 / sum(visibleQuads[i]))
center = ut.add(center, temp)
temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i]))
alignment[i] = ut.add(alignment[i], temp)
cohesion[i] = ut.sub(center, position[i])
nrm = ut.norm(cohesion[i])
if nrm != 0:
cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1]))
nrm = ut.norm(alignment[i])
if nrm != 0:
alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1]))
# compute path following force
if not leadfoll or i == leader or counting:
if not counting:
nrm = ut.norm(
ut.sub(position[i][0:2],
path[individualTarget[i]][0:2]))
if individualTarget[i] != 0:
vec1 = ut.sub(path[individualTarget[i] - 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i], path[individualTarget[i]])
if individualTarget[i] < len(path) - 1 and (
nrm <= 1
or ut.angle(vec1, vec2) >= math.pi / 2):
individualTarget[i] += 1
# if individualTarget[i] == 2 and min(individualTarget) == 2:
# print((vrep.simxGetLastCmdTime(clientID)-tt)/1000)
else:
vec1 = ut.sub(path[individualTarget[i] + 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i], path[individualTarget[i]])
if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2:
individualTarget[i] += 1
if t1 == 0 and min(individualTarget) == 1:
t1 = vrep.simxGetLastCmdTime(clientID)
# tt = vrep.simxGetLastCmdTime(clientID)
destination[i] = ut.sub(path[individualTarget[i]],
position[i])
else:
destination[i] = ut.sub(endpos[i], position[i])
nrm = ut.norm(destination[i])
if nrm != 0:
if finished[i]:
destination[i] = ut.mul(destination[i], 0.1)
else:
destination[i] = ut.mul(destination[i],
kD[0] / (nrm**kD[1]))
# compute output force without obstacle avoidance
output[i] = separation[i]
output[i] = ut.add(output[i], cohesion[i])
output[i] = ut.add(output[i], alignment[i])
output[i] = ut.add(output[i], destination[i])
# compute obstacle avoidance force
# angle = ut.angle(closest[i], output[i])
# if angle > math.pi/2+0.3:
# avoidance[i] = [0, 0, 0]
# else:
avoidance[i] = ut.sub([0, 0, 0], closest[i])
nrm = ut.norm(avoidance[i])
if nrm != 0:
avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1]))
# compute output force
output[i] = ut.add(output[i], avoidance[i])
if position[i][2] < 0.5:
output[i][2] = 0.05
# export output to VRep
for i in range(quadsNum):
vrep.simxSetObjectPosition(clientID, targets[i], quads[i],
output[i], vrep.simx_opmode_streaming)
tickets.append(nums)
valid_tickets = []
part_1 = 0
for ticket in tickets[1:]:
for n in ticket:
for a, b, c, d in fields.values():
if a <= n <= b or c <= n <= d:
break
else:
part_1 += n
break
else:
valid_tickets.append(ticket)
print "Part 1:", part_1
poss = defaultdict(set)
for i, col in enumerate(transposed(valid_tickets)):
for field, (a, b, c, d) in fields.items():
if all(a <= n <= b or c <= n <= d for n in col):
poss[field].add(i)
resolved = resolve_mapping(poss)
departures = [resolved[field] for field in resolved if 'departure' in field]
print "Part 2:", mul(tickets[0][i] for i in departures)
def voro_start(clientID, obstacles, border):
"""
Creates a Voronoi diagram around obstacles using vtk_voro
:param clientID: ID of the VRep connection
:param obstacles: list of obstacle handles
:param border: scene border [width, height]
:return: list of graph vertices, indices of edge vertices, gaps for each edge
"""
border = [[-border[0] / 2, -border[1] / 2],
[-border[0] / 2, border[1] / 2], [border[0] / 2, border[1] / 2],
[border[0] / 2, -border[1] / 2]]
obsNum = len(obstacles)
output = []
for i in range(obsNum):
# get data from V-Rep
_, position = vrep.simxGetObjectPosition(clientID, obstacles[i], -1,
vrep.simx_opmode_oneshot_wait)
_, orientation = vrep.simxGetObjectOrientation(
clientID, obstacles[i], -1, vrep.simx_opmode_oneshot_wait)
orientation = orientation[2]
_, minX = vrep.simxGetObjectFloatParameter(
clientID, obstacles[i], 15, vrep.simx_opmode_oneshot_wait)
_, minY = vrep.simxGetObjectFloatParameter(
clientID, obstacles[i], 16, vrep.simx_opmode_oneshot_wait)
_, maxX = vrep.simxGetObjectFloatParameter(
clientID, obstacles[i], 18, vrep.simx_opmode_oneshot_wait)
_, maxY = vrep.simxGetObjectFloatParameter(
clientID, obstacles[i], 19, vrep.simx_opmode_oneshot_wait)
corners = [[minX, minY], [minX, maxY], [maxX, minY], [maxX, maxY]]
# compute bounding box corners for angled object
center = ut.mul(ut.add(corners[0], corners[3]), 0.5)
radius = ut.norm(ut.sub(corners[0], center))
zero = ut.mul(ut.add(corners[0], corners[2]), 0.5)
angle = ut.angle(ut.sub(corners[0], center), ut.sub(
zero, center)) + orientation + math.pi / 2
corners[0][0] = center[0] + radius * math.cos(angle)
corners[0][1] = center[1] + radius * math.sin(angle)
angle = ut.angle(ut.sub(corners[1], center), ut.sub(
zero, center)) + orientation + math.pi / 2
corners[1][0] = center[0] + radius * math.cos(angle)
corners[1][1] = center[1] + radius * math.sin(angle)
vec = ut.sub(center, corners[0])
corners[2] = ut.add(center, vec)
vec = ut.sub(center, corners[1])
corners[3] = ut.add(center, vec)
# move to obstacle position and convert to millimeters
for j in range(4):
corners[j] = ut.add(position[0:2], corners[j])
for k in range(2):
corners[j][k] = int(corners[j][k] * 1000)
output.append(corners)
# merge overlapping obstacles
ignore = []
merged = []
for i in range(obsNum):
if i not in ignore:
tmp, visited = ut.merge_obstacles(i, output, ignore)
ignore = ignore + visited
for j in range(len(tmp)):
for k in range(2):
tmp[j][k] = int(tmp[j][k])
merged.append(tmp)
# write to file to be used by vtk_voro
file = open('voro_data.txt', 'wt', encoding='utf-8')
file.write('[BORDER]\n' + str(border[0][0]) + ' ' + str(border[0][1]) +
'\n' + str(border[1][0]) + ' ' + str(border[1][1]) + '\n' +
str(border[2][0]) + ' ' + str(border[2][1]) + '\n' +
str(border[3][0]) + ' ' + str(border[3][1]) + '\n\n')
for i in range(len(merged)):
file.write('[OBSTACLE]\n')
for j in range(len(merged[i])):
file.write(
str(merged[i][j][0]) + ' ' + str(merged[i][j][1]) + '\n')
file.write('\n')
file.close()
# execute vtk_voro
command = "start /wait cmd /c cd " + os.getcwd(
) + " && .\\vtk_voro\\build\\Debug\\vtk_voro.exe voro_data.txt"
os.system(command)
# read vtk_voro output
vertices = []
edges = []
mode = True
with open('voro_output.txt', 'rt', encoding='utf-8') as f:
for line in f:
tmp = line.split()
if tmp[0] == "---":
mode = False
continue
if mode:
for i in range(len(tmp)):
tmp[i] = float(tmp[i]) / 1000
vertices.append(tmp)
else:
for i in range(len(tmp)):
tmp[i] = int(tmp[i])
edges.append(tmp)
# sample the obstacles for computing gaps
merged.append(border)
for i in range(len(merged)):
for j in range(len(merged[i])):
for k in range(len(merged[i][j])):
merged[i][j][k] /= 1000
for i in range(len(merged)):
size = len(merged[i])
for j in range(size):
if j == size - 1:
p0 = merged[i][j]
p1 = merged[i][0]
else:
p0 = merged[i][j]
p1 = merged[i][j + 1]
vec = ut.sub(p1, p0)
length = ut.norm(vec)
vec = ut.mul(vec, 1 / length)
for k in range(int(length / 0.1)):
merged[i].append(ut.add(p0, ut.mul(vec, (k + 1) * 0.1)))
# find size of the gap for each edge
gaps = []
for edge in edges:
p0 = vertices[edge[0]]
p1 = vertices[edge[1]]
center = ut.mul(ut.add(p0, p1), 0.5)
vec = ut.sub(p1, p0)
norm = [-vec[1], vec[0]]
length = ut.norm(vec)
line = [norm[0], norm[1], -norm[0] * p0[0] - norm[1] * p0[1]]
normal = [vec[0], vec[1], -vec[0] * center[0] - vec[1] * center[1]]
mindist = [sys.maxsize, sys.maxsize]
closest = [None, None]
for obstacle in merged:
for point in obstacle:
if ut.point_line(point, normal) <= length / 2 + 0.01:
dist = ut.point_line(point, line)
if line[0] * point[0] + line[1] * point[1] + line[2] > 0:
if dist < mindist[0]:
mindist[0] = dist
closest[0] = point
elif dist < mindist[1]:
mindist[1] = dist
closest[1] = point
if None in closest:
gaps.append(None)
else:
gaps.append(ut.norm(ut.sub(closest[0], closest[1])))
return vertices, edges, gaps
# Modules
# modules means files
# packages means folders
import utils # importing utils.py file from the same directory
print(utils.mul(2,3)) # imported files can be used this way
# Different ways to import
import module
from package.subpackage import module
from package.module import func1, func2 # this way we can know what exactly we've imported
from package import *
# =================================================================================
# What is
if __name == __main__
#^^^^^ notice double equals
# __main__ is the name of the file which is being executed
# every module file gets __name__ equals to the file name
# to ensure, a block of code is only executed if the executing file is the __main__ file, we use the condition
if __name == __main__:
# rest code goes here
def parse_packets(bits, num_subpackets=None, bit_length=None, depth=0, quiet=False):
"""
Given a bitstring `bits`, parses `num_subpackets` subpackets, and returns ([packets], bit_length, version).
If the argument `num_subpackets` is given, this dictates the number of packets
to parse from `bits` before returning.
If the argument `bit_length` is given, this dictates how many bits to process
before returning however many packets the parser got through.
If `quiet` is False, the parser will print the operator and values processed,
with various nesting levels based on `depth` (incremented per recursive call).
"""
i = 0 # current pointer into `bits`
subs = [] # list of subpackets currently parsed
version = 0 # the version so far (?)
try:
while True:
# Standard Packet Header
# Read 3-bit version
version += int(bits[i:i+3], 2)
i += 3
# Read 3-bit type ID
ttype = int(bits[i:i+3], 2)
i += 3
# Packets with type ID 4 represent a literal value.
if ttype == 4:
lit = ''
# Read groups of 5 bits until the final group
# (prefixed 0) is seen. Then stop reading.
while True:
group = bits[i:i+5]
i += 5
lit += group[1:]
if group[0] == '0':
break
try:
lit = int(lit, 2)
subs.append(lit)
except Exception:
print "bad literal, moving on"
# Otherwise, process an operator.
else:
# Operator packet mode is based on its 1-bit length type ID.
len_type_id = bits[i]
i += 1
# Specifies the bit length of contained subpackets.
if len_type_id == '0':
sub_len = int(bits[i:i+15], 2)
i += 15
subpackets, new_i, new_version = parse_packets(bits[i:i+sub_len], bit_length=sub_len, depth=depth + 1, quiet=quiet)
i += new_i
version += new_version
# Specifies total number of subpackets.
elif len_type_id == '1':
sub_packets = int(bits[i:i+11], 2)
i += 11
subpackets, new_i, new_version = parse_packets(bits[i:], num_subpackets=sub_packets, depth=depth+1, quiet=quiet)
i += new_i
version += new_version
else:
print "Unknown length type ID"
# Process operators
if not quiet:
print " " * depth, OPERATORS[ttype], subpackets
if ttype == 0:
subs.append(sum(subpackets))
elif ttype == 1:
subs.append(mul(subpackets))
elif ttype == 2:
subs.append(min(subpackets))
elif ttype == 3:
subs.append(max(subpackets))
elif ttype == 5:
subs.append(1 if subpackets[0] > subpackets[1] else 0)
elif ttype == 6:
subs.append(1 if subpackets[0] < subpackets[1] else 0)
elif ttype == 7:
subs.append(1 if subpackets[0] == subpackets[1] else 0)
# Check exit conditions for loop.
if num_subpackets is not None and len(subs) == num_subpackets:
# IMPORTANT: return i and not len(i); it is not an invariant
# that we read the full `bits`, since there may have been
# some trailing data that we ignore since it falls out of
# the range of `num_subpackets` subpackets.
return subs, i, version
elif bit_length is not None and i > bit_length:
return subs, len(bits), version
elif i >= len(bits):
return subs, len(bits), version
except Exception as e:
print "Unexpected parsing error:", e
return subs, i, version
def start(clientID, quads, targets, speed, proxs, path, leadfoll=False):
"""
Boids model program for experimental graph edges evaluation
:param clientID: ID of the VRep connection
:param quads: quadrotor handles
:param targets: quadrotor target handles
:param speed: speed of quadrotors
:param proxs: proximity sensor handles
:param path: quadrotor path coordinates
:param leadfoll: True - leader/followers mode, False - all boids following path (default)
"""
# definition of constants
quadsNum = len(quads) # number of quadrotors
viewRange = 3 # view range of quadrotors
smp = 0.2 # sampling period
kS = [0.30,
2.0] # separation constants [multiplication const, power const]
kC = [0.30, 0.0] # cohesion constants [multiplication const, power const]
kA = [0.00, 0.0] # alignment constants [multiplication const, power const]
kD = [speed,
1.0] # path following constants [multiplication const, power const]
kO = [0.20, 2.0
] # obstacle avoidance constants [multiplication const, power const]
# data stream init
for i in range(quadsNum):
vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quads[i],
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(clientID, proxs[i],
vrep.simx_opmode_streaming)
# variables init
position = [[0 for _ in range(3)]
for _ in range(quadsNum)] # position of quadrotors
velocity = [[0 for _ in range(3)]
for _ in range(quadsNum)] # velocity of quadrotors
closest = [[0 for _ in range(3)]
for _ in range(quadsNum)] # coords of closest obstacle to quads
visibleQuads = [[0 for _ in range(quadsNum)]
for _ in range(quadsNum)] # visible quadrotors
individualTarget = [
0
] * quadsNum # current waypoint index for each quadrotor
# get closest boid to starting point
leader = 0
_, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1,
vrep.simx_opmode_buffer)
dist = ut.norm(ut.sub(path[1], tmp))
for i in range(1, quadsNum):
_, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_buffer)
nrm = ut.norm(ut.sub(path[1], tmp))
if nrm < dist:
dist = nrm
leader = i
# main boids program
print('Evaluating ' + str(len(path)) + ' edges:')
number = 0
cnt = [0] * quadsNum
finished = [0] * len(path)
last = [False] * quadsNum
end = False
t1 = vrep.simxGetLastCmdTime(clientID)
ec = []
while vrep.simxGetConnectionId(clientID) != -1:
time.sleep(smp)
separation = [[0 for _ in range(3)]
for _ in range(quadsNum)] # separation force
cohesion = [[0 for _ in range(3)]
for _ in range(quadsNum)] # cohesion force
alignment = [[0 for _ in range(3)]
for _ in range(quadsNum)] # alignment force
destination = [[0 for _ in range(3)]
for _ in range(quadsNum)] # path following force
avoidance = [[0 for _ in range(3)]
for _ in range(quadsNum)] # obstacle avoidance force
output = [[0 for _ in range(3)]
for _ in range(quadsNum)] # output force
# read data from VRep
for i in range(quadsNum):
_, position[i] = vrep.simxGetObjectPosition(
clientID, quads[i], -1, vrep.simx_opmode_buffer)
_, velocity[i], _ = vrep.simxGetObjectVelocity(
clientID, quads[i], vrep.simx_opmode_buffer)
_, res, closest[i], _, _ = vrep.simxReadProximitySensor(
clientID, proxs[i], vrep.simx_opmode_buffer)
if not res:
closest[i] = [0, 0, 0]
closest[i][2] = 0
# compute visible quadrotors
for i in range(quadsNum):
for j in range(quadsNum):
if i != j:
temp = ut.sub(position[i], position[j])
if ut.norm(temp) < viewRange:
visibleQuads[i][j] = 1
else:
visibleQuads[i][j] = 0
for i in range(quadsNum):
# compute separation force
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.sub(position[i], position[j])
nrm = ut.norm(temp)
if nrm != 0:
temp = ut.mul(temp, kS[0] / (nrm**kS[1]))
separation[i] = ut.add(separation[i], temp)
# compute cohesion and alignment forces
center = [0, 0, 0] # center of the swarm
if sum(visibleQuads[i]) != 0:
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.mul(position[j], 1 / sum(visibleQuads[i]))
center = ut.add(center, temp)
temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i]))
alignment[i] = ut.add(alignment[i], temp)
cohesion[i] = ut.sub(center, position[i])
nrm = ut.norm(cohesion[i])
if nrm != 0:
cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1]))
nrm = ut.norm(alignment[i])
if nrm != 0:
alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1]))
# compute path following force
check = False
if not leadfoll or i == leader or end:
nrm = ut.norm(
ut.sub(position[i][0:2], path[individualTarget[i]][0:2]))
if end:
if finished[i] == 0 and nrm < 2:
finished[i] += 1
if sum(finished) == quadsNum:
return ec
else:
if individualTarget[i] != 0:
if not last[i]:
tmp = min(individualTarget)
if individualTarget[i] == tmp and tmp != max(
individualTarget):
cnt[i] += 1
vec1 = ut.sub(path[individualTarget[i] - 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i],
path[individualTarget[i]])
if nrm <= 1 or cnt[i] > 30 or ut.angle(
vec1, vec2) >= math.pi / 2:
if cnt[i] != 0:
cnt = [0] * quadsNum
finished[individualTarget[i]] += 1
if leadfoll or finished[
individualTarget[i]] == quadsNum:
check = True
if individualTarget[i] < len(path) - 1:
individualTarget[i] += 1
else:
last[i] = True
else:
vec1 = ut.sub(path[individualTarget[i] + 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i], path[individualTarget[i]])
if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2:
finished[individualTarget[i]] += 1
if leadfoll or finished[
individualTarget[i]] == quadsNum:
check = True
individualTarget[i] += 1
if check:
t2 = vrep.simxGetLastCmdTime(clientID)
ec.append((t2 - t1) / 1000)
print('Edge #' + str(number) + ': ' +
str((t2 - t1) / 1000) + 's')
number += 1
if number == len(path):
return ec
t1 = t2
if (leadfoll and finished[-1]
== 1) or finished[-1] == quadsNum:
end = True
individualTarget = [0] * quadsNum
finished = [0] * quadsNum
destination[i] = ut.sub(path[individualTarget[i]], position[i])
nrm = ut.norm(destination[i])
if nrm != 0:
destination[i] = ut.mul(destination[i],
kD[0] / (nrm**kD[1]))
# compute output force without obstacle avoidance
output[i] = separation[i]
output[i] = ut.add(output[i], cohesion[i])
output[i] = ut.add(output[i], alignment[i])
output[i] = ut.add(output[i], destination[i])
# compute obstacle avoidance force
# angle = ut.angle(closest[i], output[i])
# if angle > math.pi/2+0.3:
# avoidance[i] = [0, 0, 0]
# else:
avoidance[i] = ut.sub([0, 0, 0], closest[i])
nrm = ut.norm(avoidance[i])
if nrm != 0:
avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1]))
# compute output force
output[i] = ut.add(output[i], avoidance[i])
if position[i][2] < 0.5:
output[i][2] = 0.05
# export output to VRep
for i in range(quadsNum):
vrep.simxSetObjectPosition(clientID, targets[i], quads[i],
output[i], vrep.simx_opmode_streaming)
def _mul_update_impl(self, x, y, out, beta):
"""
use a custom built Elementwise kernel specifically created for
pre-allocated memories
"""
utils.mul(x, y, out=out, beta=beta)
for _ in range(4):
row = ''.join(tile[0])
piece_edges[row].add(id)
piece_edges[row[::-1]].add(id)
tile = rotated(tile)
# Construct graph of neighbouring pieces
graph = defaultdict(set)
for neighbouring_pieces in piece_edges.values():
for x, y in permutations(neighbouring_pieces, 2):
graph[x].add(y)
# Corners are the only pieces that neighbour exactly 2 other tiles.
corners = [tile for tile, neighs in graph.items() if len(neighs) == 2]
print "Corner ID product:", mul(corners)
print
# Build up the full arrangement of the tiles. Arbitrarily pick
# a corner to be the top-left, and assign its neighbours.
puzzle = {}
puzzle[0, 0] = corners[0]
# Let `m` represent the m by m arrangement of tiles.
m = int(math.sqrt(len(TILES)))
# Assign tiles for the top row and left column by choosing from the subset
# of tiles that contain either 2 or 3 neighbours (ie. edges and corners).
# Make sure to not reuse tiles since that ruins this algorithm.
edges = [tile for tile, neighs in graph.items() if len(neighs) == 3]
def load_map(shift, enlarge, include_border, clientID):
"""
Loads map from voro_data.txt, creates Voronoi diagram and adjusts the output
:param shift: shift the map by [x,y]
:param enlarge: enlarge the map
:param include_border: include border when computing size of gaps
:param clientID: ID of the VRep connection (only for visualization)
:return: list of graph vertices, indices of edge vertices, gaps for each edge
"""
# read map file
border = []
obstacles = [[]]
mode = True
idx = 0
with open('voro_data.txt', 'rt', encoding='utf-8') as f:
for line in f:
tmp = line.split()
if tmp:
if tmp[0].lower() == '[border]':
mode = True
elif tmp[0].lower() == '[obstacle]':
obstacles.append([])
idx += 1
mode = False
else:
if mode:
tmp[0] = float(tmp[0]) + shift[0]
tmp[1] = float(tmp[1]) + shift[1]
for i in range(len(tmp)):
tmp[i] = enlarge * float(tmp[i]) / 1000
border.append(tmp)
else:
tmp[0] = float(tmp[0]) + shift[0]
tmp[1] = float(tmp[1]) + shift[1]
for i in range(len(tmp)):
tmp[i] = enlarge * float(tmp[i]) / 1000
obstacles[idx].append(tmp)
del obstacles[0]
# execute vtk_voro
command = "start /wait cmd /c cd " + os.getcwd(
) + " && .\\vtk_voro\\build\\Debug\\vtk_voro.exe voro_data.txt"
os.system(command)
# read vtk_voro output
vertices = []
edges = []
mode = True
with open('voro_output.txt', 'rt', encoding='utf-8') as f:
for line in f:
tmp = line.split()
if tmp[0] == "---":
mode = False
continue
if mode:
tmp[0] = float(tmp[0]) + shift[0]
tmp[1] = float(tmp[1]) + shift[1]
for i in range(len(tmp)):
tmp[i] = enlarge * float(tmp[i]) / 1000
vertices.append(tmp)
else:
for i in range(len(tmp)):
tmp[i] = int(tmp[i])
edges.append(tmp)
# for vertex in vertices:
# _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectPosition(clientID, tmp, -1, vertex, vrep.simx_opmode_oneshot_wait)
# sample the obstacles for computing gaps
if include_border:
obstacles.append(border)
# for obstacle in obstacles:
# for point in obstacle:
# _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectPosition(clientID, tmp, -1, point, vrep.simx_opmode_oneshot_wait)
for i in range(len(obstacles)):
size = len(obstacles[i])
for j in range(size):
if j == size - 1:
p0 = obstacles[i][j]
p1 = obstacles[i][0]
else:
p0 = obstacles[i][j]
p1 = obstacles[i][j + 1]
vec = ut.sub(p1, p0)
length = ut.norm(vec)
vec = ut.mul(vec, 1 / length)
for k in range(int(length / 0.1)):
obstacles[i].append(ut.add(p0, ut.mul(vec, (k + 1) * 0.1)))
# for obstacle in obstacles:
# for point in obstacle:
# _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectPosition(clientID, tmp, -1, point, vrep.simx_opmode_oneshot_wait)
# find size of the gap for each edge
gaps = []
for edge in edges:
p0 = vertices[edge[0]]
p1 = vertices[edge[1]]
center = ut.mul(ut.add(p0, p1), 0.5)
vec = ut.sub(p1, p0)
norm = [-vec[1], vec[0]]
length = ut.norm(vec)
line = [norm[0], norm[1], -norm[0] * p0[0] - norm[1] * p0[1]]
normal = [vec[0], vec[1], -vec[0] * center[0] - vec[1] * center[1]]
mindist = [sys.maxsize, sys.maxsize]
found = [None, None]
for obstacle in obstacles:
for point in obstacle:
if ut.point_line(point, normal) <= length / 2 + 0.01:
dist = ut.point_line(point, line)
if line[0] * point[0] + line[1] * point[1] + line[2] > 0:
if dist < mindist[0]:
mindist[0] = dist
found[0] = True
elif dist < mindist[1]:
mindist[1] = dist
found[1] = True
if None in found:
gaps.append(None)
else:
gaps.append(sum(mindist))
return vertices, edges, gaps
