Merge branch 'igdsl2' of https://github.com/fxia22/gibsonv2 into igdsl2

examples/demo/atus_demos/pack_lunch_0.py

@@ -94,7 +94,7 @@ objects = []
 # Lunch packing objects 
 filename = 'rigid_body.urdf'
 for i in range(4):
-    category = 'dish\\casserole_dish'
+    category = os.path.join('dish', 'casserole_dish')
     instance = 'casserole_dish00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.34)
@@ -104,7 +104,7 @@ for i in range(4):
     objects.append(obj)
     
 for i in range(2):
-    category = 'dish\\bowl\\'
+    category = os.path.join('dish', 'bowl')
     instance = 'bowl02'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.25)
@@ -114,7 +114,7 @@ for i in range(2):
     objects.append(obj)
     
 for i in range(2):
-    category = 'dish\\bowl\\'
+    category = os.path.join('dish', 'bowl')
     instance = 'bowl02'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.25)
@@ -124,7 +124,7 @@ for i in range(2):
     objects.append(obj)
 
 for i in range(4):
-    category = 'drink\\soda\\'
+    category = os.path.join('drink', 'soda')
     instance = 'soda38_bluecan'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.7)
@@ -132,7 +132,7 @@ for i in range(4):
     obj.set_position_orientation([-5.9 - 0.1*i, -5.9, 1.7], [0, 0, 0, 1])
     objects.append(obj)    
     
-category = 'food\\protein\\eggs\\'
+category = os.path.join('food', 'protein', 'eggs')
 instance = 'eggs00_eggland'
 modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
 obj = ArticulatedObject(filename=modelpath, scale=0.7)
@@ -141,7 +141,7 @@ obj.set_position_orientation([-5.4, -6, .5], [0, 0, 0, 1])
 objects.append(obj)
 
 for i in range(4):
-    category = 'food\\vegetable\\corn\\'
+    category = os.path.join('food', 'vegetable', 'corn')
     instance = 'corn00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.45)
@@ -150,7 +150,7 @@ for i in range(4):
     objects.append(obj)
 
 for i in range(2):
-    category = 'food\\fruit\\apple\\'
+    category = os.path.join('food', 'fruit', 'apple')
     instance = 'apple00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=1)
@@ -159,7 +159,7 @@ for i in range(2):
     objects.append(obj)
 
 for i in range(2):
-    category = 'food\\fruit\\apple\\'
+    category = os.path.join('food', 'fruit', 'apple')
     instance = 'apple00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=1)
@@ -168,7 +168,7 @@ for i in range(2):
     objects.append(obj)
 
 for i in range(2):
-    category = 'food\\vegetable\\pepper\\'
+    category = os.path.join('food', 'vegetable', 'pepper')
     instance = 'pepper00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.85)
@@ -177,7 +177,7 @@ for i in range(2):
     objects.append(obj)
     
 for i in range(2):
-    category = 'food\\vegetable\\pepper\\'
+    category = os.path.join('food', 'vegetable', 'pepper')
     instance = 'pepper00'
     modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
     obj = ArticulatedObject(filename=modelpath, scale=.85)

examples/demo/atus_demos/pack_lunch_demo0.py

@@ -130,9 +130,11 @@ for i in range(4):
     s.import_object(obj)
     obj.set_position_orientation([-5.9 - 0.1*i, -5.9, 1.7], [0, 0, 0, 1])
     objects.append(obj)    
-    
+
+pack_lunch_assets_folder = 'd:\\gibson2_assets\\processed\\pack_lunch_objects'
 category = 'food\\protein\\eggs\\'
 instance = 'eggs00_eggland'
+filename = 'rigid_body.urdf'
 modelpath = os.path.join(pack_lunch_assets_folder, category, instance, filename)
 obj = ArticulatedObject(filename=modelpath, scale=0.7)
 s.import_object(obj)

examples/demo/vr_demos/data_save_replay/demos_for_karen/karen_demo_scenario_1_replay.py

