@operato/scene-urdf 1.2.2

package/src/elements/urdf-viewer-element.ts

@@ -0,0 +1,495 @@
+import * as THREE from 'three'
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'
+import URDFLoader, { URDFJoint, URDFRobot } from 'urdf-loader/src/URDFLoader'
+import { XacroLoader } from 'xacro-parser'
+
+// urdf-viewer element
+// Loads and displays a 3D view of a URDF-formatted robot
+
+// Events
+// urdf-change: Fires when the URDF has finished loading and getting processed
+// urdf-processed: Fires when the URDF has finished loading and getting processed
+// geometry-loaded: Fires when all the geometry has been fully loaded
+// ignore-limits-change: Fires when the 'ignore-limits' attribute changes
+// angle-change: Fires when an angle changes
+export default class URDFViewerElement extends HTMLElement {
+  static get observedAttributes() {
+    return ['package', 'urdf', 'up', 'display-shadow', 'ambient-color', 'ignore-limits', 'no-auto-recenter']
+  }
+
+  get package() {
+    return this.getAttribute('package') || ''
+  }
+  set package(val) {
+    this.setAttribute('package', val)
+  }
+
+  get urdf() {
+    return this.getAttribute('urdf') || ''
+  }
+  set urdf(val) {
+    this.setAttribute('urdf', val)
+  }
+
+  get ignoreLimits() {
+    return this.hasAttribute('ignore-limits') || false
+  }
+  set ignoreLimits(val) {
+    val ? this.setAttribute('ignore-limits', String(val)) : this.removeAttribute('ignore-limits')
+  }
+
+  get up() {
+    return this.getAttribute('up') || '+Z'
+  }
+  set up(val) {
+    this.setAttribute('up', val)
+  }
+
+  get displayShadow() {
+    return this.hasAttribute('display-shadow') || false
+  }
+  set displayShadow(val) {
+    val ? this.setAttribute('display-shadow', '') : this.removeAttribute('display-shadow')
+  }
+
+  get ambientColor() {
+    return this.getAttribute('ambient-color') || '#455A64'
+  }
+  set ambientColor(val) {
+    val ? this.setAttribute('ambient-color', val) : this.removeAttribute('ambient-color')
+  }
+
+  get noAutoRecenter() {
+    return this.hasAttribute('no-auto-recenter') || false
+  }
+  set noAutoRecenter(val) {
+    val ? this.setAttribute('no-auto-recenter', String(true)) : this.removeAttribute('no-auto-recenter')
+  }
+
+  get jointValues() {
+    const values = {} as any
+    if (this.robot) {
+      for (const name in this.robot.joints) {
+        const joint = this.robot.joints[name]
+        values[name] = joint.jointValue.length === 1 ? joint.angle : [...joint.jointValue]
+      }
+    }
+
+    return values
+  }
+  set jointValues(val) {
+    this.setJointValues(val)
+  }
+
+  get angles() {
+    return this.jointValues
+  }
+  set angles(v) {
+    this.jointValues = v
+  }
+
+  robot?: URDFRobot
+  controls: OrbitControls
+  scene: THREE.Scene
+  camera: THREE.PerspectiveCamera
+
+  world: THREE.Object3D
+  renderer: THREE.WebGLRenderer
+  plane: THREE.Mesh
+  directionalLight: THREE.DirectionalLight
+  ambientLight: THREE.HemisphereLight
+
+  _prevload?: string
+  _requestId = 0
+  _dirty = true
+  _loadScheduled = false
+  loadMeshFunc?: (
+    url: string,
+    manager: THREE.LoadingManager,
+    onLoad: (mesh: THREE.Object3D, err?: Error) => void
+  ) => void
+  urlModifierFunc?: (url: string) => string
+
+  /* Lifecycle Functions */
+  constructor() {
+    super()
+
+    const scene = new THREE.Scene()
+
+    const ambientLight = new THREE.HemisphereLight(this.ambientColor, '#000')
+    ambientLight.groundColor.lerp(ambientLight.color, 0.5)
+    ambientLight.intensity = 0.5
+    ambientLight.position.set(0, 1, 0)
+    scene.add(ambientLight)
+
+    // Light setup
+    const dirLight = new THREE.DirectionalLight(0xffffff)
+    dirLight.position.set(4, 10, 1)
+    dirLight.shadow.mapSize.width = 2048
+    dirLight.shadow.mapSize.height = 2048
+    dirLight.shadow.normalBias = 0.001
+    dirLight.castShadow = true
+    scene.add(dirLight)
+    scene.add(dirLight.target)
+
+    // Renderer setup
+    const renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true })
+    renderer.setClearColor(0xffffff)
+    renderer.setClearAlpha(0)
+    renderer.shadowMap.enabled = true
+    renderer.shadowMap.type = THREE.PCFSoftShadowMap
+    renderer.outputEncoding = THREE.sRGBEncoding
+
+    // Camera setup
+    const camera = new THREE.PerspectiveCamera(75, 1, 0.1, 1000)
+    camera.position.z = -10
+
+    // World setup
+    const world = new THREE.Object3D()
+    scene.add(world)
+
+    const plane = new THREE.Mesh(
+      new THREE.PlaneBufferGeometry(40, 40),
+      new THREE.ShadowMaterial({ side: THREE.DoubleSide, transparent: true, opacity: 0.5 })
+    )
+    plane.rotation.x = -Math.PI / 2
+    plane.position.y = -0.5
+    plane.receiveShadow = true
+    plane.scale.set(10, 10, 10)
+    scene.add(plane)
+
+    // Controls setup
+    const controls = new OrbitControls(camera, renderer.domElement)
+    controls.rotateSpeed = 2.0
+    controls.zoomSpeed = 5
+    controls.panSpeed = 2
+    controls.enableZoom = true
+    controls.enableDamping = false
+    controls.maxDistance = 50
+    controls.minDistance = 0.25
+    controls.addEventListener('change', () => this.recenter())
+
+    this.scene = scene
+    this.world = world
+    this.renderer = renderer
+    this.camera = camera
+    this.controls = controls
+    this.plane = plane
+    this.directionalLight = dirLight
+    this.ambientLight = ambientLight
+
+    this._setUp(this.up)
+  }
+
+  connectedCallback() {
+    if (!this.querySelector('canvas')) {
+      var canvas = this.renderer.domElement
+      canvas.style.display = 'block'
+      canvas.style.width = '100%'
+      canvas.style.height = '100%'
+
+      this.appendChild(canvas)
+    }
+
+    requestAnimationFrame(() => this.recenter())
+  }
+
+  disconnectedCallback() {}
+
+  attributeChangedCallback(attr: string, oldval: any, newval: any) {
+    this.recenter()
+
+    switch (attr) {
+      case 'package':
+      case 'urdf': {
+        this._scheduleLoad()
+        break
+      }
+
+      case 'up': {
+        this._setUp(this.up)
+        break
+      }
+
+      case 'ambient-color': {
+        this.ambientLight.color.set(this.ambientColor)
+        this.ambientLight.groundColor.set('#000').lerp(this.ambientLight.color, 0.5)
+        break
+      }
+
+      case 'ignore-limits': {
+        this._setIgnoreLimits(this.ignoreLimits, true)
+        break
+      }
+    }
+  }
+
+  /* Public API */
+  updateSize() {
+    const r = this.renderer
+    const w = this.clientWidth
+    const h = this.clientHeight
+
+    const currsize = r.getSize(new THREE.Vector2())
+
+    if (currsize.width !== w || currsize.height !== h) {
+      this.recenter()
+    }
+
+    r.setPixelRatio(window.devicePixelRatio)
+    r.setSize(w, h, false)
+
+    this.camera.aspect = w / h
+    this.camera.updateProjectionMatrix()
+  }
+
+  redraw() {
+    if (this.parentNode && this._dirty) {
+      this._dirty = false
+      try {
+        this.updateSize()
+
+        if (!this.noAutoRecenter) {
+          this._updateEnvironment()
+        }
+
+        this.renderer.render(this.scene, this.camera)
+
+        // update controls after the environment in
+        // case the controls are retargeted
+        this.controls.update()
+      } catch (err) {
+        console.error(err)
+      } finally {
+        this._dirty = true
+      }
+    }
+  }
+
+  recenter() {
+    this._updateEnvironment()
+    this.redraw()
+  }
+
+  // Set the joint with jointName to
+  // angle in degrees
+  setJointValue(jointName: string, ...values: Number[]) {
+    if (!this.robot) return
+    if (!this.robot.joints[jointName]) return
+
+    if (this.robot.joints[jointName].setJointValue(values[0], values[1], values[2]) as any) {
+      this.redraw()
+      this.dispatchEvent(new CustomEvent('angle-change', { bubbles: true, cancelable: true, detail: jointName }))
+    }
+  }
+
+  setJointValues(values: Number[]) {
+    for (const name in values) this.setJointValue(name, values[name])
+  }
+
+  /* Private Functions */
+  // Updates the position of the plane to be at the
+  // lowest point below the robot and focuses the
+  // camera on the center of the scene
+  _updateEnvironment() {
+    if (!this.robot) return
+
+    this.world.updateMatrixWorld()
+
+    const bbox = new THREE.Box3()
+    bbox.setFromObject(this.robot)
+
+    const center = bbox.getCenter(new THREE.Vector3())
+    this.controls.target.y = center.y
+    this.plane.position.y = bbox.min.y - 1e-3
+
+    const dirLight = this.directionalLight
+    dirLight.castShadow = this.displayShadow
+
+    if (this.displayShadow) {
+      // Update the shadow camera rendering bounds to encapsulate the
+      // model. We use the bounding sphere of the bounding box for
+      // simplicity -- this could be a tighter fit.
+      const sphere = bbox.getBoundingSphere(new THREE.Sphere())
+      const minmax = sphere.radius
+      const cam = dirLight.shadow.camera
+      cam.left = cam.bottom = -minmax
+      cam.right = cam.top = minmax
+
+      // Update the camera to focus on the center of the model so the
+      // shadow can encapsulate it
+      const offset = dirLight.position.clone().sub(dirLight.target.position)
+      dirLight.target.position.copy(center)
+      dirLight.position.copy(center).add(offset)
+
+      cam.updateProjectionMatrix()
+    }
+  }
+
+  _scheduleLoad() {
+    // if our current model is already what's being requested
+    // or has been loaded then early out
+    if (this._prevload === `${this.package}|${this.urdf}`) return
+    this._prevload = `${this.package}|${this.urdf}`
+
+    // if we're already waiting on a load then early out
+    if (this._loadScheduled) return
+    this._loadScheduled = true
+
+    if (this.robot) {
+      this.robot.traverse((c: any) => c.dispose && c.dispose())
+      this.robot.parent!.remove(this.robot)
+      delete this.robot
+    }
+
+    requestAnimationFrame(() => {
+      this._loadUrdf(this.package, this.urdf)
+      this._loadScheduled = false
+    })
+  }
+
+  // Watch the package and urdf field and load the robot model.
+  // This should _only_ be called from _scheduleLoad because that
+  // ensures the that current robot has been removed
+  _loadUrdf(pkg: string, urdf: string) {
+    this.dispatchEvent(new CustomEvent('urdf-change', { bubbles: true, cancelable: true, composed: true }))
+
+    if (urdf) {
+      // Keep track of this request and make
+      // sure it doesn't get overwritten by
+      // a subsequent one
+      this._requestId++
+      const requestId = this._requestId
+
+      const updateMaterials = (mesh: URDFRobot) => {
+        mesh.traverse(c => {
+          if ((c as any).isMesh) {
+            c.castShadow = true
+            c.receiveShadow = true
+
+            if ((c as any).material) {
+              const mats = (Array.isArray((c as any).material) ? (c as any).material : [(c as any).material]).map(
+                (m: any) => {
+                  if (m instanceof THREE.MeshBasicMaterial) {
+                    m = new THREE.MeshPhongMaterial()
+                  }
+
+                  if (m.map) {
+                    m.map.encoding = THREE.GammaEncoding
+                  }
+
+                  return m
+                }
+              )
+
+              ;(c as any).material = mats.length === 1 ? mats[0] : mats
+            }
+          }
+        })
+      }
+
+      if (pkg.includes(':') && pkg.split(':')[1].substring(0, 2) !== '//') {
+        // E.g. pkg = "pkg_name: path/to/pkg_name, pk2: path2/to/pk2"}
+
+        // Convert pkg(s) into a map. E.g.
+        // { "pkg_name": "path/to/pkg_name",
+        //   "pk2":      "path2/to/pk2"      }
+
+        pkg = pkg.split(',').reduce((map: any, value: string) => {
+          const split = value.split(/:/).filter(x => !!x)
+          const pkgName = split.shift()!.trim()
+          const pkgPath = split.join(':').trim()
+          map[pkgName] = pkgPath
+
+          return map
+        }, {} as any)
+      }
+
+      let robot: URDFRobot
+      const manager = new THREE.LoadingManager()
+      manager.onLoad = () => {
+        // If another request has come in to load a new
+        // robot, then ignore this one
+        if (this._requestId !== requestId) {
+          robot.traverse(c => (c as any).dispose && (c as any).dispose())
+          return
+        }
+
+        this.robot = robot
+
+        this.world.add(robot)
+        updateMaterials(robot)
+
+        this._setIgnoreLimits(this.ignoreLimits)
+
+        this.dispatchEvent(new CustomEvent('urdf-processed', { bubbles: true, cancelable: true, composed: true }))
+        this.dispatchEvent(new CustomEvent('geometry-loaded', { bubbles: true, cancelable: true, composed: true }))
+
+        this.recenter()
+      }
+
+      if (this.urlModifierFunc) {
+        manager.setURLModifier(this.urlModifierFunc)
+      }
+
+      const checkXacro = /[.]/.exec(urdf) ? /[^.]+$/.exec(urdf) : undefined
+
+      if (checkXacro && checkXacro[0] === 'xacro') {
+        const xacroLoader = new XacroLoader()
+        try {
+          xacroLoader.load(
+            urdf,
+            xml => {
+              const loader = new URDFLoader(manager)
+              loader.packages = pkg
+              loader.loadMeshCb = this.loadMeshFunc!
+              loader.fetchOptions = { mode: 'cors', credentials: 'same-origin' }
+              robot = loader.parse(xml)
+            },
+            err => {
+              console.error('xacroloader error: ', err)
+            }
+          )
+        } catch (error) {
+          console.error(error)
+        }
+      } else {
+        const loader = new URDFLoader(manager)
+        loader.packages = pkg
+        loader.loadMeshCb = this.loadMeshFunc!
+        loader.fetchOptions = { mode: 'cors', credentials: 'same-origin' }
+        loader.load(urdf, model => (robot = model))
+      }
+    }
+  }
+
+  // Watch the coordinate frame and update the
+  // rotation of the scene to match
+  _setUp(up: string) {
+    if (!up) up = '+Z'
+    up = up.toUpperCase()
+    const sign = up.replace(/[^-+]/g, '')[0] || '+'
+    const axis = up.replace(/[^XYZ]/gi, '')[0] || 'Z'
+
+    const PI = Math.PI
+    const HALFPI = PI / 2
+    if (axis === 'X') this.world.rotation.set(0, 0, sign === '+' ? HALFPI : -HALFPI)
+    if (axis === 'Z') this.world.rotation.set(sign === '+' ? -HALFPI : HALFPI, 0, 0)
+    if (axis === 'Y') this.world.rotation.set(sign === '+' ? 0 : PI, 0, 0)
+  }
+
+  // Updates the current robot's angles to ignore
+  // joint limits or not
+  _setIgnoreLimits(ignore: boolean, dispatch = false) {
+    if (this.robot) {
+      Object.values(this.robot.joints).forEach(joint => {
+        joint.ignoreLimits = ignore
+        joint.setJointValue(joint.jointValue[0], joint.jointValue[1], joint.jointValue[2])
+      })
+    }
+
+    if (dispatch) {
+      this.dispatchEvent(new CustomEvent('ignore-limits-change', { bubbles: true, cancelable: true, composed: true }))
+    }
+  }
+}

