def __init__(self): config = parse_config('../../configs/turtlebot_demo.yaml') hdr_texture = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'probe_02.hdr') hdr_texture2 = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'probe_03.hdr') light_modulation_map_filename = os.path.join(gibson2.ig_dataset_path, 'scenes', 'Rs_int', 'layout', 'floor_lighttype_0.png') background_texture = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'urban_street_01.jpg') settings = MeshRendererSettings(enable_shadow=False, enable_pbr=False) self.s = Simulator(mode='headless', image_width=400, image_height=400, rendering_settings=settings) scene = StaticIndoorScene('Rs') self.s.import_scene(scene) #self.s.import_ig_scene(scene) self.robot = Turtlebot(config) self.s.import_robot(self.robot) for _ in range(5): obj = YCBObject('003_cracker_box') self.s.import_object(obj) obj.set_position_orientation( np.random.uniform(low=0, high=2, size=3), [0, 0, 0, 1]) print(self.s.renderer.instances)
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 __init__(self, config_file, scene_id=None, mode='headless', action_timestep=1 / 10.0, physics_timestep=1 / 240.0, render_to_tensor=False, device_idx=0): """ :param config_file: config_file path :param scene_id: override scene_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 scene_id is not None: self.config['scene_id'] = scene_id self.mode = mode self.action_timestep = action_timestep self.physics_timestep = physics_timestep self.texture_randomization_freq = self.config.get( 'texture_randomization_freq', None) self.object_randomization_freq = self.config.get( 'object_randomization_freq', None) self.object_randomization_idx = 0 self.num_object_randomization_idx = 10 enable_shadow = self.config.get('enable_shadow', False) enable_pbr = self.config.get('enable_pbr', True) texture_scale = self.config.get('texture_scale', 1.0) settings = MeshRendererSettings(enable_shadow=enable_shadow, enable_pbr=enable_pbr, msaa=False, texture_scale=texture_scale) self.simulator = Simulator( mode=mode, physics_timestep=physics_timestep, render_timestep=action_timestep, 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, rendering_settings=settings) self.load()
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 main(): s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0) for random_seed in range(10): scene = InteractiveIndoorScene('Rs_int', texture_randomization=False, object_randomization=True, object_randomization_idx=random_seed) s.import_ig_scene(scene) for i in range(1000): s.step() s.reload() s.disconnect()
def test_import_igsdf(scene_name, scene_source): hdr_texture = os.path.join( gibson2.ig_dataset_path, 'scenes', 'background', 'probe_02.hdr') hdr_texture2 = os.path.join( gibson2.ig_dataset_path, 'scenes', 'background', 'probe_03.hdr') if scene_source == "IG": scene_dir = get_ig_scene_path(scene_name) elif scene_source == "CUBICASA": scene_dir = get_cubicasa_scene_path(scene_name) else: scene_dir = get_3dfront_scene_path(scene_name) light_modulation_map_filename = os.path.join( scene_dir, 'layout', 'floor_lighttype_0.png') background_texture = os.path.join( gibson2.ig_dataset_path, 'scenes', 'background', 'urban_street_01.jpg') scene = InteractiveIndoorScene( scene_name, texture_randomization=False, object_randomization=False, scene_source=scene_source) settings = MeshRendererSettings(env_texture_filename=hdr_texture, env_texture_filename2=hdr_texture2, env_texture_filename3=background_texture, light_modulation_map_filename=light_modulation_map_filename, enable_shadow=True, msaa=True, light_dimming_factor=1.0) s = Simulator(mode='iggui', image_width=960, image_height=720, device_idx=0, rendering_settings=settings) s.import_ig_scene(scene) fpss = [] np.random.seed(0) _,(px,py,pz) = scene.get_random_point() s.viewer.px = px s.viewer.py = py s.viewer.pz = 1.7 s.viewer.update() for i in range(3000): if i == 2500: logId = p.startStateLogging(loggingType=p.STATE_LOGGING_PROFILE_TIMINGS, fileName='trace_beechwood') start = time.time() s.step() end = time.time() print("Elapsed time: ", end - start) print("Frequency: ", 1 / (end - start)) fpss.append(1 / (end - start)) p.stopStateLogging(logId) s.disconnect() print("end") plt.plot(fpss) plt.show()
def __init__(self, robot='turtlebot', scene='Rs_int'): config = parse_config('../../configs/turtlebot_demo.yaml') hdr_texture = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'probe_02.hdr') hdr_texture2 = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'probe_03.hdr') light_modulation_map_filename = os.path.join(gibson2.ig_dataset_path, 'scenes', 'Rs_int', 'layout', 'floor_lighttype_0.png') background_texture = os.path.join(gibson2.ig_dataset_path, 'scenes', 'background', 'urban_street_01.jpg') scene = InteractiveIndoorScene(scene, texture_randomization=False, object_randomization=False) #scene._set_first_n_objects(5) scene.open_all_doors() settings = MeshRendererSettings( env_texture_filename=hdr_texture, env_texture_filename2=hdr_texture2, env_texture_filename3=background_texture, light_modulation_map_filename=light_modulation_map_filename, enable_shadow=True, msaa=True, light_dimming_factor=1.0, optimized=True) self.s = Simulator(mode='headless', image_width=400, image_height=400, rendering_settings=settings) self.s.import_ig_scene(scene) if robot == 'turtlebot': self.robot = Turtlebot(config) else: self.robot = Fetch(config) self.s.import_robot(self.robot) for _ in range(5): obj = YCBObject('003_cracker_box') self.s.import_object(obj) obj.set_position_orientation( np.random.uniform(low=0, high=2, size=3), [0, 0, 0, 1]) print(self.s.renderer.instances)
def run_demo(self): config = parse_config( os.path.join(os.path.dirname(gibson2.__file__), '../examples/configs/turtlebot_demo.yaml')) s = Simulator(mode='gui', image_width=700, image_height=700) scene = StaticIndoorScene('Rs', pybullet_load_texture=True) s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) for i in range(1000): turtlebot.apply_action([0.1, 0.5]) s.step() 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_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_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_import_building(): download_assets() download_demo_data() s = Simulator(mode='headless') scene = StaticIndoorScene('Rs') s.import_scene(scene, texture_scale=0.4) for i in range(15): s.step() assert s.objects == list(range(2)) s.disconnect()
def main(): s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0) scene = InteractiveIndoorScene( 'Rs_int', texture_randomization=True, object_randomization=False) s.import_ig_scene(scene) for i in range(10000): if i % 1000 == 0: scene.randomize_texture() s.step() s.disconnect()
def main(): s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0) scene = InteractiveIndoorScene('Rs_int', texture_randomization=False, object_randomization=False, load_object_categories=['chair'], load_room_types=['living_room']) s.import_ig_scene(scene) for _ in range(1000): 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 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 benchmark_rendering(scene_list, rendering_presets_list, modality_list): config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml')) assets_version = get_ig_assets_version() print('assets_version', assets_version) result = {} for scene_name in scene_list: for rendering_preset in rendering_presets_list: scene = InteractiveIndoorScene(scene_name, texture_randomization=False, object_randomization=False) settings = NamedRenderingPresets[rendering_preset] if rendering_preset == 'VISUAL_RL': image_width = 128 image_height = 128 else: image_width = 512 image_height = 512 s = Simulator(mode='headless', image_width=image_width, image_height=image_height, device_idx=0, rendering_settings=settings, physics_timestep=1 / 240.0) s.import_ig_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) for mode in modality_list: for _ in range(10): s.step() _ = s.renderer.render_robot_cameras(modes=(mode)) start = time.time() for _ in range(200): _ = s.renderer.render_robot_cameras(modes=(mode)) end = time.time() fps = 200 / (end - start) result[(scene_name, rendering_preset, mode)] = fps s.disconnect() return result
def main(): s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0) scene = InteractiveIndoorScene( 'Rs_int', texture_randomization=False, object_randomization=False) s.import_ig_scene(scene) np.random.seed(0) for _ in range(10): pt = scene.get_random_point_by_room_type('living_room')[1] print('random point in living_room', pt) for _ in range(1000): s.step() s.disconnect()
def main(): config = parse_config('../configs/turtlebot_demo.yaml') settings = MeshRendererSettings() s = Simulator(mode='gui', image_width=256, image_height=256, rendering_settings=settings) scene = StaticIndoorScene('Rs', build_graph=True, pybullet_load_texture=True) s.import_scene(scene) turtlebot = Turtlebot(config) s.import_robot(turtlebot) for i in range(10000): with Profiler('Simulator step'): turtlebot.apply_action([0.1, -0.1]) s.step() lidar = s.renderer.get_lidar_all() print(lidar.shape) # TODO: visualize lidar scan s.disconnect()
def test_import_stadium(): download_assets() download_demo_data() s = Simulator(mode='headless') scene = StadiumScene() s.import_scene(scene) print(s.objects) assert s.objects == list(range(4)) s.disconnect()
def generate_trav_map(scene_name, scene_source, load_full_scene=True): if scene_source not in SCENE_SOURCE: raise ValueError('Unsupported scene source: {}'.format(scene_source)) if scene_source == "IG": scene_dir = get_ig_scene_path(scene_name) elif scene_source == "CUBICASA": scene_dir = get_cubicasa_scene_path(scene_name) else: scene_dir = get_3dfront_scene_path(scene_name) random.seed(0) scene = InteractiveIndoorScene(scene_name, build_graph=False, texture_randomization=False, scene_source=scene_source) if not load_full_scene: scene._set_first_n_objects(3) s = Simulator(mode='headless', image_width=512, image_height=512, device_idx=0) s.import_ig_scene(scene) if load_full_scene: scene.open_all_doors() for i in range(20): s.step() vertices_info, faces_info = s.renderer.dump() s.disconnect() if load_full_scene: trav_map_filename_format = 'floor_trav_{}.png' obstacle_map_filename_format = 'floor_{}.png' else: trav_map_filename_format = 'floor_trav_no_obj_{}.png' obstacle_map_filename_format = 'floor_no_obj_{}.png' gen_trav_map(vertices_info, faces_info, output_folder=os.path.join(scene_dir, 'layout'), trav_map_filename_format=trav_map_filename_format, obstacle_map_filename_format=obstacle_map_filename_format)
def main(): parser = argparse.ArgumentParser() parser.add_argument('--scene', type=str, help='Name of the scene in the iG Dataset') args = parser.parse_args() settings = MeshRendererSettings(enable_shadow=True, msaa=False) s = Simulator(mode='gui', image_width=256, image_height=256, rendering_settings=settings) scene = iGSDFScene(args.scene) s.import_ig_scene(scene) for i in range(10000): with Profiler('Simulator step'): s.step() s.disconnect()
def debug_renderer_scaling(): s = Simulator(mode='gui', image_width=512, image_height=512, physics_timestep=1 / float(100)) scene = EmptyScene() s.import_scene(scene, render_floor_plane=True) urdf_path = '/cvgl2/u/chengshu/ig_dataset_v5/objects/window/103070/103070_avg_size_0.urdf' obj = ArticulatedObject(urdf_path) s.import_object(obj) obj.set_position([0, 0, 0]) embed() z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]]) obj.set_position([0, 0, z]) embed() for _ in range(100000000000): s.step()
def test_import_igsdf(): scene = InteractiveIndoorScene('Rs_int', texture_randomization=False, object_randomization=False) s = Simulator(mode='headless', image_width=512, image_height=512, device_idx=0) s.import_ig_scene(scene) s.renderer.use_pbr(use_pbr=True, use_pbr_mapping=True) for i in range(10): # if i % 100 == 0: # scene.randomize_texture() start = time.time() s.step() end = time.time() print("Elapsed time: ", end - start) print("Frequency: ", 1 / (end - start)) s.disconnect()
def test_import_building_viewing(): download_assets() download_demo_data() config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml')) s = Simulator(mode='headless') scene = StaticIndoorScene('Rs') 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 main(): config = parse_config('../configs/fetch_p2p_nav.yaml') s = Simulator(mode='gui', physics_timestep=1 / 240.0) scene = EmptyScene() s.import_scene(scene) fetch = Fetch(config) s.import_robot(fetch) robot_id = fetch.robot_ids[0] arm_joints = joints_from_names(robot_id, [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]) #finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint']) fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) x, y, z = fetch.get_end_effector_position() #set_joint_positions(robot_id, finger_joints, [0.04,0.04]) print(x, y, z) visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02) marker = p.createMultiBody(baseVisualShapeIndex=visual_marker) #max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05] #min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0] #rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04] max_limits = [0, 0] + get_max_limits(robot_id, arm_joints) min_limits = [0, 0] + get_min_limits(robot_id, arm_joints) rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints)) joint_range = list(np.array(max_limits) - np.array(min_limits)) joint_range = [item + 1 for item in joint_range] jd = [0.1 for item in joint_range] print(max_limits) print(min_limits) def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos, threshold, maxIter): sample_fn = get_sample_fn(robotid, arm_joints) set_joint_positions(robotid, arm_joints, sample_fn()) it = 0 while it < maxIter: jointPoses = p.calculateInverseKinematics(robotid, endEffectorId, targetPos, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=joint_range, restPoses=rest_position, jointDamping=jd) set_joint_positions(robotid, arm_joints, jointPoses[2:10]) ls = p.getLinkState(robotid, endEffectorId) newPos = ls[4] dist = np.linalg.norm(np.array(targetPos) - np.array(newPos)) if dist < threshold: break it += 1 print("Num iter: " + str(it) + ", threshold: " + str(dist)) return jointPoses while True: with Profiler("Simulation step"): fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) threshold = 0.01 maxIter = 100 joint_pos = accurateCalculateInverseKinematics( robot_id, fetch.parts['gripper_link'].body_part_index, [x, y, z], threshold, maxIter)[2:10] #set_joint_positions(robot_id, finger_joints, [0.04, 0.04]) s.step() keys = p.getKeyboardEvents() for k, v in keys.items(): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)): x += 0.01 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)): x -= 0.01 if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)): y += 0.01 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)): y -= 0.01 if (k == ord('z') and (v & p.KEY_IS_DOWN)): z += 0.01 if (k == ord('x') and (v & p.KEY_IS_DOWN)): z -= 0.01 p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1]) s.disconnect()
def test_simulator(): download_assets() 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()
from gibson2.utils.motion_planning_wrapper import MotionPlanningWrapper from gibson2.envs.igibson_env import iGibsonEnv ###########################script starts######################################### ################################################################################# ###start the gui### #p.connect(p.GUI) ######################initialize gravity and time############################### #p.setGravity(0,0,-9.8) #p.setTimeStep(1./360.) ############################load a scene####################################### ############To load a Interactive Indoor scene using a simulator############### s = Simulator(mode='gui', image_width=512, image_height=512) scene = InteractiveIndoorScene('Rs_int', texture_randomization=False, object_randomization=False) s.import_ig_scene(scene) ################################################################################ ##we keep importing things to the simulator s################## ################################################################################ #scene=InteractiveIndoorScene() #scene=StadiumScene() #scene.load() #######################loading a floor######################################### #print(pybullet_data.getDataPath()) #floor=os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml")