Merge pull request #100 from fxia22/vr_new

Vr new

README.md

@@ -132,19 +132,19 @@ $ pip install -e .
 
 Should end printing 'Successfully installed gibson2'
 
-You can find all the VR demos iGibson/examples/demo/vr_demos
+You can find all the VR demos in iGibson/examples/demo/vr_demos
 
 Run:
 
-$ python vr_demo_hand.py (for a scene with an interactive hand)
+$ python vr_playground_no_pbr (for a scene without PBR)
 
 or 
 
-$ python vr_demo_rs.py (for the current state-of-the-art Gibson graphics)
+$ python vr_playground_pbr (for the current state-of-the-art Gibson graphics)
 
-To see the features of the VR software.
-
-To use the gripper asset featured in the interaction demos, please download the 'gripper' folder at this link: https://drive.google.com/drive/folders/1-lHTtUuEgs9zzcievvvVdjHP0BdN7Du4?usp=sharing, and put it in assets/models (wherever your assets folder is).
+Data saving/replay code can be found in vr_demos/data_save_replay.
+Run vr_demo_save to save a demo to a log file, and vr_demo_replay to run it again. 
+Please see the demos and gibson2/utils/vr_logging.py for more details on the data saving/replay system.
 
 To use the VR hand asset, please download and unzip the asset and put it into assets/models under the folder name 'vr_hand'.
 Link to VR hand: https://drive.google.com/file/d/117qb1r_YHHVdQuwLD83N_nd0la57j9hZ/view?usp=sharing

examples/configs/locobot_interactive_demo.yaml

@@ -4,6 +4,7 @@ scene_id: Rs_int
 build_graph: true
 load_texture: true
 pybullet_load_texture: true
+trav_map_type: no_obj
 trav_map_resolution: 0.1
 trav_map_erosion: 2
 should_open_all_doors: true

examples/demo/vr_demos/vr_playground.py → examples/demo/vr_demos/vr_playground_no_pbr.py

@@ -1,9 +1,14 @@
 """ VR playground containing various objects and VR options that can be toggled
-to experiment with the VR experience in iGibson.
+to experiment with the VR experience in iGibson. This playground operates in
+the Placida scene, which does not use PBR.
 
-All VR functions can be found in the Simulator class.
-The Simulator makes use of a MeshRendererVR, which is based
-off of a VRRendererContext in render/cpp/vr_mesh_renderer.h"""
+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
@@ -12,11 +17,11 @@
 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 VrHand
+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 translate_vr_position_by_vecs
+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')
 
@@ -27,21 +32,28 @@
 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 movement
-movement_speed = 0.01
+# Movement speed for touchpad-based movement
+movement_speed = 0.02
 
-# Initialize simulator
 # 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=fullscreen, enable_pbr=True),
+            rendering_settings=MeshRendererSettings(optimized=optimize, fullscreen=fullscreen, enable_pbr=False),
             vrEyeTracking=use_eye_tracking, vrMode=vr_mode)
 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])
+
 # This playground only uses one hand - it has enough friction to pick up some of the
 # mustard bottles
 rHand = VrHand()
@@ -73,7 +85,7 @@
     s.optimize_vertex_and_texture()
 
 # Start user close to counter for interaction
-s.setVROffset([1.0, 0, -0.4])
+s.setVROffset([-2.0, 0.0, -0.4])
 
 # Main simulation loop
 while True:
@@ -93,7 +105,7 @@
             elif eventType == 'trigger_unpress':
                 pass
 
-    # Optionally print fps during simulator step
+    # Step the simulator - this needs to be done every frame to actually run the simulation
     s.step()
 
     # VR device data
@@ -112,16 +124,17 @@
         updated_marker_pos = [origin[0] + dir[0], origin[1] + dir[1], origin[2] + dir[2]]
         gaze_marker.set_position(updated_marker_pos)
 
-    # Get coordinate system for relative movement device
-    right, _, forward = s.getDeviceCoordinateSystem(relative_movement_device)
-
     if rIsValid:
         rHand.move(rTrans, rRot)
         rHand.set_close_fraction(rTrig)
 
-        # Right hand used to control movement
-        # Move VR system based on device coordinate system and touchpad press location
-        s.setVROffset(translate_vr_position_by_vecs(rTouchX, rTouchY, right, forward, s.getVROffset(), movement_speed))
+        if enable_vr_body:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, rTouchX, rTouchY, 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, rTouchX, rTouchY, 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