package/src/index.ts

@@ -0,0 +1,2 @@
+export { default as UrdfViewer } from './urdf-viewer'
+export { default as UrdfController } from './urdf-controller'

package/src/templates/index.ts

@@ -0,0 +1,4 @@
+import URDFViewer from './urdf-viewer'
+import URDFController from './urdf-controller'
+
+export default [URDFViewer, URDFController]

package/src/templates/urdf-controller.ts

@@ -0,0 +1,16 @@
+const icon = new URL('../../icons/urdf-controller.png', import.meta.url).href
+
+export default {
+  type: 'urdf-controller',
+  description: 'urdf-controller',
+  group: '3D',
+  /* line|shape|textAndMedia|chartAndGauge|table|container|dataSource|IoT|3D|warehouse|form|etc */
+  icon,
+  model: {
+    type: 'urdf-controller',
+    left: 10,
+    top: 10,
+    width: 300,
+    height: 120
+  }
+}

package/src/templates/urdf-viewer.ts

@@ -0,0 +1,16 @@
+const icon = new URL('../../icons/urdf-viewer.png', import.meta.url).href
+
+export default {
+  type: 'urdf-viewer',
+  description: 'urdf-viewer',
+  group: '3D',
+  /* line|shape|textAndMedia|chartAndGauge|table|container|dataSource|IoT|3D|warehouse|form|etc */
+  icon,
+  model: {
+    type: 'urdf-viewer',
+    left: 10,
+    top: 10,
+    width: 300,
+    height: 200
+  }
+}

