@wandelbots/wandelbots-js-react-components 1.3.0 → 1.3.2

package/src/components/jogging/JoggingStore.tsx

@@ -0,0 +1,294 @@
+import { keyBy } from "lodash-es"
+import { autorun, makeAutoObservable, type IReactionDisposer } from "mobx"
+import type { CoordinateSystem, JoggerConnection, MotionGroupSpecification, RobotTcp } from "@wandelbots/wandelbots-js"
+import { tryParseJson } from "@wandelbots/wandelbots-js"
+
+const discreteIncrementOptions = [
+  { id: "0.1", mm: 0.1, degrees: 0.05 },
+  { id: "1", mm: 1, degrees: 0.5 },
+  { id: "5", mm: 5, degrees: 2.5 },
+  { id: "10", mm: 10, degrees: 5 },
+]
+
+const incrementOptions = [
+  { id: "continuous" },
+  ...discreteIncrementOptions,
+] as const
+
+export type DiscreteIncrementOption = (typeof discreteIncrementOptions)[number]
+export type IncrementOption = (typeof incrementOptions)[number]
+export type IncrementOptionId = IncrementOption["id"]
+
+export class JoggingStore {
+  selectedTabId: "cartesian" | "joint" | "debug" = "cartesian"
+
+  // TODO
+  isLocked: boolean = false
+
+  /**
+   * Id of selected coordinate system from among those defined on the API side
+   */
+  selectedCoordSystemId: string = "world"
+
+  /** Id of selected tool center point from among the options available on the robot */
+  selectedTcpId: string = ""
+
+  /**
+   * Id of selected increment amount for jogging. Options are defined by robot pad.
+   * When non-continuous, jogging moves the robot by a fixed number of mm or degrees
+   * each time the button is pressed, for extra precision
+   */
+  selectedIncrementId: IncrementOptionId = "continuous"
+
+  /**
+   * When on the cartesian tab, jogging can be either translating or rotating
+   * around the TCP.
+   */
+  selectedCartesianMotionType: "translate" | "rotate" = "translate"
+
+  /** True when the API is busy doing a planned increment jog motion */
+  incrementJoggingInProgress = false
+
+  /** How fast the robot goes when doing cartesian translate jogging in mm/s */
+  translationVelocityMmPerSec: number = 10
+  /** How fast the robot goes when doing cartesian or joint rotation jogging in °/s */
+  rotationVelocityDegPerSec: number = 1
+
+  /** Minimum translation velocity user can choose on the velocity slider in °/s */
+  minTranslationVelocityMmPerSec: number = 5
+  /** Maximum translation velocity user can choose on the velocity slider in °/s */
+  maxTranslationVelocityMmPerSec: number = 250
+
+  /** Minimum rotation velocity user can choose on the velocity slider in °/s */
+  minRotationVelocityDegPerSec: number = 1
+  /** Maximum rotation velocity user can choose on the velocity slider in °/s */
+  maxRotationVelocityDegPerSec: number = 60
+
+  disposers: IReactionDisposer[] = []
+
+  /**
+   * Load a jogging store with the relevant data it needs
+   * from the backend
+   */
+  static async loadFor(jogger: JoggerConnection) {
+    const { nova } = jogger
+
+    // Find out what TCPs this motion group has (we need it for jogging)
+    const [motionGroupSpec, { coordinatesystems }, { tcps }] = await Promise.all([
+      nova.api.motionGroupInfos.getMotionGroupSpecification(jogger.motionGroupId),
+
+      // Fetch coord systems so user can select between them
+      nova.api.coordinateSystems.listCoordinateSystems("ROTATION_VECTOR"),
+
+      // Same for TCPs
+      nova.api.motionGroupInfos.listTcps(
+        jogger.motionGroupId,
+        "ROTATION_VECTOR",
+      ),
+    ])
+
+    return new JoggingStore(jogger, motionGroupSpec, coordinatesystems || [], tcps || [])
+  }
+
+  constructor(
+    readonly jogger: JoggerConnection,
+    readonly motionGroupSpec: MotionGroupSpecification,
+    readonly coordSystems: CoordinateSystem[],
+    readonly tcps: RobotTcp[],
+  ) {
+    // TODO workaround for default coord system on backend having a canonical id
+    // of empty string. Can remove when fixed on API side
+    for (const cs of coordSystems) {
+      if (cs.coordinate_system === "") {
+        cs.coordinate_system = "world"
+        break
+      }
+    }
+    this.selectedCoordSystemId = coordSystems[0]?.coordinate_system || "world"
+    this.selectedTcpId = tcps[0]?.id || ""
+
+    makeAutoObservable(this, {}, { autoBind: true })
+
+    // Load user settings from local storage if available
+    this.loadFromLocalStorage()
+
+    // Automatically save user settings to local storage when save changes
+    this.disposers.push(autorun(() => this.saveToLocalStorage()))
+    ;(window as any).joggingStore = this
+  }
+
+  dispose() {
+    for (const dispose of this.disposers) {
+      dispose()
+    }
+    this.jogger.dispose()
+  }
+
+  loadFromLocalStorage() {
+    const save = tryParseJson(localStorage.getItem("joggingToolStore"))
+    if (!save) return
+
+    if (this.tabsById[save.selectedTabId]) {
+      this.selectedTabId = save.selectedTabId
+    }
+
+    if (this.coordSystemsById[save.selectedCoordSystemId]) {
+      this.selectedCoordSystemId = save.selectedCoordSystemId
+    }
+
+    if (this.tcpsById[save.selectedTcpId]) {
+      this.selectedTcpId = save.selectedTcpId
+    }
+
+    if (this.incrementOptionsById[save.selectedIncrementId]) {
+      this.selectedIncrementId = save.selectedIncrementId
+    }
+
+    if (["translate", "rotate"].includes(save.selectedCartesianMotionType)) {
+      this.selectedCartesianMotionType = save.selectedCartesianMotionType
+    }
+  }
+
+  saveToLocalStorage() {
+    localStorage.setItem(
+      "joggingToolStore",
+      JSON.stringify(this.localStorageSave),
+    )
+  }
+
+  get localStorageSave() {
+    return {
+      selectedTabId: this.selectedTabId,
+      selectedCoordSystemId: this.selectedCoordSystemId,
+      selectedTcpId: this.selectedTcpId,
+      selectedIncrementId: this.selectedIncrementId,
+      selectedCartesianMotionType: this.selectedCartesianMotionType,
+    }
+  }
+
+  get tabs() {
+    return [
+      {
+        id: "cartesian",
+        label: "Cartesian",
+      },
+      {
+        id: "joint",
+        label: "Joint",
+      },
+    ] as const
+  }
+
+  get incrementOptions() {
+    return incrementOptions
+  }
+
+  get discreteIncrementOptions() {
+    return discreteIncrementOptions
+  }
+
+  get incrementOptionsById() {
+    return keyBy(this.incrementOptions, (o) => o.id)
+  }
+
+  get tabsById() {
+    return keyBy(this.tabs, (t) => t.id)
+  }
+
+  get currentTab() {
+    return this.tabsById[this.selectedTabId] || this.tabs[0]!
+  }
+
+  get tabIndex() {
+    return this.tabs.indexOf(this.currentTab)
+  }
+
+  get coordSystemsById() {
+    return keyBy(this.coordSystems, (cs) => cs.coordinate_system)
+  }
+
+  get selectedCoordSystem() {
+    return this.coordSystemsById[this.selectedCoordSystemId]
+  }
+
+  get tcpsById() {
+    return keyBy(this.tcps, (tcp) => tcp.id)
+  }
+
+  get selectedDiscreteIncrement() {
+    return discreteIncrementOptions.find(
+      (d) => d.id === this.selectedIncrementId,
+    )
+  }
+
+  /** The selected rotation velocity converted to radians per second */
+  get rotationVelocityRadsPerSec() {
+    return (this.rotationVelocityDegPerSec * Math.PI) / 180
+  }
+
+  get velocityInCurrentUnits() {
+    return this.currentMotionType === "translate"
+      ? this.translationVelocityMmPerSec
+      : this.rotationVelocityDegPerSec
+  }
+
+  get minVelocityInCurrentUnits() {
+    return this.currentMotionType === "translate"
+      ? this.minTranslationVelocityMmPerSec
+      : this.minRotationVelocityDegPerSec
+  }
+
+  get maxVelocityInCurrentUnits() {
+    return this.currentMotionType === "translate"
+      ? this.maxTranslationVelocityMmPerSec
+      : this.maxRotationVelocityDegPerSec
+  }
+
+  /**
+   * For velocity unit purposes, joint and cartesian rotation
+   * are treated as the same type of motion
+   */
+  get currentMotionType() {
+    if (
+      this.selectedTabId === "cartesian" &&
+      this.selectedCartesianMotionType === "translate"
+    ) {
+      return "translate"
+    } else {
+      return "rotate"
+    }
+  }
+
+  onTabChange(_event: React.SyntheticEvent, newValue: number) {
+    const tab = this.tabs[newValue] || this.tabs[0]!
+    this.selectedTabId = tab.id
+  }
+
+  setSelectedCoordSystemId(id: string) {
+    this.selectedCoordSystemId = id
+  }
+
+  setSelectedTcpId(id: string) {
+    this.selectedTcpId = id
+  }
+
+  setSelectedIncrementId(id: IncrementOptionId) {
+    this.selectedIncrementId = id
+  }
+
+  setIncrementJoggingInProgress(inProgress: boolean) {
+    this.incrementJoggingInProgress = inProgress
+  }
+
+  setVelocityFromSlider(velocity: number) {
+    if (this.currentMotionType === "translate") {
+      this.translationVelocityMmPerSec = velocity
+    } else {
+      this.rotationVelocityDegPerSec = velocity
+    }
+  }
+
+  setSelectedCartesianMotionType(type: "translate" | "rotate") {
+    this.selectedCartesianMotionType = type
+  }
+}

