def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9999)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
def __init__(self):
# Receiving
self.num_robots = 0
self.num_robots = self.sock_receiver.receive_number()
print("num_robots: {}".format(self.num_robots))
# Pheromone initialization
self.pheromone = [None]*self.num_robots
for i in range(self.num_robots):
self.pheromone[i] = Pheromone('dynamic {}'.format(i), 0.5, 0)
self.pheromone.append(Pheromone('static', 180, 0))
self.phero_max = 1.0
self.phero_min = 0.0
self.is_phero_inj = True
# Socket Communication
self.sock_receiver = SocketNumpyArray()
self.sock_receiver.initalize_receiver(9999) # the 9999 is the port you can change it with your own.
self.theta = 0
self.log_timer = time.clock()
#self.log_file = open("phero_value.txt", "a+")
self.is_saved = False
self.is_loaded = False
self.is_reset = True # False for reset
for i in range(len(phero)):
self.pheromone[i].isDiffusion = False
self.pheromone[i].isEvaporation = True
self.startTime = time.time()
# Robot positions
self.positions = [0,0]*self.num_robots
self.x_idx = [0, 0]
self.y_idx = [0, 0]
# PosToIndex for pheromone circle
# x_2, y_0 = self.posToIndex(2,0)
# x_n3, y_n3 = self.posToIndex(-3,-3)
# x_0, y_0 = self.posToIndex(0,0)
# Pheromone Initilaisation
# self.pheromone.circle(x_2, y_0, 1, 1)
# self.pheromone.circle(x_3, y_0, 1, 1)
# self.pheromone.circle(x_n3, y_0, 1, 1)
# self.pheromone.circle(x_0, y_3, 1, 1)
# self.pheromone.circle(x_0,y_n3, 1, 1)
print("Initialisation completed")
def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9998)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
# Object initialisation
# Robot initial positions
self.robot_positions = [[-2.5,0],[2.5,0],[0,-2.5],[0,2.5]]
self.robot_angles = [0, pi, pi/2, -pi/2]
self.target_positions = [[2.5,0],[-2.5,0],[0,2.5],[0,-2.5]]
def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9998)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
# Target
self.target_x = 4.0
self.target_y = 0.0
self.target_index = 0
self.radius = 4
self.num_experiments = 20
def __init__(self, phero):
self.pheromone = phero
self.phero_max = 1.0
self.phero_min = 0.0
self.is_phero_inj = True
# Socket Communication
self.sock_receiver = SocketNumpyArray()
self.sock_receiver.initalize_receiver(
9998) # the 9999 is the port you can change it with your own.
self.is_service_requested = False
self.theta = 0
self.log_timer = time.clock()
self.log_file = open("phero_value.txt", "a+")
self.is_saved = False
self.is_loaded = False
self.is_reset = True # False for reset
self.pheromone.isDiffusion = True
self.pheromone.isEvaporation = False
self.startTime = time.time()
# PosToIndex for pheromone circle
# x_2, y_2 = self.posToIndex(2,2)
# x_n2, y_n2 = self.posToIndex(-2,-2)
# x_1p4, y_1p4 = self.posToIndex(1.4, 1.4)
# x_n1p4, y_n1p4 = self.posToIndex(-1.4, -1.4)
# x_0, y_0 = self.posToIndex(0,0)
# Pheromone Initilaisation
# self.pheromone.circle(x_2, y_0, 1, 0.7)
# self.pheromone.circle(x_n2, y_0, 1, 0.7)
# self.pheromone.circle(x_0, y_2, 1, 0.7)
# self.pheromone.circle(x_0, y_n2, 1, 0.7)
# self.pheromone.circle(x_1p4, y_1p4, 1, 0.5)
# self.pheromone.circle(x_1p4, y_n1p4, 1, 0.5)
# self.pheromone.circle(x_n1p4, y_n1p4, 1, 0.5)
# self.pheromone.circle(x_n1p4, y_1p4, 1, 0.5)
self.num_robots = self.sock_receiver.receive_number()
print("num_robots: {}".format(self.num_robots))
print("Initialisation completed")
class Scenario(BaseScenario):
def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9998)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
# Target
self.target_x = 4.0
self.target_y = 0.0
self.target_index = 0
self.radius = 4
self.num_experiments = 20
def make_world(self):
world = World()
# set any world properties first
world.dim_c = 2
num_agents = 1
num_obstacle = 4
num_target = 1
world.collaborative = True
# add agents
world.agents = [Agent() for i in range(num_agents)]
for i, agent in enumerate(world.agents):
agent.name = 'agent %d' % i
agent.collide = True
agent.silent = True
agent.size = 0.1
# add obstacles
world.landmarks = [Landmark() for i in range(num_obstacle)]
for i, landmark in enumerate(world.landmarks):
landmark.name = 'obstacle %d' % i
landmark.collide = True
landmark.movable = False
landmark.size = 0.1
# add target
target = Landmark()
target.name = 'target'
target.collide = False
target.movable = False
target.size = 0.1
# Merge the landmarks (obstacles + target)
world.landmarks.append(target)
# make initial conditions
self.n = num_agents
self.x = [0.0, 0.0] * num_agents
self.y = [0.0, 0.0] * num_agents
self.theta = [0.0, 0.0] * num_agents
self.reset_world(world)
# Send initial information to pheromone system
self.sock_sender.send_number(self.n)
return world
def reset_world(self, world):
# random properties for agents
for i, agent in enumerate(world.agents):
agent.color = np.array([0.35, 0.35, 0.85])
# random properties for landmarks
for i, landmark in enumerate(world.landmarks):
landmark.color = np.array([0.25, 0.25, 0.25])
if i == len(world.landmarks) - 1:
landmark.color = np.array([0.9, 0.1, 0.1])
# ========================================================================= #
# TARGET UPDATE #
# ========================================================================= #
# set random initial states
# robot position (0, 0), distance between robots and target (4 m)
angle_target = self.target_index * 2 * pi / self.num_experiments
self.target_x = self.radius * cos(angle_target)
self.target_y = self.radius * sin(angle_target)
if self.target_index < self.num_experiments - 1:
self.target_index += 1
else:
self.target_index = 0
# ========================================================================= #
# OBJECT RESET #
# ========================================================================= #
# Agent update
world.agents[0].state.p_pose = np.asarray([0.0, 0.0, 0.0])
world.agents[0].state.p_pos = world.agents[0].state.p_pose[:2]
self.target = [[self.x[1], self.y[1]], [self.x[0],
self.y[0]]] #20201130 Target
for i, agent in enumerate(world.agents):
#agent.state.p_pos = np.random.uniform(-1, +1, world.dim_p)
agent.state.p_vel = np.zeros(world.dim_p)
agent.state.c = np.zeros(world.dim_c)
agent.state.target = [self.target_x,
self.target_y] # 20201201 target
# for i, landmark in enumerate(world.landmarks):
# if landmark.name[0] == 'o':
# landmark.state.p_pos = [0, 0] #np.random.uniform(-1, +1, world.dim_p)
# landmark.state.p_vel = np.zeros(world.dim_p)
# Obstacle update
world.landmarks[0].state.p_pos = np.asarray([2, 0])
world.landmarks[0].state.p_vel = np.zeros(world.dim_p)
world.landmarks[1].state.p_pos = np.asarray([0, 2])
world.landmarks[1].state.p_vel = np.zeros(world.dim_p)
world.landmarks[2].state.p_pos = np.asarray([-2, 0])
world.landmarks[2].state.p_vel = np.zeros(world.dim_p)
world.landmarks[3].state.p_pos = np.asarray([0, -2])
world.landmarks[3].state.p_vel = np.zeros(world.dim_p)
# Target update
world.landmarks[4].state.p_pos = np.asarray(
[self.target_x, self.target_y])
world.landmarks[4].state.p_vel = np.zeros(world.dim_p)
def benchmark_data(self, agent, world):
rew = 0
collisions = 0
occupied_landmarks = 0
min_dists = 0
for l in world.landmarks:
dists = [
np.sqrt(np.sum(np.square(a.state.p_pos - l.state.p_pos)))
for a in world.agents
]
min_dists += min(dists)
rew -= min(dists)
if min(dists) < 0.1:
occupied_landmarks += 1
if agent.collide:
for a in world.agents:
if self.is_collision(a, agent):
rew -= 1
collisions += 1
return (rew, collisions, min_dists, occupied_landmarks)
def is_collision(self, agent1, agent2):
delta_pos = agent1.state.p_pos - agent2.state.p_pos
dist = np.sqrt(np.sum(np.square(delta_pos)))
dist_min = agent1.size + agent2.size
return True if dist < dist_min else False
def reward(self, agent, world):
'''
Compute rewards
'''
distance_reward = 0.0
phero_reward = 0.0
goal_reward = 0.0
collision_reward = 0.0
angular_reward = 0.0
#angular_punish_rewards = [0.0]*self.num_robots
#linear_punish_rewards = [0.0]*self.num_robots
# 1. Distance Reward
goal_progress = agent.state.distance_to_goal_prev - agent.state.distance_to_goal
if abs(goal_progress) < 0.1:
#print("Goal Progress: {}".format(goal_progress))
#print("Angle: {}".format(agent.state.angle_diff))
if goal_progress >= 0:
distance_reward = goal_progress * 1.2
else:
distance_reward = goal_progress
else:
distance_reward = 0.0
# Scaling distance_reward
distance_reward = distance_reward * (5 / world.dt)
# 2. Phero Reward
#phero_sum = np.sum(self.phero)
#phero_reward = -phero_sum*2
# 3. Goal Reward
if agent.state.distance_to_goal <= 0.3:
goal_reward = 100.0
#done = True
#self.reset(model_state, id_bots=idx[i])
# 4. Collision Penalty
for i, obstacle in enumerate(
[ob for ob in world.landmarks if 'obstacle' in ob.name]):
is_collision = self.is_collision(agent, obstacle)
if is_collision == True:
collision_reward = -50.0
# 5. Angular velocity penalty
if abs(agent.action.twist[1]) > 0.8:
angular_reward = -1
reward = distance_reward + phero_reward + goal_reward + collision_reward
print("----------------")
print("GP: {}".format(goal_progress))
print("Distance R: {}".format(distance_reward))
print("Collision R: {}".format(collision_reward))
print("Goal R: {}".format(goal_reward))
print("Angular R: {}".format(angular_reward))
print("Reward: {}".format(reward))
return reward
def observation(self, agent, world):
id = int(agent.name[-1])
# get positions of all entities in this agent's reference frame
entity_pos = []
for entity in world.landmarks: # world.entities:
entity_pos.append(entity.state.p_pos - agent.state.p_pos)
# entity colors
entity_color = []
for entity in world.landmarks: # world.entities:
entity_color.append(entity.color)
positions = np.asarray(agent.state.p_pos)
self.sock_sender.send_numpy_array(positions)
data = self.sock_sender.receive_from_server()
self.phero = phero = np.asarray(data).reshape(1, 9)
obs = np.hstack(
(phero, [agent.action.twist],
np.asarray([agent.state.distance_to_goal]).reshape(1, 1),
np.asarray([agent.state.angle_diff]).reshape(1, 1)))
#print("obs: {}".format(obs))
#print("Observation: {}".format(obs))
world.obs_n = np.shape(obs)[1]
return obs # 20201201 observation is changed (pheromone + velocity, distance, anglediff ) 1*13
def done(self, agent, world):
agent.state.done = False
# 1. Goal arrival
if agent.state.distance_to_goal <= 0.3:
print("Goal Arrived!")
agent.state.done = True
# 2. Out of range
if abs(agent.state.p_pos[0]) > 4.6 or abs(agent.state.p_pos[1]) > 4.6:
agent.state.done = True
print("out of range!!!!")
# 3. collision
for i, obstacle in enumerate(
[ob for ob in world.landmarks if 'obstacle' in ob.name]):
is_collision = self.is_collision(agent, obstacle)
if is_collision == True:
agent.state.done = True
print("Collision!!")
time.sleep(1)
done = agent.state.done
return done
def info(self, agent, world):
info = [{"targets": agent.state.target}]
return info
class Node():
def __init__(self, phero):
self.num_robots = 2
self.pheromone = [None] * self.num_robots
for i in range(len(phero)):
self.pheromone[i] = phero[i]
self.phero_max = 1.0
self.phero_min = 0.0
self.is_phero_inj = True
# Socket Communication
self.sock_receiver = SocketNumpyArray()
self.sock_receiver.initalize_receiver(9999) # the 9999 is the port you can change it with your own.
self.theta = 0
self.log_timer = time.clock()
#self.log_file = open("phero_value.txt", "a+")
self.is_saved = False
self.is_loaded = False
self.is_reset = True # False for reset
for i in range(len(phero)):
self.pheromone[i].isDiffusion = False
self.pheromone[i].isEvaporation = True
self.startTime = time.time()
# Robot positions
self.positions = [0,0]*self.num_robots
self.x_idx = [0, 0]
self.y_idx = [0, 0]
# PosToIndex for pheromone circle
# x_2, y_0 = self.posToIndex(2,0)
# x_n3, y_n3 = self.posToIndex(-3,-3)
# x_0, y_0 = self.posToIndex(0,0)
# Pheromone Initilaisation
# self.pheromone.circle(x_2, y_0, 1, 1)
# self.pheromone.circle(x_3, y_0, 1, 1)
# self.pheromone.circle(x_n3, y_0, 1, 1)
# self.pheromone.circle(x_0, y_3, 1, 1)
# self.pheromone.circle(x_0,y_n3, 1, 1)
print("Initialisation completed")
def posToIndex(self, x, y):
phero = self.pheromone
x_tmp = x
y_tmp = y
# Read pheromone value at the robot position
x_index = [0]*len(phero)
y_index = [0]*len(phero)
for i in range(len(phero)):
res = phero[i].resolution
round_dp = int(log10(res))
x_tmp[i] = round(x_tmp[i], round_dp) # round the position value so that they fit into the centre of the cell.
y_tmp[i] = round(y_tmp[i], round_dp) # e.g. 0.13 -> 0.1
x_tmp[i] = int(x_tmp[i]*res)
y_tmp[i] = int(y_tmp[i]*res)
# Position conversion from Robot into pheromone matrix (0, 0) -> (n+1, n+1) of 2n+1 matrix
x_index[i] = int(x_tmp[i] + (phero[i].num_cell-1)/2)
y_index[i] = int(y_tmp[i] + (phero[i].num_cell-1)/2)
if x_index[i] < 0 or y_index[i] < 0 or x_index[i] > phero[i].num_cell-1 or y_index[i] > phero[i].num_cell-1:
raise Exception("The pheromone matrix index is out of range.")
return x_index, y_index
def indexToPos(self, x_index, y_index):
phero = self.pheromone[0]
x = x_index - (phero.num_cell-1)/2
y = y_index - (phero.num_cell-1)/2
x = float(x) / phero.resolution
def pheroCallback(self):
# Reading from arguments
pos = self.sock_receiver.receive_array()
# twist = message.twist[-[]
# ori = pose.orientation
#print("pos0x: {}".format(pos[0].x))
phero = self.pheromone
x = [pos[0][0], pos[1][0]]
y = [pos[0][1], pos[1][1]]
print("x, y: {}, {}".format(x, y))
x_idx, y_idx = self.posToIndex(x, y)
print("x_idx, y_idx: {}, {}".format(x_idx, y_idx))
# ========================================================================= #
# Pheromone Reading #
# ========================================================================= #
'''
Pheromone Value Reading
'''
'''2 pheromone values'''
# Add two wheel position
# wheel_distance = 0.2
# pos_l = np.array([x+(cos(pi/2)*cos(self.theta)*(wheel_distance/2) - sin(pi/2)*sin(self.theta)*(wheel_distance/2)), y+(sin(pi/2)*cos(self.theta)*(wheel_distance/2) + cos(pi/2)*sin(self.theta)*(wheel_distance/2))])
# pos_r = np.array([x+(cos(pi/2)*cos(self.theta)*(wheel_distance/2) + sin(pi/2)*sin(self.theta)*(wheel_distance/2)), y+(-sin(pi/2)*cos(self.theta)*(wheel_distance/2) + cos(pi/2)*sin(self.theta)*(wheel_distance/2))])
# x_index, y_index = self.posToIndex(pos.x, pos.y)
# x_l_index, y_l_index = self.posToIndex(pos_l[0], pos_l[1])
# x_r_index, y_r_index = self.posToIndex(pos_r[0], pos_r[1])
# # Assign pheromone values from two positions and publish it
# phero_val = Float32MultiArray()
# phero_val.data = [phero.getPhero(x_l_index, y_l_index), phero.getPhero(x_r_index, y_r_index)]
# self.pub_phero.publish(phero_val)
'''9 pheromone values'''
# Position of 9 cells surrounding the robot
# x_index, y_index = self.posToIndex(x, y)
# phero_val = Float32MultiArray()
# #phero_arr = np.array( )
# for i in range(3):
# for j in range(3):
# phero_val.data.append(self.pheromone[0].getPhero(x_index+i-1, y_index+j-1)) # TODO: Randomly select the cell if the values are equal
#print("phero_avg: {}".format(np.average(np.asarray(phero_val.data))))
# self.pub_phero.publish(phero_val)
# # Assign pheromone value and publish it
# phero_val = phero.getPhero(x_index, y_index)
# self.pub_phero.publish(phero_val)
''' Read Pheromone from each pheromone grid '''
# 9 pheromone value for two robots read from the other's pheromone grid.
phero_val = [None] * self.num_robots
#phero_arr = np.array( )
for n in range(self.num_robots):
phero_val[n] = list()
for i in range(3):
for j in range(3):
phero_val[n].append(self.pheromone[1-n].getPhero(x_idx[n]+i-1, y_idx[n]+j-1)) # Read the other's pheromone
self.sock_receiver.return_to_client(np.asarray(phero_val))
# ========================================================================= #
# Pheromone Injection #
# ========================================================================= #
# Pheromone injection (uncomment it when injection is needed)
## Two robots inject pheromone in different grids
if self.is_phero_inj is True:
for i in range(len(self.pheromone)):
#phero[i].injection(x_idx[i], y_idx[i], 1, 25, self.phero_max)
phero[i].gradInjection(x_idx[i], y_idx[i], 1, 0.3, 1.2, self.phero_max)
# ========================================================================= #
# Pheromone Update #
# ========================================================================= #
# Update pheromone matrix in every 0.1s
time_cur = time.clock()
if time_cur-phero[0].step_timer >= 0.1:
phero[0].update(self.phero_min, self.phero_max)
phero[0].step_timer = time_cur
#log_time_cur = time.clock()
# Logging Pheromone grid
# if log_time_cur - self.log_timer >= 2:
# self.log_file = open("phero_value.txt", "a+")
# np.savetxt(self.log_file, self.pheromone.grid, delimiter=',')
# self.log_file.close()
# ========================================================================= #
# Save Pheromone #
# ========================================================================= #
'''Saving pheromone'''
# # Save after 20s
# time_check = time.time()
# if time_check - self.startTime >= 20 and self.is_saved is False:
# self.pheromone.save("simple_collision_diffused")
# self.is_saved = True
# Save the pheromone when robot return home.
# distance_to_origin = sqrt(x**2+y**2)
# if self.is_saved is False and distance_to_origin < 0.05:
# self.pheromone.save("foraging_static")
# self.is_saved = True
# self.is_phero_inj = False
# ========================================================================= #
# Load Pheromone #
# ========================================================================= #
'''Loading Pheromone'''
# Load the pheromone
# 1. When use continuous contoller. (1) It hasn't previously loaded, (2) pheromone injection is disabled,
# (3) service is requested by continuous controller script
# if self.is_loaded is False and self.is_phero_inj is False and self.is_service_requested is True:
# try:
# self.pheromone.load("foraging")
# self.is_loaded = True
# except IOError as io:
# print("No pheromone to load: %s"%io)
# 2. When reset is requested.
if self.is_reset == True:
try:
for i in range(self.num_robots): # Reset the pheromone grid
self.pheromone[i].reset()
#self.pheromone.load("simple_collision_diffused") # you can load any types of pheromone grid
print("Pheromone grid reset!")
self.is_reset = False # Reset the flag for next use
except IOError as io:
print("No pheromone to load: %s"%io)
class Node():
def __init__(self, phero):
self.pheromone = phero
self.phero_max = 1.0
self.phero_min = 0.0
self.is_phero_inj = True
# Socket Communication
self.sock_receiver = SocketNumpyArray()
self.sock_receiver.initalize_receiver(
9998) # the 9999 is the port you can change it with your own.
self.is_service_requested = False
self.theta = 0
self.log_timer = time.clock()
self.log_file = open("phero_value.txt", "a+")
self.is_saved = False
self.is_loaded = False
self.is_reset = True # False for reset
self.pheromone.isDiffusion = True
self.pheromone.isEvaporation = False
self.startTime = time.time()
# PosToIndex for pheromone circle
# x_2, y_2 = self.posToIndex(2,2)
# x_n2, y_n2 = self.posToIndex(-2,-2)
# x_1p4, y_1p4 = self.posToIndex(1.4, 1.4)
# x_n1p4, y_n1p4 = self.posToIndex(-1.4, -1.4)
# x_0, y_0 = self.posToIndex(0,0)
# Pheromone Initilaisation
# self.pheromone.circle(x_2, y_0, 1, 0.7)
# self.pheromone.circle(x_n2, y_0, 1, 0.7)
# self.pheromone.circle(x_0, y_2, 1, 0.7)
# self.pheromone.circle(x_0, y_n2, 1, 0.7)
# self.pheromone.circle(x_1p4, y_1p4, 1, 0.5)
# self.pheromone.circle(x_1p4, y_n1p4, 1, 0.5)
# self.pheromone.circle(x_n1p4, y_n1p4, 1, 0.5)
# self.pheromone.circle(x_n1p4, y_1p4, 1, 0.5)
self.num_robots = self.sock_receiver.receive_number()
print("num_robots: {}".format(self.num_robots))
print("Initialisation completed")
def posToIndex(self, x, y):
'''
Convert 2D coordinates (x, y) into the matrix index (x_index, y_index)
'''
phero = self.pheromone
x_tmp = x
y_tmp = y
x_index = 0
y_index = 0
# Read pheromone value at the robot position
res = self.pheromone.resolution
round_dp = int(log10(res))
x_tmp = round(
x, round_dp
) # round the position value so that they fit into the centre of the cell.
y_tmp = round(y, round_dp) # e.g. 0.13 -> 0.1
x_tmp = int(x * res)
y_tmp = int(y * res)
# Position conversion from Robot into pheromone matrix (0, 0) -> (n+1, n+1) of 2n+1 matrix
x_index = int(x_tmp + (phero.num_cell - 1) / 2)
y_index = int(y_tmp + (phero.num_cell - 1) / 2)
if x_index < 0 or y_index < 0 or x_index > phero.num_cell - 1 or y_index > phero.num_cell - 1:
raise Exception("The pheromone matrix index is out of range.")
return x_index, y_index
def indexToPos(self, x_index, y_index):
'''
Convert matrix indices into 2D coordinate (x, y)
'''
x = x_index - (self.pheromone.num_cell - 1) / 2
y = y_index - (self.pheromone.num_cell - 1) / 2
x = float(x) / self.pheromone.resolution
y = float(y) / self.pheromone.resolution
return x, y
def pheroCallback(self):
# Reading from arguments
pos = self.sock_receiver.receive_array()
print("pos: {}".format(pos))
phero = self.pheromone
x = pos[0]
y = pos[1]
print("x, y: {}, {}".format(x, y))
# ========================================================================= #
# Pheromone Reading #
# ========================================================================= #
'''
Pheromone Value Reading
'''
'''2 pheromone values'''
# Add two wheel position
# wheel_distance = 0.2
# pos_l = np.array([x+(cos(pi/2)*cos(self.theta)*(wheel_distance/2) - sin(pi/2)*sin(self.theta)*(wheel_distance/2)), y+(sin(pi/2)*cos(self.theta)*(wheel_distance/2) + cos(pi/2)*sin(self.theta)*(wheel_distance/2))])
# pos_r = np.array([x+(cos(pi/2)*cos(self.theta)*(wheel_distance/2) + sin(pi/2)*sin(self.theta)*(wheel_distance/2)), y+(-sin(pi/2)*cos(self.theta)*(wheel_distance/2) + cos(pi/2)*sin(self.theta)*(wheel_distance/2))])
# x_index, y_index = self.posToIndex(pos.x, pos.y)
# x_l_index, y_l_index = self.posToIndex(pos_l[0], pos_l[1])
# x_r_index, y_r_index = self.posToIndex(pos_r[0], pos_r[1])
# # Assign pheromone values from two positions and publish it
# phero_val = Float32MultiArray()
# phero_val.data = [phero.getPhero(x_l_index, y_l_index), phero.getPhero(x_r_index, y_r_index)]
# self.pub_phero.publish(phero_val)
'''9 pheromone values'''
# Position of 9 cells surrounding the robot
x_index, y_index = self.posToIndex(x, y)
print("x_idx, y_idx: {}, {}".format(x_index, y_index))
phero_val = []
#phero_arr = np.array( )
for i in range(3):
for j in range(3):
phero_val.append(
self.pheromone.getPhero(x_index + i - 1, y_index + j - 1))
self.sock_receiver.return_to_client(np.asarray(phero_val))
# ========================================================================= #
# Pheromone Injection #
# ========================================================================= #
''' Pheromone injection (uncomment it when injection is needed) '''
# if self.is_phero_inj is True:
# phero.injection(x_index, y_index, 0.2, 5, self.phero_max)
# ========================================================================= #
# Pheromone Update #
# ========================================================================= #
''' Pheromone Update '''
# time_cur = time.clock()
# if time_cur-phero.step_timer >= 0.1:
# phero.update(self.phero_min, self.phero_max)
# phero.step_timer = time_cur
# ========================================================================= #
# Save Pheromone #
# ========================================================================= #
'''Saving pheromone'''
# # Save after 20s
# time_check = time.time()
# if time_check - self.startTime >= 20 and self.is_saved is False:
# self.pheromone.save("simple_collision_diffused3")
# self.is_saved = True
# Save the pheromone when robot return home.
# distance_to_origin = sqrt(x**2+y**2)
# if self.is_saved is False and distance_to_origin < 0.05:
# self.pheromone.save("foraging_static")
# self.is_saved = True
# self.is_phero_inj = False
# ========================================================================= #
# Load Pheromone #
# ========================================================================= #
'''Loading Pheromone'''
# Load the pheromone
# 1. When use continuous contoller. (1) It hasn't previously loaded, (2) pheromone injection is disabled,
# (3) service is requested by continuous controller script
# if self.is_loaded is False and self.is_phero_inj is False and self.is_service_requested is True:
# try:
# self.pheromone.load("foraging")
# self.is_loaded = True
# except IOError as io:
# print("No pheromone to load: %s"%io)
# 2. When reset is requested.
if self.is_reset == True:
try:
self.pheromone.load(
"simple_collision_diffused3"
) # you can load any types of pheromone grid
self.is_reset = False # Reset the flag for next use
except IOError as io:
print("No pheromone to load: %s" % io)
def injAssign(self, req):
'''
Service Function that takes whether robot injects pheromone or not.
'''
self.is_phero_inj = req.is_inj
service_ok = True
self.is_service_requested = True
return PheroInjResponse(service_ok)
def serviceReset(self, req):
'''
When request is received, pheromone grid is reset and load the prepared pheromone grid.
'''
self.is_reset = req.reset
is_reset = True
return PheroResetResponse(is_reset)
def nextGoal(self, req):
# Turn off pheromone injection
self.is_phero_inj = False
# Convert the robot position to index for the pheromone matrix
x = req.x
y = req.y
x_index, y_index = self.posToIndex(x, y)
# read the 9 nearby values and choose the cell with maximum value
max_phero = self.pheromone.getPhero(x_index, y_index)
phero_index = np.array([0, 0])
phero_value = np.zeros((3, 3))
rand_index = 0
## Get the indices that contains maximum pheromone
for i in range(3):
for j in range(3):
if self.pheromone.getPhero(
x_index + i - 1, y_index + j - 1
) > max_phero: # TODO: Randomly select the cell if the values are equal
phero_index = np.array([i - 1, j - 1])
max_phero = self.pheromone.getPhero(
x_index + i - 1, y_index + j - 1)
print("Set new max")
elif self.pheromone.getPhero(x_index + i - 1,
y_index + j - 1) == max_phero:
phero_index = np.vstack((phero_index, [i - 1, j - 1]))
phero_value[i, j] = self.pheromone.getPhero(
x_index + i - 1, y_index + j - 1)
print(
"At the point ({}, {}), the pheromone value is {}".format(
i, j,
self.pheromone.getPhero(x_index + i - 1,
y_index + j - 1)))
#print("Append phero val")
print("Phero_index: {}".format(phero_index))
print("Phero_value: {}".format(phero_value))
# Choose the index as a next goal from the array
## Check the front cells (highest priority)
# if self.theta > (7/4) * pi or self.theta < (1/4) * pi:
# for x in phero_index:
# if x is np.array([1,0]) or x is np.array([1,1]) or x is np.array([1,2]):
# np.append(final_index, x, axis=0)
rand_index = np.random.choice(phero_index.shape[0], 1)
final_index = phero_index[rand_index]
next_x_index = x_index + final_index[0, 0]
next_y_index = y_index + final_index[0, 1]
# Reconvert index values into position.
next_x, next_y = self.indexToPos(next_x_index, next_y_index)
print("Pheromone value of goal: {}".format(
self.pheromone.getPhero(next_x_index, next_y_index)))
return PheroGoalResponse(next_x, next_y)
class Scenario(BaseScenario):
'''
Experiment 3
- 4 robots navigation + obstacle avoidance
- Dynamic & Static obstacle avoidance
'''
def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9998)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
# Object initialisation
# Robot initial positions
self.robot_positions = [[-2.5,0],[2.5,0],[0,-2.5],[0,2.5]]
self.robot_angles = [0, pi, pi/2, -pi/2]
self.target_positions = [[2.5,0],[-2.5,0],[0,2.5],[0,-2.5]]
## 20201209 - add random positions if the fixed training works.
# self.target_index = 0
# self.num_experiments = 20
# self.radius = 4
def make_world(self):
world = World()
# set any world properties first
world.dim_c = 2
num_agents = 4
num_obstacle = 1
num_target = 4
world.collaborative = False
# add agents
world.agents = [Agent() for i in range(num_agents)]
for i, agent in enumerate(world.agents):
agent.name = 'agent %d' % i
agent.collide = True
agent.silent = True
agent.size = 0.1
# add obstacles
obstacles = [Landmark() for i in range(num_obstacle)]
for i, obstacle in enumerate(obstacles):
obstacle.name = 'obstacle %d' % i
obstacle.collide = True
obstacle.movable = False
obstacle.size = 0.1
# add target
targets = [Landmark() for i in range(num_target)]
for i, target in enumerate(targets):
target.name = 'target %d' % i
target.collide = False
target.movable = False
target.size = 0.1
# Merge the landmarks (obstacles + target)
world.landmarks = obstacles + targets
# make initial conditions
self.n = num_agents
self.x = [0.0, 0.0]*num_agents
self.y = [0.0, 0.0]*num_agents
self.theta = [0.0, 0.0]*num_agents
self.reset_world(world)
return world
def reset_world(self, world):
# random properties for agent
for i, agent in enumerate(world.agents):
agent.color = np.array([0.35, 0.35, 0.85])
# random properties for landmarks
for i, landmark in enumerate(world.landmarks):
if 'obstacle' in landmark.name:
landmark.color = np.array([0.25, 0.25, 0.25])
if 'target' in landmark.name:
landmark.color = np.array([0.9, 0.1, 0.1])
# ========================================================================= #
# TARGET UPDATE #
# ========================================================================= #
# set random initial states
# robot position (0, 0), distance between robots and target (4 m)
# angle_target = self.target_index*2*pi/self.num_experiments
# self.target_x = self.radius*cos(angle_target)
# self.target_y = self.radius*sin(angle_target)
# if self.target_index < self.num_experiments-1:
# self.target_index += 1
# else:
# self.target_index = 0
# ========================================================================= #
# OBJECT RESET #
# ========================================================================= #
# Agent update
# 20201209 Fixed initial positions (later, I'll apply random initial positions)
for i, agent in enumerate(world.agents):
agent.state.p_pose = np.asarray(self.robot_positions[i] + self.robot_angles[i])
agent.state.p_pos = self.robot_positions[i]
# self.target = [[self.x[1], self.y[1]], [self.x[0], self.y[0]]]
agent.state.p_vel = np.zeros(world.dim_p)
agent.state.c = np.zeros(world.dim_c)
agent.state.target = self.target_positions[i] # 20201201 target
# for i, landmark in enumerate(world.landmarks):
# if landmark.name[0] == 'o':
# landmark.state.p_pos = [0, 0] #np.random.uniform(-1, +1, world.dim_p)
# landmark.state.p_vel = np.zeros(world.dim_p)
# Obstacle update
for i, obstacle in enumerate([ob for ob in world.landmarks if 'obstacle' in ob]):
obstacle.state.p_pos = np.asarray([0,0])
obstacle.state.p_vel = np.zeros(world.dim_p)
# Target update
for i, target in enumerate([tg for tg in world.landmarks if 'target' in tg]):
target.state.p_pos = np.asarray(self.target_positions[i])
target.state.p_vel = np.zeros(world.dim_p)
def benchmark_data(self, agent, world):
rew = 0
collisions = 0
occupied_landmarks = 0
min_dists = 0
for l in world.landmarks:
dists = [np.sqrt(np.sum(np.square(a.state.p_pos - l.state.p_pos))) for a in world.agents]
min_dists += min(dists)
rew -= min(dists)
if min(dists) < 0.1:
occupied_landmarks += 1
if agent.collide:
for a in world.agents:
if self.is_collision(a, agent):
rew -= 1
collisions += 1
return (rew, collisions, min_dists, occupied_landmarks)
def is_collision(self, agent1, agent2):
delta_pos = agent1.state.p_pos - agent2.state.p_pos
dist = np.sqrt(np.sum(np.square(delta_pos)))
dist_min = agent1.size + agent2.size
return True if dist < dist_min else False
def reward(self, agent, world):
'''
Compute rewards
'''
distance_reward = 0.0
phero_reward = 0.0
goal_reward = 0.0
collision_reward = 0.0
#angular_punish_rewards = [0.0]*self.num_robots
#linear_punish_rewards = [0.0]*self.num_robots
# 1. Distance Reward
goal_progress = agent.state.distance_to_goal_prev - agent.state.distance_to_goal
if abs(goal_progress) < 0.1:
print("Goal Progress: {}".format(goal_progress))
if goal_progress >= 0:
distance_reward = goal_progress * 1.2
else:
distance_reward = goal_progress
else:
distance_reward = 0.0
# 2. Phero Reward
# phero_sum = np.sum(self.phero)
# phero_reward = -phero_sum*2
# 3. Goal Reward
if agent.state.distance_to_goal <= 0.3:
goal_reward = 50.0
#done = True
#self.reset(model_state, id_bots=idx[i])
# 4. Collision Penalty
for i, obstacle in enumerate([ob for ob in world.landmarks if 'obstacle' in ob]):
is_collision = self.is_collision(agent, obstacle)
if is_collision == True:
collision_reward = -50.0
reward = distance_reward*(5/world.dt)+phero_reward+goal_reward+collision_reward
print("distance reward: {}".format(distance_reward))
print("collision_reward: {}".format(collision_reward))
print("goal_reward: {}".format(goal_reward))
return reward
def observation(self, agent, world):
id = int(agent.name[-1])
# get positions of all entities in this agent's reference frame
entity_pos = []
for entity in world.landmarks: # world.entities:
entity_pos.append(entity.state.p_pos - agent.state.p_pos)
# entity colors
entity_color = []
for entity in world.landmarks: # world.entities:
entity_color.append(entity.color)
positions = np.asarray(agent.state.p_pos)
self.sock_sender.send_numpy_array(positions)
data = self.sock_sender.receive_from_server()
self.phero = phero = np.asarray(data).reshape(1,9)
# 20201209 needs to work on merging static & dynamic pheromone values
obs = np.hstack((phero, [agent.action.twist], np.asarray([agent.state.distance_to_goal]).reshape(1,1), np.asarray([agent.state.angle_diff]).reshape(1,1)))
world.obs_n = np.shape(obs)[1]
return obs
def done(self, agent, world):
agent.state.done = False
# 1. Goal arrival
if agent.state.distance_to_goal <= 0.3:
agent.state.done = True
# 2. Out of range
if abs(agent.state.p_pos[0]) > 4.6 or abs(agent.state.p_pos[1]) > 4.6:
agent.state.done = True
print("out of range!!!!")
# 3. collision
for i, obstacle in enumerate([ob for ob in world.landmarks if 'obstacle' in ob]):
is_collision = self.is_collision(agent, obstacle)
if is_collision == True:
agent.state.done = True
done = agent.state.done
return done
def info(self, agent, world):
info = [{"targets": agent.state.target}]
return info
class Scenario(BaseScenario):
def __init__(self):
self.sock_sender = SocketNumpyArray()
self.sock_sender.initialize_sender('localhost', 9999)
self.n = None
self.x = None
self.y = None
self.theta = None
self.phero = None
def make_world(self):
world = World()
# set any world properties first
world.dim_c = 2
num_agents = 2
num_landmarks = 1
world.collaborative = True
# add agents
world.agents = [Agent() for i in range(num_agents)]
for i, agent in enumerate(world.agents):
agent.name = 'agent %d' % i
agent.collide = True
agent.silent = True
agent.size = 0.1
# add landmarks
world.landmarks = [Landmark() for i in range(num_landmarks)]
for i, landmark in enumerate(world.landmarks):
landmark.name = 'landmark %d' % i
landmark.collide = True
landmark.movable = False
landmark.size = 0.06
# make initial conditions
self.n = num_agents
self.x = [0.0, 0.0] * num_agents
self.y = [0.0, 0.0] * num_agents
self.theta = [0.0, 0.0] * num_agents
self.reset_world(world)
return world
def reset_world(self, world):
# random properties for agents
for i, agent in enumerate(world.agents):
agent.color = np.array([0.35, 0.35, 0.85])
# random properties for landmarks
for i, landmark in enumerate(world.landmarks):
landmark.color = np.array([0.25, 0.25, 0.25])
# set random initial states
d = 3
angle = pi * np.random.uniform(-1, +1)
self.x[0] = (d / 2) * cos(angle)
self.y[0] = (d / 2) * sin(angle)
self.x[1] = (d / 2) * cos(angle + pi)
self.y[1] = (d / 2) * sin(angle + pi)
self.theta[0] = angle + pi
self.theta[1] = angle
world.agents[0].state.p_pose = np.asarray(
[self.x[0], self.y[0], self.theta[0]])
world.agents[1].state.p_pose = np.asarray(
[self.x[1], self.y[1], self.theta[1]])
world.agents[0].state.p_pos = world.agents[0].state.p_pose[:2]
world.agents[1].state.p_pos = world.agents[1].state.p_pose[:2]
self.target = [[self.x[1], self.y[1]], [self.x[0],
self.y[0]]] #20201130 Target
for i, agent in enumerate(world.agents):
#agent.state.p_pos = np.random.uniform(-1, +1, world.dim_p)
agent.state.p_vel = np.zeros(world.dim_p)
agent.state.c = np.zeros(world.dim_c)
agent.state.target = self.target[i] # 20201201 target
for i, landmark in enumerate(world.landmarks):
landmark.state.p_pos = [0,
0] #np.random.uniform(-1, +1, world.dim_p)
landmark.state.p_vel = np.zeros(world.dim_p)
def benchmark_data(self, agent, world):
rew = 0
collisions = 0
occupied_landmarks = 0
min_dists = 0
for l in world.landmarks:
dists = [
np.sqrt(np.sum(np.square(a.state.p_pos - l.state.p_pos)))
for a in world.agents
]
min_dists += min(dists)
rew -= min(dists)
if min(dists) < 0.1:
occupied_landmarks += 1
if agent.collide:
for a in world.agents:
if self.is_collision(a, agent):
rew -= 1
collisions += 1
return (rew, collisions, min_dists, occupied_landmarks)
def is_collision(self, agent1, agent2):
delta_pos = agent1.state.p_pos - agent2.state.p_pos
dist = np.sqrt(np.sum(np.square(delta_pos)))
dist_min = agent1.size + agent2.size
return True if dist < dist_min else False
def reward(self, agent, world):
# Agents are rewarded based on minimum agent distance to each landmark, penalized for collisions
rew = 0
for l in world.landmarks:
dists = [
np.sqrt(np.sum(np.square(a.state.p_pos - l.state.p_pos)))
for a in world.agents
]
rew -= min(dists)
if agent.collide:
for a in world.agents:
if self.is_collision(a, agent):
rew -= 1
distance_reward = 0.0
phero_reward = 0.0
goal_reward = 0.0
#angular_punish_rewards = [0.0]*self.num_robots
#linear_punish_rewards = [0.0]*self.num_robots
# 1. Distance Reward
goal_progress = agent.state.distance_to_goal_prev - agent.state.distance_to_goal
if abs(goal_progress) < 0.1:
print("Goal Progress: {}".format(goal_progress))
if goal_progress >= 0:
distance_reward = goal_progress * 1.2
else:
distance_reward = goal_progress
else:
distance_reward = 0.0
# 2. Phero Reward
phero_sum = np.sum(self.phero)
phero_reward = -phero_sum * 2
# 3. Goal Reward
if agent.state.distance_to_goal <= 0.3:
goal_reward = 50.0
#done = True
#self.reset(model_state, id_bots=idx[i])
reward = distance_reward * (5 / world.dt) + phero_reward + goal_reward
return reward
def observation(self, agent, world):
id = int(agent.name[-1])
# get positions of all entities in this agent's reference frame
entity_pos = []
for entity in world.landmarks: # world.entities:
entity_pos.append(entity.state.p_pos - agent.state.p_pos)
# entity colors
entity_color = []
for entity in world.landmarks: # world.entities:
entity_color.append(entity.color)
# communication of all other agents
comm = []
other_pos = []
for other in world.agents:
if other is agent: continue
comm.append(other.state.c)
other_pos.append(other.state.p_pos - agent.state.p_pos)
positions = np.asarray([agent.state.p_pos for agent in world.agents])
#print(positions)
self.sock_sender.send_numpy_array(positions)
data = self.sock_sender.receive_from_server()
#print("Data received from receiver: {}, size: {}".format(data, data.shape))
#print("twist: {}".format(agent.action.twist))
self.phero = phero = data[id].reshape(1, 9)
#print("distances: {}, {}".format(agent.distance_to_goal_prev, agent.distance_to_goal))
# Distance to Goal (state)
#self.distance_to_goal =
# Difference of local angle and global angle to head to the goal
#self.angle_diff
#print("shape 0: {}, shape 1: {}".format(phero.shape, agent.state.p_vel))
obs = np.hstack(
(phero, [agent.action.twist],
np.asarray([agent.state.distance_to_goal]).reshape(1, 1),
np.asarray([agent.state.angle_diff]).reshape(1, 1)))
world.obs_n = np.shape(obs)[1]
print("obs size: {}".format(np.shape(obs)))
return obs # 20201201 observation is changed (pheromone + velocity, distance, anglediff ) 1*13
def done(self, agent, world):
agent.state.done = False
# 1. Goal arrival
if agent.state.distance_to_goal <= 0.3:
agent.state.done = True
# 2. Out of range
print("pos[0]: {}, pos[1]: {}".format(agent.state.p_pos[0],
agent.state.p_pos[1]))
print("abspos[0]: {}, abspos[1]: {}".format(abs(agent.state.p_pos[0]),
abs(agent.state.p_pos[1])))
if abs(agent.state.p_pos[0]) > 4.8 or abs(agent.state.p_pos[1]) > 4.8:
agent.state.done = True
print("out of range!!!!")
# 3. collision
agents = [agent for agent in world.agents]
is_collision = self.is_collision(agents[0], agents[1])
if is_collision == True:
agent.state.done = True
done = agent.state.done
return done
def info(self, agent, world):
info = [{"targets": agent.state.target}]
return info
import numpy as np
from npsocket_sn import SocketNumpyArray
sock_receiver = SocketNumpyArray()
sock_receiver.initalize_receiver(9999) # the 9999 is the port you can change it with your own.
while True:
positions = sock_receiver.receive_array() # Receiving the image as numpy array.
# Display
print("Received positions: {}, Size: {}".format(positions, np.shape(positions)))
positions_re = positions.reshape(4,1)
sock_receiver.return_to_client(positions_re)