@@ -0,0 +1,95 @@
+""" Demo for Karen - scenario 1 - REPLAY:
+
+Instructions:
+1) Walk from A to B
+2) Pick up pepper bottle at B
+3) Go to C and release pepper bottle
+
+A = dining table
+B = kitchen bar with chairs (it was hard to remove them so I kept them but picked up the pepper from the side with no chairs)
+C = fridge countertop
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_logging import VRLogReader
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# HDR files for PBR rendering
+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')
+
+# TODO for Jiangshan: set this to 1, 2 or 3 depending on which trial you want to view
+TRIAL_NUM = 1
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=True,
+                                            fullscreen=False,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='simple', image_width=504, image_height=560, rendering_settings=vr_rendering_settings)
+scene = InteractiveIndoorScene('Rs_int')
+s.import_ig_scene(scene)
+
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+
+gaze_marker = VisualMarker(radius=0.03)
+s.import_object(gaze_marker, use_pbr=False, use_pbr_mapping=False, shadow_caster=False)
+gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1.8278704545622642, 2.152284546319316, 1.031713969848457])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [-0.8, 1.55, 1.1]
+can_1 = ArticulatedObject(can_1_path, scale=0.6)
+s.import_object(can_1)
+can_1.set_position(can_pos)
+
+s.optimize_vertex_and_texture()
+
+# Note: the VRLogReader plays back the demo at the recorded fps, so there is not need to set this
+vr_log_path = 'data_logs/karen_demo_scenario_1_trial_{}.h5'.format(TRIAL_NUM)
+vr_reader = VRLogReader(log_filepath=vr_log_path)
+
+# The VR reader automatically shuts itself down and performs cleanup once the while loop has finished running
+while vr_reader.get_data_left_to_read():
+    # We need to read frame before step for various reasons - one of them is that we need to set the camera
+    # matrix for this frame before rendering in step
+    vr_reader.read_frame(s, fullReplay=True)
+    s.step()

examples/demo/vr_demos/data_save_replay/demos_for_karen/karen_demo_scenario_1_save.py

@@ -0,0 +1,141 @@
+""" Demo for Karen - scenario 1 - SAVING:
+
+Instructions:
+1) Walk from A to B
+2) Pick up pepper bottle at B
+3) Go to C and release pepper bottle
+
+A = dining table
+B = kitchen bar with chairs (it was hard to remove them so I kept them but picked up the pepper from the side with no chairs)
+C = fridge countertop
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_logging import VRLogWriter
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# HDR files for PBR rendering
+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')
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=True,
+                                            fullscreen=False,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', rendering_settings=vr_rendering_settings,
+            vr_eye_tracking=True, vr_mode=True)
+scene = InteractiveIndoorScene('Rs_int')
+s.import_ig_scene(scene)
+
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+r_hand.set_start_state(start_pos=[0, 0, 1.5])
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+l_hand.set_start_state(start_pos=[0, 0.5, 1.5])
+
+gaze_marker = VisualMarker(radius=0.03)
+s.import_object(gaze_marker, use_pbr=False, use_pbr_mapping=False, shadow_caster=False)
+gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1.8278704545622642, 2.152284546319316, 1.031713969848457])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [-0.8, 1.55, 1.1]
+can_1 = ArticulatedObject(can_1_path, scale=0.6)
+s.import_object(can_1)
+can_1.set_position(can_pos)
+
+s.optimize_vertex_and_texture()
+# Set VR starting position in the scene
+s.set_vr_offset([0, 0, -0.1])
+
+# Since vr_height_offset is set, we will use the VR HMD true height plus this offset instead of the third entry of the start pos
+s.set_vr_start_pos([1.259, -0.314, 0], vr_height_offset=-0.1)
+
+# Note: I appended "trial_n" manually to each log file, corresponding to the 3 trials I performed in each scenario
+vr_log_path = 'data_logs/karen_demo_scenario_1.h5'
+# Saves every 2 seconds or so (200 / 90fps is approx 2 seconds)
+vr_writer = VRLogWriter(frames_before_write=200, log_filepath=vr_log_path, profiling_mode=False)
+
+# Call set_up_data_storage once all actions have been registered (in this demo we only save states so there are none)
+# Despite having no actions, we need to call this function
+vr_writer.set_up_data_storage()
+
+should_simulate = True
+while should_simulate:
+    event_list = s.poll_vr_events()
+    for event in event_list:
+        device_type, event_type = event
+        if device_type == 'right_controller' or device_type == 'left_controller':
+            if event_type == 'menu_press':
+                # Quit data saving once the menu button has been pressed on either controller
+                should_simulate = False
+                
+    s.step(print_time=False)
+
+    # VR device data
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    l_is_valid, l_trans, l_rot = s.get_data_for_vr_device('left_controller')
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+
+    # VR button data
+    l_trig, l_touch_x, l_touch_y = s.get_button_data_for_controller('left_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    is_eye_data_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = s.get_eye_tracking_data()
+    if is_eye_data_valid:
+        # Move gaze marker based on eye tracking data
+        updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
+        gaze_marker.set_position(updated_marker_pos)
+
+    if r_is_valid:
+        r_hand.move(r_trans, r_rot)
+        r_hand.set_close_fraction(r_trig)
+        move_player_no_body(s, r_touch_x, r_touch_y, 0.015, 'hmd')
+
+    if l_is_valid:
+        l_hand.move(l_trans, l_rot)
+        l_hand.set_close_fraction(l_trig)
+
+    vr_writer.process_frame(s)
+
+# Note: always call this after the simulation is over to close the log file
+# and clean up resources used.
+vr_writer.end_log_session()
+s.disconnect()

examples/demo/vr_demos/data_save_replay/demos_for_karen/karen_demo_scenario_2_replay.py

@@ -0,0 +1,95 @@
+""" Demo for Karen - scenario 2 - REPLAY:
+
+Instructions:
+1) Walk from A to B
+2) Pick up pepper bottle at B
+3) Go to C and release pepper bottle
+
+A = far end of the room
+B = kitchen bar with chairs
+C = stove top with another object
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_logging import VRLogReader
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# HDR files for PBR rendering
+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')
+
+# TODO for Jiangshan: set this to 1, 2 or 3 depending on which trial you want to view
+TRIAL_NUM = 1
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=True,
+                                            fullscreen=False,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='simple', image_width=504, image_height=560, rendering_settings=vr_rendering_settings)
+scene = InteractiveIndoorScene('Rs_int')
+s.import_ig_scene(scene)
+
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+
+gaze_marker = VisualMarker(radius=0.03)
+s.import_object(gaze_marker, use_pbr=False, use_pbr_mapping=False, shadow_caster=False)
+gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1.8278704545622642, 2.152284546319316, 1.031713969848457])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [-0.8, 1.55, 1.1]
+can_1 = ArticulatedObject(can_1_path, scale=0.6)
+s.import_object(can_1)
+can_1.set_position(can_pos)
+
+s.optimize_vertex_and_texture()
+
+# Note: the VRLogReader plays back the demo at the recorded fps, so there is not need to set this
+vr_log_path = 'data_logs/karen_demo_scenario_2_trial_{}.h5'.format(TRIAL_NUM)
+vr_reader = VRLogReader(log_filepath=vr_log_path)
+
+# The VR reader automatically shuts itself down and performs cleanup once the while loop has finished running
+while vr_reader.get_data_left_to_read():
+    # We need to read frame before step for various reasons - one of them is that we need to set the camera
+    # matrix for this frame before rendering in step
+    vr_reader.read_frame(s, fullReplay=True)
+    s.step()

examples/demo/vr_demos/data_save_replay/demos_for_karen/karen_demo_scenario_2_save.py

@@ -0,0 +1,141 @@
+""" Demo for Karen - scenario 2 - SAVING:
+
+Instructions:
+1) Walk from A to B
+2) Pick up pepper bottle at B
+3) Go to C and release pepper bottle
+
+A = far end of the room
+B = kitchen bar with chairs
+C = stove top with another object
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_logging import VRLogWriter
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# HDR files for PBR rendering
+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')
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=True,
+                                            fullscreen=False,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', rendering_settings=vr_rendering_settings,
+            vr_eye_tracking=True, vr_mode=True)
+scene = InteractiveIndoorScene('Rs_int')
+s.import_ig_scene(scene)
+
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+r_hand.set_start_state(start_pos=[0, 0, 1.5])
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+l_hand.set_start_state(start_pos=[0, 0.5, 1.5])
+
+gaze_marker = VisualMarker(radius=0.03)
+s.import_object(gaze_marker, use_pbr=False, use_pbr_mapping=False, shadow_caster=False)
+gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1.8278704545622642, 2.152284546319316, 1.031713969848457])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [-0.8, 1.55, 1.1]
+can_1 = ArticulatedObject(can_1_path, scale=0.6)
+s.import_object(can_1)
+can_1.set_position(can_pos)
+
+s.optimize_vertex_and_texture()
+# Set VR starting position in the scene
+s.set_vr_offset([0, 0, -0.1])
+
+# Since vr_height_offset is set, we will use the VR HMD true height plus this offset instead of the third entry of the start pos
+s.set_vr_start_pos([1.56085938, -2.88829452, 0], vr_height_offset=-0.1)
+
+# Note: I appended "trial_n" manually to each log file, corresponding to the 3 trials I performed in each scenario
+vr_log_path = 'data_logs/karen_demo_scenario_2.h5'
+# Saves every 2 seconds or so (200 / 90fps is approx 2 seconds)
+vr_writer = VRLogWriter(frames_before_write=200, log_filepath=vr_log_path, profiling_mode=False)
+
+# Call set_up_data_storage once all actions have been registered (in this demo we only save states so there are none)
+# Despite having no actions, we need to call this function
+vr_writer.set_up_data_storage()
+
+should_simulate = True
+while should_simulate:
+    event_list = s.poll_vr_events()
+    for event in event_list:
+        device_type, event_type = event
+        if device_type == 'right_controller' or device_type == 'left_controller':
+            if event_type == 'menu_press':
+                # Quit data saving once the menu button has been pressed on either controller
+                should_simulate = False
+                
+    s.step(print_time=False)
+
+    # VR device data
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    l_is_valid, l_trans, l_rot = s.get_data_for_vr_device('left_controller')
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+
+    # VR button data
+    l_trig, l_touch_x, l_touch_y = s.get_button_data_for_controller('left_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    is_eye_data_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = s.get_eye_tracking_data()
+    if is_eye_data_valid:
+        # Move gaze marker based on eye tracking data
+        updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
+        gaze_marker.set_position(updated_marker_pos)
+
+    if r_is_valid:
+        r_hand.move(r_trans, r_rot)
+        r_hand.set_close_fraction(r_trig)
+        move_player_no_body(s, r_touch_x, r_touch_y, 0.015, 'hmd')
+
+    if l_is_valid:
+        l_hand.move(l_trans, l_rot)
+        l_hand.set_close_fraction(l_trig)
+
+    vr_writer.process_frame(s)
+
+# Note: always call this after the simulation is over to close the log file
+# and clean up resources used.
+vr_writer.end_log_session()
+s.disconnect()

examples/demo/vr_demos/muvr/muvr_demo.py

@@ -62,8 +62,9 @@ def run_muvr(mode='server', host='localhost', port='8887'):
     # Import 4 mustard bottles
     mass_list = [5, 10, 100, 500]
     mustard_start = [1, -0.2, 1]
+    m = None
     for i in range(len(mass_list)):
-        mustard = YCBObject('006_mustard_bottle')
+        m = mustard = YCBObject('006_mustard_bottle')
         s.import_object(mustard)
         mustard.set_position([mustard_start[0], mustard_start[1] - i * 0.2, mustard_start[2]])
         p.changeDynamics(mustard.body_id, -1, mass=mass_list[i])
@@ -96,6 +97,9 @@ def run_muvr(mode='server', host='localhost', port='8887'):
             # Server is the one that steps the physics simulation, not the client
             s.step()
 
+            # TODO: Remove jittery mustard
+            m.set_position([1, -0.8 + float(np.sin(sin_accumulator)) / 2.0, 1])
+            
             # Send the current frame to be rendered by the client,
             # and also ingest new client data
             vr_server.refresh_server()

examples/demo/vr_demos/robot_embodiment/vr_demo_robot_control.py

@@ -0,0 +1,186 @@
+""" VR embodiment demo with Fetch robot. """
+
+import numpy as np
+import os
+import pybullet as p
+
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.robots.fetch_vr_robot import FetchVR
+from gibson2.scenes.gibson_indoor_scene import StaticIndoorScene
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.utils import parse_config
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+fetch_config = parse_config(os.path.join('..', '..', '..', 'configs', 'fetch_p2p_nav.yaml'))
+
+# Playground configuration: edit this to change functionality
+optimize = True
+# Toggles SRAnipal eye tracking
+use_eye_tracking = True
+
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', physics_timestep = 1/90.0, render_timestep = 1/90.0, 
+            rendering_settings=MeshRendererSettings(optimized=optimize, fullscreen=False, enable_pbr=False),
+            vr_eye_tracking=use_eye_tracking, vr_mode=True)
+scene = StaticIndoorScene('Placida')
+s.import_scene(scene)
+
+# TODO: Change this to VR fetch!
+fvr = FetchVR(fetch_config)
+s.import_robot(fvr)
+# Set differential drive to control wheels
+fvr.set_position([0,-1.5,0])
+fvr.robot_specific_reset()
+fvr.keep_still()
+
+# Load robot end-effector-tracker
+effector_marker = VisualMarker(rgba_color = [1, 0, 1, 0.2], radius=0.05)
+s.import_object(effector_marker)
+# Hide marker upon initialization
+effector_marker.set_position([0,0,-5])
+
+if use_eye_tracking:
+    # Eye tracking visual marker - a red marker appears in the scene to indicate gaze direction
+    gaze_marker = VisualMarker(radius=0.03)
+    s.import_object(gaze_marker)
+    gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path)
+s.import_object(basket)
+basket.set_position([1, 0.2, 1])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+mass_list = [5, 10, 100, 500]
+mustard_start = [1, -0.2, 1]
+mustard_list = []
+for i in range(len(mass_list)):
+    mustard = YCBObject('006_mustard_bottle')
+    mustard_list.append(mustard)
+    s.import_object(mustard)
+    mustard.set_position([mustard_start[0], mustard_start[1] - i * 0.2, mustard_start[2]])
+    p.changeDynamics(mustard.body_id, -1, mass=mass_list[i])
+
+if optimize:
+    s.optimize_vertex_and_texture()
+
+fetch_height = 1.2
+
+wheel_axle_half = 0.18738 # half of the distance between the wheels
+wheel_radius = 0.054  # radius of the wheels themselves
+
+r_wheel_joint = fvr.ordered_joints[0]
+l_wheel_joint = fvr.ordered_joints[1]
+
+fetch_lin_vel_multiplier = 100
+
+# Variables used in IK to move end effector
+fetch_body_id = fvr.get_fetch_body_id()
+fetch_joint_num = p.getNumJoints(fetch_body_id)
+effector_link_id = 19
+
+# Setting to determine whether IK should also solve for end effector orientation
+# based on the VR controller
+solve_effector_orn = True
+
+# Update frequency - number of frames before update
+# TODO: Play around with this
+update_freq = 1
+
+frame_num = 0
+# Main simulation loop
+while True:
+    s.step()
+
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    # Fetch only has one arm which is entirely controlled by the right hand
+    # TODO: Use left arm for movement?
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    # Set fetch orientation directly from HMD to avoid lag when turning and resultant motion sickness
+    fvr.set_z_rotation(hmd_rot)
+
+    # Get world position and fetch position
+    hmd_world_pos = s.get_hmd_world_pos()
+    fetch_pos = fvr.get_position()
+
+     # Calculate x and y offset to get to fetch position
+    # z offset is to the desired hmd height, corresponding to fetch head height
+    offset_to_fetch = [fetch_pos[0] - hmd_world_pos[0], 
+                        fetch_pos[1] - hmd_world_pos[1], 
+                        fetch_height - hmd_world_pos[2]] 
+
+    s.set_vr_offset(offset_to_fetch)
+
+    # TODO: Consolidate this functionality into the FetchVR class
+    # Update fetch arm at user-defined frequency
+    if r_is_valid and frame_num % 10 == 0:
+        effector_marker.set_position(r_trans)
+        effector_marker.set_orientation(r_rot)
+
+        # Linear velocity is relative to current direction fetch is pointing,
+        # so only need to know how fast we should travel in that direction (Y touchpad direction is used for this)
+        lin_vel = fetch_lin_vel_multiplier * r_touch_y
+        ang_vel = 0
+
+        left_wheel_ang_vel = (lin_vel - ang_vel * wheel_axle_half) / wheel_radius
+        right_wheel_ang_vel = (lin_vel + ang_vel * wheel_axle_half) / wheel_radius
+
+        l_wheel_joint.set_motor_velocity(left_wheel_ang_vel)
+        r_wheel_joint.set_motor_velocity(right_wheel_ang_vel)
+
+        # Ignore sideays rolling dimensions of controller (x axis) since fetch can't "roll" its arm
+        r_euler_rot = p.getEulerFromQuaternion(r_rot)
+        r_rot_no_x = p.getQuaternionFromEuler([0, r_euler_rot[1], r_euler_rot[2]])
+
+        # Iteration and residual threshold values are based on recommendations from PyBullet
+        ik_joint_poses = None
+        if solve_effector_orn:
+            ik_joint_poses = p.calculateInverseKinematics(fetch_body_id,
+                                                    effector_link_id,
+                                                    r_trans,
+                                                    r_rot_no_x,
+                                                    solver=0,
+                                                    maxNumIterations=100,
+                                                    residualThreshold=.01)
+        else:
+            ik_joint_poses = p.calculateInverseKinematics(fetch_body_id,
+                                                    effector_link_id,
+                                                    r_trans,
+                                                    solver=0)
+
+        # Set joints to the results of the IK
+        if ik_joint_poses is not None:
+            for i in range(len(ik_joint_poses)):
+                next_pose = ik_joint_poses[i]
+                next_joint = fvr.ordered_joints[i]
+
+                # Set wheel joint back to original position so IK calculation does not affect movement
+                # Note: PyBullet does not currently expose the root of the IK calculation
+                if next_joint.joint_name == 'r_wheel_joint' or next_joint.joint_name == 'l_wheel_joint':
+                    next_pose, _, _ = next_joint.get_state()
+
+                p.resetJointState(fetch_body_id, next_joint.joint_index, next_pose)
+
+                # TODO: Arm is not moving with this function - debug!
+                # TODO: This could be causing some problems with movement
+                #p.setJointMotorControl2(bodyIndex=fetch_body_id,
+                #                        jointIndex=next_joint.joint_index,
+                #                        controlMode=p.POSITION_CONTROL,
+                #                        targetPosition=next_pose,
+                #                        force=500)
+        
+
+        # TODO: Implement opening/closing the end effectors
+        # Something like this: fetch.set_fetch_gripper_fraction(rTrig)
+        # TODO: Implement previous rest pose
+
+    frame_num += 1
+
+s.disconnect()