package/src/components/jogging/JoggingVelocitySlider.tsx

@@ -0,0 +1,56 @@
+import { Stack, Divider, Typography } from "@mui/material"
+import { observer, useLocalObservable } from "mobx-react-lite"
+import type { JoggingStore } from "./JoggingStore"
+import { VelocitySlider } from "../VelocitySlider"
+import { useTranslation } from "react-i18next"
+
+export const JoggingVelocitySlider = observer(
+  ({ store }: { store: JoggingStore }) => {
+    const { t } = useTranslation()
+
+    const state = useLocalObservable(() => ({
+      get valueLabelFormat() {
+        if (store.currentMotionType === "translate") {
+          return (value: number) =>
+            `v=${t("Jogging.Cartesian.Translation.velocityMmPerSec.lb", { amount: value })}`
+        } else {
+          return (value: number) =>
+            `v=${t("Jogging.Cartesian.Rotation.velocityDegPerSec.lb", { amount: value })}`
+        }
+      },
+    }))
+
+    return (
+      <Stack>
+        <Divider />
+        <Stack
+          sx={{
+            margin: "0px 20px",
+            marginTop: "0.8rem",
+            marginBottom: "0.8rem",
+          }}
+        >
+          <Stack sx={{ width: "380px", maxWidth: "90%", margin: "auto" }}>
+            <Typography
+              sx={{
+                fontWeight: "bold",
+                fontSize: "15px",
+              }}
+            >
+              {t("Jogging.Velocity.lb")}
+            </Typography>
+            <VelocitySlider
+              velocity={store.velocityInCurrentUnits}
+              min={store.minVelocityInCurrentUnits}
+              max={store.maxVelocityInCurrentUnits}
+              onVelocityChange={store.setVelocityFromSlider}
+              disabled={store.isLocked}
+              valueLabelFormat={state.valueLabelFormat}
+            />
+          </Stack>
+        </Stack>
+        <Divider />
+      </Stack>
+    )
+  },
+)

