def get_repulsion_force(self, position):
"""
Repulsion force calculation function
Parameters
----------
position : Position
Position of cell to check force at
Returns
-------
double
The value of repulsion at cell
"""
# Implementing repulsion equation
position_list_cal = [
self.position,
Position(self.position.x + 20, self.position.y),
Position(self.position.x, self.position.y + 20),
Position(self.position.x + 20, self.position.y + 20)
]
dist_value = 0
for Ever_position in position_list_cal:
dist_value += (
1 / (self._sigma * math.sqrt(2 * math.pi))
) * math.exp(-(
Position.calculate_distance_squared(Ever_position, position) /
(2 * self._sigma * self._sigma)))
return dist_value
def get_force(self, position):
dist_value = -1.1*(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(position,self.position)/(2*self._sigma*self._sigma)))
'''
if self.position.calculate_distance(position) > 300:
dist_value = 1.02*dist_value
elif self.position.calculate_distance(position) >= 200:
dist_value = 1.009*dist_value
elif self.position.calculate_distance(position) < 20:
#dist_value = 0.7*dist_value
dist_value = -1*dist_value
else :
dist_value = 1.08*dist_value
'''
if self.position.calculate_distance(position) < 80:
dist_value = -1*dist_value
print(self.position.calculate_distance(position))
#print(dist_value)
return dist_value
def move_speed(self,image,direction,speed):
angle_increment = (2*math.pi)/4 # 2pi/n
angle = -angle_increment # Going one step negative to start from zero
angle = angle + direction*angle_increment
print(angle)
toPosition = Position(40 *speed* math.cos(angle) + self.position.x,40*speed* math.sin(angle) + self.position.y)
self.move(image,toPosition)
def get_repulsion_force_pre(self, position):
print('position', self.position.x)
print('lastposition ', self.lastposition.x)
predict_position = self.position + self.position - self.lastposition
print('predictposition ', predict_position.x)
print('------------------------------------')
position_list_cal = [
predict_position,
Position(predict_position.x + 20, predict_position.y),
Position(predict_position.x, predict_position.y + 20),
Position(predict_position.x + 20, predict_position.y + 20)
]
dist_value = 0
for Ever_position in position_list_cal:
dist_value += (
1 / (self._sigma * math.sqrt(2 * math.pi))
) * math.exp(-(
Position.calculate_distance_squared(Ever_position, position) /
(2 * self._sigma * self._sigma)))
return dist_value
def randMove(self,image):
p = random.randint(0,3)
angle_increment = (2*math.pi)/4 # 2pi/n
angle = -angle_increment # Going one step negative to start from zero
possible_moves_list = []
for _ in range(4):
# Starting from angle 0
angle += angle_increment
possible_moves_list.append(Position(40 * math.cos(angle) + self.position.x,40* math.sin(angle) + self.position.y))
#possible_moves_list.append(Position(self.position.x))
toPosition = possible_moves_list[p]
while abs(toPosition.x- self.originPosition.x) > 80 or abs(toPosition.y- self.originPosition.y) > 80:
p = random.randint(0,3)
toPosition = possible_moves_list[p]
print(toPosition.x,toPosition.y)
self.move(image,toPosition)
def get_attraction_force(self, position):
"""
Attraction force calculation function
Parameters
----------
position : Position
Position of cell to check force at
Returns
-------
double
The value of attraction at cell
"""
# Implementing attraction equation
dist_value = -(1/(self._sigma*math.sqrt(2*math.pi))) * math.exp(-(Position.calculate_distance_squared(self.position,position)/(2*self._sigma*self._sigma)))
return dist_value
def get_possible_moves(self):
"""
Makes a list of points around agent
Returns
-------
list
List of points around agent
"""
angle_increment = (2*math.pi)/self._possible_moves # 2pi/n
angle = -angle_increment # Going one step negative to start from zero
possible_moves_list = []
for _ in range(self._possible_moves):
# Starting from angle 0
angle += angle_increment
possible_moves_list.append(Position(self._scan_radius * math.cos(angle) + self.position.x,self._scan_radius * math.sin(angle) + self.position.y))
#possible_moves_list.append(Position(self.position.x))
return possible_moves_list
# Main
if __name__ == '__main__':
# Defining world dimensions
xlim = 500
ylim = 500
world_size = (xlim, ylim)
# Initializing blank canvas(OpenCV) with white color
image = np.ones((world_size[1], world_size[0], 3), dtype=np.uint8) * 255
# Defining agent and goal
aPosx = 200
aPosy = 50
agent = Agent(Position(aPosx, aPosy), scan_radius=10, possible_moves=30)
Matlab_agent_x = aPosx
Matlab_agent_y = ylim - aPosy
goal = Goal(Position(250, 450),
sigma=math.sqrt(world_size[0]**2 + world_size[1]**2))
# Defining obstacles in a list
sigma_obstacles = 5
obstacles = [
Obstacle(Position(150, 180),
sigma=sigma_obstacles,
draw_radius=4 * sigma_obstacles),
Obstacle(Position(150, 280),
sigma=sigma_obstacles,
draw_radius=4 * sigma_obstacles),
Obstacle(Position(150, 380),