examples/demo/vr_demos/vr_hyperparams/vr_body_tuning.py

@@ -0,0 +1,161 @@
+""" VR playground containing various objects and VR options that can be toggled
+to experiment with the VR experience in iGibson. This playground operates in a
+PBR scene. Please see vr_playground_no_pbr.py for a non-PBR experience.
+
+Important - VR functionality and where to find it:
+
+1) Most VR functions can be found in the gibson2/simulator.py
+2) VR utility functions are found in gibson2/utils/vr_utils.py
+3) The VR renderer can be found in gibson2/render/mesh_renderer.py
+4) The underlying VR C++ code can be found in vr_mesh_render.h and .cpp in gibson2/render/cpp
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# Playground configuration: edit this to change functionality
+optimize = False
+# Toggles fullscreen companion window
+fullscreen = False
+
+# HDR files for PBR rendering
+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')
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=optimize,
+                                            fullscreen=fullscreen,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', physics_timestep = 1/90.0, render_timestep = 1/90.0, rendering_settings=vr_rendering_settings,
+            vr_eye_tracking=False, vr_mode=True)
+scene = InteractiveIndoorScene('Rs_int')
+# Turn this on when debugging to speed up loading
+scene._set_first_n_objects(5)
+s.import_ig_scene(scene)
+
+# TODO: Remove later
+p.setGravity(0, 0, 0)
+
+# Player body is represented by a translucent blue cylinder
+""" if enable_vr_body:
+    vr_body = VrBody()
+    s.import_object(vr_body, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+    vr_body.init_body([0,0]) """
+
+vr_body_fpath = os.path.join(assets_path, 'models', 'vr_body', 'vr_body.urdf')
+vrb = ArticulatedObject(vr_body_fpath, scale=0.1)
+s.import_object(vrb, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+#vrb.set_position([0, 0, 1])
+vrb_cid = p.createConstraint(vrb.body_id, -1, -1, -1, p.JOINT_FIXED, 
+                                            [0, 0, 0], [0, 0, 0], [0, 0, 1.2])
+
+# The hand can either be 'right' or 'left'
+# It has enough friction to pick up the basket and the mustard bottles
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+# This sets the hand constraints so it can move with the VR controller
+r_hand.set_start_state(start_pos=[0, 0, 1.5])
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+# This sets the hand constraints so it can move with the VR controller
+l_hand.set_start_state(start_pos=[0, 0.5, 1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1, 1.55, 1.2])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [[-0.8, 1.55, 1.2], [-0.6, 1.55, 1.2], [-0.4, 1.55, 1.2]]
+cans = []
+for i in range (len(can_pos)):
+    can_1 = ArticulatedObject(can_1_path, scale=0.6)
+    cans.append(can_1)
+    s.import_object(can_1)
+    can_1.set_position(can_pos[i])
+
+# TODO: Remove this test
+#r_hand.set_hand_no_collision(can_1.body_id)
+#r_hand.set_hand_no_collision(basket.body_id)
+#r_hand.set_hand_no_collision(vr_body.body_id)
+#p.setCollisionFilterPair(can_1.body_id, basket.body_id, -1, -1, 0) # the last argument is 0 for disabling collision, 1 for enabling collision
+#p.setCollisionFilterPair(can_1.body_id, r_hand.body_id, -1, -1, 0)
+#p.setCollisionFilterPair(can_1.body_id, l_hand.body_id, -1, -1, 0)
+
+if optimize:
+    s.optimize_vertex_and_texture()
+
+# Set VR starting position in the scene
+s.set_vr_offset([0, 0, 0])
+
+while True:
+    s.step()
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    l_is_valid, l_trans, l_rot = s.get_data_for_vr_device('left_controller')
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+    l_trig, l_touch_x, l_touch_y = s.get_button_data_for_controller('left_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    if hmd_is_valid:
+        p.changeConstraint(vrb_cid, hmd_trans, vrb.get_orientation(), maxForce=2000)
+
+
+    """if enable_vr_body:
+        if not r_is_valid:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, 0, 0, movement_speed, relative_movement_device)
+        else:
+            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device) """
+
+    if r_is_valid:
+        r_hand.move(r_trans, r_rot)
+        r_hand.set_close_fraction(r_trig)
+
+        # Right hand used to control movement
+        # Move VR system based on device coordinate system and touchpad press location
+        move_player_no_body(s, r_touch_x, r_touch_y, 0.03, 'hmd')
+
+        # Trigger haptic pulse on right touchpad, modulated by trigger close fraction
+        # Close the trigger to create a stronger pulse
+        # Note: open trigger has closed fraction of 0.05 when open, so cutoff haptic input under 0.1
+        # to avoid constant rumbling
+        s.trigger_haptic_pulse('right_controller', r_trig if r_trig > 0.1 else 0)
+
+    if l_is_valid:
+        l_hand.move(l_trans, l_rot)
+        l_hand.set_close_fraction(l_trig)
+        s.trigger_haptic_pulse('left_controller', l_trig if l_trig > 0.1 else 0)
+
+s.disconnect()

examples/demo/vr_demos/vr_hyperparams/vr_hand_tuning.py

@@ -0,0 +1,145 @@
+""" VR demo for tuning physics parameters. Has mustard bottles, pears and a basket that can be grasped. """
+
+import numpy as np
+import os
+import pybullet as p
+
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.gibson_indoor_scene import StaticIndoorScene
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+lunch_pack_folder = os.path.join(assets_path, 'pack_lunch')
+small_fruit_path = os.path.join(lunch_pack_folder, 'food', 'fruit', 'pear', 'pear00', 'rigid_body.urdf')
+
+# Playground configuration: edit this to change functionality
+optimize = True
+# Toggles fullscreen companion window
+fullscreen = False
+# Toggles SRAnipal eye tracking
+use_eye_tracking = True
+# Enables the VR collision body
+enable_vr_body = True
+# Toggles movement with the touchpad (to move outside of play area)
+touchpad_movement = True
+# Set to one of hmd, right_controller or left_controller to move relative to that device
+relative_movement_device = 'hmd'
+# Movement speed for touchpad-based movement
+movement_speed = 0.03
+
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', rendering_settings=MeshRendererSettings(optimized=optimize, fullscreen=fullscreen, enable_pbr=False),
+            vr_eye_tracking=use_eye_tracking, vr_mode=True)
+scene = StaticIndoorScene('Placida')
+s.import_scene(scene)
+
+# Player body is represented by a translucent blue cylinder
+if enable_vr_body:
+    vr_body = VrBody()
+    s.import_object(vr_body)
+    vr_body.init_body([0,0])
+
+# The hand can either be 'right' or 'left'
+# It has enough friction to pick up the basket and the mustard bottles
+r_hand = VrHand(hand='right')
+s.import_object(r_hand)
+# This sets the hand constraints so it can move with the VR controller
+r_hand.set_start_state(start_pos=[0, 0, 1.5])
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand)
+# This sets the hand constraints so it can move with the VR controller
+l_hand.set_start_state(start_pos=[0, 0.5, 1.5])
+
+if use_eye_tracking:
+    # Eye tracking visual marker - a red marker appears in the scene to indicate gaze direction
+    gaze_marker = VisualMarker(radius=0.03)
+    s.import_object(gaze_marker)
+    gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path)
+s.import_object(basket)
+basket.set_position([1, 0.2, 1])
+p.changeDynamics(basket.body_id, -1, mass=1)
+
+# Experiment with heavier mustard bottles
+mass_list = [0.5, 1, 2, 5]
+mustard_start = [1, -0.2, 1]
+mustard_list = []
+for i in range(len(mass_list)):
+    mustard = YCBObject('006_mustard_bottle')
+    mustard_list.append(mustard)
+    s.import_object(mustard)
+    mustard.set_position([mustard_start[0], mustard_start[1] - i * 0.2, mustard_start[2]])
+    p.changeDynamics(mustard.body_id, -1, mass=mass_list[i])
+
+fruit_start = [1, -1, 1]
+for i in range(3):
+    fruit = ArticulatedObject(small_fruit_path, scale=0.9)
+    s.import_object(fruit)
+    fruit.set_position([fruit_start[0], fruit_start[1] - i * 0.2, fruit_start[2]])
+    # Normal-sized pears weigh around 200 grams
+    p.changeDynamics(fruit.body_id, -1, mass=0.2)
+
+if optimize:
+    s.optimize_vertex_and_texture()
+
+# Start user close to counter for interaction
+# Small negative offset to account for lighthouses not being set up entirely correctly
+s.set_vr_offset([-0.5, 0.0, -0.1])
+
+# Main simulation loop
+while True:
+    # Step the simulator - this needs to be done every frame to actually run the simulation
+    s.step()
+
+    # VR device data
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    l_is_valid, l_trans, l_rot = s.get_data_for_vr_device('left_controller')
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+
+    # VR button data
+    l_trig, l_touch_x, l_touch_y = s.get_button_data_for_controller('left_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    # VR eye tracking data
+    if use_eye_tracking:
+        is_eye_data_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = s.get_eye_tracking_data()
+        if is_eye_data_valid:
+            # Move gaze marker based on eye tracking data
+            updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
+            gaze_marker.set_position(updated_marker_pos)
+
+    if enable_vr_body:
+        if not r_is_valid:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, 0, 0, movement_speed, relative_movement_device)
+        else:
+            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+
+    if r_is_valid:
+        r_hand.move(r_trans, r_rot)
+        r_hand.set_close_fraction(r_trig)
+
+        # Right hand used to control movement
+        # Move VR system based on device coordinate system and touchpad press location
+        move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+
+        # Trigger haptic pulse on right touchpad, modulated by trigger close fraction
+        # Close the trigger to create a stronger pulse
+        # Note: open trigger has closed fraction of 0.05 when open, so cutoff haptic input under 0.1
+        # to avoid constant rumbling
+        s.trigger_haptic_pulse('right_controller', r_trig if r_trig > 0.1 else 0)
+
+    if l_is_valid:
+        l_hand.move(l_trans, l_rot)
+        l_hand.set_close_fraction(l_trig)
+        s.trigger_haptic_pulse('left_controller', l_trig if l_trig > 0.1 else 0)
+
+s.disconnect()

examples/demo/vr_demos/vr_hyperparams/vr_timestep_tuning.py

@@ -0,0 +1,152 @@
+""" VR playground containing various objects and VR options that can be toggled
+to experiment with the VR experience in iGibson. This playground operates in a
+PBR scene. Please see vr_playground_no_pbr.py for a non-PBR experience.
+
+Important - VR functionality and where to find it:
+
+1) Most VR functions can be found in the gibson2/simulator.py
+2) VR utility functions are found in gibson2/utils/vr_utils.py
+3) The VR renderer can be found in gibson2/render/mesh_renderer.py
+4) The underlying VR C++ code can be found in vr_mesh_render.h and .cpp in gibson2/render/cpp
+"""
+
+import numpy as np
+import os
+import pybullet as p
+import time
+
+import gibson2
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+from gibson2.objects.object_base import Object
+from gibson2.objects.articulated_object import ArticulatedObject
+from gibson2.objects.vr_objects import VrBody, VrHand
+from gibson2.objects.visual_marker import VisualMarker
+from gibson2.objects.ycb_object import YCBObject
+from gibson2.simulator import Simulator
+from gibson2.utils.vr_utils import move_player_no_body
+from gibson2 import assets_path
+sample_urdf_folder = os.path.join(assets_path, 'models', 'sample_urdfs')
+groceries_folder = os.path.join(assets_path, 'models', 'groceries')
+
+# Playground configuration: edit this to change functionality
+optimize = True
+# Toggles fullscreen companion window
+fullscreen = False
+# Toggles SRAnipal eye tracking
+use_eye_tracking = True
+# Toggles movement with the touchpad (to move outside of play area)
+touchpad_movement = True
+# Set to one of hmd, right_controller or left_controller to move relative to that device
+relative_movement_device = 'hmd'
+# Movement speed for touchpad-based movement
+movement_speed = 0.03
+
+# HDR files for PBR rendering
+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')
+
+# VR rendering settings
+vr_rendering_settings = MeshRendererSettings(optimized=optimize,
+                                            fullscreen=fullscreen,
+                                            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, 
+                                            enable_pbr=True,
+                                            msaa=True,
+                                            light_dimming_factor=1.0)
+# Initialize simulator with specific rendering settings
+s = Simulator(mode='vr', rendering_settings=vr_rendering_settings,
+            vr_eye_tracking=use_eye_tracking, vr_mode=True)
+scene = InteractiveIndoorScene('Beechwood_0_int')
+# Turn this on when debugging to speed up loading
+scene._set_first_n_objects(10)
+s.import_ig_scene(scene)
+
+# The hand can either be 'right' or 'left'
+# It has enough friction to pick up the basket and the mustard bottles
+r_hand = VrHand(hand='right')
+s.import_object(r_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+# This sets the hand constraints so it can move with the VR controller
+r_hand.set_start_state(start_pos=[0, 0, 1.5])
+
+l_hand = VrHand(hand='left')
+s.import_object(l_hand, use_pbr=False, use_pbr_mapping=False, shadow_caster=True)
+# This sets the hand constraints so it can move with the VR controller
+l_hand.set_start_state(start_pos=[0, 0.5, 1.5])
+
+if use_eye_tracking:
+    # Eye tracking visual marker - a red marker appears in the scene to indicate gaze direction
+    gaze_marker = VisualMarker(radius=0.03)
+    s.import_object(gaze_marker, use_pbr=False, use_pbr_mapping=False, shadow_caster=False)
+    gaze_marker.set_position([0,0,1.5])
+
+basket_path = os.path.join(sample_urdf_folder, 'object_ZU6u5fvE8Z1.urdf')
+basket = ArticulatedObject(basket_path, scale=0.8)
+s.import_object(basket)
+basket.set_position([-1, 1.55, 1.2])
+p.changeDynamics(basket.body_id, -1, mass=5)
+
+can_1_path = os.path.join(groceries_folder, 'canned_food', '1', 'rigid_body.urdf')
+can_pos = [[-0.8, 1.55, 1.2], [-0.6, 1.55, 1.2], [-0.4, 1.55, 1.2]]
+cans = []
+for i in range (len(can_pos)):
+    can_1 = ArticulatedObject(can_1_path, scale=0.6)
+    cans.append(can_1)
+    s.import_object(can_1)
+    can_1.set_position(can_pos[i])
+
+if optimize:
+    s.optimize_vertex_and_texture()
+
+# Set VR starting position in the scene
+s.set_vr_offset([0, 0, -0.1])
+
+while True:
+    s.step(print_time=True)
+
+    # VR device data
+    hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
+    l_is_valid, l_trans, l_rot = s.get_data_for_vr_device('left_controller')
+    r_is_valid, r_trans, r_rot = s.get_data_for_vr_device('right_controller')
+
+    # VR button data
+    l_trig, l_touch_x, l_touch_y = s.get_button_data_for_controller('left_controller')
+    r_trig, r_touch_x, r_touch_y = s.get_button_data_for_controller('right_controller')
+
+    # VR eye tracking data
+    if use_eye_tracking:
+        is_eye_data_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = s.get_eye_tracking_data()
+        if is_eye_data_valid:
+            # Move gaze marker based on eye tracking data
+            updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
+            gaze_marker.set_position(updated_marker_pos)
+
+    if r_is_valid:
+        r_hand.move(r_trans, r_rot)
+        r_hand.set_close_fraction(r_trig)
+
+        # Right hand used to control movement
+        # Move VR system based on device coordinate system and touchpad press location
+        move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+
+        # Trigger haptic pulse on right touchpad, modulated by trigger close fraction
+        # Close the trigger to create a stronger pulse
+        # Note: open trigger has closed fraction of 0.05 when open, so cutoff haptic input under 0.1
+        # to avoid constant rumbling
+        s.trigger_haptic_pulse('right_controller', r_trig if r_trig > 0.1 else 0)
+
+    if l_is_valid:
+        l_hand.move(l_trans, l_rot)
+        l_hand.set_close_fraction(l_trig)
+        s.trigger_haptic_pulse('left_controller', l_trig if l_trig > 0.1 else 0)
+
+s.disconnect()

examples/demo/vr_demos/vr_playground_old_gibson_no_pbr.py

@@ -93,7 +93,7 @@ if optimize:
     s.optimize_vertex_and_texture()
 
 # Start user close to counter for interaction
-s.set_vr_offset([-0.5, 0.0, -0.5])
+s.set_vr_offset([-0.5, 0.0, 0])
 
 # State of mustard hiding, toggled by a menu press
 hide_mustard = False
@@ -131,17 +131,20 @@ while True:
             updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
             gaze_marker.set_position(updated_marker_pos)
 
+    if enable_vr_body:
+        if not r_is_valid:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, 0, 0, movement_speed, relative_movement_device)
+        else:
+            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+
     if r_is_valid:
         r_hand.move(r_trans, r_rot)
         r_hand.set_close_fraction(r_trig)
 
-        if enable_vr_body:
+        # Right hand used to control movement
-            # See VrBody class for more details on this method
+        # Move VR system based on device coordinate system and touchpad press location
-            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+        move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
-        else:
-            # Right hand used to control movement
-            # Move VR system based on device coordinate system and touchpad press location
-            move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
 
         # Trigger haptic pulse on right touchpad, modulated by trigger close fraction
         # Close the trigger to create a stronger pulse

examples/demo/vr_demos/vr_playground_pbr.py

@@ -118,7 +118,7 @@ if optimize:
     s.optimize_vertex_and_texture()
 
 # Set VR starting position in the scene
-s.set_vr_offset([0, 0, -0.6])
+s.set_vr_offset([0, 0, -0.1])
 
 # State of can hiding, toggled by a menu press
 hide_can = False
@@ -154,17 +154,20 @@ while True:
             updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
             gaze_marker.set_position(updated_marker_pos)
 
+    if enable_vr_body:
+        if not r_is_valid:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, 0, 0, movement_speed, relative_movement_device)
+        else:
+            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+
     if r_is_valid:
         r_hand.move(r_trans, r_rot)
         r_hand.set_close_fraction(r_trig)
 
-        if enable_vr_body:
+        # Right hand used to control movement
-            # See VrBody class for more details on this method
+        # Move VR system based on device coordinate system and touchpad press location
-            vr_body.move_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
+        move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
-        else:
-            # Right hand used to control movement
-            # Move VR system based on device coordinate system and touchpad press location
-            move_player_no_body(s, r_touch_x, r_touch_y, movement_speed, relative_movement_device)
 
         # Trigger haptic pulse on right touchpad, modulated by trigger close fraction
         # Close the trigger to create a stronger pulse

gibson2/objects/vr_objects.py

@@ -1,27 +1,29 @@
+import numpy as np
 import os
 import pybullet as p
 
 from gibson2 import assets_path
-from gibson2.objects.object_base import Object
 from gibson2.objects.articulated_object import ArticulatedObject
 from gibson2.utils.utils import multQuatLists
 from gibson2.utils.vr_utils import translate_vr_position_by_vecs
 
 
-class VrBody(Object):
+class VrBody(ArticulatedObject):
     """
     A simple cylinder representing a VR user's body. This stops
     them from moving through physical objects and wall, as well
     as other VR users.
     """
-    def __init__(self, height=1.2):
+    def __init__(self):
-        super(VrBody, self).__init__()
+        self.vr_body_fpath = os.path.join(assets_path, 'models', 'vr_body', 'vr_body.urdf')
-        # Height of VR body
+        super(VrBody, self).__init__(filename=self.vr_body_fpath, scale=1)
-        self.height = 0.6
+        # Height of VR body - this is relatively tall since we have disabled collision with the floor
+        # TODO: Fine tune this height variable!
+        self.height = 0.8
         # Distance between shoulders
         self.shoulder_width = 0.1
         # Width of body from front to back
-        self.body_width = 0.05
+        self.body_width = 0.01
         # This is the start that center of the body will float at
         # We give it 0.2m of room off the floor to avoid any collisions
         self.start_height = self.height/2 + 0.2
@@ -37,6 +39,9 @@ class VrBody(Object):
         self.start_y_rot = 0.0
         # Need this extra factor to amplify HMD movement vector, since body doesn't reach HMD each frame (since constraints don't set position)
         self.hmd_vec_amp = 2
+        # This is multiplication factor for backwards distance behind the HMD - this is the distance in m that the torso will be behind the HMD
+        # TODO: Change this back after experimenting 
+        self.back_disp_factor = 0.2
 
     # TIMELINE: Call this after loading the VR body into the simulator
     def init_body(self, start_pos):
@@ -44,20 +49,21 @@ class VrBody(Object):
         Initialize VR body to start in a specific location.
         Start pos should just contain an x and y value
         """
