@operato/scene-urdf 1.2.2

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

@@ -0,0 +1,297 @@
+import { LitElement, css, html, PropertyValues } from 'lit'
+import { customElement, property } from 'lit/decorators.js'
+import * as THREE from 'three'
+import { registerDragEvents } from './drag-n-drop'
+import { ScrollbarStyles } from '@operato/styles'
+import { URDFRobot } from 'urdf-loader'
+import URDFManipulatorElement from './urdf-manipulator-element'
+
+const DEG2RAD = Math.PI / 180
+const RAD2DEG = 1 / DEG2RAD
+
+@customElement('urdf-controller')
+export default class URDFControllerElement extends LitElement {
+  static styles = [
+    ScrollbarStyles,
+    css`
+      :host {
+        display: flex;
+        flex-direction: column;
+        width: 100%;
+        margin: 15px 0;
+        overflow: hidden;
+        user-select: none;
+      }
+
+      :host > * {
+        margin: 5px 0;
+      }
+
+      input[type='number'] {
+        color: white;
+        border: none;
+        font-weight: 300;
+        background: rgba(255, 255, 255, 0.25);
+        padding: 1px 2px;
+      }
+
+      ul {
+        list-style: none;
+        padding: 0;
+        margin: 0;
+      }
+
+      input[type='range'] {
+        -webkit-appearance: none;
+        border: none;
+        outline: none;
+        width: 100%;
+        flex: 1;
+        height: 16px;
+        background-color: transparent;
+      }
+      input[type='range']::-webkit-slider-runnable-track {
+        width: 100%;
+        height: 1px;
+        background: white;
+        border: none;
+        border-radius: 5px;
+      }
+      input[type='range']::-webkit-slider-thumb {
+        -webkit-appearance: none;
+        border: none;
+        height: 10px;
+        width: 10px;
+        border-radius: 50%;
+        background: white;
+        margin-top: -5px;
+      }
+      input[type='range']:focus {
+        outline: none;
+      }
+      input[type='range']:focus::-webkit-slider-runnable-track {
+        background: white;
+      }
+
+      input[type='range']::-moz-range-track {
+        width: 100%;
+        height: 1px;
+        background: white;
+        border: none;
+        border-radius: 5px;
+      }
+      input[type='range']::-moz-range-thumb {
+        border: none;
+        height: 10px;
+        width: 10px;
+        border-radius: 50%;
+        background: white;
+      }
+
+      input[type='range']:-moz-focusring {
+        outline: 1px solid white;
+        outline-offset: -1px;
+      }
+
+      input[type='range']::-ms-track {
+        width: 100%;
+        height: 1px;
+        background: white;
+        border-radius: 10px;
+        color: transparent;
+        border: none;
+        outline: none;
+      }
+      input[type='range']::-ms-thumb {
+        height: 10px;
+        width: 10px;
+        border-radius: 50%;
+        background: white;
+        border: none;
+        outline: none;
+        margin-top: 2px;
+      }
+
+      input:focus {
+        outline: none;
+        opacity: 1;
+      }
+
+      /* list of joint sliders */
+      ul {
+        flex: 1;
+        overflow-y: auto;
+      }
+
+      li {
+        font-size: 16px;
+        display: flex;
+        align-items: center;
+        padding: 1px 0;
+
+        width: 100%;
+        user-select: text;
+
+        transition: background 0.25s ease;
+      }
+
+      li[robot-hovered] {
+        background: rgba(255, 255, 255, 0.35);
+      }
+
+      li:hover {
+        background: rgba(255, 255, 255, 0.35);
+      }
+
+      li span {
+        padding: 0 5px;
+        max-width: 125px;
+        text-overflow: ellipsis;
+        overflow: hidden;
+      }
+
+      li input[type='number'] {
+        width: 50px;
+        overflow: hidden;
+      }
+    `
+  ]
+
+  @property({ type: Object }) viewer?: URDFManipulatorElement
+  @property({ type: Object }) robot?: URDFRobot
+
+  render() {
+    const { joints = {} } = this.robot || {}
+    const { ignoreLimits = false } = this.viewer || {}
+    const jointsArray = Object.values(joints)
+
+    return html`
+      <ul>
+        ${jointsArray.map(joint => {
+          var { jointType, name, angle, limit } = joint
+          var degVal: number | string = Number(angle || 0)
+
+          if (jointType === 'revolute' || jointType === 'continuous') {
+            degVal *= RAD2DEG
+          }
+
+          if (Math.abs(degVal) > 1) {
+            degVal = degVal.toFixed(1)
+          } else {
+            degVal = degVal.toPrecision(2)
+          }
+
+          var sliderMin
+          var sliderMax
+          var inputMin
+          var inputMax
+
+          if (ignoreLimits || jointType === 'continuous') {
+            sliderMin = -6.28
+            sliderMax = 6.28
+
+            inputMin = -6.28 * RAD2DEG
+            inputMax = 6.28 * RAD2DEG
+          } else {
+            sliderMin = limit.lower
+            sliderMax = limit.upper
+
+            inputMin = Number(limit.lower) * RAD2DEG
+            inputMax = Number(limit.upper) * RAD2DEG
+          }
+
+          return html`
+            <li joint-type=${jointType} joint-name=${name}>
+              <span title=${name}>${name}</span>
+              <input
+                type="range"
+                .value=${String(angle)}
+                step="0.0001"
+                min=${sliderMin}
+                max=${sliderMax}
+                @input=${(e: Event) => {
+                  this.viewer?.setJointValue(name, (e.target as any).value)
+                }}
+              />
+              <input
+                type="number"
+                .value=${degVal}
+                step="0.0001"
+                min=${inputMin}
+                max=${inputMax}
+                @change=${(e: Event) => {
+                  this.viewer?.setJointValue(name, Number((e.target as HTMLInputElement).value) * DEG2RAD)
+                }}
+              />
+            </li>
+          `
+        })}
+      </ul>
+    `
+  }
+
+  updated(changes: PropertyValues<this>) {
+    if (changes.has('viewer')) {
+      this._setupViewer(this.viewer)
+    }
+  }
+
+  _setupViewer(viewer?: URDFManipulatorElement) {
+    if (!viewer) {
+      return
+    }
+
+    this.robot = viewer.robot
+
+    viewer.addEventListener('urdf-processed', () => {
+      this.robot = viewer.robot
+    })
+
+    viewer.addEventListener('ignore-limits-change', () => {
+      this.requestUpdate()
+    })
+
+    viewer.addEventListener('angle-change', (e: Event) => {
+      this.requestUpdate()
+    })
+
+    viewer.addEventListener('joint-mouseover', (e: Event) => {
+      const j = this.renderRoot.querySelector(`li[joint-name="${(e as CustomEvent).detail}"]`)
+      if (j) {
+        j.setAttribute('robot-hovered', 'true')
+      }
+    })
+
+    viewer.addEventListener('joint-mouseout', (e: Event) => {
+      const j = this.renderRoot.querySelector(`li[joint-name="${(e as CustomEvent).detail}"]`)
+      if (j) {
+        j.removeAttribute('robot-hovered')
+      }
+    })
+
+    let originalNoAutoRecenter: boolean
+    viewer.addEventListener('manipulate-start', (e: Event) => {
+      const j = this.renderRoot.querySelector(`li[joint-name="${(e as CustomEvent).detail}"]`)
+      if (j) {
+        // ??
+        j.scrollIntoView({ block: 'nearest' })
+        window.scrollTo(0, 0)
+      }
+
+      originalNoAutoRecenter = viewer.noAutoRecenter
+      viewer.noAutoRecenter = true
+    })
+
+    viewer.addEventListener('manipulate-end', (e: Event) => {
+      viewer.noAutoRecenter = originalNoAutoRecenter
+    })
+
+    registerDragEvents(viewer, () => {
+      this.setColor('#263238')
+    })
+  }
+
+  setColor(color: string) {
+    this.viewer!.highlightColor = '#' + new THREE.Color(0xffffff).lerp(new THREE.Color(color), 0.35).getHexString()
+  }
+}

