diff --git a/tesseract_viewer_python/cmake/setup.py.in b/tesseract_viewer_python/cmake/setup.py.in index 5a101caa4..6a0b24328 100644 --- a/tesseract_viewer_python/cmake/setup.py.in +++ b/tesseract_viewer_python/cmake/setup.py.in @@ -9,7 +9,7 @@ setup(name='tesseract_robotics_viewer', author_email='wason@wasontech.com', url='http://robotraconteur.com/', packages=['tesseract_robotics_viewer','tesseract_robotics_viewer.resources'], - package_data={'tesseract_robotics_viewer.resources':['static/index.html','static/tesseract_viewer.js','geometries.json']}, + package_data={'tesseract_robotics_viewer.resources':['static/index.html','static/app.js','geometries.json']}, license='Apache-2.0', install_requires=['numpy','tesseract_robotics'], long_description='Tesseract viewer package for Python' diff --git a/tesseract_viewer_python/package.xml b/tesseract_viewer_python/package.xml index 22ba64c2d..9a1e25d7b 100644 --- a/tesseract_viewer_python/package.xml +++ b/tesseract_viewer_python/package.xml @@ -1,7 +1,7 @@ tesseract_viewer_python - 0.1.0 + 0.2.0 The tesseract_viewer_python package John Wason Apache 2.0 diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/app.js b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/app.js new file mode 100644 index 000000000..0995c11f8 --- /dev/null +++ b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/app.js @@ -0,0 +1,322 @@ +import * as THREE from 'https://unpkg.com/three@0.127.0/build/three.module.js'; + +import { OrbitControls } from 'https://unpkg.com/three@0.127.0/examples/jsm/controls/OrbitControls.js'; +import { GLTFLoader } from 'https://unpkg.com/three@0.127.0/examples/jsm/loaders/GLTFLoader.js' +import { VRButton } from 'https://unpkg.com/three@0.127.0/examples/jsm/webxr/VRButton.js' + +class TesseractViewer { + + constructor() + { + this._scene = null; + this._clock = null; + this._camera = null; + this._renderer = null; + this._light = null; + this._scene_etag = null; + this._trajectory_etag = null; + this._disable_update_trajectory = false; + this._animation_mixer = null; + this._animation = null; + this._animation_action = null; + this._root_z_up = null; + this._root_env = null; + + } + + async createScene() { + this._scene = new THREE.Scene(); + 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; + this._camera = camera; + + const renderer = new THREE.WebGLRenderer( { antialias: true } ); + renderer.setPixelRatio( window.devicePixelRatio ); + renderer.setSize( window.innerWidth, window.innerHeight ); + renderer.outputEncoding = THREE.sRGBEncoding; + renderer.xr.enabled = true; + + renderer.setClearColor("#000000"); + + this._renderer = renderer; + + + window.addEventListener( 'resize', onWindowResize, false ); + + function onWindowResize(){ + + camera.aspect = window.innerWidth / window.innerHeight; + camera.updateProjectionMatrix(); + + renderer.setSize( window.innerWidth, window.innerHeight ); + } + + const light = new THREE.HemisphereLight( 0xffffbb, 0x202018, 1 ); + this._scene.add( light ); + this._light = light; + + document.body.appendChild( renderer.domElement ); + + const controls = new OrbitControls( camera, renderer.domElement ); + + document.body.appendChild( VRButton.createButton( renderer ) ); + + renderer.setAnimationLoop( this.render.bind(this) ); + + const gridHelper = new THREE.GridHelper( 10, 10 ); + this._scene.add( gridHelper ); + + const root_z_up = new THREE.Object3D(); + root_z_up.rotateX(-Math.PI / 2.0); + this._scene.add(root_z_up); + + const root_env = new THREE.Object3D(); + root_z_up.add(root_env); + + this._root_z_up = root_z_up; + this._root_env = root_env; + + this._animation_mixer = new THREE.AnimationMixer( this._root_env ); + + await this.updateScene(); + + let _this = this; + const queryString = window.location.search; + const urlParams = new URLSearchParams(queryString); + let do_update = true; + if (urlParams.has("noupdate")) { + if (urlParams.get("noupdate") === "true") { + do_update = false; + } + } + if (do_update) { + setTimeout(() => _this.updateTrajectory(), 2000); + } + + } + + render() { + // Render the scene + this._renderer.render(this._scene, this._camera); + + var delta = this._clock.getDelta(); + if ( this._animation_mixer ) this._animation_mixer.update( delta ); + }; + + async updateScene() { + let fetch_res; + try { + fetch_res = await fetch("tesseract_scene.gltf", { method: "HEAD" }); + } + catch (_a) { + let _this = this; + setTimeout(() => _this.updateScene(), 1000); + return; + } + let etag = fetch_res.headers.get('etag'); + if (etag !== null) { + if (this._scene_etag !== null) { + if (this._scene_etag != etag) { + this._scene_etag = null; + let _this = this; + setTimeout(() => _this.updateScene(), 0); + return; + } + else { + let _this = this; + setTimeout(() => _this.updateScene(), 1000); + return; + } + } + } + const loader = new GLTFLoader(); + + let gltf = await new Promise((resolve, reject) => { + loader.load('tesseract_scene.gltf', data=> resolve(data), null, reject); + }); + + if (this._root_env) + { + for( var i = this._root_env.children.length - 1; i >= 0; i--) { + let obj = this._root_env.children[i]; + this._root_env.remove(obj); + } + } + + this._root_env.add(gltf.scene); + + if (gltf.animations.length > 0) + { + + this._animation_mixer.stopAllAction(); + this._animation_mixer.uncacheRoot(this._root_env); + + let animation_action = this._animation_mixer.clipAction(gltf.animations[0]); + animation_action.play(); + + this._animation = gltf.animations[0]; + this._animation_action = animation_action; + } + + if (etag !== null) { + this._scene_etag = etag; + let _this = this; + setTimeout(() => _this.updateScene(), 1000); + } + } + + async updateTrajectory() { + + if (this._disable_update_trajectory) { + return; + } + let fetch_res; + let _this = this; + try { + fetch_res = await fetch("tesseract_trajectory.json", { method: "HEAD" }); + } + catch (_a) { + setTimeout(() => _this.updateTrajectory(), 1000); + return; + } + if (!fetch_res.ok) { + setTimeout(() => _this.updateTrajectory(), 1000); + return; + } + let etag = fetch_res.headers.get('etag'); + if (etag == null || this._trajectory_etag == etag) { + console.log("No updated trajectory"); + setTimeout(() => _this.updateTrajectory(), 1000); + return; + } + try { + let trajectory_response = await fetch("./tesseract_trajectory.json"); + let trajectory_json = await trajectory_response.json(); + console.log(trajectory_json) + this.setTrajectory(trajectory_json.joint_names, trajectory_json.trajectory); + } + catch (e) { + console.log("Trajectory not available"); + console.log(e); + } + if (etag !== null) { + this._trajectory_etag = etag; + setTimeout(() => _this.updateTrajectory(), 1000); + } + + } + disableUpdateTrajectory() { + this._disable_update_trajectory = true; + } + enableUpdateTrajectory() { + this._disable_update_trajectory = false; + } + setJointPositions(joint_names, joint_positions) { + let trajectory = [[...joint_positions, 0], [...joint_positions, 100000]]; + this.setTrajectory(joint_names, trajectory); + } + + setTrajectory(joint_names, trajectory) { + + this._animation_mixer.stopAllAction(); + this._animation_mixer.uncacheRoot(this._root_env); + + let anim = this.trajectoryToAnimation(joint_names, trajectory); + let animation_action = this._animation_mixer.clipAction(anim); + animation_action.play(); + + this._animation = anim; + this._animation_action = animation_action; + } + + trajectoryToAnimation(joint_names, trajectory) { + let joints = this.findJoints(joint_names); + let tracks = [] + joint_names.forEach((joint_name, joint_index) => { + let joint = joints[joint_name]; + switch (joint.type) { + case 1: + { + let values = []; + let times = [] + trajectory.forEach(ee => { + let axis_vec = new THREE.Vector3().fromArray(joint.axes); + let quat = new THREE.Quaternion().setFromAxisAngle(axis_vec, ee[joint_index]); + let quat_array = quat.toArray(); + values.push(...quat_array); + times.push(ee[ee.length - 1]) + }); + let track = new THREE.QuaternionKeyframeTrack(joint.joint.name + ".quaternion", times, values); + tracks.push(track); + } + break; + case 2: + { + let values = []; + let times = [] + trajectory.forEach(ee => { + let axis_vec = new THREE.Vector3().fromArray(joint.axes); + let vec = axis_vec.multiplyScalar(ee[joint_index]); + let vec_array = vec.toArray(); + values.push(...vec_array); + times.push(ee[ee.length - 1]) + }); + let track = new THREE.VectorKeyframeTrack(joint.joint.name + ".position", times, values); + tracks.push(track); + } + break; + default: + throw new Error("Unknown joint type"); + } + }); + + let animation_clip = new THREE.AnimationClip("tesseract_trajectory", -1, tracks); + + return animation_clip; + } + + findJoints(joint_names) + { + let ret = {} + this._root_env.traverse(tf => { + if (tf.userData && tf.userData["tesseract_joint"]) + { + let t_j = tf.userData["tesseract_joint"]; + + if (joint_names && joint_names.indexOf(t_j["name"]) == -1) { + return; + } + let t = {}; + t.joint_name = t_j["name"]; + t.node_name = tf.name; + t.joint = tf; + t.axes = t_j.axis; + t.type = t_j.type; + ret[t.joint_name] = t; + } + }); + return ret; + } +} + +window.addEventListener("DOMContentLoaded", async function () { + let viewer = new TesseractViewer(); + window.tesseract_viewer = viewer; + await viewer.createScene(); + window.addEventListener("message", function (event) { + let data = event.data; + if (data.command === "joint_positions") { + viewer.disableUpdateTrajectory(); + viewer.setJointPositions(data.joint_names, data.joint_positions); + } + if (data.command === "joint_trajectory") { + viewer.disableUpdateTrajectory(); + viewer.setTrajectory(data.joint_names, data.joint_trajectory); + } + }); + viewer.render(); +}) \ No newline at end of file diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/index.html b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/index.html index 455f0acb1..c698d9860 100644 --- a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/index.html +++ b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/index.html @@ -1,39 +1,19 @@ - - - - - Tesseract Viewer - - - - - - - - - - - - - //touch-action="none" for best results from PEP - - - - - + + + + Tesseract Viewer + + + + + + + + + + \ No newline at end of file diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.js b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.js deleted file mode 100644 index f55ace40a..000000000 --- a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.js +++ /dev/null @@ -1,407 +0,0 @@ -// Copyright 2019 Wason Technology, LLC -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: John Wason (wason@wasontech.com) -// Date: 12/10/2019 -var __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) { - function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); } - return new (P || (P = Promise))(function (resolve, reject) { - function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } } - function rejected(value) { try { step(generator["throw"](value)); } catch (e) { reject(e); } } - function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); } - step((generator = generator.apply(thisArg, _arguments || [])).next()); - }); -}; -class TesseractViewer { - constructor(canvasElement) { - this._scene_etag = null; - this._trajectory_etag = null; - this._disable_update_trajectory = false; - // Create canvas and engine. - this._canvas = document.getElementById(canvasElement); - this._engine = new BABYLON.Engine(this._canvas, true); - } - createScene() { - return __awaiter(this, void 0, void 0, function* () { - // Create the scene space - this._scene = new BABYLON.Scene(this._engine); - //scene.clearColor = new BABYLON.Color3(.4, .4, .4); - this._scene.useRightHandedSystem = true; - this._environment = this._scene.createDefaultEnvironment({ enableGroundShadow: true, groundYBias: 0 }); - this._environment.setMainColor(BABYLON.Color3.FromHexString("#74b9ff")); - // Add a camera to the scene and attach it to the canvas - this._camera = new BABYLON.ArcRotateCamera("Camera", Math.PI / 2, Math.PI / 2, 2, new BABYLON.Vector3(0, 1, 0), this._scene); - this._camera.attachControl(this._canvas, true); - this._camera.setPosition(new BABYLON.Vector3(2.5, 1.5, -1)); - // Add lights to the scene - this._light = new BABYLON.HemisphericLight("light1", new BABYLON.Vector3(0, -1, 0), this._scene); - this._light.intensity = 0.5; - this._root = new BABYLON.TransformNode("root0"); - this._root.rotation.x = -1.5707963267948966; - yield this.updateScene(); - console.log("Loaded!"); - this._scene.transformNodes.forEach(function (tf) { - //this._addAxis(tf, 0.5); - //console.log(tf) - }); - //console.log(this._scene.transformNodes); - this._scene.meshes.forEach(function (m) { - try { - m.createNormals(true); - } - catch (e) { - console.log(e); - } - //m.parent=root; - //addAxis(tf, 0.5); - }); - yield this.enableVR(); - let _this = this; - const queryString = window.location.search; - const urlParams = new URLSearchParams(queryString); - let do_update = true; - if (urlParams.has("noupdate")) { - if (urlParams.get("noupdate") === "true") { - do_update = false; - } - } - if (do_update) { - setTimeout(() => _this.updateTrajectory(), 2000); - } - //this._scene.debugLayer.show(); - }); - } - doRender() { - // Run the render loop. - this._engine.runRenderLoop(() => { - this._scene.render(); - }); - // The canvas/window resize event handler. - window.addEventListener('resize', () => { - this._engine.resize(); - }); - } - addAxis(parent, size) { - var axisX = BABYLON.Mesh.CreateLines("axisX", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(size, 0, 0), new BABYLON.Vector3(size * 0.95, 0.05 * size, 0), - new BABYLON.Vector3(size, 0, 0), new BABYLON.Vector3(size * 0.95, -0.05 * size, 0) - ], this._scene); - axisX.color = new BABYLON.Color3(1, 0, 0); - axisX.parent = parent; - //var xChar = makeTextPlane("X", "red", size / 10); - //xChar.position = new BABYLON.Vector3(0.9 * size, -0.05 * size, 0); - var axisY = BABYLON.Mesh.CreateLines("axisY", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, size, 0), new BABYLON.Vector3(-0.05 * size, size * 0.95, 0), - new BABYLON.Vector3(0, size, 0), new BABYLON.Vector3(0.05 * size, size * 0.95, 0) - ], this._scene); - axisY.color = new BABYLON.Color3(0, 1, 0); - axisY.parent = parent; - //var yChar = makeTextPlane("Y", "green", size / 10); - //yChar.position = new BABYLON.Vector3(0, 0.9 * size, -0.05 * size); - var axisZ = BABYLON.Mesh.CreateLines("axisZ", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, 0, size), new BABYLON.Vector3(0, -0.05 * size, size * 0.95), - new BABYLON.Vector3(0, 0, size), new BABYLON.Vector3(0, 0.05 * size, size * 0.95) - ], this._scene); - axisZ.color = new BABYLON.Color3(0, 0, 1); - axisZ.parent = parent; - //var zChar = makeTextPlane("Z", "blue", size / 10); - //zChar.position = new BABYLON.Vector3(0, 0.05 * size, 0.9 * size); - } - enableVR() { - return __awaiter(this, void 0, void 0, function* () { - // Enable VR - var ground = BABYLON.MeshBuilder.CreateGround("ground", { width: 10, height: 10 }, this._scene); - ground.material = new BABYLON.GridMaterial("mat", this._scene); - if (navigator.xr !== undefined) { - const xrHelper = yield this._scene.createDefaultXRExperienceAsync({ - // define floor meshes - floorMeshes: [ground] - }); - } - ground.visibility = 0.1; - //vrHelper.enableTeleportation({floorMeshes: [environment.ground]}); - }); - } - updateScene() { - return __awaiter(this, void 0, void 0, function* () { - let fetch_res; - try { - fetch_res = yield fetch("tesseract_scene.babylon", { method: "HEAD" }); - } - catch (_a) { - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - return; - } - let etag = fetch_res.headers.get('etag'); - if (etag !== null) { - if (this._scene_etag !== null) { - if (this._scene_etag != etag) { - location.reload(); - return; - } - else { - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - return; - } - } - } - yield BABYLON.SceneLoader.AppendAsync("./", "tesseract_scene.babylon", this._scene); - if (etag !== null) { - this._scene_etag = etag; - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - } - }); - } - updateTrajectory() { - return __awaiter(this, void 0, void 0, function* () { - if (this._disable_update_trajectory) { - return; - } - let fetch_res; - let _this = this; - try { - fetch_res = yield fetch("tesseract_trajectory.json", { method: "HEAD" }); - } - catch (_a) { - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - if (!fetch_res.ok) { - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - let etag = fetch_res.headers.get('etag'); - if (etag == null || this._trajectory_etag == etag) { - console.log("No updated trajectory"); - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - try { - if (this._joint_trajectory !== null) { - this._joint_trajectory.stop(); - this._joint_trajectory = null; - } - } - catch (_b) { } - try { - let trajectory_response = yield fetch("./tesseract_trajectory.json"); - let trajectory_json = yield trajectory_response.json(); - this._joint_trajectory = JointTrajectoryAnimation.Parse(trajectory_json, this._scene); - this._joint_trajectory.start(); - } - catch (e) { - console.log("Trajectory not available"); - console.log(e); - } - if (etag !== null) { - this._trajectory_etag = etag; - setTimeout(() => _this.updateTrajectory(), 1000); - } - }); - } - disableUpdateTrajectory() { - this._disable_update_trajectory = true; - } - enableUpdateTrajectory() { - this._disable_update_trajectory = false; - } - setJointPositions(joint_names, joint_positions) { - let trajectory = [[...joint_positions, 0], [...joint_positions, 100000]]; - this.setTrajectory(joint_names, trajectory); - } - /* - trajectory format: - [ - [0.1,0.2,0.3,0.4,0.5,0.6, 0] // waypoint 1, last element is time - [0.11,0.21,0.31,0.41,0.51,0.61, 1] // waypoint 2, last element is time - ... // more waypoints - ] - Position is in radians or meters, time is in seconds - */ - setTrajectory(joint_names, trajectory) { - try { - if (this._joint_trajectory !== null) { - this._joint_trajectory.stop(); - this._joint_trajectory = null; - } - } - catch (_a) { } - this._joint_trajectory = new JointTrajectoryAnimation(this._scene, joint_names, trajectory, true, 0); - this._joint_trajectory.start(); - } -} -class JointTrajectoryAnimation { - constructor(scene, joint_names, trajectory, use_time, loop_time) { - this._timerid = 0; - if (joint_names.length == 0) { - throw new Error("joint_names must not be zero count"); - } - if (trajectory.length == 0) { - throw new Error("trajectory must not be zero count"); - } - this._max_time = -1; - trajectory.forEach((t) => { - if (use_time) { - if (t.length - 1 != joint_names.length) { - throw new Error("Trajectory waypoints must have same count as joint_names"); - } - let waypoint_time = t.slice(-1)[0]; - if (this._max_time >= waypoint_time) { - throw new Error("Trajectory waypoint time must me monotonically increasing"); - } - this._max_time = waypoint_time; - } - else { - if (t.length != joint_names.length) { - throw new Error("Trajectory waypoints must have same count as joint_names"); - } - } - }); - this._joint_names = joint_names; - this._trajectory = trajectory; - this._use_time = use_time; - this._loop_time = loop_time; - this._scene = scene; - this.findJoints(); - } - findJoints() { - let joints = new Map(); - let axes = new Map(); - let type = new Map(); - this._joint_names.forEach((joint_name) => { - let tf = this._scene.getTransformNodeByName("joint_" + joint_name); - let metadata = tf.metadata; - if (metadata.hasOwnProperty("tesseract_joint") - && metadata.tesseract_joint.hasOwnProperty("axis")) { - joints.set(joint_name, tf); - let axis_array = tf.metadata.tesseract_joint.axis; - axes.set(joint_name, new BABYLON.Vector3(axis_array[0], axis_array[1], axis_array[2])); - type.set(joint_name, tf.metadata.tesseract_joint.type); - } - }); - this._joints = joints; - this._joint_axes = axes; - this._joint_type = type; - } - resetJointPos() { - this._joints.forEach((tf) => { - tf.position = new BABYLON.Vector3(0, 0, 0); - tf.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1); - }); - } - getMaxTime() { - if (this._use_time) { - return this._max_time; - } - else { - return this._loop_time; - } - } - setTrajectoryTime(t) { - let joint_n = this._joint_names.length; - let n = this._trajectory.length; - let times = []; - for (let i = 0; i < n; i++) { - if (this._use_time) { - times.push(this._trajectory[i][joint_n]); - } - else { - times.push(i * (this._loop_time / n)); - } - } - let joint_pos = null; - for (let i = 0; i < n - 1; i++) { - if (times[i] == t) { - joint_pos = this._trajectory[i].slice(0, joint_n); - break; - } - if (times[i] < t) { - let joint_pos1 = this._trajectory[i].slice(0, joint_n); - let joint_pos2 = this._trajectory[i + 1].slice(0, joint_n); - let t1 = times[i]; - let t2 = times[i + 1]; - joint_pos = []; - for (let j = 0; j < joint_n; j++) { - joint_pos.push(joint_pos1[j] + ((joint_pos2[j] - joint_pos1[j]) / (t2 - t1)) * (t - t1)); - } - } - } - if (joint_pos === null) { - joint_pos = this._trajectory.slice(-1)[0].slice(0, joint_n); - } - for (let i = 0; i < joint_n; i++) { - let joint_name = this._joint_names[i]; - let joint = this._joints.get(joint_name); - let axes = this._joint_axes.get(joint_name); - let type = this._joint_type.get(joint_name); - if (type == 2) { - joint.position = axes.scale(joint_pos[i]); - } - else { - joint.rotationQuaternion = new BABYLON.Quaternion(0, 0, 0, 1); - joint.rotate(axes, joint_pos[i], BABYLON.Space.LOCAL); - } - } - } - start() { - if (this._timerid != 0) { - return; - } - this._t0 = new Date().getTime() / 1000; - var _this = this; - this._timerid = setInterval(() => _this.intervalCallback(), 50); - } - stop() { - if (this._timerid == 0) { - return; - } - clearInterval(this._timerid); - this._timerid = 0; - } - intervalCallback() { - let max_t = this.getMaxTime(); - let t_total = new Date().getTime() / 1000 - this._t0; - let t = t_total % max_t; - this.setTrajectoryTime(t); - } - static Parse(parsedTrajectory, scene) { - let trajectory = new JointTrajectoryAnimation(scene, parsedTrajectory.joint_names, parsedTrajectory.trajectory, parsedTrajectory.use_time, parsedTrajectory.loop_time); - return trajectory; - } -} -window.addEventListener('DOMContentLoaded', function () { - return __awaiter(this, void 0, void 0, function* () { - // Create the game using the 'renderCanvas'. - let viewer = new TesseractViewer('renderCanvas'); - window.tesseract_viewer = viewer; - // Create the scene. - yield viewer.createScene(); - window.addEventListener("message", function (event) { - let data = event.data; - if (data.command === "joint_positions") { - viewer.disableUpdateTrajectory(); - viewer.setJointPositions(data.joint_names, data.joint_positions); - } - if (data.command === "joint_trajectory") { - viewer.disableUpdateTrajectory(); - viewer.setTrajectory(data.joint_names, data.joint_trajectory); - } - }); - // Start render loop. - viewer.doRender(); - }); -}); diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.ts b/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.ts deleted file mode 100644 index a1d84af2e..000000000 --- a/tesseract_viewer_python/tesseract_robotics_viewer/resources/static/tesseract_viewer.ts +++ /dev/null @@ -1,552 +0,0 @@ -// Copyright 2019 Wason Technology, LLC -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: John Wason (wason@wasontech.com) -// Date: 12/10/2019 - -/// -/// - -// tsc tesseract_viewer.ts --lib es6,DOM -m es2015 -target es6 - -import { _BabylonLoaderRegistered, SceneComponentConstants, FreeCameraTouchInput } from "babylonjs"; - -class TesseractViewer { - - private _canvas: HTMLCanvasElement; - private _engine: BABYLON.Engine; - private _scene: BABYLON.Scene; - private _camera: BABYLON.ArcRotateCamera; - private _light: BABYLON.Light; - private _environment: BABYLON.EnvironmentHelper; - private _root: BABYLON.TransformNode; - private _joint_trajectory: JointTrajectoryAnimation; - private _scene_etag = null; - private _trajectory_etag= null; - private _disable_update_trajectory = false; - - constructor(canvasElement : string) { - // Create canvas and engine. - this._canvas = document.getElementById(canvasElement) as HTMLCanvasElement; - this._engine = new BABYLON.Engine(this._canvas, true); - } - - async createScene() : Promise { - // Create the scene space - this._scene = new BABYLON.Scene(this._engine); - //scene.clearColor = new BABYLON.Color3(.4, .4, .4); - this._scene.useRightHandedSystem=true; - - this._environment = this._scene.createDefaultEnvironment({ enableGroundShadow: true, groundYBias: 0 }); - this._environment.setMainColor(BABYLON.Color3.FromHexString("#74b9ff")); - - // Add a camera to the scene and attach it to the canvas - this._camera = new BABYLON.ArcRotateCamera("Camera", Math.PI / 2, Math.PI / 2, 2, new BABYLON.Vector3(0,1,0), this._scene); - this._camera.attachControl(this._canvas, true); - this._camera.setPosition(new BABYLON.Vector3(2.5, 1.5, -1)); - - // Add lights to the scene - this._light = new BABYLON.HemisphericLight("light1", new BABYLON.Vector3(0, -1, 0), this._scene); - this._light.intensity = 0.5 - - this._root = new BABYLON.TransformNode("root0"); - - this._root.rotation.x = -1.5707963267948966; - - await this.updateScene(); - - console.log("Loaded!"); - this._scene.transformNodes.forEach(function (tf) - { - //this._addAxis(tf, 0.5); - //console.log(tf) - }); - //console.log(this._scene.transformNodes); - this._scene.meshes.forEach(function (m) - { - try - { - m.createNormals(true); - } - catch (e) - { - console.log(e) - } - //m.parent=root; - //addAxis(tf, 0.5); - }); - - - await this.enableVR(); - let _this = this; - - const queryString = window.location.search; - const urlParams = new URLSearchParams(queryString); - let do_update = true; - if (urlParams.has("noupdate")) - { - if (urlParams.get("noupdate") === "true") - { - do_update = false; - } - } - - if (do_update) - { - setTimeout(() => _this.updateTrajectory(),2000); - } - - //this._scene.debugLayer.show(); - - } - - doRender() : void { - // Run the render loop. - this._engine.runRenderLoop(() => { - this._scene.render(); - }); - - // The canvas/window resize event handler. - window.addEventListener('resize', () => { - this._engine.resize(); - }); - } - - addAxis(parent: BABYLON.Node, size: number) : void { - var axisX = BABYLON.Mesh.CreateLines("axisX", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(size, 0, 0), new BABYLON.Vector3(size * 0.95, 0.05 * size, 0), - new BABYLON.Vector3(size, 0, 0), new BABYLON.Vector3(size * 0.95, -0.05 * size, 0) - ], this._scene); - axisX.color = new BABYLON.Color3(1, 0, 0); - axisX.parent = parent; - //var xChar = makeTextPlane("X", "red", size / 10); - //xChar.position = new BABYLON.Vector3(0.9 * size, -0.05 * size, 0); - var axisY = BABYLON.Mesh.CreateLines("axisY", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, size, 0), new BABYLON.Vector3(-0.05 * size, size * 0.95, 0), - new BABYLON.Vector3(0, size, 0), new BABYLON.Vector3(0.05 * size, size * 0.95, 0) - ], this._scene); - axisY.color = new BABYLON.Color3(0, 1, 0); - axisY.parent = parent; - //var yChar = makeTextPlane("Y", "green", size / 10); - //yChar.position = new BABYLON.Vector3(0, 0.9 * size, -0.05 * size); - var axisZ = BABYLON.Mesh.CreateLines("axisZ", [ - BABYLON.Vector3.Zero(), new BABYLON.Vector3(0, 0, size), new BABYLON.Vector3(0, -0.05 * size, size * 0.95), - new BABYLON.Vector3(0, 0, size), new BABYLON.Vector3(0, 0.05 * size, size * 0.95) - ], this._scene); - axisZ.color = new BABYLON.Color3(0, 0, 1); - axisZ.parent = parent; - //var zChar = makeTextPlane("Z", "blue", size / 10); - //zChar.position = new BABYLON.Vector3(0, 0.05 * size, 0.9 * size); - } - - async enableVR(): Promise - { - // Enable VR - var ground = BABYLON.MeshBuilder.CreateGround("ground", { width: 10, height: 10 }, this._scene); - ground.material = new BABYLON.GridMaterial("mat", this._scene); - if ((navigator as any).xr !== undefined) - { - const xrHelper = await this._scene.createDefaultXRExperienceAsync({ - // define floor meshes - floorMeshes: [ground] - }); - } - ground.visibility = 0.1; - //vrHelper.enableTeleportation({floorMeshes: [environment.ground]}); - - } - - async updateScene(): Promise - { - let fetch_res: Response; - try - { - fetch_res = await fetch("tesseract_scene.babylon", {method: "HEAD"}); - } - catch - { - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - return; - } - - let etag = fetch_res.headers.get('etag'); - if (etag !== null) - { - if (this._scene_etag !== null) - { - if (this._scene_etag != etag) - { - location.reload(); - return; - } - else - { - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - return; - } - } - } - - await BABYLON.SceneLoader.AppendAsync("./", "tesseract_scene.babylon", this._scene); - if (etag !== null) - { - this._scene_etag = etag; - let _this = this; - setTimeout(() => _this.updateScene(), 1000); - } - } - - async updateTrajectory(): Promise - { - if (this._disable_update_trajectory) - { - return; - } - let fetch_res: Response; - let _this = this; - try - { - fetch_res = await fetch("tesseract_trajectory.json", {method: "HEAD"}); - } - catch - { - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - - if (!fetch_res.ok) - { - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - let etag = fetch_res.headers.get('etag'); - if (etag == null || this._trajectory_etag == etag) - { - console.log("No updated trajectory"); - setTimeout(() => _this.updateTrajectory(), 1000); - return; - } - - try - { - if (this._joint_trajectory !== null) - { - this._joint_trajectory.stop(); - this._joint_trajectory = null; - } - } - catch {} - - try - { - let trajectory_response = await fetch("./tesseract_trajectory.json"); - let trajectory_json = await trajectory_response.json(); - this._joint_trajectory = JointTrajectoryAnimation.Parse(trajectory_json, this._scene); - this._joint_trajectory.start(); - } - catch (e) - { - console.log("Trajectory not available"); - console.log(e); - } - - if (etag !== null) - { - this._trajectory_etag = etag; - setTimeout(() => _this.updateTrajectory(), 1000); - } - } - - public disableUpdateTrajectory() : void - { - this._disable_update_trajectory = true; - } - - public enableUpdateTrajectory() : void - { - this._disable_update_trajectory = false; - } - - public setJointPositions(joint_names : string[], joint_positions: number[]) - { - let trajectory = [[...joint_positions,0],[...joint_positions,100000]]; - this.setTrajectory(joint_names, trajectory); - } - - /* - trajectory format: - [ - [0.1,0.2,0.3,0.4,0.5,0.6, 0] // waypoint 1, last element is time - [0.11,0.21,0.31,0.41,0.51,0.61, 1] // waypoint 2, last element is time - ... // more waypoints - ] - Position is in radians or meters, time is in seconds - */ - public setTrajectory(joint_names: string[], trajectory: number[][]) - { - try - { - if (this._joint_trajectory !== null) - { - this._joint_trajectory.stop(); - this._joint_trajectory = null; - } - } - catch {} - this._joint_trajectory = new JointTrajectoryAnimation(this._scene, joint_names, trajectory, true, 0); - this._joint_trajectory.start(); - } - -} - -class JointTrajectoryAnimation -{ - private _joint_names : string[]; - private _use_time: boolean; - private _loop_time: number; - private _trajectory: number[][]; - - private _scene: BABYLON.Scene; - private _joints: Map; - private _joint_axes: Map; - private _joint_type: Map; - - private _max_time: number; - private _t0: number; - private _timerid = 0; - - public constructor(scene: BABYLON.Scene, joint_names: string[], - trajectory: number[][], use_time: boolean, loop_time: number) - { - if (joint_names.length == 0) - { - throw new Error("joint_names must not be zero count"); - } - - if (trajectory.length == 0) - { - throw new Error("trajectory must not be zero count"); - } - - this._max_time = -1; - trajectory.forEach( (t) => - { - if (use_time) - { - if (t.length-1 != joint_names.length) - { - throw new Error("Trajectory waypoints must have same count as joint_names") - } - let waypoint_time = t.slice(-1)[0]; - if (this._max_time >= waypoint_time) - { - throw new Error("Trajectory waypoint time must me monotonically increasing"); - } - this._max_time = waypoint_time; - } - else - { - if (t.length != joint_names.length) - { - throw new Error("Trajectory waypoints must have same count as joint_names") - } - } - }); - - this._joint_names = joint_names; - this._trajectory = trajectory; - this._use_time = use_time; - this._loop_time = loop_time; - this._scene = scene; - this.findJoints(); - } - - private findJoints() : void - { - let joints = new Map(); - let axes = new Map(); - let type = new Map(); - - this._joint_names.forEach((joint_name) => { - let tf = this._scene.getTransformNodeByName("joint_" + joint_name); - let metadata = tf.metadata; - if (metadata.hasOwnProperty("tesseract_joint") - && metadata.tesseract_joint.hasOwnProperty("axis") ) - { - joints.set(joint_name, tf); - let axis_array = tf.metadata.tesseract_joint.axis; - axes.set(joint_name, new BABYLON.Vector3(axis_array[0], axis_array[1], axis_array[2])); - type.set(joint_name, tf.metadata.tesseract_joint.type); - } - }); - - this._joints = joints; - this._joint_axes = axes; - this._joint_type = type; - } - - public resetJointPos() : void - { - this._joints.forEach((tf) => { - tf.position = new BABYLON.Vector3(0,0,0); - tf.rotationQuaternion = new BABYLON.Quaternion(0,0,0,1); - }); - } - - public getMaxTime() : number - { - if (this._use_time) - { - return this._max_time; - } - else - { - return this._loop_time; - } - } - - public setTrajectoryTime(t: number) : void - { - let joint_n = this._joint_names.length; - let n = this._trajectory.length; - - let times = []; - for (let i=0; i _this.intervalCallback(),50); - } - - public stop(): void - { - if (this._timerid == 0) - { - return; - } - clearInterval(this._timerid); - this._timerid = 0; - } - - private intervalCallback(): void - { - let max_t = this.getMaxTime(); - let t_total = new Date().getTime()/1000 - this._t0; - let t = t_total % max_t; - this.setTrajectoryTime(t) - - } - - public static Parse(parsedTrajectory: any, scene: BABYLON.Scene) - { - let trajectory = new JointTrajectoryAnimation(scene, - parsedTrajectory.joint_names, parsedTrajectory.trajectory, - parsedTrajectory.use_time, parsedTrajectory.loop_time); - - return trajectory; - } -} - -window.addEventListener('DOMContentLoaded', async function() { - // Create the game using the 'renderCanvas'. - let viewer = new TesseractViewer('renderCanvas'); - - (window as any).tesseract_viewer = viewer; - - // Create the scene. - await viewer.createScene(); - - window.addEventListener("message", function(event: Event) - { - let data = (event as MessageEvent).data; - if (data.command === "joint_positions") - { - viewer.disableUpdateTrajectory(); - viewer.setJointPositions(data.joint_names, data.joint_positions) - } - if (data.command === "joint_trajectory") - { - viewer.disableUpdateTrajectory(); - viewer.setTrajectory(data.joint_names, data.joint_trajectory) - } - }); - - // Start render loop. - viewer.doRender(); - }); \ No newline at end of file diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_babylon_json.py b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_babylon_json.py deleted file mode 100644 index f6814680d..000000000 --- a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_babylon_json.py +++ /dev/null @@ -1,259 +0,0 @@ -# Copyright 2019 Wason Technology, LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# Author: John Wason (wason@wasontech.com) -# Date: 12/10/2019 - -import json -import numpy as np -import math -from tesseract_robotics import tesseract_geometry -from tesseract_robotics.tesseract_common import Quaterniond -import pkgutil -import re -import base64 - -def tesseract_env_to_babylon_json(t_env, origin_offset=[0,0,0]): - return json.dumps(tesseract_env_to_babylon_json_dict(t_env,origin_offset)) - -def tesseract_env_to_babylon_json_dict(t_env, origin_offset=[0,0,0]): - - assert len(list(origin_offset)) == 3 - - link_names = t_env.getLinkNames() - joint_names = t_env.getJointNames() - - link_map = dict() - joint_map = dict() - for l in link_names: - link_map[l] = t_env.getLink(l) - - for j in joint_names: - joint_map[j] = t_env.getJoint(j) - - root_link_name = t_env.getRootLinkName() - - transform_nodes, meshes, materials = _process_link_recursive(link_map, joint_map, root_link_name, None) - - transform_nodes.append({"name": "root", "id": "root", "isVisible": "true", "isEnabled": "true", "position": list(origin_offset), "parentId": "root0"}) - - geometries = json.loads(pkgutil.get_data("tesseract_robotics_viewer.resources","geometries.json"))["geometries"] - - babylon_dict = {"geometries": geometries, "transformNodes": transform_nodes, "meshes": meshes, "materials": materials} - return babylon_dict - - -def _find_child_joints(joint_map, parent_link_name): - return [j for j in joint_map.values() if j.parent_link_name == parent_link_name] - -def _np_transform_to_babylon(eigen_tf): - p = eigen_tf.translation().tolist() - q0 = Quaterniond(eigen_tf.rotation()) - q = [q0.x(), q0.y(), q0.z(), q0.w()] - return p,q - -def _process_link_recursive(link_map, joint_map, link_name, parent_joint_name): - transform_nodes = [] - meshes = [] - materials = [] - link = link_map[link_name] - - tf_link = {"name": "link_" + link_name, "id": "link_" + link_name, "isVisible": "true", "isEnabled": "true"} - if (parent_joint_name is not None): - tf_link["parentId"] = "joint_" + parent_joint_name - else: - tf_link["parentId"] = "root" - transform_nodes.append(tf_link) - - visual_i = 0 - - for visual in link.visual: - visual_i += 1 - visual_u_name = visual.name + str(visual_i) - visual_name = "link_" + link_name + "_visual_" + visual_u_name - np_tf_visual = visual.origin - visual_p, visual_q = _np_transform_to_babylon(np_tf_visual) - tf_visual = {"name": visual_name, "isVisible": "true", "isEnabled": "true", - "parentId": "link_" + link_name, "materialId": "material_" + visual_name} - tf_visual["position"] = visual_p - tf_visual["rotationQuaternion"] = visual_q - tf_visual["billboardMode"] = 0 - - tf_material = None - - visual_geom = visual.geometry - if (isinstance(visual_geom,tesseract_geometry.PolygonMesh)): - - mesh=visual_geom - vertices = mesh.getVertices() - positions = np.zeros((len(vertices)*3,),dtype=np.float64) - for i in range(len(vertices)): - positions[i*3:(i*3+3)] = vertices[i].flatten() - - triangles = mesh.getFaces().flatten() - triangle_count = int(len(triangles)/4) - indices = [0]*(triangle_count*3) - for i in range(triangle_count): - assert triangles[i*4]==3, "Only triangle meshes supported" - indices[i*3] = int(triangles[i*4+3]) - indices[i*3+1] = int(triangles[i*4+2]) - indices[i*3+2] = int(triangles[i*4+1]) - - tf_visual["positions"] = positions.tolist() - tf_visual["indices"] = indices - - tf_visual["scaling"] = list(mesh.getScale().flatten()) - tf_visual["normals"] = [0,0,1]*int(len(tf_visual["positions"])/3) - - submesh = {} - submesh["materialIndex"] = 0 - submesh["verticesStart"] = 0 - submesh["verticesCount"] = len(positions)/3 - submesh["indexStart"] = 0 - submesh["indexCount"] = len(indices) - tf_visual["subMeshes"] = [submesh] - - if not mesh.getResource().getUrl().lower().endswith('.stl'): - mesh_material = mesh.getMaterial() - if mesh_material is not None: - - tf_material = {"name": "material_" + visual_name} - - color = mesh_material.getBaseColorFactor().flatten() - tf_color = [color[0], color[1], color[2]] - tf_color2 = [color[0]*0.5, color[1]*0.5, color[2]*0.5] - - """tf_material["ambient"] = tf_color - tf_material["diffuse"] = tf_color - tf_material["specular"] = tf_color2 - tf_material["alpha"] = 1 - tf_material["backFaceCulling"] = True""" - - tf_material["customType"] = "BABYLON.PBRMaterial" - tf_material["albedo"] = tf_color - tf_material["alpha"] = color[3] - tf_material["alphaCutOff"] = 0.4 - tf_material["backFaceCulling"] = True - tf_material["roughness"] = mesh_material.getRoughnessFactor() - tf_material["metallic"] = mesh_material.getMetallicFactor() - tf_material["maxSimultaneousLights"] = 4 - tf_material["environmentIntensity"] = 0.1 - - mesh_textures = mesh.getTextures() - if mesh_textures is not None and len(mesh_textures) > 0: - mesh_tex = mesh_textures[0] - mesh_tex_image = mesh_tex.getTextureImage() - tex_name = mesh_tex_image.getUrl() - tex_mimetype = "image/jpg" - if tex_name.endswith('png'): - tex_mimetype = "image/png" - tex_data = base64.b64encode(bytearray(mesh_tex_image.getResourceContents())).decode('ascii') - tex_data_url = "data:" + tex_mimetype + ";base64," + tex_data - - tf_texture = {"name": "material_texture_" + visual_name, "url": "material_texture_" + visual_name, "level": 1,"hasAlpha": True,"coordinatesMode":0,"uOffset":0,"vOffset":0,"uScale":1,\ - "vScale":1,"uAng":0,"vAng":0,"wAng":0,"wrapU":0,"wrapV":0,"coordinatesIndex":0, "isRenderTarget":False, \ - "base64String": tex_data_url} - - #tf_material["useAlphaFromAlbedoTexture"] = True - tf_material["albedoTexture"] = tf_texture - mesh_uvs = mesh_tex.getUVs() - tf_uvs = [0]*(len(mesh_uvs)*2) - for i in range(len(mesh_uvs)): - mesh_uv = mesh_uvs[i].flatten() - tf_uvs[i*2] = mesh_uv[0] - tf_uvs[i*2+1] = mesh_uv[1] - tf_visual["uvs"] = tf_uvs - - - elif (isinstance(visual_geom,tesseract_geometry.Box)): - box=visual_geom - tf_visual["geometryId"] = "cube_geometry" - tf_visual["scaling"] = [0.5*box.getX(), 0.5*box.getY(), 0.5*box.getZ()] - submesh = {} - submesh["materialIndex"] = 0 - submesh["verticesStart"] = 0 - submesh["verticesCount"] = 30 - submesh["indexStart"] = 0 - submesh["indexCount"] = 36 - tf_visual["subMeshes"] = [submesh] - - elif (isinstance(visual_geom,tesseract_geometry.Sphere)): - sphere=visual_geom - tf_visual["geometryId"] = "sphere_geometry" - tf_visual["scaling"] = [sphere.getRadius(), sphere.getRadius(), sphere.getRadius()] - submesh = {} - submesh["materialIndex"] = 0 - submesh["verticesStart"] = 0 - submesh["verticesCount"] = 2592 - submesh["indexStart"] = 0 - submesh["indexCount"] = 2880 - tf_visual["subMeshes"] = [submesh] - - elif (isinstance(visual_geom,tesseract_geometry.Cylinder)): - cylinder=visual_geom - tf_visual["geometryId"] = "cylinder_geometry" - tf_visual["scaling"] = [cylinder.getRadius(), cylinder.getRadius(), 0.5*cylinder.getLength()] - submesh = {} - submesh["materialIndex"] = 0 - submesh["verticesStart"] = 0 - submesh["verticesCount"] = 309 - submesh["indexStart"] = 0 - submesh["indexCount"] = 372 - tf_visual["subMeshes"] = [submesh] - - - else: - #Unsupported geometry type - continue - - meshes.append(tf_visual) - - if tf_material is None: - tf_material = {"name": "material_" + visual_name} - - material = visual.material - - if material is None: - tf_color = [0.5,0.5,0.5] - else: - color = material.color.flatten() - tf_color = [color[0], color[1], color[2]] - - tf_material["ambient"] = tf_color - tf_material["diffuse"] = tf_color - tf_material["specular"] = tf_color - tf_material["alpha"] = 1 - tf_material["backFaceCulling"] = True - - materials.append(tf_material) - - - child_joints = _find_child_joints(joint_map, link_name) - for j in child_joints: - np_tf_joint = j.parent_to_joint_origin_transform - joint_p, joint_q = _np_transform_to_babylon(np_tf_joint) - tf_joint_parent = {"name": "jointparent_" + j.getName(), "isVisible": "true", "isEnabled": "true", "parentId": "link_" + link_name} - tf_joint_parent["position"] = joint_p - tf_joint_parent["rotationQuaternion"] = joint_q - transform_nodes.append(tf_joint_parent) - tf_joint = {"name": "joint_" + j.getName(), "isVisible": "true", "isEnabled": "true", "parentId": "jointparent_" + j.getName()} - tf_joint["metadata"] = {"tesseract_joint": {"axis": list(j.axis.flatten()), "type": int(j.type), "name": j.getName()}} - transform_nodes.append(tf_joint) - j_transform_nodes, j_meshes, j_materials = _process_link_recursive(link_map, joint_map, j.child_link_name, j.getName()) - transform_nodes.extend(j_transform_nodes) - meshes.extend(j_meshes) - materials.extend(j_materials) - - return transform_nodes, meshes, materials - diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py new file mode 100644 index 000000000..24268d1f5 --- /dev/null +++ b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py @@ -0,0 +1,589 @@ +# Copyright 2022 Wason Technology, LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: John Wason (wason@wasontech.com) +# Date: 3/25/2022 + +import json +import struct +import numpy as np +import math +from tesseract_robotics import tesseract_geometry +from tesseract_robotics.tesseract_common import Quaterniond, AngleAxisd +import pkgutil +import re +import base64 +import io +from tesseract_robotics.tesseract_command_language import isStateWaypoint, isMoveInstruction + +def tesseract_env_to_gltf(t_env, origin_offset=[0,0,0], name = None, trajectory = None): + gltf_dict, gltf_buf_io = tesseract_env_to_gltf_dict_and_buf(t_env,origin_offset,name) + if trajectory is not None: + _append_trajectory_animation(gltf_dict, gltf_buf_io, trajectory) + buf_bytes = gltf_buf_io.getvalue() + buf_uri = "data:application/octet-stream;base64," + base64.b64encode(buf_bytes).decode("ascii") + gltf_dict["buffers"] = [ + { + "byteLength": len(buf_bytes), + "uri": buf_uri + } + ] + return json.dumps(gltf_dict, indent=4) + +_GLB_CHUNK_TYPE_JSON = 0x4E4F534A +_GLB_CHUNK_TYPE_BIN = 0x004E4942 +_GLB_MAGIC = 0x46546C67 + +def tesseract_env_to_glb(t_env, origin_offset=[0,0,0], name = None): + gltf_dict, gltf_buf_io = tesseract_env_to_gltf_dict_and_buf(t_env,origin_offset,name) + while gltf_buf_io.tell() % 4 != 0: + gltf_buf_io.write(b'\0') + buf_bytes = gltf_buf_io.getvalue() + + gltf_dict["buffers"] = [ + { + "byteLength": len(buf_bytes), + } + ] + + gltf_str = json.dumps(gltf_dict) + if len(gltf_str) % 4 != 0: + gltf_str += " " * (4 - (len(gltf_str) % 4)) + gltf_bytes = gltf_str.encode("utf-8") + + full_length = 12 + 8 + len(gltf_bytes) + 8 + len(buf_bytes) + header_bytes = struct.pack(" 0: + link_node["children"] = child_inds + + return link_node, link_ind + +def _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, shapes_mesh_inds): + + visual_u_name = visual.name + str(visual_i) + visual_name = "link_" + link_name + "_visual_" + visual_u_name + visual_node, visual_ind = _append_node(gltf_dict, visual.origin, visual_name) + + tf_material = None + + visual_geom = visual.geometry + if (isinstance(visual_geom,tesseract_geometry.PolygonMesh)): + + mesh=visual_geom + vertices = mesh.getVertices() + positions = np.zeros((len(vertices),3),dtype=np.float32) + for i in range(len(vertices)): + positions[i,:] = vertices[i].flatten() + + indices = mesh.getFaces().flatten().astype(np.uint32).reshape((-1,4))[:,1:4].flatten() + + _, positions_ind = _append_accessor(gltf_dict, gltf_buf_io, positions) + _, indices_ind = _append_accessor(gltf_dict, gltf_buf_io, indices) + + normals_ind = None + normals = mesh.getNormals() + if normals is not None: + normals2 = np.zeros((len(normals),3),dtype=np.float32) + for i in range(len(normals)): + normals2[i,:] = normals[i].flatten() + _, normals_ind = _append_accessor(gltf_dict, gltf_buf_io, normals2) + + mesh_dict, mesh_ind = _append_dict_list(gltf_dict, "meshes", { + "primitives": [ + { + "attributes": { + "POSITION": positions_ind + }, + "indices": indices_ind + } + ], + "name": visual_name + "_mesh" + }) + + if normals_ind is not None: + mesh_dict["primitives"][0]["attributes"]["NORMAL"] = normals_ind + + visual_node["scale"] = list(mesh.getScale().flatten()) + + if not mesh.getResource().getUrl().lower().endswith('.stl'): + mesh_material = mesh.getMaterial() + if mesh_material is not None: + + tf_material = { + "name": "material_" + visual_name, + "alphaMode": "MASK", + "alphaCutoff": 0.4 + } + + base_color_factor = mesh_material.getBaseColorFactor().flatten().tolist() + + tf_material["pbrMetallicRoughness"] = { + "baseColorFactor": base_color_factor, + "roughnessFactor": mesh_material.getRoughnessFactor(), + "metallicFactor": mesh_material.getMetallicFactor(), + } + + mesh_textures = mesh.getTextures() + if mesh_textures is not None and len(mesh_textures) > 0: + mesh_tex = mesh_textures[0] + mesh_tex_image = mesh_tex.getTextureImage() + tex_name = mesh_tex_image.getUrl() + tex_mimetype = "image/jpg" + if tex_name.endswith('png'): + tex_mimetype = "image/png" + + mesh_tex_image_bytes = bytearray(mesh_tex_image.getResourceContents()) + tex_img = cv2.imdecode(np.frombuffer(mesh_tex_image_bytes,dtype=np.uint8),flags=1) + img_h, img_w, _ = tex_img.shape + _, image_bufview_ind = _append_bufview(gltf_dict, gltf_buf_io, mesh_tex_image_bytes) + _, image_ind = _append_dict_list(gltf_dict, "images", { + "mimeType": tex_mimetype, + "bufferView": image_bufview_ind + }) + + _, tex_ind = _append_dict_list(gltf_dict, "textures", { + "source": image_ind + }) + + tf_material["pbrMetallicRoughness"]["baseColorTexture"] = { + "index": tex_ind, + "texCoord": 0 + } + + mesh_uvs = mesh_tex.getUVs() + tf_uvs = np.zeros((len(mesh_uvs),2),dtype=np.float32) + for i in range(len(mesh_uvs)): + tf_uvs1 = mesh_uvs[i].flatten() + tf_uvs[i,:] = [tf_uvs1[0], 1.0-tf_uvs1[1]] + _, tex_ind = _append_accessor(gltf_dict, gltf_buf_io, tf_uvs) + + mesh_dict["primitives"][0]["attributes"]["TEXCOORD_0"] = tex_ind + + elif (isinstance(visual_geom,tesseract_geometry.Box)): + box=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cube_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [0.5*box.getX(), 0.5*box.getY(), 0.5*box.getZ()] + + elif (isinstance(visual_geom,tesseract_geometry.Sphere)): + sphere=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "sphere_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [sphere.getRadius(), sphere.getRadius(), sphere.getRadius()] + + elif (isinstance(visual_geom,tesseract_geometry.Cylinder)): + cylinder=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cylinder_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [cylinder.getRadius(), cylinder.getRadius(), 0.5*cylinder.getLength()] + + if tf_material is None: + tf_material = {"name": "material_" + visual_name} + + material = visual.material + + if material is None: + tf_color = [0.5,0.5,0.5,1] + else: + tf_color = material.color.flatten().tolist() + + tf_material["pbrMetallicRoughness"] = { + "baseColorFactor": tf_color, + "roughnessFactor": 0.3, + "metallicFactor": 0.3 + } + + material_dict, material_ind = _append_dict_list(gltf_dict, "materials", tf_material) + + mesh_dict["primitives"][0]["material"] = material_ind + + visual_node["mesh"] = mesh_ind + + + return visual_node, visual_ind + +_COMPONENT_TYPE_INT8 = 5120 +_COMPONENT_TYPE_UINT8 = 5121 +_COMPONENT_TYPE_INT16 = 5122 +_COMPONENT_TYPE_UINT16 = 5123 +_COMPONENT_TYPE_UINT32 = 5125 +_COMPONENT_TYPE_FLOAT32 = 5126 + +_TYPE_SCALAR = "SCALAR" # 1 component +_TYPE_VEC2 = "VEC2" # 2 components +_TYPE_VEC3 = "VEC3" # 3 components +_TYPE_VEC4 = "VEC4" # 4 components +_TYPE_MAT2 = "MAT2" # 4 components +_TYPE_MAT2 = "MAT3" # 3 components +_TYPE_MAT2 = "MAT4" # 16 components + +def _append_accessor(gltf_dict, gltf_buf_io, dat): + + dat_min = None + dat_max = None + + if isinstance(dat,bytearray) or isinstance(dat,bytes): + dat_count = len(dat) + componentType = _COMPONENT_TYPE_UINT8 + type_ = _TYPE_SCALAR + elif isinstance(dat,np.ndarray): + + if dat.dtype == np.int8: + componentType = _COMPONENT_TYPE_INT8 + elif dat.dtype == np.uint8: + componentType = _COMPONENT_TYPE_UINT8 + elif dat.dtype == np.int16: + componentType = _COMPONENT_TYPE_INT16 + elif dat.dtype == np.uint16: + componentType = _COMPONENT_TYPE_UINT16 + elif dat.dtype == np.uint32: + componentType = _COMPONENT_TYPE_UINT32 + elif dat.dtype == np.float32: + componentType = _COMPONENT_TYPE_FLOAT32 + else: + assert False, "Invalid accessor componentType for np array" + + dat_count = dat.shape[0] + + if dat.ndim == 1: + dat_min = [dat.min(0).tolist()] + dat_max = [dat.max(0).tolist()] + type_ = _TYPE_SCALAR + elif dat.ndim == 2: + dat_min = dat.min(0).tolist() + dat_max = dat.max(0).tolist() + if dat.shape[1] == 2: + type_ = _TYPE_VEC2 + elif dat.shape[1] == 3: + type_ = _TYPE_VEC3 + elif dat.shape[1] == 4: + type_ = _TYPE_VEC4 + else: + assert False, "Invalid accessor np array shape" + else: + assert False, "Invalid accessor np array shape" + + dat = dat.flatten().tobytes() + + else: + assert False, "Invalid accessor data type" + + assert isinstance(dat, bytes) or isinstance(dat,bytearray) + + byteOffset = gltf_buf_io.tell() + byteLength = len(dat) + + _, bufview_ind = _append_dict_list(gltf_dict, "bufferViews", { + "buffer": 0, + "byteLength": byteLength, + "byteOffset": byteOffset + }) + + gltf_buf_io.write(dat) + + accessor, accessor_ind = _append_dict_list(gltf_dict, "accessors", { + "bufferView": bufview_ind, + "componentType": componentType, + "type": type_, + "count": dat_count + }) + + if dat_min is not None and dat_max is not None: + accessor["min"] = dat_min + accessor["max"] = dat_max + + return accessor, accessor_ind + +def _append_bufview(gltf_dict, gltf_buf_io, dat): + byteOffset = gltf_buf_io.tell() + byteLength = len(dat) + + bufview_dict, bufview_ind = _append_dict_list(gltf_dict, "bufferViews", { + "buffer": 0, + "byteLength": byteLength, + "byteOffset": byteOffset + }) + + gltf_buf_io.write(dat) + + return bufview_dict, bufview_ind + +def _append_trajectory_animation(gltf_dict, gltf_buf_io, tesseract_trajectory): + + start_instruction_o = tesseract_trajectory[0] + assert isMoveInstruction(start_instruction_o) + start_waypoint_o = start_instruction_o.as_MoveInstruction().getWaypoint() + assert isStateWaypoint(start_waypoint_o) + start_waypoint = start_waypoint_o.as_StateWaypoint() + + joint_names = list(start_waypoint.joint_names) + + trajectory2 = [] + trajectory_time2 = [] + for i in range(len(tesseract_trajectory)): + instr = tesseract_trajectory[i] + assert isMoveInstruction(instr) + wp = instr.as_MoveInstruction().getWaypoint() + assert isStateWaypoint(wp) + state_wp = wp.as_StateWaypoint() + trajectory2.append(state_wp.position.flatten().tolist()) + trajectory_time2.append(state_wp.time) + + print(trajectory2) + print(trajectory_time2) + + joint_inds = {} + joint_axes = {} + joint_types = {} + + for i in range(len(gltf_dict["nodes"])): + n = gltf_dict["nodes"][i] + if "extras" in n: + if "tesseract_joint" in n["extras"]: + e1 = n["extras"]["tesseract_joint"] + joint_inds[e1["name"]] = i + joint_axes[e1["name"]] = np.array(e1["axis"],dtype=np.float32) + joint_types[e1["name"]] = e1["type"] + + _, t_ind = _append_accessor(gltf_dict, gltf_buf_io,np.array(trajectory_time2, dtype=np.float32)) + + trajectory2_np = np.array(trajectory2,dtype=np.float32) + + animation = { + "channels": [], + "samplers": [] + } + + for i in range(len(joint_names)): + joint_name = joint_names[i] + trajectory2_i = trajectory2_np[:,i] + #_, o_ind[i] = _append_accessor(gltf_dict, gltf_buf_io, trajectory2_i) + joint_type = joint_types[joint_name] + joint_axis = joint_axes[joint_name] + + if joint_type == 1: + sampler_np = np.zeros((len(trajectory2_i),4),dtype=np.float32) + joint_axisd = np.array(joint_axis,dtype=np.float64) + for j in range(len(trajectory2_i)): + a_ax = AngleAxisd(float(trajectory2_i[j]),joint_axisd) + qd = Quaterniond(a_ax) + sampler_np[j,:] = [qd.x(), qd.y(), qd.z(), qd.w()] + _, s_ind = _append_accessor(gltf_dict, gltf_buf_io, sampler_np) + + animation["samplers"].append({ + "input": t_ind, + "output": s_ind, + "interpolation": "LINEAR" + }) + + animation["channels"].append({ + "sampler": i, + "target": { + "node": joint_inds[joint_name], + "path": "rotation" + } + }) + elif joint_type == 2: + sampler_np = np.zeros((len(trajectory2_i),3),dtype=np.float32) + joint_axisd = np.array(joint_axis,dtype=np.float64) + for j in range(len(trajectory2_i)): + vec = float(trajectory2_i[j]) * joint_axisd + sampler_np[j,:] = vec.tolist() + _, s_ind = _append_accessor(gltf_dict, gltf_buf_io, sampler_np) + + animation["samplers"].append({ + "input": t_ind, + "output": s_ind, + "interpolation": "LINEAR" + }) + + animation["channels"].append({ + "sampler": i, + "target": { + "node": joint_inds[joint_name], + "path": "translation" + } + }) + + else: + assert False, "Unknown joint type " + str(joint_type) + + _append_dict_list(gltf_dict, "animations", animation) + + + print(animation) + +def _append_shapes_mesh_accessors(gltf_dict, gltf_buf_io, name): + o = _geometry_shapes[name] + + _, positions_ind = _append_accessor(gltf_dict, gltf_buf_io, o["positions"]) + _, normals_ind = _append_accessor(gltf_dict, gltf_buf_io, o["normals"]) + _, indices_ind = _append_accessor(gltf_dict, gltf_buf_io, o["indices"]) + + return { + "positions_ind": positions_ind, + "normals_ind": normals_ind, + "indices_ind": indices_ind + } + +def _append_shape_mesh(gltf_dict, gltf_buf_io, shape_name, visual_name, shapes_mesh_inds): + accessors_inds = shapes_mesh_inds.get(shape_name, None) + if accessors_inds is None: + accessors_inds = _append_shapes_mesh_accessors(gltf_dict, gltf_buf_io, shape_name) + shapes_mesh_inds[shape_name] = accessors_inds + + mesh_dict, mesh_ind = _append_dict_list(gltf_dict, "meshes", { + "primitives": [ + { + "attributes": { + "POSITION": accessors_inds["positions_ind"], + "NORMALS": accessors_inds["normals_ind"] + }, + "indices": accessors_inds["indices_ind"] + } + ], + "name": visual_name + "_mesh" + }) + return mesh_dict, mesh_ind + +def _load_shapes(): + geometries = json.loads(pkgutil.get_data("tesseract_robotics_viewer.resources","geometries.json"))["geometries"] + + geometry_names = ["cube_geometry", "sphere_geometry", "cylinder_geometry"] + + for name in geometry_names: + g = next(filter(lambda x: x["id"] == name, geometries["vertexData"])) + o = dict() + o["name"] = name + o["positions"] = np.array(g["positions"],dtype=np.float32).reshape((-1,3)) + o["normals"] = np.array(g["normals"],dtype=np.float32).reshape((-1,3)) + #o["uvs"] = np.array(g["uvs"],dtype=np.float32).reshape((-1,2)) + o["indices"] = np.array(g["indices"],dtype=np.uint32) + + _geometry_shapes[name] = o + +_geometry_shapes = dict() + +_load_shapes() + + + diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_viewer.py b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_viewer.py index 23b518e8b..cbe6bcf30 100644 --- a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_viewer.py +++ b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_viewer.py @@ -18,7 +18,7 @@ from __future__ import absolute_import import threading -from tesseract_robotics_viewer.tesseract_env_to_babylon_json import tesseract_env_to_babylon_json +from tesseract_robotics_viewer.tesseract_env_to_gltf import tesseract_env_to_gltf, tesseract_env_to_glb import pkg_resources import mimetypes import posixpath @@ -46,6 +46,7 @@ class _TesseractViewerRequestHandler(BaseHTTPRequestHandler): def __init__(self, request, client_address, server ): self.scene_json = server.viewer.scene_json + self.scene_glb = server.viewer.scene_glb self.trajectory_json = server.viewer.trajectory_json if sys.version_info[0] < 3: BaseHTTPRequestHandler.__init__(self,request,client_address,server) @@ -58,11 +59,16 @@ def do_file(self, send_data): path = "/index.html" path=path[1:] - if path == 'tesseract_scene.babylon': + if path == 'tesseract_scene.gltf': if self.scene_json is None: file_data = "{}".encode() else: file_data = self.scene_json.encode() + elif path == 'tesseract_scene.glb': + if self.scene_glb is None: + file_data = b"" + else: + file_data = self.scene_glb elif path == 'tesseract_trajectory.json': if self.trajectory_json is None: self.send_response(404) @@ -83,7 +89,9 @@ def do_file(self, send_data): self.send_response(200) _, ext = posixpath.splitext(path) - if ext in mimetypes.types_map: + if ext == '.js': + ctype = 'text/javascript' + elif ext in mimetypes.types_map: ctype = mimetypes.types_map[ext] else: ctype = 'application/octet-stream' @@ -115,17 +123,19 @@ def log_message(self, format, *args): class TesseractViewer(): def __init__(self, server_address = ('',8000)): self.scene_json = None + self.scene_glb = None self.trajectory_json = None self._lock = threading.Lock() self._serve_thread = None self._httpd = HTTPServer(server_address,_TesseractViewerRequestHandler) setattr(self._httpd,"viewer",self) - def update_environment(self, tesseract_env, origin_offset = [0,0,0]): + def update_environment(self, tesseract_env, origin_offset = [0,0,0], trajectory = None): assert isinstance(tesseract_env, tesseract_environment.Environment) with self._lock: - self.scene_json = tesseract_env_to_babylon_json(tesseract_env, origin_offset) + self.scene_json = tesseract_env_to_gltf(tesseract_env, origin_offset, trajectory=trajectory) + self.scene_glb = tesseract_env_to_glb(tesseract_env, origin_offset) def update_joint_positions(self, joint_names, joint_positions): # Create "infinite" animation with constant joint angles @@ -203,3 +213,20 @@ def save(self, directory, overwrite=False): with open(os.path.join(directory,k), "wb") as f: f.write(v) + def save_scene_gltf(self, fname, overwrite=False): + assert self.scene_json is not None, "Tesseract environment not set" + s = self.scene_json + if isinstance(s,str): + s = s.encode() + + fmode = 'wb' if overwrite else 'xb' + with open(fname,fmode) as f: + f.write(s) + + def save_scene_glb(self, fname, overwrite=False): + assert self.scene_json is not None, "Tesseract environment not set" + s = self.scene_glb + + fmode = 'wb' if overwrite else 'xb' + with open(fname,fmode) as f: + f.write(s) \ No newline at end of file