+        # TODO: Change this constraint to add rotation from the hmd!
         x, y = start_pos
         self.movement_cid = p.createConstraint(self.body_id, -1, -1, -1, p.JOINT_FIXED, 
                                             [0, 0, 0], [0, 0, 0], [x, y, self.start_height])
         self.start_rot = self.get_orientation()
 
-    def _load(self):
+    def rotate_offset_vec(self, offset_vec, theta):
-        # Use a box to represent the player body
+        """
-        col_cy = p.createCollisionShape(p.GEOM_BOX, halfExtents=[self.body_width, self.shoulder_width, self.height/2])
+        Rotate offset vector by an angle theta in the xy plane (z axis rotation). This offset vector has a z component of 0.
-        # Make body a translucent blue
+        """
-        vis_cy = p.createVisualShape(p.GEOM_BOX, halfExtents=[self.body_width, self.shoulder_width, self.height/2], rgbaColor=[0.65,0.65,0.65,1])
+        x = offset_vec[0]
-        body_id = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=col_cy, 
+        y = offset_vec[1]
-                                    baseVisualShapeIndex=vis_cy)
+        x_new = x * np.cos(theta) - y * np.sin(theta)
-
+        y_new = y * np.cos(theta) + x * np.sin(theta)
-        return body_id
+        return np.array([x_new, y_new, 0])
     
     def move_body(self, s, rTouchX, rTouchY, movement_speed, relative_device):
         """
@@ -68,12 +74,33 @@ class VrBody(Object):
         """
         # Calculate right and forward vectors relative to input device
         right, _, forward = s.get_device_coordinate_system(relative_device)