package/src/components/robots/ABB_1200_07_7.tsx

@@ -0,0 +1,123 @@
+import { useGLTF } from "@react-three/drei"
+import type * as THREE from "three"
+import type { RobotProps } from "./SupportedRobot"
+import { animated } from "@react-spring/three"
+import RobotAnimator from "./RobotAnimator"
+
+export function ABB_1200_07_7({ modelURL, connectedMotionGroup, ...props }: RobotProps) {
+  const gltf = useGLTF(modelURL) as any
+  const nodes = gltf.nodes
+  const materials = gltf.materials
+  const rotationOffsets = [0, -Math.PI / 2, 0, 0, 0, 0]
+
+  function setRotation(jointObjects: THREE.Object3D[], jointValues: number[]) {
+    jointObjects.forEach(
+      (object, index) =>
+        (object.rotation.y = jointValues[index]! + rotationOffsets[index]!),
+    )
+  }
+
+  return (
+    <>
+      <RobotAnimator
+        connectedMotionGroup={connectedMotionGroup}
+        robotRootObjectName="Scene"
+        onRotationChanged={setRotation}
+      />
+      <group {...props} dispose={null}>
+        <group name="Scene">
+          <group name="IRB1200_7_70_IRC5" rotation={[Math.PI / 2, 0, 0]}>
+            <animated.group
+              name="IRB1200_7_70_IRC5_J01"
+              rotation={[-Math.PI / 2, 0, 0]}
+            >
+              <animated.group
+                name="IRB1200_7_70_IRC5_J02"
+                position={[0, 0.399, 0]}
+                rotation={[-Math.PI / 2, -Math.PI / 2, 0]}
+              >
+                <animated.group
+                  name="IRB1200_7_70_IRC5_J03"
+                  position={[0.35, 0, 0]}
+                >
+                  <animated.group
+                    name="IRB1200_7_70_IRC5_J04"
+                    position={[0.042, 0, 0]}
+                    rotation={[-Math.PI / 2, 0, 0]}
+                  >
+                    <animated.group
+                      name="IRB1200_7_70_IRC5_J05"
+                      position={[0, 0.351, 0]}
+                      rotation={[Math.PI / 2, 0, 0]}
+                    >
+                      <animated.group
+                        name="IRB1200_7_70_IRC5_J06"
+                        rotation={[Math.PI / 2, 0, -Math.PI]}
+                      >
+                        <group
+                          name="IRB1200_7_70_IRC5_FLG"
+                          position={[0, 0.082, 0]}
+                        />
+                        <mesh
+                          name="IRB1200_7_70_IRC5_L06"
+                          castShadow
+                          receiveShadow
+                          geometry={nodes.IRB1200_7_70_IRC5_L06.geometry}
+                          material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                        />
+                      </animated.group>
+                      <mesh
+                        name="IRB1200_7_70_IRC5_L05"
+                        castShadow
+                        receiveShadow
+                        geometry={nodes.IRB1200_7_70_IRC5_L05.geometry}
+                        material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                      />
+                    </animated.group>
+                    <mesh
+                      name="IRB1200_7_70_IRC5_L04"
+                      castShadow
+                      receiveShadow
+                      geometry={nodes.IRB1200_7_70_IRC5_L04.geometry}
+                      material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                    />
+                  </animated.group>
+                  <mesh
+                    name="IRB1200_7_70_IRC5_L03"
+                    castShadow
+                    receiveShadow
+                    geometry={nodes.IRB1200_7_70_IRC5_L03.geometry}
+                    material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                  />
+                </animated.group>
+                <mesh
+                  name="IRB1200_7_70_IRC5_L02"
+                  castShadow
+                  receiveShadow
+                  geometry={nodes.IRB1200_7_70_IRC5_L02.geometry}
+                  material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                />
+              </animated.group>
+              <mesh
+                name="IRB1200_7_70_IRC5_L01"
+                castShadow
+                receiveShadow
+                geometry={nodes.IRB1200_7_70_IRC5_L01.geometry}
+                material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+                rotation={[Math.PI / 2, 0, 0]}
+              />
+            </animated.group>
+            <mesh
+              name="IRB1200_7_70_IRC5_L00"
+              castShadow
+              receiveShadow
+              geometry={nodes.IRB1200_7_70_IRC5_L00.geometry}
+              material={materials["IRB1200_7-70_IRC5.003_Bake"]}
+              rotation={[-Math.PI / 2, 0, 0]}
+            />
+          </group>
+        </group>
+      </group>
+    </>
+  )
+}

