Add WebXR Locomotion (player movement) to tesseract_viewer_python (#78)

* Add webxr locomotion to tesseract_viewer_python

* Remove debug print statements

* Bump version

Author: johnwason

tesseract_viewer_python/examples/shapes_viewer_ssl_webxr_headset.py

@@ -0,0 +1,134 @@
+# SSL may be necessary for some systems that use WebXR. By default WebXR is not allowed to run on insecure origins.
+# The user will need to accept the security warning for a self signed certificate.
+# Also allow all incoming addresses to connect to the server by binding to 0.0.0.0 instead of localhost.
+# Firewalls may also need to be configured to allow incoming connections.
+# Point your VR headset to the https address of the computer running the server. For example
+# https://192.168.1.10:8000 if the server is running on port 8000 on the computer with IP address
+# 192.168.1.10. 
+#
+# To generate a self-signed certificate, run the following command (openssl must be installed):
+#
+# openssl req -newkey rsa:2048 -nodes -keyout key.pem -x509 -days 365 -out cert.pem
+#
+# When viewed on a VR headset web browser, click the "Enter VR" button to enter VR mode.
+
+from tesseract_robotics.tesseract_environment import Environment
+from tesseract_robotics.tesseract_common import ResourceLocator, SimpleLocatedResource
+import os
+import re
+import traceback
+from tesseract_robotics_viewer import TesseractViewer
+import numpy as np
+import time
+import sys
+import ssl
+
+shapes_urdf="""
+<robot name="multipleshapes">
+  
+  <link name="world"/>
+  <link name="cylinder_link">
+    <visual>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+      <material name="red">
+        <color rgba="0.8 0 0 1"/>
+      </material>
+    </visual>
+  </link>
+
+  <joint name="clyinder_joint" type="revolute">
+    <parent link="world"/>
+    <child link="cylinder_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-2.0944" upper="2.0944" velocity="6.2832"/>
+  </joint>
+
+  <link name="box_link">
+    <visual>
+      <geometry>
+        <box size="0.6 0.1 0.2"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 0.8 0 1"/>
+      </material>
+    </visual>
+  </link>
+
+  <joint name="box_joint" type="revolute">
+    <parent link="world"/>
+    <child link="box_link"/>
+    <origin xyz="1 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-2.0944" upper="2.0944" velocity="6.2832"/>
+  </joint>
+
+  <link name="sphere_link">
+    <visual>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 0.8 1"/>
+      </material>
+    </visual>
+  </link>
+
+  <joint name="sphere_joint" type="revolute">
+    <parent link="world"/>
+    <child link="sphere_link"/>
+    <origin xyz="2 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-2.0944" upper="2.0944" velocity="6.2832"/>
+  </joint>
+
+</robot>
+"""
+
+TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"]
+
+class TesseractSupportResourceLocator(ResourceLocator):
+    def __init__(self):
+        super().__init__()
+    
+    def locateResource(self, url):
+        try:
+            try:
+                if os.path.exists(url):
+                    return SimpleLocatedResource(url, url, self)
+            except:
+                pass
+            url_match = re.match(r"^package:\/\/tesseract_support\/(.*)$",url)
+            if (url_match is None):
+                print("url_match failed")
+                return None
+            if not "TESSERACT_SUPPORT_DIR" in os.environ:
+                return None
+            tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"]
+            filename = os.path.join(tesseract_support, os.path.normpath(url_match.group(1)))
+            ret = SimpleLocatedResource(url, filename, self)
+            return ret
+        except:
+            traceback.print_exc()
+
+
+t_env = Environment()
+
+# locator must be kept alive by maintaining a reference
+locator=TesseractSupportResourceLocator()
+t_env.init(shapes_urdf, locator)
+
+ssl_context = ssl.create_default_context(ssl.Purpose.CLIENT_AUTH)
+ssl_context.load_cert_chain(certfile='cert.pem', keyfile='key.pem')
+viewer = TesseractViewer(("0.0.0.0", 8000), ssl_context)
+
+viewer.update_environment(t_env, [0,0,0])
+
+viewer.start_serve_background()
+
+if sys.version_info[0] < 3:
+    raw_input("press enter")
+else:
+    input("press enter")
+

tesseract_viewer_python/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>tesseract_viewer_python</name>
-  <version>0.4.0</version>
+  <version>0.5.0</version>
   <description>The tesseract_viewer_python package</description>
   <maintainer email="wason@wasontech.com">John Wason</maintainer>
   <license>Apache 2.0</license>

tesseract_viewer_python/setup.py

@@ -14,7 +14,7 @@
       description='Tesseract Viewer Python Library',
       author='John Wason',
       author_email='wason@wasontech.com',
-      url='http://robotraconteur.com/',
+      url='https://github.com/tesseract-robotics/tesseract_python',
       packages=['tesseract_robotics_viewer','tesseract_robotics_viewer.resources'],
       package_data={'tesseract_robotics_viewer.resources':['static/index.html','static/app.js','geometries.json']},
 	  license='Apache-2.0',

tesseract_viewer_python/tesseract_robotics_viewer/resources/static/app.js

@@ -6,6 +6,7 @@ import { VRButton } from 'https://unpkg.com/three@0.153.0/examples/jsm/webxr/VRB
 import { LineMaterial } from 'https://unpkg.com/three@0.153.0/examples/jsm/lines/LineMaterial.js'
 import { Line2 } from 'https://unpkg.com/three@0.153.0/examples/jsm/lines/Line2.js'
 import { LineGeometry } from 'https://unpkg.com/three@0.153.0/examples/jsm/lines/LineGeometry.js'
+import { XRControllerModelFactory } from 'https://unpkg.com/three@0.153.0/examples/jsm/webxr/XRControllerModelFactory.js';
 import 'https://cdn.jsdelivr.net/npm/robust-websocket@1.0.0/robust-websocket.min.js';
 
 class TesseractViewer {
@@ -31,6 +32,17 @@ class TesseractViewer {
         this._update_trajectory_timer = null;
         this._update_markers_timer = null;
         this._update_scene_timer = null;
+        this._xr_dolly = null;
+        this._xr_controller1_grip = null;
+        this._xr_controller2_grip = null;
+        this._xr_controller1_model = null;
+        this._xr_controller2_model = null;
+        this._xr_gamepad1 = null;
+        this._xr_gamepad2 = null;
+        this._controls = null;
+        this._xr_drag_controller_start = null;
+        this._xr_drag_controller_orientation = null;
+        this._xr_drag_dolly_start = null;
 
     }
 
@@ -39,9 +51,7 @@ class TesseractViewer {
         this._clock = new THREE.Clock();
 
         const camera = new THREE.PerspectiveCamera( 45, window.innerWidth/window.innerHeight, 0.1, 1000 );
-        camera.position.x = 3;
-        camera.position.y = 3;
-        camera.position.z = -1.5;
+        camera.position.set(3, 3, -1.5)
         this._camera = camera;
 
         const renderer = new THREE.WebGLRenderer( { antialias: true } );
@@ -71,14 +81,20 @@ class TesseractViewer {
 
         document.body.appendChild( renderer.domElement );
 
-        const controls = new OrbitControls( camera, renderer.domElement );
+        this._controls = new OrbitControls( camera, renderer.domElement );
+
+        let _this = this;
 
         // Only add VR button if it is supported
         if ( 'xr' in navigator )
         {
             if (await navigator.xr.isSessionSupported( 'immersive-vr' ))
             {
                 document.body.appendChild( VRButton.createButton( renderer ) );
+
+                renderer.xr.addEventListener('sessionstart', function() {
+                    _this.enterXR();
+                });
             }
         }
 
@@ -101,7 +117,6 @@ class TesseractViewer {
 
         await this.updateScene();
 
-        let _this = this;
         const queryString = window.location.search;
         const urlParams = new URLSearchParams(queryString);
         let do_update = true;
@@ -119,7 +134,19 @@ class TesseractViewer {
 
     createWebSocket() {
         // Create a new WebSocket instance
-        const socket = new RobustWebSocket('ws://localhost:8000/websocket', null, {
+        
+        // Get host and port from current URL
+        const host = window.location.hostname;
+        const port = window.location.port;
+        let ws_protocol = "ws";
+
+        if (window.location.protocol === "https:") {
+            ws_protocol = "wss";
+        }
+
+        const ws_url = ws_protocol + "://" + host + ":" + port + "/websocket";
+
+        const socket = new RobustWebSocket(ws_url, null, {
             shouldReconnect: (event,ws) => { return 1000; }
         });
 
@@ -157,7 +184,9 @@ class TesseractViewer {
 
         var delta = this._clock.getDelta();
         if ( this._animation_mixer ) this._animation_mixer.update( delta );
-    };
+
+        this.xrLocomotion();
+    }
 
     async fetchIfModified(url, etag) {
         let fetch_res;
@@ -704,8 +733,147 @@ class TesseractViewer {
             tf.parent.remove(tf);
         });
     }
+
+    initXRControllers(dolly) {
+        const controller1 = this._renderer.xr.getController(0);
+        const controller2 = this._renderer.xr.getController(1);
+
+        dolly.add(controller1);
+        dolly.add(controller2);
+
+        const controllerModelFactory = new XRControllerModelFactory();
+
+        const controllerGrip1 = this._renderer.xr.getControllerGrip(0);
+        this._xr_controller1_grip = controllerGrip1;
+        this._xr_controller1_model = controllerModelFactory.createControllerModel(controllerGrip1);
+        controllerGrip1.add(this._xr_controller1_model);
+        dolly.add(controllerGrip1);
+
+        const controllerGrip2 = this._renderer.xr.getControllerGrip(1);
+        this._xr_controller2_grip = controllerGrip2;
+        this._xr_controller2_model = controllerModelFactory.createControllerModel(controllerGrip2);
+        controllerGrip2.add(this._xr_controller2_model);
+        dolly.add(controllerGrip2);
+
+        let _this = this;
+
+        controllerGrip1.addEventListener("connected", (e)=> {
+            _this._xr_gamepad1 = e.data.gamepad;
+        })
+
+        controllerGrip2.addEventListener("connected", (e)=> {
+            _this._xr_gamepad2 = e.data.gamepad;
+        })
+    }
+
+    enterXR() {
+        this._controls.saveState();
+        this._xr_dolly = new THREE.Object3D();
+        this.initXRControllers(this._xr_dolly)
+        this._xr_dolly.add(this._camera);
+        this._camera.position.set(0, 0, 0);
+        this._camera.rotation.set(0, 0, 0);
+        this._scene.add(this._xr_dolly);
+        this._xr_dolly.position.set(2.5,0,0);
+        this._xr_dolly.rotateY(Math.PI / 2.0);
+
+        this._renderer.xr.getSession().addEventListener('end', () => {
+            this._scene.add(this._camera);
+            this._scene.remove(this._xr_dolly);
+            this._xr_dolly.remove(this._camera);
+            this._xr_dolly = null;
+            this._xr_controller1_grip.remove(this._xr_controller1_model);
+            this._xr_controller2_grip.remove(this._xr_controller2_model);
+            this._xr_controller1_grip = null;
+            this._xr_controller2_grip = null;
+            this._xr_controller1_model = null;
+            this._xr_controller2_model = null;
+            this._controls.reset();
+            this._controls.update();
+        });
+
+
+    }
+    
+    xrLocomotion()
+    {
+        if (this._xr_dolly && this._xr_gamepad2) {
+            try
+            {
+            
+            if (this._xr_gamepad2.buttons[5].pressed)
+            {
+                if (!this._xr_drag_controller_start)
+                {
+                    this._xr_drag_controller_start = this._xr_controller2_grip.position.clone();
+                    let world_quat = new THREE.Quaternion();
+                    this._xr_drag_controller_orientation = this._camera.getWorldQuaternion(world_quat);
+                    
+                    this._xr_drag_dolly_start = this._xr_dolly.position.clone();
+                }
+
+                let controller_diff = this._xr_controller2_grip.position.clone().sub(this._xr_drag_controller_start);
+                controller_diff.applyQuaternion(this._xr_drag_controller_orientation.clone());
+                let y_diff = controller_diff.y;
+                y_diff = Math.floor(y_diff / 0.1);
+                if (y_diff < 0) {
+                    y_diff = y_diff + 1;
+                }
+                controller_diff.y = y_diff * 0.1;
+                let dolly_pos = this._xr_drag_dolly_start.clone().sub(controller_diff);
+                this._xr_dolly.position.copy(dolly_pos);
+            }
+            else
+            {
+                if (this._xr_drag_controller_start)
+                {
+                    this._xr_drag_controller_start = null;
+                    this._xr_drag_dolly_start = null;
+                }
+            }
+
+            }
+            catch (e)
+            {
+                console.log(e);
+            }
+
+            if (this._xr_gamepad2.axes.length == 4) {
+                let axis_2 = this._xr_gamepad2.axes[2];
+                if (axis_2 > 0.2)
+                {
+                    let scale = -(axis_2 - 0.2)/0.8;
+                    this._xr_dolly.rotateY(0.01 * scale);
+                }
+                if (axis_2 < -0.2)
+                {
+                    let scale = -(axis_2 + 0.2)/0.8;
+                    this._xr_dolly.rotateY(0.01 * scale);
+                }
+                
+                let axis_3 = this._xr_gamepad2.axes[3];
+                let dolly_world_quat = new THREE.Quaternion();
+                this._xr_dolly.getWorldQuaternion(dolly_world_quat);
+                let dolly_forward = new THREE.Vector3(0,0,-1);
+                dolly_forward.applyQuaternion(dolly_world_quat);
+
+                if (axis_3 > 0.2)
+                {
+                    let scale = -(axis_3 - 0.2)/0.8;
+                    this._xr_dolly.position.add(dolly_forward.clone().multiplyScalar(0.01 * scale));
+                }
+                if (axis_3 < -0.2)
+                {
+                    let scale = -(axis_3 + 0.2)/0.8;
+                    this._xr_dolly.position.add(dolly_forward.clone().multiplyScalar(0.01 * scale));
+                }
+            }
+        }
+    };
 }
 
+
+
 window.addEventListener("DOMContentLoaded", async function () {
     let viewer = new TesseractViewer();
     window.tesseract_viewer = viewer;