+        # Backwards HMD direction
+        back_dir = np.array(forward) * -1
+        # Project backwards direction onto horizontal plane to get body direction - just remove z component
+        back_dir[2] = 0.0
+        # Normalize back dir
+        back_dir = back_dir / np.linalg.norm(back_dir)
+        back_dir = back_dir * self.back_disp_factor
         
         # Get HMD data
         hmd_is_valid, hmd_trans, hmd_rot = s.get_data_for_vr_device('hmd')
         # Set the body to the HMD position on the first frame that it is valid, to aid calculation accuracy
         if self.first_frame and hmd_is_valid:
-            self.set_position(hmd_trans)
+            body_pos = hmd_trans + back_dir
+            # TODO: Need to do the rotation here as well
+            self.set_position(body_pos)
+
+            # Set collision filter between body and floor so we can bend down without any obstruction
+            # This is an alternative solution to scaling the body height as the player bends down
+            #self.floor_ids = s.get_floor_ids()
+            #for f_id in self.floor_ids:
+            #    p.setCollisionFilterPair(f_id, self.body_id, -1, -1, 0) # the last argument is 0 for disabling collision, 1 for enabling collision
+
+            #for obj_id in s.objects:
+            #    p.setCollisionFilterPair(obj_id, self.body_id, -1, -1, 0) # the last argument is 0 for disabling collision, 1 for enabling collision
+
+            # TODO: Disable collision with VR hands as well
+
             self.first_frame = False
 
         # First frame will not register HMD offset, since no previous hmd position has been recorded
