def benchmark_environment(
targets,
hazards,
model,
*,
startpos=Coordinate(cfg.SCREEN_X * 0.1, cfg.SCREEN_Y * 0.8, 0),
bit_data=[random.uniform(np.pi / 2, np.pi), 0.0, 0.0],
num_runs=1,
mode="display"):
# Verify mode
if mode != "display" and mode != "path":
print("Invalid mode selected!")
os._exit(0)
env = gym.make('drill-v0', startLocation=startpos, activate_hazards=True)
env.load_predefined_env(targets, hazards)
obs = np.array(env.get_state())
done = False
while not done:
action, states = model.predict(obs)
obs, rewards, done, info = env.step(action)
if mode == "display":
env.display_environment()
else:
path = env.get_path()
env.close()
def _create_unique_random_target(start_pos,x_bound,y_bound, z_bound,r_bound,existing_targets):
target_center = Coordinate(np.random.uniform(x_bound[0],x_bound[1]),(np.random.uniform(y_bound[0],y_bound[1])),(np.random.uniform(z_bound[0],z_bound[1])))
target_radius = np.random.uniform(r_bound[0],r_bound[1])
target_candidate = TargetBall(target_center.x,target_center.y,target_center.z,target_radius)
for target in existing_targets:
if _is_overlapping(target,target_candidate) or _is_within(start_pos,target_center,target_radius):
target_candidate =_create_unique_random_target(start_pos,x_bound,y_bound,z_bound,r_bound,existing_targets)
break
return target_candidate
def _findNearest(start_location,candidates):
current_shortest_distance = -1 # Init with an impossible distance
current_closest_target_index = 0
for candidate_index in range(len(candidates)):
candidate = candidates[candidate_index]
distance = Coordinate.getEuclideanDistance(candidate.center,start_location)
distance -= candidate.radius
if distance < current_shortest_distance or current_shortest_distance == -1:
current_shortest_distance = distance
current_closest_target_index = candidate_index
return current_closest_target_index
def _create_unique_random_hazard(start_pos, x_bound, y_bound, z_bound, r_bound,
existing_obstacles):
hazard_center = Coordinate(np.random.uniform(x_bound[0], x_bound[1]),
(np.random.uniform(y_bound[0], y_bound[1])),
(np.random.uniform(z_bound[0], z_bound[1])))
hazard_radius = np.random.uniform(r_bound[0], r_bound[1])
hazard_candidate = Hazard(hazard_center.x, hazard_center.y,
hazard_center.z, hazard_radius)
for obstacle in existing_obstacles:
if _is_overlapping(obstacle, hazard_candidate) or _is_within(
start_pos, hazard_center, hazard_radius):
hazard_candidate = _create_unique_random_hazard(
start_pos, x_bound, y_bound, z_bound, r_bound,
existing_obstacles)
break
return hazard_candidate
from gym.envs.registration import register
from gym_drill.envs.Coordinate import Coordinate
import gym_drill.envs.environment_config as cfg
import numpy as np
register(
id='drill-v0',
entry_point='gym_drill.envs:DrillEnv',
# Default values
kwargs={
# Starting at the top, center of the screen
"startLocation" : Coordinate(0.5*cfg.SCREEN_X,0.5*cfg.SCREEN_Y,0),
# Random Azimuth and Inclination angle. Angular velocity and acceleration of both are set to zero
"bitInitialization" : [np.random.uniform(0,2*np.pi),np.random.uniform(0,np.pi/4), 0.0, 0.0, 0.0, 0.0]
}
)
import gym
import gym_drill
import random
import numpy as np
from gym_drill.envs.Coordinate import Coordinate
STARTLOCATION = Coordinate(1000, 1000, 1000)
BIT_INITIALIZATION = [3.8 * np.pi / 4, 0.0, 0.0]
env_name = 'drill-v0'
env = gym.make(env_name,
startLocation=STARTLOCATION,
bitInitialization=BIT_INITIALIZATION,
activate_hazards=True)
print("Obs space", env.observation_space)
print("action space", env.action_space)
class Agent():
def __init__(self, env):
self.action_size = env.action_space.n
print("Action size", self.action_size)
def get_action(self):
action = random.choice(range(self.action_size))
return action
agent = Agent(env)
def _create_unique_random_hazard(start_pos,x_bound,y_bound,z_bound,r_bound,existing_obstacles):
hazard_center = Coordinate(np.random.uniform(x_bound[0],x_bound[1]),(np.random.uniform(y_bound[0],y_bound[1])),(np.random.uniform(z_bound[0],z_bound[1] )))
hazard_radius = np.random.uniform(r_bound[0],r_bound[1])
hazard_candidate = Hazard(hazard_center.x,hazard_center.y,hazard_center.z,hazard_radius)
for obstacle in existing_obstacles:
def _is_overlapping(t1,t2):
total_radii = t1.radius + t2.radius
distance = Coordinate.getEuclideanDistance(t1.center,t2.center)
return distance < total_radii
return angle_between_vectors
def angle_between_vectors(v1, v2):
# Returns the angle in radians between vectors 'v1' and 'v2'
v1_u = v1 / np.linalg.norm(v1)
v2_u = v2 / np.linalg.norm(v2)
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
if __name__ == '__main__':
from gym_drill.envs import environment_config as cfg
#targets, hazards = _read_env_from_file("environments.txt",1)
#for t in targets:
# print(t)
#for h in hazards:
# print(h)
targets = _init_targets(cfg.NUM_TARGETS,cfg.TARGET_BOUND_X,cfg.TARGET_BOUND_Y,cfg.TARGET_BOUND_Z,cfg.TARGET_RADII_BOUND,Coordinate(100,100,900))
for t in targets:
print(t)
=======
return (bitPosition.x - targetPosition.x)**2 + (bitPosition.y - targetPosition.y)**2 < targetRadius**2
if __name__ == '__main__':
targets,hazards = read_env_from_file("environments.txt",2)
for t in targets:
print(t)
for h in hazards:
print(h)
>>>>>>> 2914a7ab17b685790fbbcc82ad796d416a700bb1:2D-version/gym-drill/gym_drill/envs/environment_support.py
def __init__(self,x,y,z,rad):
self.center = Coordinate(x,y,z)
self.radius = rad
self.penalty = HAZARD_PENALTY
self.is_hit = False
def __init__(self, x, y, z, rad):
self.center = Coordinate(x, y, z)
self.radius = rad
self.reached = False # Indicate that the target has been reached.
self.reward = 100
TARGET_BOUND_X = [0.5*SCREEN_X,0.9*SCREEN_X]
TARGET_BOUND_Y = [0.1*SCREEN_Y,0.6*SCREEN_Y]
TARGET_BOUND_Z = [0.5*SCREEN_Z,0.9*SCREEN_Z]
TARGET_RADII_BOUND = [20,50]
TARGET_BOUNDS = [TARGET_BOUND_X,TARGET_BOUND_Y,TARGET_BOUND_Z,TARGET_RADII_BOUND]
HAZARD_BOUND_X = [0,SCREEN_X]
HAZARD_BOUND_Y = [0,SCREEN_Y]
HAZARD_BOUND_Z = [0,SCREEN_Z]
HAZARD_RADII_BOUND = [20,50]
HAZARD_BOUNDS = [HAZARD_BOUND_X,HAZARD_BOUND_Y,HAZARD_BOUND_Z,HAZARD_RADII_BOUND]
targets = []
for _ in range(4):
target_center = Coordinate(np.random.uniform(TARGET_BOUND_X[0],TARGET_BOUND_X[1]),np.random.uniform(TARGET_BOUND_Y[0],TARGET_BOUND_Y[1]),np.random.uniform(TARGET_BOUND_Z[0],TARGET_BOUND_Z[1]))
target_radius = np.random.uniform(TARGET_RADII_BOUND[0],TARGET_RADII_BOUND[1])
target_candidate = TargetBall(target_center.x,target_center.y,target_center.z,target_radius)
targets.append(target_candidate)
<<<<<<< HEAD:gym-drill/gym_drill/envs/ObservationSpace.py
hazards = []
=======
hazards = []
# Additional data
DIAGONAL = np.sqrt(SCREEN_X**2 + SCREEN_Y**2)
TARGET_DISTANCE_BOUND = [0,DIAGONAL]
RELATIVE_ANGLE_BOUND = [-np.pi,np.pi]
EXTRA_DATA_BOUNDS = [TARGET_DISTANCE_BOUND,RELATIVE_ANGLE_BOUND] # [Distance, angle between current direction and target direction]
TARGET_BOUNDS = [
TARGET_BOUND_X, TARGET_BOUND_Y, TARGET_BOUND_Z, TARGET_RADII_BOUND
]
HAZARD_BOUND_X = [0, SCREEN_X]
HAZARD_BOUND_Y = [0, SCREEN_Y]
HAZARD_BOUND_Z = [0, SCREEN_Z]
HAZARD_RADII_BOUND = [20, 50]
HAZARD_BOUNDS = [
HAZARD_BOUND_X, HAZARD_BOUND_Y, HAZARD_BOUND_Z, HAZARD_RADII_BOUND
]
targets = []
for _ in range(4):
target_center = Coordinate(
np.random.uniform(TARGET_BOUND_X[0], TARGET_BOUND_X[1]),
np.random.uniform(TARGET_BOUND_Y[0], TARGET_BOUND_Y[1]),
np.random.uniform(TARGET_BOUND_Z[0], TARGET_BOUND_Z[1]))
target_radius = np.random.uniform(TARGET_RADII_BOUND[0],
TARGET_RADII_BOUND[1])
target_candidate = TargetBall(target_center.x, target_center.y,
target_center.z, target_radius)
targets.append(target_candidate)
hazards = []
for _ in range(4):
hazard_center = Coordinate(
np.random.uniform(HAZARD_BOUND_X[0], HAZARD_BOUND_X[1]),
np.random.uniform(HAZARD_BOUND_Y[0], HAZARD_BOUND_Y[1]),
np.random.uniform(HAZARD_BOUND_Z[0], HAZARD_BOUND_Z[1]))
hazard_radius = np.random.uniform(HAZARD_RADII_BOUND[0],
HAZARD_RADII_BOUND[1])
object_hor_pos_vector = np.array([obj.center.x, obj.center.y])
curr_drill_hor_pos_vector = np.array([bitPosition.x, bitPosition.y])
appr_vec = object_hor_pos_vector - curr_drill_hor_pos_vector
head_vec = np.array([np.cos(azimuth_head), np.sin(azimuth_head)])
angle_between_vectors = np.math.atan2(np.linalg.det([appr_vec, head_vec]),
np.dot(appr_vec, head_vec))
return angle_between_vectors
def angle_between_vectors(v1, v2):
# Returns the angle in radians between vectors 'v1' and 'v2'
v1_u = v1 / np.linalg.norm(v1)
v2_u = v2 / np.linalg.norm(v2)
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
if __name__ == '__main__':
from gym_drill.envs import environment_config as cfg
#targets, hazards = _read_env_from_file("environments.txt",1)
#for t in targets:
# print(t)
#for h in hazards:
# print(h)
targets = _init_targets(cfg.NUM_TARGETS, cfg.TARGET_BOUND_X,
cfg.TARGET_BOUND_Y, cfg.TARGET_BOUND_Z,
cfg.TARGET_RADII_BOUND, Coordinate(100, 100, 900))
for t in targets:
print(t)
def __init__(self,x,y,z,rad):
self.center = Coordinate(x,y,z)
self.radius = rad