package/src/elements/urdf-drag-controls.ts

@@ -0,0 +1,248 @@
+import { Raycaster, Vector3, Plane, Vector2, Scene, Camera, Object3D, Ray } from 'three'
+import { URDFJoint } from 'urdf-loader'
+
+// Find the nearest parent that is a joint
+function isJoint(j: URDFJoint) {
+  return j.isURDFJoint && j.jointType !== 'fixed'
+}
+
+function findNearestJoint(child: Object3D) {
+  let curr: Object3D | null = child
+  while (curr) {
+    if (isJoint(curr as URDFJoint)) {
+      return curr
+    }
+
+    curr = curr.parent
+  }
+
+  return curr
+}
+
+const prevHitPoint = new Vector3()
+const newHitPoint = new Vector3()
+const pivotPoint = new Vector3()
+const tempVector = new Vector3()
+const tempVector2 = new Vector3()
+const projectedStartPoint = new Vector3()
+const projectedEndPoint = new Vector3()
+const plane = new Plane()
+export class URDFDragControls {
+  scene: Scene
+
+  enabled: boolean
+  raycaster: Raycaster
+  initialGrabPoint: Vector3
+  hitDistance: number
+
+  hovered: any
+  manipulating: any
+
+  constructor(scene: Scene) {
+    this.enabled = true
+    this.scene = scene
+
+    this.raycaster = new Raycaster()
+    this.initialGrabPoint = new Vector3()
+
+    this.hitDistance = -1
+    this.hovered = null
+    this.manipulating = null
+  }
+
+  update() {
+    const { raycaster, hovered, manipulating, scene } = this
+
+    if (manipulating) {
+      return
+    }
+
+    let hoveredJoint = null
+    const intersections = raycaster.intersectObject(scene, true)
+    if (intersections.length !== 0) {
+      const hit = intersections[0]
+      this.hitDistance = hit.distance
+      hoveredJoint = findNearestJoint(hit.object)
+      this.initialGrabPoint.copy(hit.point)
+    }
+
+    if (hoveredJoint !== hovered) {
+      if (hovered) {
+        this.onUnhover(hovered)
+      }
+
+      this.hovered = hoveredJoint
+
+      if (hoveredJoint) {
+        this.onHover(hoveredJoint as URDFJoint)
+      }
+    }
+  }
+
+  updateJoint(joint: URDFJoint, angle: number) {
+    joint.setJointValue(angle)
+  }
+
+  onDragStart(joint: URDFJoint) {}
+
+  onDragEnd(joint: URDFJoint) {}
+
+  onHover(joint: URDFJoint) {}
+
+  onUnhover(joint: URDFJoint) {}
+
+  getRevoluteDelta(joint: URDFJoint, startPoint: Vector3, endPoint: Vector3) {
+    // set up the plane
+    tempVector.copy(joint.axis).transformDirection(joint.matrixWorld).normalize()
+    pivotPoint.set(0, 0, 0).applyMatrix4(joint.matrixWorld)
+    plane.setFromNormalAndCoplanarPoint(tempVector, pivotPoint)
+
+    // project the drag points onto the plane
+    plane.projectPoint(startPoint, projectedStartPoint)
+    plane.projectPoint(endPoint, projectedEndPoint)
+
+    // get the directions relative to the pivot
+    projectedStartPoint.sub(pivotPoint)
+    projectedEndPoint.sub(pivotPoint)
+
+    tempVector.crossVectors(projectedStartPoint, projectedEndPoint)
+
+    const direction = Math.sign(tempVector.dot(plane.normal))
+    return direction * projectedEndPoint.angleTo(projectedStartPoint)
+  }
+
+  getPrismaticDelta(joint: URDFJoint, startPoint: Vector3, endPoint: Vector3) {
+    tempVector.subVectors(endPoint, startPoint)
+    plane.normal.copy(joint.axis).transformDirection(joint.parent!.matrixWorld).normalize()
+
+    return tempVector.dot(plane.normal)
+  }
+
+  moveRay(toRay: Ray) {
+    const { raycaster, hitDistance, manipulating } = this
+    const { ray } = raycaster
+
+    if (manipulating) {
+      ray.at(hitDistance, prevHitPoint)
+      toRay.at(hitDistance, newHitPoint)
+
+      let delta = 0
+      if (manipulating.jointType === 'revolute' || manipulating.jointType === 'continuous') {
+        delta = this.getRevoluteDelta(manipulating, prevHitPoint, newHitPoint)
+      } else if (manipulating.jointType === 'prismatic') {
+        delta = this.getPrismaticDelta(manipulating, prevHitPoint, newHitPoint)
+      }
+
+      if (delta) {
+        this.updateJoint(manipulating, manipulating.angle + delta)
+      }
+    }
+
+    this.raycaster.ray.copy(toRay)
+    this.update()
+  }
+
+  setGrabbed(grabbed: boolean) {
+    const { hovered, manipulating } = this
+    if (grabbed) {
+      if (manipulating !== null || hovered === null) {
+        return
+      }
+
+      this.manipulating = hovered
+      this.onDragStart(hovered)
+    } else {
+      if (this.manipulating === null) {
+        return
+      }
+
+      this.onDragEnd(this.manipulating)
+      this.manipulating = null
+      this.update()
+    }
+  }
+}
+
+export class PointerURDFDragControls extends URDFDragControls {
+  camera: Camera
+  domElement: HTMLElement
+
+  _mouseDown: (e: MouseEvent) => void
+  _mouseMove: (e: MouseEvent) => void
+  _mouseUp: (e: MouseEvent) => void
+
+  constructor(scene: Scene, camera: Camera, domElement: HTMLElement) {
+    super(scene)
+
+    this.camera = camera
+    this.domElement = domElement
+
+    const raycaster = new Raycaster()
+    const mouse = new Vector2()
+
+    function updateMouse(e: MouseEvent) {
+      mouse.x = (e.offsetX / domElement.offsetWidth) * 2 - 1
+      mouse.y = -(e.offsetY / domElement.offsetHeight) * 2 + 1
+    }
+
+    this._mouseDown = (e: MouseEvent) => {
+      updateMouse(e)
+      raycaster.setFromCamera(mouse, this.camera)
+      this.moveRay(raycaster.ray)
+      this.setGrabbed(true)
+    }
+
+    this._mouseMove = (e: MouseEvent) => {
+      updateMouse(e)
+      raycaster.setFromCamera(mouse, this.camera)
+      this.moveRay(raycaster.ray)
+    }
+
+    this._mouseUp = (e: MouseEvent) => {
+      updateMouse(e)
+      raycaster.setFromCamera(mouse, this.camera)
+      this.moveRay(raycaster.ray)
+      this.setGrabbed(false)
+    }
+
+    domElement.addEventListener('pointerdown', this._mouseDown)
+    domElement.addEventListener('pointermove', this._mouseMove)
+    domElement.addEventListener('pointerup', this._mouseUp)
+  }
+
+  getRevoluteDelta(joint: URDFJoint, startPoint: Vector3, endPoint: Vector3) {
+    const { camera, initialGrabPoint } = this
+
+    // set up the plane
+    tempVector.copy(joint.axis).transformDirection(joint.matrixWorld).normalize()
+    pivotPoint.set(0, 0, 0).applyMatrix4(joint.matrixWorld)
+    plane.setFromNormalAndCoplanarPoint(tempVector, pivotPoint)
+
+    tempVector.copy(camera.position).sub(initialGrabPoint).normalize()
+
+    // if looking into the plane of rotation
+    if (Math.abs(tempVector.dot(plane.normal)) > 0.3) {
+      return super.getRevoluteDelta(joint, startPoint, endPoint)
+    } else {
+      // get the up direction
+      tempVector.set(0, 1, 0).transformDirection(camera.matrixWorld)
+
+      // get points projected onto the plane of rotation
+      plane.projectPoint(startPoint, projectedStartPoint)
+      plane.projectPoint(endPoint, projectedEndPoint)
+
+      tempVector.set(0, 0, -1).transformDirection(camera.matrixWorld)
+      tempVector.cross(plane.normal)
+      tempVector2.subVectors(endPoint, startPoint)
+
+      return tempVector.dot(tempVector2)
+    }
+  }
+
+  dispose() {
+    const { domElement } = this
+    domElement.removeEventListener('pointerdown', this._mouseDown)
+    domElement.removeEventListener('pointermove', this._mouseMove)
+    domElement.removeEventListener('pointerup', this._mouseUp)
+  }
+}

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