@@ -81,31 +108,40 @@ class VrBody(Object):
                 self.prev_hmd_wp = s.get_hmd_world_pos()
 
         # Get offset to VR body
-        offset_to_body = self.get_position() - self.prev_hmd_wp
+        #    offset_to_body = self.get_position() - self.prev_hmd_wp - back_dir
         # Move the HMD to be aligned with the VR body
         # Set x and y coordinate offsets, but keep current system height (otherwise we teleport into the VR body)
-        s.set_vr_offset([offset_to_body[0], offset_to_body[1], s.get_vr_offset()[2]])
+        #    s.set_vr_offset([offset_to_body[0], offset_to_body[1], s.get_vr_offset()[2]])
             
         # Get current HMD world position and VR offset
         hmd_wp = s.get_hmd_world_pos()
-        curr_offset = s.get_vr_offset()
+        #  curr_offset = s.get_vr_offset()
         # Translate VR offset using controller information
-        translated_offset = translate_vr_position_by_vecs(rTouchX, rTouchY, right, forward, curr_offset, movement_speed)
+        #  translated_offset = translate_vr_position_by_vecs(rTouchX, rTouchY, right, forward, curr_offset, movement_speed)
         # New player position calculated - amplify delta in HMD positiion to account for constraints not moving body exactly to new position each frame
-        new_player_pos = (hmd_wp - self.prev_hmd_wp) * self.hmd_vec_amp + translated_offset + self.prev_hmd_wp
+        #    new_player_pos = (hmd_wp - self.prev_hmd_wp) * self.hmd_vec_amp + translated_offset + self.prev_hmd_wp + back_dir
+        new_body_pos = hmd_wp + back_dir
         # Attempt to set the vr body to this new position (will stop if collides with wall, for example)
         # This involves setting translation and rotation constraint
-        x, y, z = new_player_pos
+        x, y, z = new_body_pos
         new_center = z - self.dist_below_hmd - self.height/2
 
         # Extract only z rotation from HMD so we can spin the body on the vertical axis
-        _, _, curr_z = p.getEulerFromQuaternion(self.get_orientation())
+        _, _, old_body_z = p.getEulerFromQuaternion(self.get_orientation())
+        delta_hmd_z = 0
         if hmd_is_valid:
             _, _, hmd_z = p.getEulerFromQuaternion(hmd_rot)
-            curr_z = hmd_z
+            delta_hmd_z = hmd_z - old_body_z
 
         # Use starting x and y rotation so our body does not get knocked over when we collide with low objects
-        new_rot = p.getQuaternionFromEuler([self.start_x_rot, self.start_y_rot, curr_z])
+        new_rot = p.getQuaternionFromEuler([self.start_x_rot, self.start_y_rot, old_body_z + delta_hmd_z])
+        # Finally move the body based on the rotation - it pivots around the HMD in a circle whose circumference
+        # is defined by self.back_disp_factor. We can calculate this translation vector by drawing a vector triangle
+        # where the two radii are back_dir and the angle is delta_hmd_z. Some 2D trigonometry gets us the final result
+        self.rot_trans_vec = self.rotate_offset_vec(back_dir, -1 * delta_hmd_z) - back_dir
+        # Add translated vector to current offset value
+        x += self.rot_trans_vec[0]
+        y += self.rot_trans_vec[1]
         p.changeConstraint(self.movement_cid, [x, y, new_center], new_rot, maxForce=2000)
 
         # Update previous HMD world position at end of frame
@@ -174,25 +210,40 @@ class VrHand(ArticulatedObject):
         for jointIndex in range(p.getNumJoints(self.body_id)):
             # Make masses larger for greater stability
             # Mass is in kg, friction is coefficient
-            p.changeDynamics(self.body_id, jointIndex, mass=0.2, lateralFriction=3)
+            p.changeDynamics(self.body_id, jointIndex, mass=0.2, lateralFriction=4)
             open_pos = self.open_pos[jointIndex]
             p.resetJointState(self.body_id, jointIndex, open_pos)
             p.setJointMotorControl2(self.body_id, jointIndex, p.POSITION_CONTROL, targetPosition=open_pos, force=500)
-        # Keep base light for easier hand movement
+        p.changeDynamics(self.body_id, -1, mass=0.2, lateralFriction=2)
-        p.changeDynamics(self.body_id, -1, mass=0.05, lateralFriction=0.8)
         # Create constraint that can be used to move the hand
         self.movement_cid = p.createConstraint(self.body_id, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], start_pos)
 
+    # TODO: Get this working!
+    def set_hand_no_collision(self, no_col_id):
+        """
+        Sets VrHand to not collide with the body specified by no_col_id.
+        """
+        p.setCollisionFilterPair(self.body_id, no_col_id, -1, -1, 0)
+        hand_joint_num = p.getNumJoints(self.body_id)
+        no_col_joint_num = p.getNumJoints(no_col_id)
+        # Set all links to ignore collision, if no_col_id has joints
+        if no_col_joint_num == 0:
+            return
+
+        for i in range(hand_joint_num):
+            for j in range(no_col_joint_num):
+                p.setCollisionFilterPair(self.body_id, no_col_id, i, j, 0)
+
     # Close frac of 1 indicates fully closed joint, and close frac of 0 indicates fully open joint
     # Joints move smoothly between their values in self.open_pos and self.close_pos
-    def set_close_fraction(self, close_frac, maxForce=500):
+    def set_close_fraction(self, close_frac):
         for jointIndex in range(p.getNumJoints(self.body_id)):
             open_pos = self.open_pos[jointIndex]
             close_pos = self.close_pos[jointIndex]
             interp_frac = (close_pos - open_pos) * close_frac
             target_pos = open_pos + interp_frac
-            p.setJointMotorControl2(self.body_id, jointIndex, p.POSITION_CONTROL, targetPosition=target_pos, force=maxForce)
+            p.setJointMotorControl2(self.body_id, jointIndex, p.POSITION_CONTROL, targetPosition=target_pos, force=2000)
 
-    def move(self, trans, rot, maxForce=500):
+    def move(self, trans, rot):
         final_rot = multQuatLists(rot, self.base_rot)
-        p.changeConstraint(self.movement_cid, trans, final_rot, maxForce=maxForce)
+        p.changeConstraint(self.movement_cid, trans, final_rot, maxForce=2000)

gibson2/render/cpp/egl_mesh_renderer.cpp