@@ -0,0 +1,102 @@
+""" 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
+
+from gibson2.render.mesh_renderer.mesh_renderer_cpu import MeshRendererSettings
+from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene
+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')
+
+# Playground configuration: edit this to change functionality
+optimize = True
+vr_mode = True
+# Toggles fullscreen companion window
+fullscreen = False
+# Toggles SRAnipal eye tracking
+use_eye_tracking = True
+# Enables the VR collision body
+enable_vr_body = False
+# 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.02
+
+# 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=fullscreen, enable_pbr=True, enable_shadow=False),
+            vrEyeTracking=use_eye_tracking, vrMode=vr_mode)
+scene = InteractiveIndoorScene('Beechwood_0_int')
+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])
+
+# This playground only uses one hand - it has enough friction to pick up some of the
+# mustard bottles
+rHand = VrHand()
+s.import_object(rHand)
+# This sets the hand constraints so it can move with the VR controller
+rHand.set_start_state(start_pos=[0.0, 0.5, 1.5])
+
+# Add playground objects to the scene
+# 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])
+
+if optimize:
+    s.optimize_vertex_and_texture()
+
+# Start user close to counter for interaction
+#s.setVROffset([-2.0, 0.0, -0.4])
+
+while True:
+    # Step the simulator - this needs to be done every frame to actually run the simulation
+    s.step()
+
+    rIsValid, rTrans, rRot = s.getDataForVRDevice('right_controller')
+    rTrig, rTouchX, rTouchY = s.getButtonDataForController('right_controller')
+
+    # VR eye tracking data
+    is_eye_data_valid, origin, dir, left_pupil_diameter, right_pupil_diameter = s.getEyeTrackingData()
+    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 rIsValid:
+        rHand.move(rTrans, rRot)
+        rHand.set_close_fraction(rTrig)
+
+        if enable_vr_body:
+            # See VrBody class for more details on this method
+            vr_body.move_body(s, rTouchX, rTouchY, 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, rTouchX, rTouchY, movement_speed, relative_movement_device)
+
+s.disconnect()

gibson2/envs/locomotor_env.py

@@ -503,14 +503,14 @@ def get_reward(self, collision_links=[], action=None, info={}):
             item for sublist in collision_links for item in sublist]
         reward = self.slack_reward  # |slack_reward| = 0.01 per step
 
-
         if self.reward_type in ['l2', 'geodesic']:
             if self.reward_type == 'l2':
                 new_potential = self.get_l2_potential()
             elif self.reward_type == 'geodesic':
                 new_potential = self.get_geodesic_potential()
             potential_reward = self.potential - new_potential
-            reward += potential_reward * self.potential_reward_weight  # |potential_reward| ~= 0.1 per step
+            # |potential_reward| ~= 0.1 per step
+            reward += potential_reward * self.potential_reward_weight
             self.potential = new_potential
 
         collision_reward = float(len(collision_links_flatten) > 0)
@@ -626,17 +626,25 @@ def reset_agent(self):
         """
         reset_success = False
         max_trials = 100
+
+        # move robot away from the scene
+        self.robots[0].set_position([0.0, 0.0, 100.0])
+        # cache pybullet state
+        state_id = p.saveState()
         for _ in range(max_trials):
             self.reset_initial_and_target_pos()
             if self.test_valid_position('robot', self.robots[0], self.initial_pos, self.initial_orn) and \
                     self.test_valid_position('robot', self.robots[0], self.target_pos):
                 reset_success = True
+                p.restoreState(state_id)
                 break
+            p.restoreState(state_id)
 
         if not reset_success:
             logging.warning("WARNING: Failed to reset robot without collision")
 
         self.land('robot', self.robots[0], self.initial_pos, self.initial_orn)
+        p.removeState(state_id)
 
     def reset_initial_and_target_pos(self):
         """

gibson2/objects/articulated_object.py

@@ -565,10 +565,10 @@ def _load(self):
             for j in range(p.getNumJoints(body_id)):
                 info = p.getJointInfo(body_id, j)
                 jointType = info[2]
-                if jointType == p.JOINT_REVOLUTE or jointType == p.JOINT_PRISMATIC:
+                if jointType in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
                     p.setJointMotorControl2(
-                        body_id, j, p.VELOCITY_CONTROL, force=self.joint_friction, targetVelocity=0)
-
+                        body_id, j, p.VELOCITY_CONTROL,
+                        targetVelocity=0.0, force=self.joint_friction)
             self.body_ids.append(body_id)
         return self.body_ids
 
@@ -585,3 +585,14 @@ def reset(self):
             pos, orn = p.multiplyTransforms(
                 pos, orn, inertial_pos, inertial_orn)
             p.resetBasePositionAndOrientation(body_id, pos, orn)
+
+            # reset joint position to 0.0
+            for j in range(p.getNumJoints(body_id)):
+                info = p.getJointInfo(body_id, j)
+                jointType = info[2]
+                if jointType in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
+                    p.resetJointState(
+                        body_id, j, targetValue=0.0, targetVelocity=0.0)
+                    p.setJointMotorControl2(
+                        body_id, j, p.VELOCITY_CONTROL,
+                        targetVelocity=0.0, force=self.joint_friction)