def __init__(self, show_gui=False):
# set up our initial configurations
self.image_height = 480
self.image_width = 640
self.camera_height = 0.9
self.camera_x = 0.0
self.camera_y = -0.8
self.objects = {}
if show_gui:
self.simulator = Simulator(image_width=self.image_width,
image_height=self.image_height)
else:
self.simulator = Simulator(image_width=self.image_width,
image_height=self.image_height,
mode='headless',
timestep=0.001)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
pb.loadURDF('plane.urdf')
# set up renderer
self.adjust_camera([self.camera_x, self.camera_y, self.camera_height],
[0, 0, 0], [-1, 0, 0])
self.simulator.renderer.set_light_pos([0.65, 0.0, 10.0])
self.simulator.renderer.set_fov(53)
def __init__(self,
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(
mode=mode,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
image_width=self.config.get('image_width', 128),
image_height=self.config.get('image_height', 128),
vertical_fov=self.config.get('vertical_fov', 90),
device_idx=device_idx,
auto_sync=False)
self.simulator_loop = int(self.action_timestep /
self.simulator.timestep)
self.load()
def __init__(self, config_file, mode='headless', device_idx=0):
self.config = parse_config(config_file)
self.simulator = Simulator(mode=mode,
use_fisheye=self.config.get(
'fisheye', False),
resolution=self.config['resolution'],
device_idx=device_idx)
self.load()
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.calc_state()
s.step()
s.disconnect()
def run_demo(self):
config = parse_config(os.path.join(gibson2.assets_path, 'example_configs/turtlebot_demo.yaml'))
s = Simulator(mode='gui', image_width=700, image_height=700)
scene = BuildingScene('Rs_interactive', is_interactive=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for i in range(10000):
turtlebot.apply_action([0.1,0.5])
s.step()
s.disconnect()
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
config = parse_config(
os.path.join(gibson2.root_path, '../test/test_continuous.yaml'))
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.apply_action(np.array([0] * 2))
fetch.calc_state()
s.step()
s.disconnect()
def test_import_many_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
for i in range(30):
obj = YCBObject('003_cracker_box')
s.import_object(obj)
for j in range(1000):
s.step()
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 33)
def test_import_human():
s = Simulator(mode='gui')
scene = StadiumScene()
s.import_scene(scene)
obj = Pedestrian()
s.import_object(obj)
for j in range(100):
s.step()
obj.reset_position_orientation([j * 0.001, 0, 0], [0, 0, 0, 1])
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 4)
def test_import_building():
s = Simulator(mode='headless')
scene = BuildingScene('Ohoopee')
s.import_scene(scene, texture_scale=0.4)
for i in range(15):
s.step()
assert s.objects == list(range(2))
s.disconnect()
def test_jr2():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
jr2 = JR2(config)
s.import_robot(jr2)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def benchmark(render_to_tensor=False, resolution=512):
config = parse_config('../configs/turtlebot_demo.yaml')
s = Simulator(mode='headless', image_width=resolution, image_height=resolution, render_to_tensor=render_to_tensor)
scene = BuildingScene('Rs',
build_graph=True,
pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
n_frame = 500
start = time.time()
for i in range(n_frame):
turtlebot.apply_action([0.1,0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
physics_render_elapsed = time.time() - start
print("physics simulation + rendering rgb, resolution {}, render_to_tensor {}: {} fps".format(resolution,
render_to_tensor,
n_frame/physics_render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
render_elapsed = time.time() - start
print("Rendering rgb, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame/render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('3d'))
render_elapsed = time.time() - start
print("Rendering 3d, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame / render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('normal'))
render_elapsed = time.time() - start
print("Rendering normal, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame / render_elapsed))
s.disconnect()
def test_quadrotor():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
quadrotor = Quadrotor(config)
s.import_robot(quadrotor)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_humanoid():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_turtlebot():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_import_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('003_cracker_box')
s.import_object(obj)
objs = s.objects
s.disconnect()
assert objs == list(range(5))
def test_humanoid_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
humanoid.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = humanoid.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def __init__(self,
config_file,
model_id=None,
mode='headless',
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.simulator = Simulator(
mode=mode,
use_fisheye=self.config.get('fisheye', False),
resolution=self.config.get('resolution', 64),
fov=self.config.get('fov', 90),
device_idx=device_idx)
self.load()
def test_turtlebot_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = turtlebot.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def test_import_stadium():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
print(s.objects)
assert s.objects == list(range(4))
s.disconnect()
def main():
config = parse_config('../configs/turtlebot_demo.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
scene = BuildingScene('Rs',
build_graph=True,
pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for _ in range(10):
obj = YCBObject('003_cracker_box')
obj.load()
obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3), [0,0,0,1])
for i in range(10000):
with Profiler('Simulator step'):
turtlebot.apply_action([0.1,0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def test_jr2():
s = Simulator(mode='gui')
try:
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
jr2 = JR2_Kinova(config)
s.import_robot(jr2)
jr2.set_position([-6, 0, 0.1])
obj3 = InteractiveObj(os.path.join(gibson2.assets_path, 'models',
'scene_components', 'door.urdf'),
scale=2)
s.import_interactive_object(obj3)
obj3.set_position_rotation(
[-5, -1, 0], [0, 0, np.sqrt(0.5), np.sqrt(0.5)])
jr2.apply_action([0.005, 0.005, 0, 0, 0, 0, 0, 0, 0, 0])
for _ in range(400):
s.step()
jr2.apply_action([
0, 0, 0, 0, 3.1408197119196117, -1.37402907967774,
-0.8377005721485424, -1.9804208517373096, 0.09322135043256494,
2.62937740156038
])
for _ in range(400):
s.step()
finally:
s.disconnect()
def test_simulator():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('006_mustard_bottle')
for i in range(10):
s.import_object(obj)
obj = YCBObject('002_master_chef_can')
for i in range(10):
s.import_object(obj)
for i in range(1000):
s.step()
s.disconnect()
class BaseEnv(gym.Env):
'''
a basic environment, step, observation and reward not implemented
'''
def __init__(self,
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(
mode=mode,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
image_width=self.config.get('image_width', 128),
image_height=self.config.get('image_height', 128),
vertical_fov=self.config.get('vertical_fov', 90),
device_idx=device_idx,
auto_sync=False)
self.simulator_loop = int(self.action_timestep /
self.simulator.timestep)
self.load()
def reload(self, config_file):
"""
Reload another config file, this allows one to change the envrionment on the fly
:param config_file: new config file path
"""
self.config = parse_config(config_file)
self.simulator.reload()
self.load()
def reload_model(self, model_id):
"""
Reload another model, this allows one to change the envrionment on the fly
:param model_id: new model_id
"""
self.config['model_id'] = model_id
self.simulator.reload()
self.load()
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'empty':
scene = EmptyScene()
elif self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
waypoint_resolution=self.config.get('waypoint_resolution',
0.2),
num_waypoints=self.config.get('num_waypoints', 10),
build_graph=self.config.get('build_graph', False),
trav_map_resolution=self.config.get('trav_map_resolution',
0.1),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
is_interactive=self.config.get('is_interactive', False),
pybullet_load_texture=self.config.get('pybullet_load_texture',
False),
)
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
elif self.config['robot'] == 'Locobot':
robot = Locobot(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot)
def clean(self):
"""
Clean up
"""
if self.simulator is not None:
self.simulator.disconnect()
def simulator_step(self):
"""
Step the simulation, this is different from environment step where one can get observation and reward
"""
self.simulator.step()
def step(self, action):
"""
Overwritten by subclasses
"""
return NotImplementedError()
def reset(self):
"""
Overwritten by subclasses
"""
return NotImplementedError()
def set_mode(self, mode):
self.simulator.mode = mode
class BaseEnv(gym.Env):
'''
a basic environment, step, observation and reward not implemented
'''
def __init__(self,
config_file,
model_id=None,
mode='headless',
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.simulator = Simulator(
mode=mode,
use_fisheye=self.config.get('fisheye', False),
resolution=self.config.get('resolution', 64),
fov=self.config.get('fov', 90),
device_idx=device_idx)
self.load()
def reload(self, config_file):
"""
Reload another config file, this allows one to change the envrionment on the fly
:param config_file: new config file path
"""
self.config = parse_config(config_file)
self.simulator.reload()
self.load()
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
build_graph=self.config.get('build_graph', False),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
should_load_replaced_objects=self.config.get(
'should_load_replaced_objects', False))
# scene: class_id = 0
# robot: class_id = 1
# objects: class_id > 1
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True),
class_id=0)
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot, class_id=1)
def clean(self):
"""
Clean up
"""
if self.simulator is not None:
self.simulator.disconnect()
def simulator_step(self):
"""
Step the simulation, this is different from environment step where one can get observation and reward
"""
self.simulator.step()
def step(self, action):
return NotImplementedError
def reset(self):
return NotImplementedError
def set_mode(self, mode):
self.simulator.mode = mode
class BaseEnv(gym.Env):
'''
a basic environment, step, observation and reward not implemented
'''
def __init__(self, config_file, mode='headless', device_idx=0):
self.config = parse_config(config_file)
self.simulator = Simulator(mode=mode,
use_fisheye=self.config.get(
'fisheye', False),
resolution=self.config['resolution'],
device_idx=device_idx)
self.load()
def reload(self, config_file):
self.config = parse_config(config_file)
self.simulator.reload()
self.load()
def load(self):
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(self.config['model_id'])
self.simulator.import_scene(scene)
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot)
def clean(self):
if not self.simulator is None:
self.simulator.disconnect()
def simulator_step(self):
self.simulator.step()
def step(self, action):
return NotImplementedError
def reset(self):
return NotImplementedError
def set_mode(self, mode):
self.simulator.mode = mode
def rollout(traj, headless=False):
SCENE, traj_id, instr_id, instr, starting_coords = traj
# instr = "Keep going and don't stop"
# starting_coords = [0, 0, 0]
seq = tok.encode_sentence(instr)
tokens = tok.split_sentence(instr)
seq_lengths = [np.argmax(seq == padding_idx, axis=0)]
seq = torch.from_numpy(np.expand_dims(seq, 0)).cuda()
# seq_lengths[seq_lengths == 0] = seq.shape[1] # Full length
ctx, h_t, c_t, ctx_mask = encoder(seq, seq_lengths)
question = h_t
pre_feat = torch.zeros(batch_size, opts.img_feat_input_dim).cuda()
pre_ctx_attend = torch.zeros(batch_size, opts.rnn_hidden_size).cuda()
# Gibson stuff
# 72 fov for 600, 60 for 480
# mode = gui for debug, headless for run
s = Simulator(mode='gui', resolution=640, fov=75, panorama=True)
scene = BuildingScene(SCENE)
# scene = StadiumScene()
ids = s.import_scene(scene)
robot = Turtlebot(config)
ped_id = s.import_robot(robot)
heading_feat_tensor = torch.Tensor(heading_elevation_feat()).view([im_per_ob, 128]).cuda()
s.step()
robot.set_position(starting_coords)
def apply_action(bot: robot, action_idx: int, depth_ok: list, headless=False) -> bool:
print(action_idx)
# action_idx is expected to be 0-13, TODO: make nicer...
if action_idx == 0 or action_idx > 12 or not depth_ok[action_idx - 1]:
print("STOP")
return True
action_idx -= 1
#if action_idx < 3 or (action_idx < 12 and action_idx > 9):
bot.turn_right(0.5235988 * action_idx)
s.step()
if(not headless):
time.sleep(0.2)
bot.move_forward(0.5)
return False
# else:
# if action_idx < 7:
# bot.turn_left(1.57)
# else:
# bot.turn_right(1.57)
bot_is_running = True
while bot_is_running:
s.step()
gib_out = s.renderer.render_robot_cameras(modes=('rgb', '3d'))
rgb = gib_out[::2]
depth = np.array(gib_out[1::2])
processed_rgb = list(map(transform_img, rgb))
batch_obs = np.concatenate(processed_rgb)
imgnet_input = torch.Tensor(batch_obs).cuda()
imgnet_output = torch.zeros([im_per_ob, 2048]).cuda()
# depth processing and filtering
# depth: [36, ]
depth *= depth
depth = depth[:, :, :, :3].sum(axis=3)
depth = np.sqrt(depth)
# filter out 0 distances that are presumably from infinity dist
depth[depth < 0.0001] = 10
# TODO: generalize to non-horizontal moves
depth_ok = depth[12:24, 200:440, 160:480].min(axis=2).min(axis=1)
fig=plt.figure(figsize=(8, 2))
for n, i in enumerate([0, 3, 6, 9]):
fig.add_subplot(1, 4, n + 1)
plt.imshow(depth[12 + i])
plt.show()
# depth_ok *= depth_ok > 1
print(depth_ok)
depth_ok = depth_ok > 0.8
print(depth_ok)
# Each observation has 36 inputs
# We pass rgb images through frozen embedder
for i in range(im_per_ob // B_S):
def hook_fn(m, last_input, o):
imgnet_output[i*B_S:(i+1)*B_S, :] = \
o.detach().squeeze(2).squeeze(2)
imgnet_input[B_S * i : B_S * (i + 1)]
# imgnet_output[B_S * i : B_S * (i + 1)] = resnet(minibatch).detach()
imgnet_output = torch.cat([imgnet_output, heading_feat_tensor], 1)
pano_img_feat = imgnet_output.view([1, im_per_ob, 2176])
navigable_feat = torch.zeros([1, 16, 2176]).cuda()
navigable_feat[0, 1:13] = imgnet_output[12:24] * torch.Tensor(depth_ok).cuda().view(12, 1)
# TODO: make nicer as stated above
navigable_index = [list(map(int, depth_ok))]
print(navigable_index)
# NB: depth_ok replaces navigable_index
h_t, c_t, pre_ctx_attend, img_attn, ctx_attn, logit, value, navigable_mask = model(
pano_img_feat, navigable_feat, pre_feat, question, h_t, c_t, ctx,
pre_ctx_attend, navigable_index, ctx_mask)
print("ATTN")
print(ctx_attn[0])
print(img_attn[0])
plt.bar(range(len(tokens)), ctx_attn.detach().cpu()[0][:len(tokens)])
plt.xticks(range(len(tokens)), tokens)
plt.show()
plt.bar(range(16), img_attn.detach().cpu()[0])
plt.show()
print("NMASK")
print(navigable_mask)
logit.data.masked_fill_((navigable_mask == 0).data, -float('inf'))
m = torch.Tensor([[False] + list(map(lambda b: not b, navigable_index[0])) + [False, False, False]], dtype=bool).cuda()
logit.data.masked_fill_(m, -float('inf'))
action = _select_action(logit, [False])
ended = apply_action(robot, action[0], depth_ok)
bot_is_running = not ended or not headless
if not headless:
time.sleep(.3)
import argparse
import numpy as np
import yaml
import math
from pyquaternion import Quaternion
from transformations import euler_from_matrix
import pybullet as pb
from gibson2.core.simulator import Simulator
from gibson2.core.physics.interactive_objects import InteractiveObj
import matplotlib.pyplot as plt
'''
Analyzes a model for possible ways to place it flat on a surface.
Use by running without --ask_probs or --save_yaml to see rotations, and then with to save them with probabilities.
'''
simulator = Simulator(image_width=640, image_height=640) #, mode='headless')
parser = argparse.ArgumentParser(
description='Analyze objects that can be placed in a container.')
parser.add_argument('object_file', type=str)
parser.add_argument('--ask_probs',
action='store_true',
help='Whether to ask probability per rotation.')
parser.add_argument(
'--save_yaml',
action='store_true',
help='Whether to save orientations and probabilities to yaml.')
parser.add_argument('--threshold',
type=float,
default=0.02,
help='Threshold for including orientations or not.')
args = parser.parse_args()
def __init__(self,
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
automatic_reset=False,
device_idx=0,
render_to_tensor=False):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
# gibson external camera
self.initial_pos = np.array([0, 1.3, 4])
self.initial_view_direction = np.array([0, 0, -1])
self.up = np.array([0, 1, 0])
# output res for RL
self.out_image_height = 84
self.out_image_width = 84
# class: stadium=0, robot=1, drawer=2, handle=3
self.num_object_classes = 4
self.automatic_reset = automatic_reset
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(
mode=mode,
gravity=0,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
image_width=self.config.get('image_width', 128),
image_height=self.config.get('image_height', 128),
vertical_fov=self.config.get('vertical_fov', 90),
device_idx=device_idx,
render_to_tensor=render_to_tensor,
auto_sync=False)
self.simulator_loop = int(self.action_timestep /
self.simulator.timestep)
self.load()
self.hand_start_pos = [0.05, 1, 1.05]
self.hand_start_orn = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi])
self.set_robot_pos_orn(self.robots[0], self.hand_start_pos,
self.hand_start_orn)
# load drawer
self.drawer_start_pos = [0, 2.2, 1]
self.drawer_start_orn = p.getQuaternionFromEuler([0, 0, -np.pi / 2])
self.handle_idx = 3
self.drawer = InteractiveObj(
os.path.join(gibson2.assets_path, 'models', 'drawer',
'drawer_one_sided_handle.urdf'))
self.import_drawer_handle()
self.drawer.set_position_orientation(self.drawer_start_pos,
self.drawer_start_orn)
self.handle_init_pos = p.getLinkState(self.drawer_id,
self.handle_idx)[0][1]
self.simulator.sync()
self._state_id = p.saveState()
class HandDrawerEnv(BaseEnv):
def __init__(self,
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
automatic_reset=False,
device_idx=0,
render_to_tensor=False):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
# gibson external camera
self.initial_pos = np.array([0, 1.3, 4])
self.initial_view_direction = np.array([0, 0, -1])
self.up = np.array([0, 1, 0])
# output res for RL
self.out_image_height = 84
self.out_image_width = 84
# class: stadium=0, robot=1, drawer=2, handle=3
self.num_object_classes = 4
self.automatic_reset = automatic_reset
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(
mode=mode,
gravity=0,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
image_width=self.config.get('image_width', 128),
image_height=self.config.get('image_height', 128),
vertical_fov=self.config.get('vertical_fov', 90),
device_idx=device_idx,
render_to_tensor=render_to_tensor,
auto_sync=False)
self.simulator_loop = int(self.action_timestep /
self.simulator.timestep)
self.load()
self.hand_start_pos = [0.05, 1, 1.05]
self.hand_start_orn = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi])
self.set_robot_pos_orn(self.robots[0], self.hand_start_pos,
self.hand_start_orn)
# load drawer
self.drawer_start_pos = [0, 2.2, 1]
self.drawer_start_orn = p.getQuaternionFromEuler([0, 0, -np.pi / 2])
self.handle_idx = 3
self.drawer = InteractiveObj(
os.path.join(gibson2.assets_path, 'models', 'drawer',
'drawer_one_sided_handle.urdf'))
self.import_drawer_handle()
self.drawer.set_position_orientation(self.drawer_start_pos,
self.drawer_start_orn)
self.handle_init_pos = p.getLinkState(self.drawer_id,
self.handle_idx)[0][1]
self.simulator.sync()
self._state_id = p.saveState()
"""
modify import_articulated_object() in simulator.py to
render drawer and drawer handle separately for semantic inforamtion
"""
def import_drawer_handle(self):
self.drawer_id = self.drawer.load()
# render drawer
class_id = self.simulator.next_class_id
self.simulator.next_class_id += 1
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(self.drawer_id):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
link_name = p.getJointInfo(self.drawer_id, link_id)[12]
if link_name != b'handle_left' and link_name != b'handle_right' and link_name != b'handle_grip':
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.simulator.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.simulator.renderer.visual_objects) - 1)
link_ids.append(link_id)
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.simulator.renderer.add_instance_group(
object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=self.drawer_id,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
# render drawer handle
class_id = self.simulator.next_class_id
self.simulator.next_class_id += 1
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(self.drawer_id):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
link_name = p.getJointInfo(self.drawer_id, link_id)[12]
if link_name == b'handle_left' or link_name == b'handle_right' or link_name == b'handle_grip':
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.simulator.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.simulator.renderer.visual_objects) - 1)
link_ids.append(link_id)
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.simulator.renderer.add_instance_group(
object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=self.drawer_id,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
def set_robot_pos_orn(self, robot, pos, orn):
robot.set_position_orientation(pos, orn)
def get_drawer_handle_pos(self):
return np.array(p.getLinkState(self.drawer_id, self.handle_idx)[0])
# legacy get state from gibson
def get_state_legacy(self):
state = OrderedDict()
if 'rgb' in self.output:
state['rgb'] = self.get_rgb()
if 'depth' in self.output:
state['depth'] = self.get_depth()
if 'normal' in self.output:
state['normal'] = self.get_normal()
if 'seg' in self.output:
state['seg'] = self.get_seg()
return state
def get_state(self):
state = np.empty((0, self.image_height, self.image_width),
dtype=np.uint8)
if 'depth' in self.output:
depth = self.get_depth_external()[..., None].transpose(2, 0, 1)
state = np.concatenate((state, depth), axis=0)
if 'seg' in self.output:
seg = self.get_seg_external()[..., None].transpose(2, 0, 1)
state = np.concatenate((state, seg), axis=0)
return state.astype(np.uint8)
def _sigmoids(self, x, value_at_1, sigmoid):
if sigmoid in ('cosine', 'linear', 'quadratic'):
if not 0 <= value_at_1 < 1:
raise ValueError(
'`value_at_1` must be nonnegative and smaller than 1, '
'got {}.'.format(value_at_1))
else:
if not 0 < value_at_1 < 1:
raise ValueError(
'`value_at_1` must be strictly between 0 and 1, '
'got {}.'.format(value_at_1))
if sigmoid == 'gaussian':
scale = np.sqrt(-2 * np.log(value_at_1))
return np.exp(-0.5 * (x * scale)**2)
elif sigmoid == 'hyperbolic':
scale = np.arccosh(1 / value_at_1)
return 1 / np.cosh(x * scale)
elif sigmoid == 'long_tail':
scale = np.sqrt(1 / value_at_1 - 1)
return 1 / ((x * scale)**2 + 1)
elif sigmoid == 'cosine':
scale = np.arccos(2 * value_at_1 - 1) / np.pi
scaled_x = x * scale
return np.where(
abs(scaled_x) < 1, (1 + np.cos(np.pi * scaled_x)) / 2, 0.0)
elif sigmoid == 'linear':
scale = 1 - value_at_1
scaled_x = x * scale
return np.where(abs(scaled_x) < 1, 1 - scaled_x, 0.0)
elif sigmoid == 'quadratic':
scale = np.sqrt(1 - value_at_1)
scaled_x = x * scale
return np.where(abs(scaled_x) < 1, 1 - scaled_x**2, 0.0)
elif sigmoid == 'tanh_squared':
scale = np.arctanh(np.sqrt(1 - value_at_1))
return 1 - np.tanh(x * scale)**2
else:
raise ValueError('Unknown sigmoid type {!r}.'.format(sigmoid))
def is_close_reward(self,
x,
bounds=(0.0, 0.0),
margin=0.0,
sigmoid='gaussian',
value_at_margin=_DEFAULT_VALUE_AT_MARGIN):
lower, upper = bounds
if lower > upper:
raise ValueError('Lower bound must be <= upper bound.')
if margin < 0:
raise ValueError('`margin` must be non-negative.')
in_bounds = np.logical_and(lower <= x, x <= upper)
if margin == 0:
value = np.where(in_bounds, 1.0, 0.0)
else:
d = np.where(x < lower, lower - x, x - upper) / margin
value = np.where(in_bounds, 1.0,
self._sigmoids(d, value_at_margin, sigmoid))
return float(value) if np.isscalar(x) else value
def get_reward_new(self):
handle_center = self.get_drawer_handle_pos()
grip_center = self.robots[0].get_palm_position()
reach_dist = np.linalg.norm(grip_center - handle_center)
pull_dist = self.max_pull_dist - (self.handle_init_pos -
handle_center[1])
close_threshold = 0.05
reach_reward = -self.reach_reward_factor * reach_dist
pull_reward = self.is_close_reward(pull_dist, (0, close_threshold),
close_threshold * 2)
return reach_reward + pull_reward
def get_reward(self):
handle_center = self.get_drawer_handle_pos()
grip_center = self.robots[0].get_palm_position()
reach_dist = np.linalg.norm(grip_center - handle_center)
pull_dist = self.handle_init_pos - handle_center[1]
reach_rew = -self.reach_reward_factor * reach_dist
if reach_dist < 0.05:
self.reach_completed = True
else:
self.reach_completed = False
def pull_reward():
if self.reach_completed:
pull_rew = self.pull_reward_factor * pull_dist
return pull_rew
else:
return 0
pull_rew = pull_reward()
reward = reach_rew + pull_rew
return reward
def is_goal_pulled(self):
epsilon = 0.01
return (self.handle_init_pos -
self.get_drawer_handle_pos()[1]) > self.max_pull_dist - epsilon
def get_termination(self, info={}):
done = False
# if self.is_goal_pulled() and self.reach_completed:
# done = True
# info['success'] = True
if self.current_step >= self.max_step:
done = True
# info['success'] = False
if done:
info['episode_length'] = self.current_step
return done, info
def get_depth(self):
"""
:return: depth sensor reading, normalized to [0.0, 1.0]
"""
depth = -self.simulator.renderer.render_robot_cameras(
modes=('3d'))[0][:, :, 2:3]
# 0.0 is a special value for invalid entries
depth[depth < self.depth_low] = 0.0
depth[depth > self.depth_high] = 0.0
# re-scale depth to [0.0, 1.0]
depth /= self.depth_high
return depth
def get_rgb(self):
"""
:return: RGB sensor reading, normalized to [0.0, 1.0]
"""
return self.simulator.renderer.render_robot_cameras(
modes=('rgb'))[0][:, :, :3]
def get_normal(self):
"""
:return: surface normal reading
"""
return self.simulator.renderer.render_robot_cameras(
modes='normal')[0][:, :, :3]
def get_seg(self):
"""
:return: semantic segmentation mask, normalized to [0.0, 1.0]
"""
seg = self.simulator.renderer.render_robot_cameras(
modes='seg')[0][:, :, 0:1]
if self.num_object_classes is not None:
seg = np.clip(seg * 255.0 / self.num_object_classes, 0.0, 1.0)
return seg
def get_depth_external(self):
self.simulator.renderer.set_camera(
self.initial_pos, self.initial_pos + self.initial_view_direction,
self.up)
depth = -self.simulator.renderer.render(modes=('3d'))[0][:, :, 2:3]
depth[depth < self.depth_low] = 0.0
depth[depth > self.depth_high] = 0.0
depth /= self.depth_high
return (depth * 255).astype(np.uint8)[:, :, 0]
def get_seg_external(self):
self.simulator.renderer.set_camera(
self.initial_pos, self.initial_pos + self.initial_view_direction,
self.up)
seg = self.simulator.renderer.render(modes='seg')[0][:, :, 0:1]
if self.num_object_classes is not None:
seg = seg * 255
# only mask handle
seg[seg < 3.1] = 0
seg = np.clip(seg / self.num_object_classes, 0.0, 1.0)
return (seg * 255).astype(np.uint8)[:, :, 0]
def get_external_camera(self):
"""
pybullet external view rendering
"""
# pybullet external camera position
self._view_matrix = [
0.5708255171775818, -0.6403688788414001, 0.5138930082321167, 0.0,
0.821071445941925, 0.4451974034309387, -0.3572688400745392, 0.0,
-0.0, 0.6258810758590698, 0.7799185514450073, 0.0,
-1.0701078176498413, -0.883043646812439, -1.8267910480499268, 1.0
]
self._projection_matrix = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
-1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0
]
self._external_width = 1024
self._external_height = 768
(_, _, px, _,
_) = p.getCameraImage(width=self._external_width,
height=self._external_height,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
viewMatrix=self._view_matrix,
projectionMatrix=self._projection_matrix)
rgb_array = np.array(px, dtype=np.uint8)
rgb_array = np.reshape(
np.array(px), (self._external_height, self._external_width, -1))
rgb_array = rgb_array[:, :, :3]
return rgb_array
def render(self, mode='rgb_array'):
self._view_matrix = [
0.5708255171775818, -0.6403688788414001, 0.5138930082321167, 0.0,
0.821071445941925, 0.4451974034309387, -0.3572688400745392, 0.0,
-0.0, 0.6258810758590698, 0.7799185514450073, 0.0,
-1.0701078176498413, -0.883043646812439, -1.8267910480499268, 1.0
]
self._projection_matrix = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
-1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0
]
self._external_width = 1024
self._external_height = 768
(_, _, px, _,
_) = p.getCameraImage(width=self._external_width,
height=self._external_height,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
viewMatrix=self._view_matrix,
projectionMatrix=self._projection_matrix)
rgb_array = np.array(px, dtype=np.uint8)
rgb_array = np.reshape(
np.array(px), (self._external_height, self._external_width, -1))
rgb_array = rgb_array[:, :, :3]
return rgb_array
def save_rgb_image(self, path):
rgb = self.get_rgb()
Image.fromarray((rgb * 255).astype(np.uint8)).save(path)
def save_depth_image(self, path):
depth = self.get_depth()
Image.fromarray((depth * 255).astype(np.uint8)[:, :, 0]).save(path)
def save_seg_image(self, path):
seg = self.get_seg()
Image.fromarray((seg * 255).astype(np.uint8)[:, :, 0]).save(path)
def save_normal_image(self, path):
normal = self.get_normal()
Image.fromarray((normal * 255).astype(np.uint8)).save(path)
def save_external_image(self, path):
external = self.get_external_camera()
Image.fromarray(external).save(path)
def save_depth_image_external(self, path):
depth = self.get_depth_external()
Image.fromarray(depth).save(path)
def save_seg_image_external(self, path):
seg = self.get_seg_external()
Image.fromarray(seg).save(path)
def load_task_setup(self):
self.max_pull_dist = 0.5
self.reach_reward_factor = 1.0
self.pull_reward_factor = 1.0
self.max_step = self.config.get('max_step', 500)
# interface for drq
self._max_episode_steps = self.max_step
# legacy observation space from gibson
def load_observation_space_legacy(self):
self.output = self.config['output']
self.image_width = self.config.get('image_width', 128)
self.image_height = self.config.get('image_height', 128)
observation_space = OrderedDict()
if 'rgb' in self.output:
self.rgb_space = gym.spaces.Box(low=0.0,
high=1.0,
shape=(self.image_height,
self.image_width, 3),
dtype=np.float32)
observation_space['rgb'] = self.rgb_space
if 'depth' in self.output:
self.depth_low = self.config.get('depth_low', 0.5)
self.depth_high = self.config.get('depth_high', 5.0)
self.depth_space = gym.spaces.Box(low=0.0,
high=1.0,
shape=(self.image_height,
self.image_width, 1),
dtype=np.float32)
observation_space['depth'] = self.depth_space
if 'seg' in self.output:
self.seg_space = gym.spaces.Box(low=0.0,
high=1.0,
shape=(self.image_height,
self.image_width, 1),
dtype=np.float32)
observation_space['seg'] = self.seg_space
if 'normal' in self.output:
self.normal_space = gym.spaces.Box(low=0.0,
high=1.0,
shape=(self.image_height,
self.image_width, 3),
dtype=np.float32)
observation_space['normal'] = self.normal_space
self.observation_space = gym.spaces.Dict(observation_space)
# observation space for drq
def load_observation_space(self):
self.output = self.config['output']
self.image_width = self.config.get('image_width', 128)
self.image_height = self.config.get('image_height', 128)
self.depth_low = self.config.get('depth_low', 0.5)
self.depth_high = self.config.get('depth_high', 5.0)
channels = 0
if 'rgb' in self.output: channels += 3
if 'depth' in self.output: channels += 1
if 'seg' in self.output: channels += 1
if 'normal' in self.output: channels += 3
shape = [channels, self.image_height, self.image_width]
self.observation_space = gym.spaces.Box(low=0,
high=255,
shape=shape,
dtype=np.uint8)
def load_action_space(self):
self.action_space = self.robots[0].action_space
def load_miscellaneous_variables(self):
self.current_step = 0
self.current_episode = 0
def load(self):
super(HandDrawerEnv, self).load()
self.load_task_setup()
self.load_observation_space()
self.load_action_space()
self.load_miscellaneous_variables()
def reset_variables(self):
self.current_episode += 1
self.current_step = 0
def reset(self):
p.restoreState(self._state_id)
self.simulator.sync()
self.reset_variables()
return self.get_state()
def run_simulation(self):
for _ in range(self.simulator_loop):
self.simulator_step()
self.simulator.sync()
def step(self, action):
self.current_step += 1
if action is not None:
self.robots[0].apply_action(action)
self.run_simulation()
state = self.get_state()
info = {}
reward = self.get_reward()
done, info = self.get_termination()
if done and self.automatic_reset:
info['last_observation'] = state
state = self.reset()
return state, reward, done, info
import yaml
from gibson2.core.physics.robot_locomotors import Turtlebot
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import BuildingScene, StadiumScene
from gibson2.utils.utils import parse_config
import pytest
import pybullet as p
import numpy as np
from gibson2.core.render.profiler import Profiler
config = parse_config('../configs/turtlebot_p2p_nav.yaml')
s = Simulator(mode='gui', resolution=512, render_to_tensor=False)
scene = BuildingScene('Bolton')
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
p.resetBasePositionAndOrientation(scene.ground_plane_mjcf[0],
posObj=[0, 0, -10],
ornObj=[0, 0, 0, 1])
for i in range(2000):
with Profiler('simulator step'):
turtlebot.apply_action([0.1, 0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