@@ -280,6 +280,7 @@ PYBIND11_MODULE(EGLRendererContext, m) {
 	pymodule.def("updateUVData", &EGLRendererContext::updateUVData, "TBA");
     pymodule.def("updateDynamicData", &EGLRendererContext::updateDynamicData, "TBA");
     pymodule.def("renderOptimized", &EGLRendererContext::renderOptimized, "TBA");
+    pymodule.def("renderOptimized", &EGLRendererContext::renderOptimized, "TBA");
     pymodule.def("clean_meshrenderer_optimized", &EGLRendererContext::clean_meshrenderer_optimized, "TBA");
 
     //for skybox

gibson2/render/mesh_renderer/mesh_renderer_vr.py

@@ -34,8 +34,8 @@ class MeshRendererVR(MeshRenderer):
             self.P = left_proj
             # Set camera to be at the camera position of the VR eye
             self.camera = left_cam_pos
-            # Set camera once for both VR eyes
+            # Set camera once for both VR eyes - use the right eye since this is what we save in data save and replay
-            self.set_light_position_direction([self.camera[0], self.camera[1], 10], [self.camera[0], self.camera[1], 0])
+            self.set_light_position_direction([right_cam_pos[0], right_cam_pos[1], 10], [right_cam_pos[0], right_cam_pos[1], 0])
             
             super().render(modes=('rgb'), return_buffer=False, render_shadow_pass=True)
             self.vrsys.postRenderVRForEye("left", self.color_tex_rgb)

gibson2/robots/fetch_vr_robot.py

@@ -0,0 +1,126 @@
+import gym
+import numpy as np
+import pybullet as p
+
+from gibson2.external.pybullet_tools.utils import joints_from_names, set_joint_positions
+from gibson2.robots.robot_locomotor import LocomotorRobot
+
+
+class FetchVR(LocomotorRobot):
+    """
+    Fetch robot used in VR embodiment demos.
+    """
+    def __init__(self, config):
+        self.config = config
+        self.wheel_velocity = config.get('wheel_velocity', 1.0)
+        self.torso_lift_velocity = config.get('torso_lift_velocity', 1.0)
+        self.arm_velocity = config.get('arm_velocity', 1.0)
+        self.wheel_dim = 2
+        self.torso_lift_dim = 1
+        self.arm_dim = 7
+        LocomotorRobot.__init__(self,
+                                "fetch/fetch_vr.urdf",
+                                action_dim=self.wheel_dim + self.torso_lift_dim + self.arm_dim,
+                                scale=config.get("robot_scale", 1.0),
+                                is_discrete=config.get("is_discrete", False),
+                                control="velocity",
+                                self_collision=True)
+
+    def set_up_continuous_action_space(self):
+        self.action_high = np.array([self.wheel_velocity] * self.wheel_dim +
+                                    [self.torso_lift_velocity] * self.torso_lift_dim +
+                                    [self.arm_velocity] * self.arm_dim)
+        self.action_low = -self.action_high
+        self.action_space = gym.spaces.Box(shape=(self.action_dim,),
+                                           low=-1.0,
+                                           high=1.0,
+                                           dtype=np.float32)
+
+    def robot_specific_reset(self):
+        super(FetchVR, self).robot_specific_reset()
+
+        # roll the arm to its body
+        robot_id = self.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'
+                                       ])
+
+        rest_position = (0.02, np.pi / 2.0 - 0.4, np.pi / 2.0 - 0.1, -0.4, np.pi / 2.0 + 0.1, 0.0, np.pi / 2.0, 0.0)
+        # might be a better pose to initiate manipulation
+        # rest_position = (0.30322468280792236, -1.414019864768982,
+        #                  1.5178184935241699, 0.8189625336474915,
+        #                  2.200358942909668, 2.9631312579803466,
+        #                  -1.2862852996643066, 0.0008453550418615341)
+
+        set_joint_positions(robot_id, arm_joints, rest_position)
+
+    def get_end_effector_position(self):
+        return self.parts['gripper_link'].get_position()
+    
+    # Return body id of fetch robot
+    def get_fetch_body_id(self):
+        return self.robot_body.bodies[self.robot_body.body_index]
+
+    def set_z_rotation(self, hmd_rot):
+        """
+        Sets the z rotation of the fetch VR robot using the provided HMD rotation.
+        """
+        # Get z component of hmd rotation
+        _, _, hmd_z = p.getEulerFromQuaternion(hmd_rot)
+        prev_x, prev_y, _ = p.getEulerFromQuaternion(self.get_orientation())
+        # Preserve pre-existing x and y rotations, just force z rotation to be same as HMD
+        fetch_rot = p.getQuaternionFromEuler([prev_x, prev_y, hmd_z])
+        self.set_orientation(fetch_rot)
+
+    # Set open/close fraction of the end grippers
+    def set_fetch_gripper_fraction(self, frac, maxForce=500):
+        min_joint = 0.0
+        max_joint = 0.05
+        right_finger_joint_idx = 20
+        left_finger_joint_idx = 21
+        # TODO: Set more friction on grippers using p.changeDynamics?
+        #  min_joint + frac * (max_joint - min_joint)
+        target_pos = 0.05
+        p.setJointMotorControl2(self.get_fetch_body_id(),
+                                right_finger_joint_idx, 
+                                p.POSITION_CONTROL, 
+                                targetPosition=target_pos, 
+                                force=maxForce)
+        
+        p.setJointMotorControl2(self.get_fetch_body_id(),
+                                left_finger_joint_idx, 
+                                p.POSITION_CONTROL, 
+                                targetPosition=target_pos, 
+                                force=maxForce)
+
+    def get_end_effector_position(self):
+        return self.parts['gripper_link'].get_position()
+
+    def load(self):
+        ids = super(FetchVR, self).load()
+        robot_id = self.robot_ids[0]
+
+        # disable collision between torso_lift_joint and shoulder_lift_joint
+        #                   between torso_lift_joint and torso_fixed_joint
+        #                   between caster_wheel_joint and estop_joint
+        #                   between caster_wheel_joint and laser_joint
+        #                   between caster_wheel_joint and torso_fixed_joint
+        #                   between caster_wheel_joint and l_wheel_joint
+        #                   between caster_wheel_joint and r_wheel_joint
+        p.setCollisionFilterPair(robot_id, robot_id, 3, 13, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 3, 22, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 0, 20, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 0, 21, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 0, 22, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 0, 1, 0)
+        p.setCollisionFilterPair(robot_id, robot_id, 0, 2, 0)
+
+        return ids

gibson2/simulator.py

@@ -90,6 +90,8 @@ class Simulator:
         # renderer + VR
         self.vr_eye_tracking = vr_eye_tracking
         self.vr_mode = vr_mode
+        # Starting position for the VR (default set to None if no starting position is specified by the user)
+        self.vr_start_pos = None
         self.max_haptic_duration = 4000
         self.image_width = image_width
         self.image_height = image_height
@@ -100,6 +102,17 @@ class Simulator:
         self.optimized_renderer = rendering_settings.optimized
         self.rendering_settings = rendering_settings
         self.viewer = None
+
+        # Settings for adjusting physics and render timestep in vr
+        # Fraction to multiple previous render timestep by in low-pass filter
+        self.lp_filter_frac = 0.9
+
+        # Variables for data saving and replay in VR
+        self.last_physics_timestep = -1
+        self.last_render_timestep = -1
+        self.last_physics_step_num = -1
+        self.last_frame_dur = -1
+
         self.load()
 
         self.class_name_to_class_id = get_class_name_to_class_id()
@@ -116,6 +129,12 @@ class Simulator:
         self.render_timestep = render_timestep
         p.setTimeStep(self.physics_timestep)
 
+    def set_render_timestep(self, render_timestep):
+        """
+        :param render_timestep: render timestep to set in the Simulator
+        """
+        self.render_timestep = render_timestep
+
     def add_viewer(self):
         """
         Attach a debugging viewer to the renderer.
@@ -603,13 +622,44 @@ class Simulator:
             if instance.dynamic:
                 self.update_position(instance)
 
-    def step(self):
+    def step(self, print_time=False, use_render_timestep_lpf=True):
         """
         Step the simulation at self.render_timestep and update positions in renderer
         """
-        for _ in range(int(self.render_timestep / self.physics_timestep)):
+        physics_start_time = time.time()
+        physics_timestep_num = int(
+            self.render_timestep / self.physics_timestep)
+        for _ in range(physics_timestep_num):
             p.stepSimulation()
         self.sync()
+        physics_dur = time.time() - physics_start_time
+
+        render_start_time = time.time()
+        render_dur = time.time() - render_start_time
+        # Update render timestep using low-pass filter function
+        if use_render_timestep_lpf:
+            self.render_timestep = self.lp_filter_frac * \
+                self.render_timestep + (1 - self.lp_filter_frac) * render_dur
+        frame_dur = physics_dur + render_dur
+
+        # Set variables for data saving and replay
+        self.last_physics_timestep = physics_dur
+        self.last_render_timestep = render_dur
+        self.last_physics_step_num = physics_timestep_num
+        self.last_frame_dur = frame_dur
+
+        # Sets the VR starting position if one has been specified by the user
+        self.perform_vr_start_pos_move()
+
+        if print_time:
+            print('Total frame duration: {} and FPS: {}'.format(
+                round(frame_dur, 2), round(1/max(frame_dur, 0.002), 2)))
+            print('Total physics duration: {} and FPS: {}'.format(
+                round(physics_dur, 2), round(1/max(physics_dur, 0.002), 2)))
+            print('Number of 1/120 physics steps: {}'.format(physics_timestep_num))
+            print('Total render duration: {} and FPS: {}'.format(
+                round(render_dur, 2), round(1/max(render_dur, 0.002), 2)))
+            print('-------------------------')
 
     def sync(self):
         """
@@ -622,6 +672,22 @@ class Simulator:
         if (self.use_ig_renderer or self.use_vr_renderer or self.use_simple_viewer) and self.viewer is not None:
             self.viewer.update()
 
+    # Sets the VR position on the first step iteration where the hmd tracking is valid. Not to be confused
+    # with self.set_vr_start_pos, which simply records the desired start position before the simulator starts running.
+    def perform_vr_start_pos_move(self):
+        # Update VR start position if it is not None and the hmd is valid
+        # This will keep checking until we can successfully set the start position
+        if self.vr_start_pos:
+            hmd_is_valid, _, _, _ = self.renderer.vrsys.getDataForVRDevice(
+                'hmd')
+            if hmd_is_valid:
+                offset_to_start = np.array(
+                    self.vr_start_pos) - self.get_hmd_world_pos()
+                if self.vr_height_offset:
+                    offset_to_start[2] = self.vr_height_offset
+                self.set_vr_offset(offset_to_start)
+                self.vr_start_pos = None
+
     # Returns event data as list of lists. Each sub-list contains deviceType and eventType. List is empty is all
     # events are invalid.
     # deviceType: left_controller, right_controller