package/src/components/robots/AxisConfig.ts

@@ -0,0 +1,3 @@
+export type AxisConfig = number[]
+
+export const defaultAxisConfig: AxisConfig = Array(6).fill(2 * Math.PI)

package/src/components/robots/DHRobot.tsx

@@ -0,0 +1,129 @@
+import { Vector3, Matrix4, Quaternion } from "three"
+import { Line } from "@react-three/drei"
+import type { LineGeometry } from "three/examples/jsm/lines/LineGeometry.js"
+import type { DHParameter } from "@wandelbots/wandelbots-api-client"
+import RobotAnimator from "./RobotAnimator"
+import type * as THREE from "three"
+import {
+  getAllJointsByName,
+  type RobotSceneJoint,
+} from "../utils/robotTreeQuery"
+import type { DHRobotProps } from "./SupportedRobot"
+
+const CHILD_LINE = "line"
+const CHILD_MESH = "mesh"
+
+export function DHRobot({ connectedMotionGroup, ...props }: DHRobotProps) {
+  // reused in every update
+  const accumulatedMatrix = new Matrix4()
+
+  // Updates accumulatedMatrix with every execution
+  // Reset the matrix to identity if you start a new position update
+  function getLinePoints(
+    dhParameter: DHParameter,
+    jointRotation: number,
+  ): {
+    a: THREE.Vector3
+    b: THREE.Vector3
+  } {
+    const position = new Vector3()
+    const quaternion = new Quaternion()
+    const scale = new Vector3()
+    accumulatedMatrix.decompose(position, quaternion, scale)
+    const prevPosition = position.clone() // Update the previous position
+
+    const matrix = new Matrix4()
+      .makeRotationY(
+        dhParameter.theta! +
+          jointRotation * (dhParameter.reverse_rotation_direction ? -1 : 1),
+      ) // Rotate around Z
+      .multiply(new Matrix4().makeTranslation(0, dhParameter.d! / 1000, 0)) // Translate along Z
+      .multiply(new Matrix4().makeTranslation(dhParameter.a! / 1000, 0, 0)) // Translate along X
+      .multiply(new Matrix4().makeRotationX(dhParameter.alpha!)) // Rotate around X
+
+    // Accumulate transformations
+    accumulatedMatrix.multiply(matrix)
+    accumulatedMatrix.decompose(position, quaternion, scale)
+    return { a: prevPosition, b: position }
+  }
+
+  function setJointLineRotation(
+    jointIndex: number,
+    line: THREE.Line,
+    mesh: THREE.Mesh,
+    jointValue: number,
+  ) {
+    const dh_parameters =
+      connectedMotionGroup.motionGroupSpecification.dh_parameters
+    if (!dh_parameters) {
+      return
+    }
+
+    const dh_parameter = dh_parameters[jointIndex]
+    if (!dh_parameter) {
+      return
+    }
+
+    const { a, b } = getLinePoints(dh_parameter, jointValue)
+    const lineGeometry = line.geometry as LineGeometry
+    lineGeometry.setPositions([a.toArray(), b.toArray()].flat())
+
+    mesh.position.set(b.x, b.y, b.z)
+  }
+
+  function setRotation(joints: THREE.Object3D[], jointValues: number[]) {
+    accumulatedMatrix.identity()
+    joints.forEach((joint, jointIndex) => {
+      setJointLineRotation(
+        jointIndex,
+        joint.getObjectByName(CHILD_LINE) as THREE.Line,
+        joint.getObjectByName(CHILD_MESH) as THREE.Mesh,
+        jointValues[jointIndex]!,
+      )
+    })
+  }
+
+  function getAllJoints(rootObject: THREE.Object3D): RobotSceneJoint[] {
+    return getAllJointsByName(rootObject, "^group_[0-9]+$")
+  }
+
+  return (
+    <>
+      <RobotAnimator
+        connectedMotionGroup={connectedMotionGroup}
+        robotRootObjectName="Scene"
+        onRotationChanged={setRotation}
+        jointCollector={getAllJoints}
+      />
+      <group {...props} name="Scene">
+        <mesh>
+          <sphereGeometry args={[0.01, 32, 32]} />
+          <meshStandardMaterial color={"black"} depthTest={true} />
+        </mesh>
+        {connectedMotionGroup.motionGroupSpecification.dh_parameters!.map(
+          (param, index) => {
+            const { a, b } = getLinePoints(
+              param,
+              connectedMotionGroup.rapidlyChangingMotionState.state
+                .joint_position.joints[index] ?? 0,
+            )
+            return (
+              <group name={`group_${index}`} key={"group_" + index}>
+                <Line
+                  name={CHILD_LINE}
+                  points={[a, b]}
+                  color={"white"}
+                  lineWidth={5}
+                />
+                <mesh name={CHILD_MESH} key={"mesh_" + index} position={b}>
+                  <sphereGeometry args={[0.01, 32, 32]} />
+                  <meshStandardMaterial color={"black"} depthTest={true} />
+                </mesh>
+              </group>
+            )
+          },
+        )}
+      </group>
+    </>
+  )
+}