def multi_agent_example():
"""A basic example of using multiple agents"""
env = holodeck.make("UrbanCity")
cmd0 = np.array([0, 0, -2, 10])
cmd1 = np.array([0, 0, 5, 10])
for i in range(10):
env.reset()
# This will queue up a new agent to spawn into the environment, given that the coordinates are not blocked.
sensors = [
Sensors.PIXEL_CAMERA, Sensors.LOCATION_SENSOR,
Sensors.VELOCITY_SENSOR
]
agent = AgentDefinition("uav1", agents.UavAgent, sensors)
env.spawn_agent(agent, [1, 1, 5])
env.set_control_scheme("uav0",
ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
env.set_control_scheme("uav1",
ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
env.tick(
) # Tick the environment once so the second agent spawns before we try to interact with it.
env.act("uav0", cmd0)
env.act("uav1", cmd1)
for _ in range(1000):
states = env.tick()
uav0_terminal = states["uav0"][Sensors.TERMINAL]
uav1_reward = states["uav1"][Sensors.REWARD]
def __init__(self, res=(128, 128), prev_frames=4):
super(MazeWorldEnvironment, self).__init__()
self._env = holodeck.make('MazeWorld',
window_res=(256, 256),
cam_res=res)
self._res = res
self._prev_frames = prev_frames
self._imgs = np.zeros((res[0], res[1], prev_frames), dtype=np.float32)
def test_default_worlds():
uav_worlds = [
'EuropeanForest', 'RedwoodForest', 'UrbanCity', 'CyberPunkCity',
'InfiniteForest'
]
sphere_worlds = ['MazeWorld']
android_worlds = ['AndroidPlayground']
test_time = 200
for world in uav_worlds:
env = holodeck.make(world)
env.set_control_scheme("uav0",
ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
print("Testing World Commands")
world_command_test(env, "uav0", [0, 0, 1, 10], test_time)
print("Testing camera")
camera_test(env, "uav0", [0, 0, 1, 10], test_time)
for world in sphere_worlds:
env = holodeck.make(world)
print("Testing World Commands")
world_command_test(env, "sphere0", 0, test_time)
print("Testing camera")
camera_test(env, "sphere0", 0, test_time)
for world in android_worlds:
env = holodeck.make(world)
print("Testing World Commands")
world_command_test(env, "android0", np.ones(94) * 10, test_time)
print("Testing camera")
camera_test(env, "android0", np.ones(94) * 10, test_time)
def multi_agent_example():
"""A basic example of using multiple agents"""
env = holodeck.make("CyberPunkCity-FollowSight")
cmd0 = np.array([0, 0, -2, 10])
cmd1 = np.array([0, 0, 0])
for i in range(10):
env.reset()
env.tick()
env.act("uav0", cmd0)
env.act("nav0", cmd1)
for _ in range(1000):
states = env.tick()
pixels = states["uav0"]["RGBCamera"]
def sphere_example():
"""A basic example of how to use the sphere agent."""
env = holodeck.make("MazeWorld-FinishMazeSphere")
# This command is to constantly rotate to the right
command = 2
for i in range(10):
env.reset()
for _ in range(1000):
state, reward, terminal, _ = env.step(command)
# To access specific sensor data:
pixels = state["RGBCamera"]
orientation = state["OrientationSensor"]
def sphere_example():
"""A basic example of how to use the sphere agent."""
env = holodeck.make("MazeWorld")
# This command is to constantly rotate to the right
command = 2
for i in range(10):
env.reset()
for _ in range(1000):
state, reward, terminal, _ = env.step(command)
# To access specific sensor data:
pixels = state[Sensors.PIXEL_CAMERA]
orientation = state[Sensors.ORIENTATION_SENSOR]
def test_load_scenario(scenario):
"""Tests that every scenario can be loaded without any errors
TODO: We need some way of communicating with the engine to verify that the expected level was loaded.
If the level isn't found, then Unreal just picks a default one, so we're missing that case
Args:
scenario (str): Scenario to test
"""
env = holodeck.make(scenario, show_viewport=False)
for _ in range(30):
env.tick()
env.__on_exit__()
def env_scenario(request):
"""Gets an environment for the scenario matching request.param. Creates the env
or uses a cached one. Calls .reset() for you
"""
global envs
scenario = request.param
if scenario in envs:
env = envs[scenario]
env.reset()
return env, scenario
env = holodeck.make(scenario, show_viewport=False)
env.reset()
envs[scenario] = env
return env, scenario
def android_example():
"""A basic example of how to use the android agent."""
env = holodeck.make("AndroidPlayground-MaxDistance")
# The Android's command is a 94 length vector representing torques to be applied at each of his joints
command = np.ones(94) * 10
for i in range(10):
env.reset()
for j in range(1000):
if j % 50 == 0:
command *= -1
state, reward, terminal, _ = env.step(command)
# To access specific sensor data:
pixels = state["RGBCamera"]
orientation = state["OrientationSensor"]
def generate_mazeworld_walkthrough():
"""Runs through Mazeworld and records state at every tic, so that tests can analyze the results
without having to run through the maze multiple times
Returns: list of 4tuples, output from step()
"""
def on_step(state_reward_terminal_):
on_step.states.append(state_reward_terminal_)
on_step.states = list()
env = holodeck.make("MazeWorld-FinishMazeSphere", show_viewport=False)
finish.navigate(env, on_step)
env.__on_exit__()
return on_step.states
def uav_example():
"""A basic example of how to use the UAV agent."""
env = holodeck.make("UrbanCity-MaxDistance")
# This line can be used to change the control scheme for an agent
# env.agents["uav0"].set_control_scheme(ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
for i in range(10):
env.reset()
# This command tells the UAV to not roll or pitch, but to constantly yaw left at 10m altitude.
command = np.array([0, 0, 2, 1000])
for _ in range(1000):
state, reward, terminal, _ = env.step(command)
# To access specific sensor data:
pixels = state["RGBCamera"]
velocity = state["VelocitySensor"]
def world_command_examples():
"""A few examples to showcase commands for manipulating the worlds."""
env = holodeck.make("MazeWorld")
# This is the unaltered MazeWorld
for _ in range(300):
_ = env.tick()
env.reset()
# The set_day_time_command sets the hour between 0 and 23 (military time). This example sets it to 6 AM.
env.set_day_time(6)
for _ in range(300):
_ = env.tick()
env.reset() # reset() undoes all alterations to the world
# The start_day_cycle command starts rotating the sun to emulate day cycles.
# The parameter sets the day length in minutes.
env.start_day_cycle(5)
for _ in range(1500):
_ = env.tick()
env.reset()
# The set_fog_density changes the density of the fog in the world. 1 is the maximum density.
env.set_fog_density(.25)
for _ in range(300):
_ = env.tick()
env.reset()
# The set_weather_command changes the weather in the world. The two available options are "rain" and "cloudy".
# The rainfall particle system is attached to the agent, so the rain particles will only be found around each agent.
# Every world is clear by default.
env.set_weather("rain")
for _ in range(500):
_ = env.tick()
env.reset()
env.set_weather("cloudy")
for _ in range(500):
_ = env.tick()
env.reset()
env.teleport_camera([1000, 1000, 1000], [0, 0, 0])
for _ in range(500):
_ = env.tick()
env.reset()
def uav_example():
"""A basic example of how to use the UAV agent."""
env = holodeck.make("UrbanCity")
# This changes the control scheme for the uav
env.set_control_scheme("uav0", ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
for i in range(10):
env.reset()
# This command tells the UAV to not roll or pitch, but to constantly yaw left at 10m altitude.
command = np.array([0, 0, 2, 10])
for _ in range(1000):
state, reward, terminal, _ = env.step(command)
# To access specific sensor data:
pixels = state[Sensors.PIXEL_CAMERA]
velocity = state[Sensors.VELOCITY_SENSOR]
def multi_agent_example():
"""A basic example of using multiple agents"""
env = holodeck.make("CyberPunkCity-Follow")
cmd0 = np.array([0, 0, -2, 10])
cmd1 = np.array([0, 0, 0])
for i in range(10):
env.reset()
env.tick()
env.act("uav0", cmd0)
env.act("nav0", cmd1)
for _ in range(1000):
states = env.tick()
pixels = states["uav0"]["RGBCamera"]
location = states["uav0"]["LocationSensor"]
task = states["uav0"]["FollowTask"]
reward = task[0]
terminal = task[1]
def test_using_make_with_custom_config():
"""
Validate that we can use holodeck.make with a custom configuration instead
of loading it from a config file
"""
conf = {"name": "test_randomization", "agents": []}
# pick a random world from the installed packages
pkg = random.choice(list(holodeck.packagemanager._iter_packages()))
world = random.choice(pkg[0]["worlds"])["name"]
conf["world"] = world
conf["package_name"] = pkg[0]["name"]
print("world: {} package: {}".format(world, pkg[0]["name"]))
with holodeck.make(scenario_cfg=conf, show_viewport=False) as env:
for _ in range(0, 10):
env.tick()
def main():
env = holodeck.make("MazeWorld")
action_values = dict()
for episode in range(100):
# print("Episode", episode)
state, reward, terminal, _ = env.reset()
prev_state = simplify_state(state)
avg_expected = 0.0
total_reward = 0
next_threshold = THRESHOLD_DIST
for _ in tqdm(range(600)):
# for _ in range(600):
action = choose_action(prev_state, action_values)
state, reward, terminal, _ = env.step(action)
simple_state = simplify_state(state)
reward -= 1
if simple_state[0] > next_threshold:
next_threshold += THRESHOLD_DIST
reward += 1
total_reward += reward
# Update action values
key = (prev_state, action)
if key not in action_values:
action_values[key] = 10.0
expected_value = get_expected_action_value(simple_state,
action_values)
avg_expected += expected_value
action_values[key] = ((action_values[key] *
(1 - STEP_SIZE)) + STEP_SIZE *
(reward + GAMMA *
(expected_value - action_values[key])))
prev_state = simple_state
print("Episode", episode, "\tavg expected:", avg_expected / 600,
"\tTotal reward:", total_reward)
if key.char == "q":
command[3] = 1
if key.char == "r":
command[3] = -1
except AttributeError:
print('special key {0} pressed'.format(key))
# set things up to save
x = []
z = []
u = []
record = False
# This is where the magic actually uhappens
with holodeck.make("Rooms-IEKF", ticks_per_sec=ticks) as env:
# start keyboard listener
listener = keyboard.Listener(on_press=on_press, on_release=on_release)
listener.start()
#listen till we need to quit (by pressing q)
while True:
if command[3] == 1:
break
if command[3] == -1:
record = True
#send to holodeck
env.act("uav0", command)
state = env.tick()
def __init__(self, scenario_name='', scenario_cfg=None, max_steps=200,
gif_freq=500, steps_per_action=4, image_dir='images/test',
viewport=False):
# Load holodeck environment
if scenario_cfg is not None and \
scenario_cfg['package_name'] not in holodeck.installed_packages():
holodeck.install(scenario_cfg['package_name'])
self._env = holodeck.make(scenario_name=scenario_name,
scenario_cfg=scenario_cfg,
show_viewport=viewport)
# Get action space from holodeck env and store for use with rlpyt
if self.is_action_continuous:
self._action_space = FloatBox(-1, 1, self._env.action_space.shape)
else:
self._action_space = IntBox(self._env.action_space.get_low(),
self._env.action_space.get_high(),
())
# Calculate observation space with all sensor data
max_width = 0
max_height = 0
num_img = 0
num_lin = 0
for sensor in self._env._agent.sensors.values():
if 'Task' in sensor.name:
continue
shape = sensor.sensor_data.shape
if len(shape) == 3:
max_width = max(max_width, shape[0])
max_height = max(max_height, shape[1])
num_img += shape[2]
else:
num_lin += np.prod(shape)
if num_img > 0 and num_lin == 0:
self.has_img = True
self.has_lin = False
self._observation_space = FloatBox(0, 1,
(num_img, max_width, max_height))
elif num_lin > 0 and num_img == 0:
self.has_img = False
self.has_lin = True
self._observation_space = FloatBox(-256, 256, (num_lin,))
else:
self.has_img = True
self.has_lin = True
self._observation_space = Composite([
FloatBox(0, 1, (num_img, max_width, max_height)),
FloatBox(-256, 256, (num_lin,))],
HolodeckObservation)
# Set data members
self._max_steps = max_steps
self._image_dir = image_dir
self._steps_per_action = steps_per_action
self.curr_step = 0
self.gif_freq = gif_freq
self.rollout_count = -1
self.gif_images = []
def main():
if DATA:
screen_pos = [2000, 0] # x, y position on screen
data_view = data_viewer(*screen_pos) # initialize view of data plots
# Holodeck Setup
env = holodeck.make("Ocean", show_viewport=True)
env.reset()
# wave intensity: 1-13, wave size: 1-8, wave dir: 0-360 deg
env.set_ocean_state(6, 3, 90)
env.set_day_time(5)
env.set_weather('Cloudy')
env.set_aruco_code(False)
alt = -PLAN.MAV.pd0*100
if DEBUG_HOLO:
env.set_control_scheme("uav0", ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
uav_cmd = np.array([0, -0.2, 0, alt])
uav_cmd = np.array([0,0,0,0])
boat_cmd = 0
env.act("uav0", uav_cmd)
env.act("boat0", boat_cmd)
# Just for getting UAV off the platform
state = env.set_state("uav0", [-1500, 0, 6000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
state = env.set_state("uav0", [-1500, 0, 2000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
env.teleport("boat0", location=[3500, 0, 0], rotation=[0, 0, 0])
# Initialization
pos = np.array([OFFSET_N, OFFSET_E, alt])
att = np.array([0, 0, 0])
vel = np.array([0, 0, 0]) * 100
angvel = np.array([0, 0, 0])
state = env.set_state("uav0", pos, att, vel, angvel)["uav0"]
# Camera
cam_alt = 300
env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if DEBUG_HOLO:
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
for i in range(1):
state = env.tick()["uav0"]
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if SHOW_PIXELS:
pixels = state["RGBCamera"]
cv2.imshow('Camera', pixels)
cv2.waitKey(0)
print("READY FOR SIM")
# sys.exit(0)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
path_manage = path_manager()
# waypoint definition
waypoints = msg_waypoints()
waypoints.type = 'straight_line'
waypoints.type = 'fillet'
waypoints.type = 'dubins'
waypoints.num_waypoints = 4
Va = PLAN.Va0
d = 1000.0
h = -PLAN.MAV.pd0
waypoints.ned[:waypoints.num_waypoints] = np.array([[0, 0, -h],
[d, 0, -h],
[0, d, -h],
[d, d, -h]])
waypoints.airspeed[:waypoints.num_waypoints] = np.array([Va, Va, Va, Va])
waypoints.course[:waypoints.num_waypoints] = np.array([np.radians(0),
np.radians(45),
np.radians(45),
np.radians(-135)])
# initialize the simulation time
sim_time = SIM.start_time
delta = np.zeros(4)
mav.update(delta) # propagate the MAV dynamics
mav.update_sensors()
# main simulation loop
# print("Waiting for keypress")
# input()
print("Press Q to exit...")
while sim_time < SIM.end_time:
#-------observer-------------
measurements = mav.update_sensors() # get sensor measurements
# estimate states from measurements
estimated_state = obsv.update(measurements)
#-------path manager-------------
path = path_manage.update(waypoints, PLAN.R_min, estimated_state)
#-------path follower-------------
autopilot_commands = path_follow.update(path, estimated_state)
# autopilot_commands = path_follow.update(path, mav.true_state)
#-------controller-------------
delta, commanded_state = ctrl.update(autopilot_commands, estimated_state)
#-------physical system------j-------
current_wind = wind.update() # get the new wind vector
mav.update(delta, current_wind) # propagate the MAV dynamics
#-------update holodeck-------------
update_holodeck(env, mav.true_state)
draw_holodeck(env, waypoints)
#-------update viewer-------------
if DATA:
data_view.update(mav.true_state, # true states
estimated_state, # estimated states
commanded_state, # commanded states
SIM.ts_simulation)
#-------increment time-------------
sim_time += SIM.ts_simulation
def on_release(key):
if key.char == "a":
command[1] = 0
if key.char == "d":
command[1] = 0
if key.char == "w":
command[0] = 0
if key.char == "s":
command[0] = 0
if key.char == "q":
command[0] = 10000
#set up holodeck
env = holodeck.make("Rooms-DataGen")
# env = HolodeckEnvironment(scenario=config, start_world=True)
env.reset()
#get file ready
filename = sys.argv[1]
open(filename, "w").close()
output = open(filename, "a")
#put in header
num_angles = 64
angles = np.linspace(0, 360, num_angles)
output.write(f"{num_angles} {a2s(angles)}\n")
# ...or, in a non-blocking fashion:
listener = keyboard.Listener(on_press=on_press, on_release=on_release)
from tqdm import tqdm
import time
import holodeck
from holodeck import agents
from holodeck.environments import *
from holodeck import sensors
from IPython.core.debugger import Pdb
import os
import cv2
print('\nHOLODECK PATH: {}\n'.format(os.path.dirname(holodeck.__file__)))
np.set_printoptions(precision=3, suppress=True, sign=' ', floatmode='fixed')
if __name__ == '__main__':
env = holodeck.make("Ocean")
RF = ROSflightHolodeck()
env.reset()
env.tick()
# wave intensity: 1-13(int), wave size: 1-8(int), wave direction: 0-360 degreese (float)
env.set_ocean_state(3, 3, 90)
env.set_aruco_code(False)
x0 = np.array(
[
0,
0,
0, # position [0-2]
1,
0,
