diff --git a/README.md b/README.md index af3e82f05..0dbb2b72f 100644 --- a/README.md +++ b/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 diff --git a/examples/configs/locobot_interactive_demo.yaml b/examples/configs/locobot_interactive_demo.yaml index 1b11448c7..628ccdfe4 100644 --- a/examples/configs/locobot_interactive_demo.yaml +++ b/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 diff --git a/examples/demo/pbr_test/pbr_test.py b/examples/demo/pbr_test/pbr_test.py deleted file mode 100644 index dc6888f65..000000000 --- a/examples/demo/pbr_test/pbr_test.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python -from gibson2.simulator import Simulator -from gibson2.scenes.igibson_indoor_scene import InteractiveIndoorScene -from gibson2.robots.turtlebot_robot import Turtlebot -from gibson2.utils.utils import parse_config -import os -import gibson2 -import time -import random -import matplotlib.pyplot as plt -from gibson2.utils.assets_utils import get_scene_path - -def benchmark_scene(scene_name, optimized=False): - config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml')) - random.seed(0) - scene = InteractiveIndoorScene(scene_name, texture_randomization=False) - s = Simulator(mode='iggui', - optimized_renderer=optimized, - env_texture_filename=os.path.join(gibson2.assets_path, 'hdr', 'photo_studio_01_2k.hdr'), - vrMode=False, - vrFullscreen=False) - s.import_ig_scene(scene) - turtlebot = Turtlebot(config) - s.import_robot(turtlebot) - s.renderer.use_pbr(use_pbr=False, use_pbr_mapping=False) - #s.renderer.use_pbr(use_pbr=True, use_pbr_mapping=True) - if optimized: - s.optimize_vertex_and_texture() - fps = [] - physics_fps = [] - render_fps = [] - for i in range(50000): - # if i % 100 == 0: - # scene.randomize_texture() - start = time.time() - s.step() - physics_end = time.time() - _ = s.renderer.render_robot_cameras(modes=('rgb')) - end = time.time() - #print("Elapsed time: ", end - start) - print("Render Frequency: ", 1 / (end - physics_end)) - print("Physics Frequency: ", 1 / (physics_end - start)) - print("Step Frequency: ", 1 / (end - start)) - fps.append(1 / (end - start)) - physics_fps.append(1 / (physics_end - start)) - render_fps.append(1 / (end - physics_end)) - s.disconnect() - plt.figure(figsize=(7,15)) - ax = plt.subplot(3, 1, 1) - plt.hist(render_fps) - ax.set_xlabel('Render fps') - ax.set_title('Scene {} version {}\noptimized {}'.format(scene_name, assets_version, optimized)) - ax = plt.subplot(3, 1, 2) - plt.hist(physics_fps) - ax.set_xlabel('Physics fps') - ax = plt.subplot(3, 1, 3) - plt.hist(fps) - ax.set_xlabel('Step fps') - plt.savefig('scene_benchmark_{}_o_{}.pdf'.format(scene_name, optimized)) - -def main(): - #benchmark_scene('Rs', True) - #benchmark_scene('Rs', False) - benchmark_scene('Rs', False) - #benchmark_scene('Wainscott_0', False) - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/examples/demo/vr_demos/vr_playground.py b/examples/demo/vr_demos/vr_playground_no_pbr.py similarity index 74% rename from examples/demo/vr_demos/vr_playground.py rename to examples/demo/vr_demos/vr_playground_no_pbr.py index b9f3180de..4106b9d6c 100644 --- a/examples/demo/vr_demos/vr_playground.py +++ b/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 diff --git a/examples/demo/vr_demos/vr_playground_pbr.py b/examples/demo/vr_demos/vr_playground_pbr.py new file mode 100644 index 000000000..99c4e8ffa --- /dev/null +++ b/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() \ No newline at end of file diff --git a/gibson2/envs/locomotor_env.py b/gibson2/envs/locomotor_env.py index e898aebfd..cbd750966 100644 --- a/gibson2/envs/locomotor_env.py +++ b/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): """ diff --git a/gibson2/objects/articulated_object.py b/gibson2/objects/articulated_object.py index c1ba60ae9..7d3abb732 100644 --- a/gibson2/objects/articulated_object.py +++ b/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) diff --git a/gibson2/objects/vr_objects.py b/gibson2/objects/vr_objects.py index a444e230b..28fc2639c 100644 --- a/gibson2/objects/vr_objects.py +++ b/gibson2/objects/vr_objects.py @@ -5,6 +5,104 @@ from gibson2.objects.articulated_object import ArticulatedObject from gibson2.objects.object_base import Object from gibson2.utils.utils import multQuatLists +from gibson2.utils.vr_utils import translate_vr_position_by_vecs + + +class VrBody(Object): + """ + 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): + super(VrBody, self).__init__() + self.height = 0.6 + self.radius = 0.1 + # 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 + # This is the distance of the top of the body below the HMD, so as to not obscure vision + self.dist_below_hmd = 0.4 + # Body needs to keep track of first frame so it can set itself to the player's + # coordinates on that first frame + self.first_frame = True + # Keep track of previous hmd world position for movement calculations + self.prev_hmd_wp = None + # Keep track of start x and y rotation so we can lock object to these values + self.start_x_rot = 0.0 + self.start_y_rot = 0.0 + + # TIMELINE: Call this after loading the VR body into the simulator + def init_body(self, start_pos): + """ + Initialize VR body to start in a specific location. + Start pos should just contain an x and y value + """ + 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): + # Use a box to represent the player body + col_cy = p.createCollisionShape(p.GEOM_BOX, halfExtents=[self.radius, self.radius, self.height/2]) + # Make body a translucent blue + vis_cy = p.createVisualShape(p.GEOM_BOX, halfExtents=[self.radius, self.radius, self.height/2], rgbaColor=[0,0,0.8,1]) + body_id = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=col_cy, + baseVisualShapeIndex=vis_cy) + + return body_id + + def move_body(self, s, rTouchX, rTouchY, movement_speed, relative_device): + """ + Moves VrBody to new position, via constraints. Takes in the simulator, s, so that + it can obtain the VR data needed to perform the movement calculation. Also takes + in right touchpad information, movement speed and the device relative to which movement + is calculated. + """ + # Calculate right and forward vectors relative to input device + right, _, forward = s.getDeviceCoordinateSystem(relative_device) + + # Get HMD data + hmdIsValid, hmdTrans, hmdRot = s.getDataForVRDevice('hmd') + if self.first_frame and hmdIsValid: + self.set_position(hmdTrans) + self.first_frame = False + + # First frame will not register HMD offset, since no previous hmd position has been recorded + if self.prev_hmd_wp is None: + self.prev_hmd_wp = s.getHmdWorldPos() + + # Get offset to VR body + offset_to_body = self.get_position() - self.prev_hmd_wp + # 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.setVROffset([offset_to_body[0], offset_to_body[1], s.getVROffset()[2]]) + + # Get current HMD world position and VR offset + hmd_wp = s.getHmdWorldPos() + curr_offset = s.getVROffset() + # Translate VR offset using controller information + translated_offset = translate_vr_position_by_vecs(rTouchX, rTouchY, right, forward, curr_offset, movement_speed) + # New player position calculated + new_player_pos = hmd_wp + translated_offset + # 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 + 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()) + if hmdIsValid: + _, _, hmd_z = p.getEulerFromQuaternion(hmdRot) + curr_z = hmd_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]) + p.changeConstraint(self.movement_cid, [x, y, new_center], new_rot, maxForce=500) + + # Update previous HMD world position at end of frame + self.prev_hmd_wp = hmd_wp class VrHand(ArticulatedObject): diff --git a/gibson2/render/cpp/glfw_mesh_renderer.cpp b/gibson2/render/cpp/glfw_mesh_renderer.cpp index fed7e33fc..5307438fe 100644 --- a/gibson2/render/cpp/glfw_mesh_renderer.cpp +++ b/gibson2/render/cpp/glfw_mesh_renderer.cpp @@ -33,7 +33,7 @@ namespace py = pybind11; -int GLFWRendererContext::init(bool render_window, bool fullscreen) { +int GLFWRendererContext::init() { verbosity = 20; // Initialize GLFW context and window @@ -51,12 +51,12 @@ int GLFWRendererContext::init(bool render_window, bool fullscreen) { glfwWindowHint(GLFW_SAMPLES, 0); // Hide GLFW window if user requests - if (!render_window) { + if (!m_render_window) { glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE); } - if (fullscreen) { + if (m_fullscreen) { this->window = glfwCreateWindow(m_windowWidth, m_windowHeight, "Gibson Renderer Output", glfwGetPrimaryMonitor(), NULL); } else { @@ -107,7 +107,7 @@ PYBIND11_MODULE(GLFWRendererContext, m) { py::class_ pymodule = py::class_(m, "GLFWRendererContext"); - pymodule.def(py::init()); + pymodule.def(py::init()); pymodule.def("init", &GLFWRendererContext::init); pymodule.def("release", &GLFWRendererContext::release); pymodule.def("render_companion_window_from_buffer", &GLFWRendererContext::render_companion_window_from_buffer); diff --git a/gibson2/render/cpp/glfw_mesh_renderer.h b/gibson2/render/cpp/glfw_mesh_renderer.h index c8bfb1870..3fbf4b0dd 100644 --- a/gibson2/render/cpp/glfw_mesh_renderer.h +++ b/gibson2/render/cpp/glfw_mesh_renderer.h @@ -10,14 +10,20 @@ class GLFWRendererContext: public MeshRendererContext { public: int m_glVersionMajor, m_glVersionMinor; - GLFWRendererContext(int w, int h, int glVersionMajor, int glVersionMinor): MeshRendererContext(w, h) { + bool m_render_window; + bool m_fullscreen; + + GLFWRendererContext(int w, int h, int glVersionMajor, int glVersionMinor, + bool render_window = false, bool fullscreen = false): MeshRendererContext(w, h) { m_glVersionMajor = glVersionMajor; m_glVersionMinor = glVersionMinor; + m_render_window = render_window; + m_fullscreen = fullscreen; }; GLFWwindow *window = NULL; // By default don't render window and don't use fullscreen - int init(bool render_window = false, bool fullscreen = false); + int init(); void release(); void render_companion_window_from_buffer(GLuint readBuffer); }; diff --git a/gibson2/render/cpp/vr_mesh_renderer.cpp b/gibson2/render/cpp/vr_mesh_renderer.cpp index 7f1105626..71ba0ab77 100644 --- a/gibson2/render/cpp/vr_mesh_renderer.cpp +++ b/gibson2/render/cpp/vr_mesh_renderer.cpp @@ -702,7 +702,7 @@ PYBIND11_MODULE(VRRendererContext, m) { py::class_ pymodule = py::class_(m, "VRRendererContext"); - pymodule.def(py::init()); + pymodule.def(py::init()); pymodule.def("init", &VRRendererContext::init); pymodule.def("release", &VRRendererContext::release); pymodule.def("render_companion_window_from_buffer", &VRRendererContext::render_companion_window_from_buffer); diff --git a/gibson2/render/cpp/vr_mesh_renderer.h b/gibson2/render/cpp/vr_mesh_renderer.h index cd6d27f53..fef106ffb 100644 --- a/gibson2/render/cpp/vr_mesh_renderer.h +++ b/gibson2/render/cpp/vr_mesh_renderer.h @@ -90,7 +90,7 @@ class VRRendererContext : public GLFWRendererContext { EyeTrackingData eyeTrackingData; - VRRendererContext(int w, int h, int glVersionMajor, int glVersionMinor) : GLFWRendererContext(w, h, glVersionMajor, glVersionMinor), m_pHMD(NULL), nearClip(0.1f), farClip(30.0f) {}; + VRRendererContext(int w, int h, int glVersionMajor, int glVersionMinor, bool render_window = false, bool fullscreen = false) : GLFWRendererContext(w, h, glVersionMajor, glVersionMinor, render_window, fullscreen), m_pHMD(NULL), nearClip(0.1f), farClip(30.0f) {}; py::list getButtonDataForController(char* controllerType); diff --git a/gibson2/render/mesh_renderer/mesh_renderer_cpu.py b/gibson2/render/mesh_renderer/mesh_renderer_cpu.py index b28fe2deb..676e0f15b 100644 --- a/gibson2/render/mesh_renderer/mesh_renderer_cpu.py +++ b/gibson2/render/mesh_renderer/mesh_renderer_cpu.py @@ -660,23 +660,25 @@ def __init__(self, width=512, height=512, vertical_fov=90, device_idx=0, renderi from gibson2.render.mesh_renderer import GLFWRendererContext self.r = GLFWRendererContext.GLFWRendererContext(width, height, int(self.rendering_settings.glfw_gl_version[0]), - int(self.rendering_settings.glfw_gl_version[1]) + int(self.rendering_settings.glfw_gl_version[1]), + False, + rendering_settings.fullscreen ) elif self.platform == 'Windows': from gibson2.render.mesh_renderer import VRRendererContext self.r = VRRendererContext.VRRendererContext(width, height, int(self.rendering_settings.glfw_gl_version[0]), - int(self.rendering_settings.glfw_gl_version[1]) + int(self.rendering_settings.glfw_gl_version[1]), + True, + rendering_settings.fullscreen ) else: from gibson2.render.mesh_renderer import EGLRendererContext self.r = EGLRendererContext.EGLRendererContext( width, height, device) - if self.platform == 'Windows': - self.r.init(True, rendering_settings.fullscreen) - else: - self.r.init(False, rendering_settings.fullscreen) + + self.r.init() self.glstring = self.r.getstring_meshrenderer() diff --git a/gibson2/scenes/indoor_scene.py b/gibson2/scenes/indoor_scene.py index 5250d6943..4b49c14ac 100644 --- a/gibson2/scenes/indoor_scene.py +++ b/gibson2/scenes/indoor_scene.py @@ -50,7 +50,7 @@ def __init__(self, self.trav_map_original_size = None self.trav_map_size = None self.trav_map_erosion = trav_map_erosion - self.trav_map_type=trav_map_type + self.trav_map_type = trav_map_type self.build_graph = build_graph self.num_waypoints = num_waypoints self.waypoint_interval = int(waypoint_resolution / trav_map_resolution) diff --git a/gibson2/utils/vr_utils.py b/gibson2/utils/vr_utils.py index 4b180e3ba..6bb18640c 100644 --- a/gibson2/utils/vr_utils.py +++ b/gibson2/utils/vr_utils.py @@ -1,6 +1,17 @@ +"""This module contains vr utility functions.""" + import numpy as np from gibson2.utils.utils import normalizeListVec +def move_player_no_body(s, rTouchX, rTouchY, movement_speed, relative_device): + """Moves the VR player when they are not using a VR body. Takes in the simulator, + information from the right touchpad, player movement speed and the device relative to which + we would like to move.""" + curr_offset = s.getVROffset() + right, _, forward = s.getDeviceCoordinateSystem(relative_device) + new_offset = translate_vr_position_by_vecs(rTouchX, rTouchY, right, forward, curr_offset, movement_speed) + s.setVROffset(new_offset) + def get_normalized_translation_vec(right_frac, forward_frac, right, forward): """Generates a normalized translation vector that is a linear combination of forward and right.""" vr_offset_vec = [right[i] * right_frac + forward[i] * forward_frac for i in range(3)]