def test_multiagent(): s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) turtlebot1 = Turtlebot(config) turtlebot2 = Turtlebot(config) turtlebot3 = Turtlebot(config) s.import_robot(turtlebot1) s.import_robot(turtlebot2) s.import_robot(turtlebot3) turtlebot1.set_position([1, 0, 0.5]) turtlebot2.set_position([0, 0, 0.5]) turtlebot3.set_position([-1, 0, 0.5]) nbody = p.getNumBodies() for i in range(100): #turtlebot1.apply_action(1) #turtlebot2.apply_action(1) #turtlebot3.apply_action(1) s.step() s.disconnect() assert nbody == 7
def main(): config = parse_config('../configs/jr2_reaching.yaml') s = Simulator(mode='gui', image_width=512, image_height=512) #scene = BuildingScene('Rs', # build_graph=True, # pybullet_load_texture=True) scene = StadiumScene() s.import_scene(scene) jr = JR2_Kinova_Head(config) #turtlebot = Turtlebot(config) s.import_robot(jr) for _ in range(10): obj = YCBObject('003_cracker_box') s.import_object(obj) 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'): jr.apply_action([0, 0, 0.1, 0.1, 0, 0, 0, 0, 0]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
def main(): config = parse_config('../configs/jr2_reaching.yaml') s = Simulator(mode='gui', image_width=512, image_height=512) scene = StadiumScene() s.import_scene(scene) jr = JR2_Kinova(config) s.import_robot(jr) marker = VisualMarker(visual_shape=p.GEOM_SPHERE, rgba_color=[0, 0, 1, 0.3], radius=0.5) s.import_object(marker) marker.set_position([0, 4, 1]) for i in range(10000): with Profiler('Simulator step'): jr.apply_action([0, 0, 0, 0, 0, 0, 0]) s.step() rgb = s.renderer.render_robot_cameras(modes=('rgb')) s.disconnect()
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 test_import_building_viewing(): s = Simulator(mode='headless') scene = BuildingScene('Ohoopee') s.import_scene(scene) assert s.objects == list(range(2)) turtlebot1 = Turtlebot(config) turtlebot2 = Turtlebot(config) turtlebot3 = Turtlebot(config) s.import_robot(turtlebot1) s.import_robot(turtlebot2) s.import_robot(turtlebot3) turtlebot1.set_position([0.5, 0, 0.5]) turtlebot2.set_position([0, 0, 0.5]) turtlebot3.set_position([-0.5, 0, 0.5]) for i in range(10): s.step() #turtlebot1.apply_action(np.random.randint(4)) #turtlebot2.apply_action(np.random.randint(4)) #turtlebot3.apply_action(np.random.randint(4)) s.disconnect()
def test_import_box(): s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) print(s.objects) # wall = [pos, dim] wall = [[[0, 7, 1.01], [10, 0.2, 1]], [[0, -7, 1.01], [6.89, 0.1, 1]], [[7, -1.5, 1.01], [0.1, 5.5, 1]], [[-7, -1, 1.01], [0.1, 6, 1]], [[-8.55, 5, 1.01], [1.44, 0.1, 1]], [[8.55, 4, 1.01], [1.44, 0.1, 1]]] obstacles = [[[-0.5, 2, 1.01], [3.5, 0.1, 1]], [[4.5, -1, 1.01], [1.5, 0.1, 1]], [[-4, -2, 1.01], [0.1, 2, 1]], [[2.5, -4, 1.01], [1.5, 0.1, 1]]] for i in range(len(wall)): curr = wall[i] obj = BoxShape(curr[0], curr[1]) s.import_object(obj) for i in range(len(obstacles)): curr = obstacles[i] obj = BoxShape(curr[0], curr[1]) s.import_object(obj) config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml')) turtlebot1 = Turtlebot(config) turtlebot2 = Turtlebot(config) s.import_robot(turtlebot1) s.import_robot(turtlebot2) turtlebot1.set_position([6., -6., 0.]) turtlebot2.set_position([-3., 4., 0.]) for i in range(100): s.step() 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_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_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_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_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 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_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_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 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_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 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_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_ant(): s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) ant = Ant(config) s.import_robot(ant) ant2 = Ant(config) s.import_robot(ant2) ant2.set_position([0, 2, 2]) nbody = p.getNumBodies() for i in range(100): s.step() s.disconnect() assert nbody == 6
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_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()
def test_ant(): s = Simulator(mode='headless', timestep=1 / 40.0) scene = StadiumScene() s.import_scene(scene) ant = Ant(config) s.import_robot(ant) ant2 = Ant(config) s.import_robot(ant2) ant2.set_position([0, 2, 2]) nbody = p.getNumBodies() for i in range(100): s.step() #ant.apply_action(np.random.randint(17)) #ant2.apply_action(np.random.randint(17)) s.disconnect() assert nbody == 6
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 show_action_sensor_space(): s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) turtlebot.set_position([0, 1, 0.5]) ant = Ant(config) s.import_robot(ant) ant.set_position([0, 2, 0.5]) h = Humanoid(config) s.import_robot(h) h.set_position([0, 3, 2]) jr = JR2(config) s.import_robot(jr) jr.set_position([0, 4, 0.5]) jr2 = JR2_Kinova(config) s.import_robot(jr2) jr2.set_position([0, 5, 0.5]) husky = Husky(config) s.import_robot(husky) husky.set_position([0, 6, 0.5]) quad = Quadrotor(config) s.import_robot(quad) quad.set_position([0, 7, 0.5]) for robot in s.robots: print(type(robot), len(robot.ordered_joints), robot.calc_state().shape) for i in range(100): s.step() 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_import_rbo_object(): s = Simulator(mode='headless') try: scene = StadiumScene() s.import_scene(scene) obj = RBOObject('book') s.import_articulated_object(obj) obj2 = RBOObject('microwave') s.import_articulated_object(obj2) obj.set_position([0, 0, 2]) obj2.set_position([0, 1, 2]) obj3 = InteractiveObj( os.path.join(gibson2.assets_path, 'models', 'scene_components', 'door.urdf')) s.import_articulated_object(obj3) for i in range(100): s.step() finally: 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
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()