@@ -677,9 +743,24 @@ class Simulator:
         is_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = self.renderer.vrsys.getEyeTrackingData()
         return [is_valid, origin, dir, left_pupil_diameter, right_pupil_diameter]
 
+    # Sets the starting position of the VR system in iGibson space
+    def set_vr_start_pos(self, start_pos=None, vr_height_offset=None):
+        if not self.use_vr_renderer or not start_pos:
+            return
+
+        # The VR headset will actually be set to this position during the first frame.
+        # This is because we need to know where the headset is in space when it is first picked
+        # up to set the initial offset correctly.
+        self.vr_start_pos = start_pos
+        # This value can be set to specify a height offset instead of an absolute height.
+        # We might want to adjust the height of the camera based on the height of the person using VR,
+        # but still offset this height. When this option is non-zero it offsets the height by the amount
+        # specified instead of overwriting the VR system height output.
+        self.vr_height_offset = vr_height_offset
+
     # Sets the world position of the VR system in iGibson space
     def set_vr_pos(self, pos=None):
-        if not self.use_vr_renderer:
+        if not self.use_vr_renderer or not pos:
             return
 
         offset_to_pos = np.array(pos) - self.get_hmd_world_pos()
@@ -745,6 +826,17 @@ class Simulator:
                 self.renderer.update_hidden_state([instance])
                 return
 
+    def get_floor_ids(self):
+        """
+        Gets the body ids for all floor objects in the scene. This is used internally
+        by the VrBody class to disable collisions with the floor.
+        """
+        floor_ids = []
+        for body_id in self.objects:
+            if body_id in self.scene.objects_by_id.keys() and self.scene.objects_by_id[body_id].category == 'floors':
+                floor_ids.append(body_id)
+        return floor_ids
+
     @staticmethod
     def update_position(instance):
         """

gibson2/task/task_base.py

@@ -178,7 +178,7 @@ class iGTNTask(TaskNetTask):
         pass
 
 def main():
-    igtn_task = iGTNTask('kinematic_checker_testing')
+    igtn_task = iGTNTask('kinematic_checker_testing', 2)
     igtn_task.initialize_simulator()
 
     for i in range(500):

gibson2/utils/vr_logging.py

@@ -8,13 +8,8 @@ HDF5 hierarchy:
 
 ------ N x action_path (group) - these are paths introduced by the user and are of the form Y x group_name + dataset name
 
---- frame_data (group)
+--- frame_data (dataset)
-
+------ DATA: [frame_number, last_frame_physics_time, last_frame_render_time, last_frame_physics_step_num, last_frame_time] (len 5 x float)
------- frame_number (dataset)
---------- DATA: int
-
------- last_frame_time (dataset) - the time the last frame took to simulate and render
---------- DATA: float
 
 --- physics_data (group)
 
@@ -31,6 +26,8 @@ the computer's display when the VR is running
 ------------ DATA: 4x4 mat
 --------- right_eye_proj (dataset)
 ------------ DATA: 4x4 mat
+--------- right_camera_pos (dataset)
+------------ DATA: [x, y, z] (len 3)
 
 ------ vr_device_data (group)
 
@@ -91,8 +88,6 @@ class VRLogWriter():
         self.frame_counter = 0
         # Counts number of frames and does not reset
         self.persistent_frame_count = 0
-        # Time when last frame ended (not valid for first frame, set to current time to get a reasonable estimate)
-        self.last_frame_end_time = time.time()
         # Handle of HDF5 file
         self.hf = None
         # Name path data - used to extract data from data map and save to hd5
@@ -104,10 +99,7 @@ class VRLogWriter():
     def generate_name_path_data(self):
         """Generates lists of name paths for resolution in hd5 saving.
         Eg. ['vr', 'vr_camera', 'right_eye_view']."""
-        self.name_path_data.extend([
+        self.name_path_data.extend([['frame_data']])
-                ['frame_data', 'frame_number'],
-                ['frame_data', 'last_frame_duration'],
-        ])
 
         for n in self.pb_ids:
             self.name_path_data.append(['physics_data', 'body_id_{0}'.format(n)])
@@ -115,6 +107,7 @@ class VRLogWriter():
         self.name_path_data.extend([
                 ['vr', 'vr_camera', 'right_eye_view'],
                 ['vr', 'vr_camera', 'right_eye_proj'],
+                ['vr', 'vr_camera', 'right_camera_pos'],
                 ['vr', 'vr_device_data', 'hmd'],
                 ['vr', 'vr_device_data', 'left_controller'],
                 ['vr', 'vr_device_data', 'right_controller'],
@@ -128,9 +121,7 @@ class VRLogWriter():
         map is reset after every self.frames_before_write frames, by refresh_data_map."""
         self.data_map = dict()
         self.data_map['action'] = dict()
-        self.data_map['frame_data'] = dict()
+        self.data_map['frame_data'] = np.full((self.frames_before_write, 5), self.default_fill_sentinel)
-        self.data_map['frame_data']['frame_number'] = np.full((self.frames_before_write, 1), self.default_fill_sentinel)
-        self.data_map['frame_data']['last_frame_duration'] = np.full((self.frames_before_write, 1), self.default_fill_sentinel)
 
         self.data_map['physics_data'] = dict()
         for pb_id in self.pb_ids:
@@ -142,7 +133,8 @@ class VRLogWriter():
         self.data_map['vr'] = {
             'vr_camera': {
                 'right_eye_view': np.full((self.frames_before_write, 4, 4), self.default_fill_sentinel),
-                'right_eye_proj': np.full((self.frames_before_write, 4, 4), self.default_fill_sentinel)
+                'right_eye_proj': np.full((self.frames_before_write, 4, 4), self.default_fill_sentinel),
+                'right_camera_pos': np.full((self.frames_before_write, 3), self.default_fill_sentinel)
             }, 
             'vr_device_data': {
                 'hmd': np.full((self.frames_before_write, 8), self.default_fill_sentinel),
@@ -229,11 +221,21 @@ class VRLogWriter():
         act_data = self.get_data_for_name_path(full_action_path.split('/'))
         act_data[self.frame_counter, ...] = action
 
-    def write_frame_data_to_map(self):
+    def write_frame_data_to_map(self, s):
-        """Writes frame data to the data map."""
+        """Writes frame data to the data map.
-        self.data_map['frame_data']['frame_number'][self.frame_counter, ...] = self.persistent_frame_count
+        
-        self.data_map['frame_data']['last_frame_duration'][self.frame_counter, ...] = time.time() - self.last_frame_end_time
+        Args:
-        self.last_frame_end_time = time.time()
+            s (simulator): used to extract information about VR system
+        """
+        frame_data = np.array([
+            self.persistent_frame_count,
+            s.last_physics_timestep,
+            s.last_render_timestep,
+            s.last_physics_step_num,
+            s.last_frame_dur
+        ])
+        
+        self.data_map['frame_data'][self.frame_counter, ...] = frame_data[:]
 
     def write_vr_data_to_map(self, s):
         """Writes all VR data to map. This will write data
@@ -250,6 +252,7 @@ class VRLogWriter():
         # At end of each frame, renderer has camera information for VR right eye
         self.data_map['vr']['vr_camera']['right_eye_view'][self.frame_counter, ...] = s.renderer.V
         self.data_map['vr']['vr_camera']['right_eye_proj'][self.frame_counter, ...] = s.renderer.P
+        self.data_map['vr']['vr_camera']['right_camera_pos'][self.frame_counter, ...] = s.renderer.camera
 
         for device in ['hmd', 'left_controller', 'right_controller']:
             is_valid, trans, rot = s.get_data_for_vr_device(device)
@@ -292,7 +295,7 @@ class VRLogWriter():
         Args:
             s (simulator): used to extract information about VR system
         """
-        self.write_frame_data_to_map()
+        self.write_frame_data_to_map(s)
         self.write_vr_data_to_map(s)
         self.write_pybullet_data_to_map()
         self.frame_counter += 1
@@ -330,6 +333,7 @@ class VRLogWriter():
 
     def end_log_session(self):
         """Closes hdf5 log file at end of logging session."""
+        print('VR LOGGER INFO: Ending log writing session after {} frames'.format(self.persistent_frame_count))
         self.hf.close()
 
 class VRLogReader():
@@ -373,13 +377,17 @@ class VRLogReader():
         if self.frame_counter >= self.total_frame_num:
             return
 
-        # Get recorded frame duration for this frame
+        # Get all frame statistics for the most recent frame
-        frame_duration = self.hf['frame_data']['last_frame_duration'][self.frame_counter][0]
+        _, _, render_t, _, frame_duration = list(self.hf['frame_data'][self.frame_counter])
+        s.set_render_timestep(render_t)
 
         read_start_time = time.time()
         # Each frame we first set the camera data
         s.renderer.V = self.hf['vr/vr_camera/right_eye_view'][self.frame_counter]
         s.renderer.P = self.hf['vr/vr_camera/right_eye_proj'][self.frame_counter]
+        right_cam_pos = self.hf['vr/vr_camera/right_camera_pos'][self.frame_counter]
+        s.renderer.camera = right_cam_pos
+        s.renderer.set_light_position_direction([right_cam_pos[0], right_cam_pos[1], 10], [right_cam_pos[0], right_cam_pos[1], 0])
 
         if fullReplay:
             # If doing full replay we update the physics manually each frame