@@ -0,0 +1,133 @@
+import { customElement } from 'lit/decorators.js'
+import * as THREE from 'three'
+import { PointerURDFDragControls } from './urdf-drag-controls'
+import { Material, MeshPhongMaterial } from 'three'
+import URDFViewerElement from './urdf-viewer-element'
+import { URDFJoint } from 'urdf-loader'
+
+// urdf-manipulator element
+// Displays a URDF model that can be manipulated with the mouse
+
+// Events
+// joint-mouseover: Fired when a joint is hovered over
+// joint-mouseout: Fired when a joint is no longer hovered over
+// manipulate-start: Fires when a joint is manipulated
+// manipulate-end: Fires when a joint is done being manipulated
+
+@customElement('urdf-viewer')
+export default class URDFManipulatorElement extends URDFViewerElement {
+  static get observedAttributes() {
+    return ['highlight-color', ...super.observedAttributes]
+  }
+
+  highlightMaterial: MeshPhongMaterial
+  dragControls: PointerURDFDragControls
+
+  get disableDragging() {
+    return this.hasAttribute('disable-dragging')
+  }
+
+  set disableDragging(val) {
+    val ? this.setAttribute('disable-dragging', String(!!val)) : this.removeAttribute('disable-dragging')
+  }
+
+  get highlightColor() {
+    return this.getAttribute('highlight-color') || '#FFFFFF'
+  }
+
+  set highlightColor(val) {
+    val ? this.setAttribute('highlight-color', val) : this.removeAttribute('highlight-color')
+  }
+
+  constructor() {
+    super()
+
+    // The highlight material
+    this.highlightMaterial = new THREE.MeshPhongMaterial({
+      shininess: 10,
+      color: this.highlightColor,
+      emissive: this.highlightColor,
+      emissiveIntensity: 0.25
+    })
+
+    const isJoint = (j: URDFJoint) => {
+      return j.isURDFJoint && j.jointType !== 'fixed'
+    }
+
+    // Highlight the link geometry under a joint
+    const highlightLinkGeometry = (m: THREE.Material, revert: boolean) => {
+      const traverse = (c: any) => {
+        // Set or revert the highlight color
+        if (c.type === 'Mesh') {
+          if (revert) {
+            c.material = c.__origMaterial
+            delete c.__origMaterial
+          } else {
+            c.__origMaterial = c.material
+            c.material = this.highlightMaterial
+          }
+        }
+
+        // Look into the children and stop if the next child is
+        // another joint
+        if (c === m || !isJoint(c)) {
+          for (let i = 0; i < c.children.length; i++) {
+            traverse(c.children[i])
+          }
+        }
+      }
+
+      traverse(m)
+    }
+
+    const el = this.renderer.domElement
+
+    const dragControls = new PointerURDFDragControls(this.scene, this.camera, el)
+
+    dragControls.onDragStart = joint => {
+      this.dispatchEvent(new CustomEvent('manipulate-start', { bubbles: true, cancelable: true, detail: joint.name }))
+      this.controls.enabled = false
+      this.redraw()
+    }
+
+    dragControls.onDragEnd = joint => {
+      this.dispatchEvent(new CustomEvent('manipulate-end', { bubbles: true, cancelable: true, detail: joint.name }))
+      this.controls.enabled = true
+      this.redraw()
+    }
+
+    dragControls.updateJoint = (joint, angle) => {
+      this.setJointValue(joint.name, angle)
+    }
+
+    dragControls.onHover = (joint: URDFJoint) => {
+      highlightLinkGeometry(joint as any, false)
+      this.dispatchEvent(new CustomEvent('joint-mouseover', { bubbles: true, cancelable: true, detail: joint.name }))
+      this.redraw()
+    }
+
+    dragControls.onUnhover = joint => {
+      highlightLinkGeometry(joint as any, true)
+      this.dispatchEvent(new CustomEvent('joint-mouseout', { bubbles: true, cancelable: true, detail: joint.name }))
+      this.redraw()
+    }
+
+    this.dragControls = dragControls
+  }
+
+  disconnectedCallback() {
+    super.disconnectedCallback()
+    this.dragControls.dispose()
+  }
+
+  attributeChangedCallback(attr: string, oldval: any, newval: any) {
+    super.attributeChangedCallback(attr, oldval, newval)
+
+    switch (attr) {
+      case 'highlight-color':
+        this.highlightMaterial.color.set(this.highlightColor)
+        this.highlightMaterial.emissive.set(this.highlightColor)
+        break
+    }
+  }
+}