def is_object_in_bin(self):
"""Based on position of the bin and the object, calculates if the object is being held by the gripper.
"""
pos_bin = self.get_position(self.bin_handle)
pos_cyl = self.get_position(self.cylinder_handle)
return utility.distance(pos_bin, pos_cyl) < self.cylinder_bin_distance
def is_object_in_bin(self):
"""Based on position of the bin and the object, calculates if the object is being held by the gripper.
"""
pos_bin = self.get_position(self.bin_handle)
pos_cyl = self.get_position(self.cylinder_handle)
return utility.distance(pos_bin, pos_cyl) < self.cylinder_bin_distance
def warning_detection(width, height, image, vp, left_lane, right_lane):
"""
Returns the distance between the edges of the warning box and the points of intersection
with the lanes.
:param width: Image's width
:param height: Image's height
:param image: The actual image
:param vp: The coordinate representing the vanishing point
:param left_lane: The left lane
:param right_lane: The right lane
:return: tuple (dm, ds)
"""
half_width = int(width / 2)
half_height = int(height / 2)
bottom_left = (vp[0] - half_width, vp[1] + half_height)
bottom_right = (vp[0] + half_width, vp[1] + half_height)
cv2.rectangle(image,
(vp[0] - half_width, vp[1]),
(vp[0] + half_width, vp[1] + half_height),
(0, 0, 255), thickness=2)
warning_y = (int(vp[0] + half_width / 2), int(vp[1] + half_height))
cv2.rectangle(image, (vp[0] - int(width / 4), vp[1]), warning_y, (244, 64, 130), thickness=2)
m = line_intersection((bottom_left, bottom_right), left_lane)
s = line_intersection((bottom_left, bottom_right), right_lane)
if m is not None and s is not None:
a_m = m[0]
b_m = m[1]
a_s = s[0]
b_s = s[1]
# Draw the left distance of the screen
cv2.line(image, tuple(m), bottom_left, color=(66, 199, 244), thickness=4)
# Draw the intersection
cv2.circle(image, tuple(m), radius=4, color=(66, 199, 89), thickness=5)
# Draw the right distance of the screen
cv2.line(image, tuple(s), bottom_right, color=(66, 199, 244), thickness=4)
# Draw the intersection
cv2.circle(image, tuple(s), radius=4, color=(66, 199, 89), thickness=5)
d_m = distance((a_m, b_m), bottom_left)
d_s = distance((a_s, b_s), bottom_right)
return d_m, d_s
def test_node_lookup():
target_id = utility.default_id
n = Node(1000)
n.load_kbucket('nodeA.csv')
x = n._node_lookup(target_id)
print('node lookup answer:', x)
for triple in x:
print(utility.distance(triple.id, target_id))
def bus_stop_generator(roads, inner_r, outer_r, k=10, seeds=None):
"""
Bus stop placement using the poisson-disc algorithm
"""
assert inner_r < outer_r
if seeds is None:
seeds = []
all_points = list(seeds)
road_points_gen = _road_point_generator(roads)
if not all_points: # Check if all_points is empty
road = tuple(next(road_points_gen))
yield road
all_points.append(road) # Seed point
active_points = list(
all_points) # Use a list because random.choice require a sequence
def check_dist(p, test_points, limit=inner_r):
"""
Return true if any of the test_points is within the limit distance of P
"""
for test_point in test_points:
if distance((p[0], p[1]), (test_point[0], test_point[1])) < limit:
# p is within limit distance
return True
# p is not within the limit distance of of the test points
return False
while len(active_points) > 0:
# Pick a random point from the set of active points to be the center of the poisson disc
center = random.choice(active_points)
# Limit the search to K points
gen = firstn(
k,
filter(
lambda point: inner_r <= distance(
(center[0], center[1]), (point[0], point[1])) <= outer_r,
road_points_gen))
# Search for candidate point
try:
# Search for a point, or raise StopIteration is none can be found
point = next(
filter(lambda p: not check_dist(p, iter(all_points)), gen))
assert not check_dist(point, all_points)
# A new point was found
active_points.append(point)
all_points.append(tuple(point))
yield point
except StopIteration:
# No point was found, mark center as an inactive point
active_points.remove(center)
def sample(self, pos: Tuple[float, float]) -> float:
"""
Samples the noise at the given position
:param pos: the position to sample at
:return: a float in the range [0,1)
"""
scale = 4 / self._radius
noise01 = (noise.pnoise3(pos[0] * scale, pos[1] * scale, self._offset, octaves=self._octaves) + 1) / 2
gradient = (1 - (distance(pos, self._centre) / self._radius))
return (smoothstep(noise01) + gradient * self._centre_weight) / (1 + self._centre_weight)
def distance_check(self): my_coordinates = (self.my_player.x, self.my_player.y) player_coordinates = (self.player.x, self.player.y) distance = utility.distance(my_coordinates, player_coordinates) is_in_range = self.player.x != 0 and distance <= configuration.maximal_enchant_distance if not is_in_range: packets.send_chat(configuration.enchant_range_error) return is_in_range
def find_closest_depot(pos):
closest_depot = None
closest_distance = -1
for i, depot in enumerate(depots):
d = distance(depot.pos, pos)
if closest_depot is None or d < closest_distance:
closest_depot = (depot, i)
closest_distance = d
return closest_depot[0], closest_depot[1], closest_distance
def check_dist(p, test_points, limit=inner_r):
"""
Return true if any of the test_points is within the limit distance of P
"""
for test_point in test_points:
if distance((p[0], p[1]), (test_point[0], test_point[1])) < limit:
# p is within limit distance
return True
# p is not within the limit distance of of the test points
return False
def __update_actionstate(self, action_id):
"""Caches various environment parameters. Used for reward calculation among other things.
"""
self.actionstate_curr['action_id'] = action_id
self.actionstate_curr['current_state_id'] = self.get_current_state()
self.actionstate_curr['state'] = self.states[
self.actionstate_curr['current_state_id']]
self.actionstate_curr['cylinder_position'] = self.get_object_position()
self.actionstate_curr['bin_position'] = self.get_destination_position()
self.actionstate_curr['claw_position'] = self.get_arm_position()
self.actionstate_curr['claw_cylinder_distance'] = utility.distance(
self.actionstate_curr['claw_position'],
self.actionstate_curr['cylinder_position'])
self.actionstate_curr['bin_cylinder_distance'] = utility.distance(
self.actionstate_curr['bin_position'],
self.actionstate_curr['cylinder_position'])
self.actionstate_curr['cylinder_in_bin'] = self.robot.is_object_in_bin(
)
self.actionstate_curr['is_cylinder_held'] = self.robot.is_object_held()
def talk(self, player, network):
nodes = utility.distance(player, network)
if len(nodes) != 0:
responder = random.choice(nodes)
responder.stop = 1
responder.talking = 1
self.conversationManager.setText(responder.startDialog())
self.conversationManager.setPosFromTalkerRect(responder.rect)
self.stop = 1
self.talkingwith = responder
self.delay = 1
def vEarth(v,eCords):
# d ex d ey
# 2*ex * ---- + 2*ey * ----
# dt dt ex*vX + ey*vY
# vEarth = --------------------------- = -------------------
# 2 * (ex^2 + ey^2)^0.5 (ex^2 + ey^2)^0.5
(vX,vY) = v
(ex,ey) = eCords
vEarth = (ex*vX + ey*vY) / utility.distance(ex,ey)
return vEarth
def choose_action_explore(self, obs_dict, index):
# maintain the data structure for map exploration
area_x = int(obs_dict[index]['pos_x'] / SQUARE_SIZE)
area_y = int(obs_dict[index]['pos_y'] / SQUARE_SIZE)
explored_index = area_x * (self.size_y / SQUARE_SIZE) + area_y
if explored_index >= (self.size_x / SQUARE_SIZE) * (self.size_y /
SQUARE_SIZE):
print('explored_index out of range!')
explored_index = (self.size_x / SQUARE_SIZE) * (self.size_y /
SQUARE_SIZE) - 1
if explored_index in self.to_explore_set:
self.to_explore_set.remove(explored_index)
if len(self.to_explore_set) == 0:
self.to_explore_set = set(
range(
round((self.size_x / SQUARE_SIZE) *
(self.size_y / SQUARE_SIZE))))
# if agent has a destination to explore
if index in self.explore_destination_set:
# delete the destination when getting close enough
if ut.distance(obs_dict[index]['pos_x'], obs_dict[index]['pos_y'],
self.explore_destination_set[index][0],
self.explore_destination_set[index][1]) <= 5:
# print('trying to delete the key')
del self.explore_destination_set[index]
# print('delete succesfully')
# if the agent has no destination, randomly select a destination from the to_explore_set
if index not in self.explore_destination_set:
if len(self.to_explore_set) == 0:
print('Error! to_explore_set is empty')
selected_ele = random.sample(self.to_explore_set, 1)
# print('selected_ele: ', selected_ele)
ele_index_x = int(selected_ele[0] / (self.size_y / SQUARE_SIZE))
# print('ele_index_x: ', ele_index_x)
ele_index_y = int(selected_ele[0] % (self.size_y / SQUARE_SIZE))
# print('ele_index_y', ele_index_y)
ele_pos_x = int(ele_index_x * SQUARE_SIZE + SQUARE_SIZE / 2)
ele_pos_y = int(ele_index_y * SQUARE_SIZE + SQUARE_SIZE / 2)
# print('ele_pos_x: ', ele_pos_x, 'ele_pos_y', ele_pos_y)
self.explore_destination_set[index] = (ele_pos_x, ele_pos_y)
# print('self.explore_destination_set[index]: ', self.explore_destination_set[index])
# print('angle: ',
# ut.angle(obs_dict[index]['pos_x'], obs_dict[index]['pos_y'], self.explore_destination_set[index][0],
# self.explore_destination_set[index][1]))
# navigating towards the explore destination
return [
ut.angle(obs_dict[index]['pos_x'], obs_dict[index]['pos_y'],
self.explore_destination_set[index][0],
self.explore_destination_set[index][1]), 1, 0, 0
]
def schedule_route(route):
if not len(route):
return route
new_route = []
prev_cust = random.choice(route)
route.remove(prev_cust)
new_route.append(prev_cust)
while len(route):
prev_cust = min(route,
key=lambda x: distance(customers[x - 1].pos, customers[
prev_cust - 1].pos))
route.remove(prev_cust)
new_route.append(prev_cust)
return new_route
def update_for_in_trap(self, t, traps): #******
"""
Update simulation for flies in traps.
* If flies are in traps. If so record trap info and time.
"""
sources = traps.param['source_locations'] #Of format [(0,0),]
for trap_num, trap_loc in enumerate(sources):
dist_vals = distance((self.x_position, self.y_position), trap_loc)
mask_trapped = dist_vals < traps.param['trap_radius']
self.mode[mask_trapped] = self.Mode_Trapped
self.trap_num[mask_trapped] = trap_num
self.x_trap_loc[mask_trapped] = trap_loc[0]
self.y_trap_loc[mask_trapped] = trap_loc[1]
self.x_velocity[mask_trapped] = 0.0
self.y_velocity[mask_trapped] = 0.0
# Get time stamp for newly trapped flies
mask_newly_trapped = mask_trapped & (self.t_in_trap == scipy.inf)
self.t_in_trap[mask_newly_trapped] = t
def is_consistent_route(route, depot, include_reason=False):
route_load = 0
route_duration = 0
last_pos = depot.pos
for c in route:
customer = customers[c - 1]
route_load += customer.demand
route_duration += distance(last_pos,
customer.pos) + customer.service_duration
last_pos = customer.pos
route_duration += find_closest_depot(last_pos)[2]
if include_reason:
if route_load > depot.max_load:
return False, 1
if depot.max_duration != 0 and route_duration > depot.max_duration:
return False, 2
return True, 0
return route_load <= depot.max_load and (
depot.max_duration == 0 or route_duration <= depot.max_duration)
def evaluate_route(route, depot, return_load=False):
if len(route) == 0:
if return_load:
return 0, 0
return 0
route_load = 0
route_length = 0
customer = None
last_pos = depot.pos
for cid in route:
customer = customers[cid - 1]
route_load += customer.demand
route_length += distance(last_pos, customer.pos)
route_length += customer.service_duration
last_pos = customer.pos
route_length += find_closest_depot(customer.pos)[1]
if return_load:
return route_length, route_load
return route_length
def obs_construct(self, obs_raw_dict):
# self.obs_dict = copy.deepcopy(obs_raw_dict)
for i in range(self.fighter_num):
self.obs_dict[i] = obs_raw_dict['fighter_obs_list'][i]
self.obs_dict['strike_list'] = obs_raw_dict['joint_obs_dict'][
'strike_list']
# continue if the fighter is dead
if not self.obs_dict[i]['alive']:
continue
detected_enemy_list = []
myself_pos_x = self.obs_dict[i]['pos_x']
myself_pos_y = self.obs_dict[i]['pos_y']
for j in range(len(self.obs_dict[i]['r_visible_list'])):
enemy_dict = self.obs_dict[i]['r_visible_list'][j]
enemy_pos_x = enemy_dict['pos_x']
enemy_pos_y = enemy_dict['pos_y']
enemy_dict['distance'] = ut.distance(myself_pos_x,
myself_pos_y, enemy_pos_x,
enemy_pos_y)
enemy_dict['angle'] = ut.angle(myself_pos_x, myself_pos_y,
enemy_pos_x, enemy_pos_y)
detected_enemy_list.append(enemy_dict)
####commented by liyuan.
'''
for j in range(len(self.obs_dict[i]['j_recv_list'])):
enemy_dict = self.obs_dict[i]['j_recv_list'][j]
enemy_pos_x = enemy_dict['pos_x']
enemy_pos_y = enemy_dict['pos_y']
enemy_dict['distance'] = ut.distance(myself_pos_x, myself_pos_y, enemy_pos_x, enemy_pos_y)
enemy_dict['angle'] = ut.angle(myself_pos_x, myself_pos_y, enemy_pos_x, enemy_pos_y)
detected_enemy_list.append(enemy_dict)
'''
if len(detected_enemy_list) > 0:
detected_enemy_list = sorted(detected_enemy_list,
key=lambda k: k['distance'])
self.obs_dict[i]['target_list'] = detected_enemy_list
return self.obs_dict
def create_random_chromosome(groups):
routes = []
for d in range(len(groups)):
depot = depots[d]
group = groups[d][:]
random.shuffle(group)
routes.append([[]])
r = 0
route_cost = 0
route_load = 0
last_pos = depot.pos
for c in group:
customer = customers[c - 1]
cost = distance(
last_pos,
customer.pos) + customer.service_duration + find_closest_depot(
customer.pos)[2]
if route_cost + cost > depot.max_duration or route_load + customer.demand > depot.max_load:
r += 1
routes[d].append([])
routes[d][r].append(c)
return encode(routes)
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After lifting", ra)
pos = ra.get_position(ra.bin_handle)
status("After getting position of bin", ra)
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After going to bin top", ra)
ra.enable_grip(False)
status("After releasing", ra)
dist = utility.distance(ra.bin_position, ra.get_position(ra.bin_handle))
print(dist)
if dist > config.TOLERANCE:
print("Bin is not in position")
else:
print("Bin is in position")
print("Reset ==================================================")
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
print(pos)
ra.goto_position(pos)
pos = ra.get_position(ra.cylinder_handle)
print(pos)
"""
def create_heuristic_chromosome(groups):
# Group customers in routes according to savings
routes = [[] for i in range(len(depots))]
missing_customers = list(map(lambda x: x.id, customers))
for d in range(len(groups)):
depot = depots[d]
savings = []
for i in range(len(groups[d])):
ci = customers[groups[d][i] - 1]
savings.append([])
for j in range(len(groups[d])):
if j <= i:
savings[i].append(0)
else:
cj = customers[groups[d][j] - 1]
savings[i].append(
distance(depot.pos, ci.pos) +
distance(depot.pos, cj.pos) - distance(ci.pos, cj.pos))
savings = np.array(savings)
order = np.flip(np.argsort(savings, axis=None), 0)
for saving in order:
i = saving // len(groups[d])
j = saving % len(groups[d])
ci = groups[d][i]
cj = groups[d][j]
ri = -1
rj = -1
for r, route in enumerate(routes[d]):
if ci in route:
ri = r
if cj in route:
rj = r
route = None
if ri == -1 and rj == -1:
if len(routes[d]) < depot.max_vehicles:
route = [ci, cj]
elif ri != -1 and rj == -1:
if routes[d][ri].index(ci) in (0, len(routes[d][ri]) - 1):
route = routes[d][ri] + [cj]
elif ri == -1 and rj != -1:
if routes[d][rj].index(cj) in (0, len(routes[d][rj]) - 1):
route = routes[d][rj] + [ci]
elif ri != rj:
route = routes[d][ri] + routes[d][rj]
if route:
if is_consistent_route(route, depot, True)[1] == 2:
route = schedule_route(route)
if is_consistent_route(route, depot):
if ri == -1 and rj == -1:
routes[d].append(route)
missing_customers.remove(ci)
if ci != cj:
missing_customers.remove(cj)
elif ri != -1 and rj == -1:
routes[d][ri] = route
missing_customers.remove(cj)
elif ri == -1 and rj != -1:
routes[d][rj] = route
missing_customers.remove(ci)
elif ri != -1 and rj != -1:
if ri > rj:
routes[d].pop(ri)
routes[d].pop(rj)
else:
routes[d].pop(rj)
routes[d].pop(ri)
routes[d].append(route)
# Order customers within routes
for i, depot_routes in enumerate(routes):
for j, route in enumerate(depot_routes):
new_route = schedule_route(route)
routes[i][j] = new_route
chromosome = encode(routes)
chromosome.extend(missing_customers)
return chromosome
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After lifting", ra)
pos = ra.get_position(ra.bin_handle)
status("After getting position of bin", ra)
pos[2] = utility.rnd(0.10)
ra.goto_position(pos)
status("After going to bin top", ra)
ra.enable_grip(False)
status("After releasing", ra)
dist = utility.distance(ra.bin_position, ra.get_position(ra.bin_handle))
print(dist)
if dist > config.TOLERANCE:
print("Bin is not in position")
else:
print("Bin is in position")
print("Reset ==================================================")
ra.restart_sim()
pos = ra.get_position(ra.cylinder_handle)
print(pos)
ra.goto_position(pos)
pos = ra.get_position(ra.cylinder_handle)
print(pos)
def is_bin_inplace(env):
if utility.distance(
env.robot.bin_position,
env.actionstate_curr['bin_position']) > config.TOLERANCE_FINER:
return False
return True
def is_bin_inplace(env):
if utility.distance(env.robot.bin_position, env.actionstate_curr['bin_position']) > config.TOLERANCE_FINER:
return False
return True
