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()