package/src/urdf-controller.ts

@@ -0,0 +1,82 @@
+/*
+ * Copyright © HatioLab Inc. All rights reserved.
+ */
+
+const NATURE = {
+  mutable: false,
+  resizable: true,
+  rotatable: true,
+  properties: [
+    {
+      type: 'id-input',
+      label: 'urdf-reference',
+      name: 'reference',
+      property: {
+        component: 'urdf-viewer'
+      }
+    }
+  ]
+}
+
+import { Component, HTMLOverlayContainer } from '@hatiolab/things-scene'
+import './elements/urdf-controller-element'
+import URDFControllerElement from './elements/urdf-controller-element'
+import URDFManipulatorElement from './elements/urdf-manipulator-element'
+import UrdfViewer from './urdf-viewer'
+
+export default class UrdfController extends HTMLOverlayContainer {
+  _reference?: UrdfViewer
+
+  static get nature() {
+    return NATURE
+  }
+
+  oncreate_element(controller: URDFControllerElement) {}
+
+  dispose() {
+    super.dispose()
+  }
+
+  /*
+   * 컴포넌트의 생성 또는 속성 변화 시에 호출되며,
+   * 그에 따른 html element의 반영이 필요한 부분을 구현한다.
+   *
+   * ThingsComponent state => HTML element properties
+   */
+  setElementProperties(controller: URDFControllerElement) {
+    var reference = this.reference
+    controller.viewer = reference ? (reference.element as URDFManipulatorElement) : undefined
+  }
+
+  /*
+   * 컴포넌트가 ready 상태가 되거나, 컴포넌트의 속성이 변화될 시 setElementProperties 뒤에 호출된다.
+   * 변화에 따른 기본적인 html 속성이 super.reposition()에서 진행되고, 그 밖의 작업이 필요할 때, 오버라이드 한다.
+   */
+  reposition() {
+    super.reposition()
+  }
+
+  get tagName() {
+    return 'urdf-controller'
+  }
+
+  get reference() {
+    var { reference } = this.state
+    if (!reference) {
+      return
+    }
+
+    if (!this._reference) {
+      this._reference = this.root.findById(reference) as UrdfViewer
+    }
+
+    return this._reference
+  }
+
+  set reference(reference) {
+    delete this._reference
+    this.state.reference = reference
+  }
+}
+
+Component.register('urdf-controller', UrdfController)