def next_step(self, current_vertex_ind, vertexes, current_time):
if not self.queue: #if queue of next visits is empty
max_cluster = self.clusters[0]
max_price = 0
for cluster in self.clusters: # find the cluster with the highest price
cluster_price = 0
for v_i in cluster: # calculate cluster's price
v = vertexes[v_i]
cluster_price += global_utils.price(
v.cst(current_time), v.p)
if cluster_price > max_price:
max_cluster = cluster
max_price = cluster_price
# sort cluster so the robot visit the highest price in the cluster first
vs_of_cluster = [(i,
global_utils.price(vertexes[i].cst(current_time),
vertexes[i].p))
for i in max_cluster]
vs_of_cluster = [
v for v in vs_of_cluster if v[1] > 0
] # filter out all vertexes in cluster that has price=0
vs_of_cluster.sort(key=lambda v: v[1], reverse=True)
self.queue.extend(map(lambda v: v[0], vs_of_cluster))
if self.queue:
next_v = self.queue.popleft()
else:
next_v = current_vertex_ind
return next_v
def branch(self):
for i, v in enumerate(self.vertexes):
if i == self.state_vertex_ind:
continue
self.price = 0
c_time_stamp = self.time_stamp + \
self.distance_matrix[self.state_vertex_ind][i]
c_vertexes = []
for _, w in enumerate(self.vertexes):
c_vertexes.append(self.copy_vertex(w))
c_state_vertex_ind = i
c_vertexes[c_state_vertex_ind].visit(c_time_stamp)
# calculate price of state by end result
child = State(c_time_stamp, c_state_vertex_ind, c_vertexes,
self.distance_matrix, self)
# total world price divided by time passed
# child.price = sum([global_utils.price(c.cst(c_time_stamp), c.p) for c in child.vertexes])*(c_time_stamp-self.time_stamp) + self.price
child.price = sum([
global_utils.price(v.cst(c_time_stamp), v.p)
for v in child.vertexes
])
# print('parent point: ',
# self.vertexes[self.state_vertex_ind].point, 'child point', self.vertexes[child.state_vertex_ind].point, ' price: ', child.price, ' time: ', self.time_stamp)
self.children.append(child)
return self.children
def price_function(self, factor, node, distance):
if distance == 0:
return 0
vertex = self.vertexes[node.index]
# cost = global_utils.price(vertex.cst(
# self.current_time+distance), vertex.p)
cost = global_utils.price(vertex.cst(self.current_time + distance),
vertex.p)
# TODO: replace with constant
return factor * cost * ((1 / distance)**CONSTANTS['G'])
def next_step(self, current_vertex_ind, vertexes, current_time):
max_ind = current_vertex_ind
max_price = 0
for i, v in enumerate(
vertexes): # find the vertex with the maximum price
current_v_price = global_utils.price(v.cst(current_time), v.p)
if current_v_price > max_price:
max_ind = i
max_price = current_v_price
return max_ind
def next_step(self, current_vertex_ind, vertexes, current_time):
# 1. calculate potential field force on robot
current_point = vertexes[current_vertex_ind].point
fv = (0, 0)
for i, v in enumerate(vertexes):
if i == current_vertex_ind:
continue
# 1.0 calc distance
d = numpy.linalg.norm(current_point - v.point)
# 1.1 calc length of vector
length = global_utils.price(v.cst(current_time), v.p) / d**2
# 1.2 calc direction vector
fv += length * ((v.point - current_point) / d)
# 1.3 obstacles
fv += self.obstacle_forces(current_point)
# 2. make sure that path is valid
# 3. if vertex is in radius and screaming and not current vertex, return vertex index
# 4. move virtual step toward force vector
# 5. repeat until 3 is satisfied
return None