@wandelbots/nova-js 3.2.0-pr.dev-e2e-jogging-test.143.4f02caf → 3.2.0-pr.feat-v2.155.e91b019

package/src/index.ts

@@ -3,3 +3,4 @@ export * from "./lib/availableStorage"
 export * from "./lib/converters"
 export * from "./lib/errorHandling"
 export * from "./LoginWithAuth0"
+export * from "./lib/v1/wandelscriptUtils"

package/src/lib/converters.ts

@@ -1,5 +1,3 @@
-import type { Pose } from "@wandelbots/nova-api/v1"
-
 /** Try to parse something as JSON; return undefined if we can't */
 // biome-ignore lint/suspicious/noExplicitAny: it's json
 export function tryParseJson(json: unknown): any {
@@ -39,27 +37,6 @@ export function degreesToRadians(degrees: number): number {
   return degrees * (Math.PI / 180)
 }
 
-/**
- * Convert a Pose object representing a motion group position
- * into a string which represents that pose in Wandelscript.
- */
-export function poseToWandelscriptString(
-  pose: Pick<Pose, "position" | "orientation">,
-) {
-  const position = [pose.position.x, pose.position.y, pose.position.z]
-  const orientation = [
-    pose.orientation?.x ?? 0,
-    pose.orientation?.y ?? 0,
-    pose.orientation?.z ?? 0,
-  ]
-
-  const positionValues = position.map((v) => v.toFixed(1))
-  // Rotation needs more precision since it's in radians
-  const rotationValues = orientation.map((v) => v.toFixed(4))
-
-  return `(${positionValues.concat(rotationValues).join(", ")})`
-}
-
 /**
  * Check for coordinate system id equivalence, accounting for the "world" default
  * on empty/undefined values.
@@ -73,3 +50,8 @@ export function isSameCoordinateSystem(
 
   return firstCoordSystem === secondCoordSystem
 }
+
+/**
+ * Helpful const for converting {x, y, z} to [x, y, z] and vice versa
+ */
+export const XYZ_TO_VECTOR = { x: 0, y: 1, z: 2 }

package/src/lib/v1/MotionStreamConnection.ts

@@ -81,7 +81,7 @@ export class MotionStreamConnection {
 
     // Wait for the first message to get the initial state
     const firstMessage = await motionStateSocket.firstMessage()
-    console.log("got first message", firstMessage)
+    console.log("got first message", tryParseJson(firstMessage.data))
     const initialMotionState = tryParseJson(firstMessage.data)
       ?.result as MotionGroupStateResponse
 

package/src/lib/v1/NovaClient.ts

@@ -1,3 +1,8 @@
+/**
+ * @fileoverview
+ * @deprecated The nova v1 client is deprecated. Please use the v2 client from `@wandelbots/nova-js/v2` instead.
+ */
+
 import type { Configuration as BaseConfiguration } from "@wandelbots/nova-api/v1"
 import type { AxiosRequestConfig } from "axios"
 import axios, { isAxiosError } from "axios"
@@ -54,6 +59,7 @@ function permissiveInstanceUrlParse(url: string): string {
 
 /**
  * Client for connecting to a Nova instance and controlling robots.
+ * @deprecated The nova v1 client is deprecated. Please use the v2 client from `@wandelbots/nova-js/v2` instead.
  */
 export class NovaClient {
   readonly api: NovaCellAPIClient

package/src/lib/v1/index.ts

@@ -1,3 +1,8 @@
+/**
+ * @fileoverview
+ * @deprecated The nova v1 client is deprecated. Please use the v2 client from `@wandelbots/nova-js/v2` instead.
+ */
+
 export * from "@wandelbots/nova-api/v1"
 export * from "./ConnectedMotionGroup"
 export * from "./getLatestTrajectories"
@@ -6,3 +11,4 @@ export * from "./MotionStreamConnection"
 export * from "./NovaCellAPIClient"
 export * from "./NovaClient"
 export * from "./ProgramStateConnection"
+export * from "./wandelscriptUtils"

package/src/lib/v1/mock/MockNovaInstance.ts

@@ -11,7 +11,6 @@ import * as pathToRegexp from "path-to-regexp"
 import type { AutoReconnectingWebsocket } from "../../AutoReconnectingWebsocket"
 
 /**
- * EXPERIMENTAL
  * Ultra-simplified mock Nova server for testing stuff
  */
 export class MockNovaInstance {

package/src/lib/v1/wandelscriptUtils.ts

@@ -0,0 +1,22 @@
+import type { Pose } from "@wandelbots/nova-api/v1"
+
+/**
+ * Convert a Pose object representing a motion group position
+ * into a string which represents that pose in Wandelscript.
+ */
+export function poseToWandelscriptString(
+  pose: Pick<Pose, "position" | "orientation">,
+) {
+  const position = [pose.position.x, pose.position.y, pose.position.z]
+  const orientation = [
+    pose.orientation?.x ?? 0,
+    pose.orientation?.y ?? 0,
+    pose.orientation?.z ?? 0,
+  ]
+
+  const positionValues = position.map((v) => v.toFixed(1))
+  // Rotation needs more precision since it's in radians
+  const rotationValues = orientation.map((v) => v.toFixed(4))
+
+  return `(${positionValues.concat(rotationValues).join(", ")})`
+}

package/src/lib/v2/ConnectedMotionGroup.ts

@@ -0,0 +1,415 @@
+import type {
+  MotionGroupDescription,
+  MotionGroupState,
+  RobotControllerState,
+  OperationMode,
+  SafetyStateType,
+} from "@wandelbots/nova-api/v2"
+import { makeAutoObservable, runInAction } from "mobx"
+import * as THREE from "three"
+import type { AutoReconnectingWebsocket } from "../AutoReconnectingWebsocket"
+import { tryParseJson } from "../converters"
+import { jointValuesEqual, tcpMotionEqual } from "./motionStateUpdate"
+import type { NovaClient } from "./NovaClient"
+import type { Vector3Simple } from "./types/vector3"
+
+const MOTION_DELTA_THRESHOLD = 0.0001
+
+export type RobotTcpLike = {
+  id: string
+  readable_name: string
+  position: Vector3Simple
+  orientation: Vector3Simple
+}
+
+export type MotionGroupOption = {
+  selectionId: string
+}
+
+/**
+ * Store representing the current state of a connected motion group.
+ */
+export class ConnectedMotionGroup {
+  static async connect(nova: NovaClient, motionGroupId: string) {
+    const [_motionGroupIndex, controllerId] = motionGroupId.split("@") as [
+      string,
+      string,
+    ]
+
+    const controller =
+      await nova.api.controller.getCurrentRobotControllerState(controllerId)
+    const motionGroup = controller?.motion_groups.find(
+      (mg) => mg.motion_group === motionGroupId,
+    )
+    if (!controller || !motionGroup) {
+      throw new Error(
+        `Controller ${controllerId} or motion group ${motionGroupId} not found`,
+      )
+    }
+
+    const motionStateSocket = nova.openReconnectingWebsocket(
+      `/controllers/${controllerId}/motion-groups/${motionGroupId}/state-stream`,
+    )
+
+    // Wait for the first message to get the initial state
+    const firstMessage = await motionStateSocket.firstMessage()
+    const initialMotionState = tryParseJson(firstMessage.data)
+      ?.result as MotionGroupState
+
+    if (!initialMotionState) {
+      throw new Error(
+        `Unable to parse initial motion state message ${firstMessage.data}`,
+      )
+    }
+
+    console.log(
+      `Connected motion state websocket to motion group ${motionGroup.motion_group}. Initial state:\n  `,
+      initialMotionState,
+    )
+
+    // Check if robot is virtual or physical
+    const config = await nova.api.controller.getRobotController(
+      controller.controller,
+    )
+    const isVirtual = config.configuration.kind === "VirtualController"
+
+    // If there's a configured mounting, we need it to show the right
+    // position of the robot model
+    const description = await nova.api.motionGroup.getMotionGroupDescription(
+      controllerId,
+      motionGroup.motion_group,
+    )
+
+    // Find out what TCPs this motion group has (we need it for jogging)
+    // There are converted into a RobotTcpLike for easier use in the UI
+    const tcps: RobotTcpLike[] = Object.entries(description.tcps || {}).map(
+      ([id, tcp]) => ({
+        id,
+        readable_name: tcp.name,
+        position: tcp.pose.position as Vector3Simple,
+        orientation: tcp.pose.orientation as Vector3Simple,
+      }),
+    )
+
+    // Open the websocket to monitor controller state for e.g. e-stop
+    const controllerStateSocket = nova.openReconnectingWebsocket(
+      `/controllers/${controller.controller}/state-stream?response_rate=1000`,
+    )
+
+    // Wait for the first message to get the initial state
+    const firstControllerMessage = await controllerStateSocket.firstMessage()
+    const initialControllerState = tryParseJson(firstControllerMessage.data)
+      ?.result as RobotControllerState
+
+    if (!initialControllerState) {
+      throw new Error(
+        `Unable to parse initial controller state message ${firstControllerMessage.data}`,
+      )
+    }
+
+    console.log(
+      `Connected controller state websocket to controller ${controller.controller}. Initial state:\n  `,
+      initialControllerState,
+    )
+
+    return new ConnectedMotionGroup(
+      nova,
+      controller,
+      motionGroup,
+      initialMotionState,
+      motionStateSocket,
+      isVirtual,
+      tcps,
+      description,
+      initialControllerState,
+      controllerStateSocket,
+    )
+  }
+
+  connectedJoggingSocket: WebSocket | null = null
+  // biome-ignore lint/suspicious/noExplicitAny: legacy code
+  planData: any | null // tmp
+  joggingVelocity: number = 10
+
+  // Not mobx-observable as this changes very fast; should be observed
+  // using animation frames
+  rapidlyChangingMotionState: MotionGroupState
+
+  // Response rate on the websocket should be a bit slower on this one since
+  // we don't use the motion data
+  controllerState: RobotControllerState
+
+  /**
+   * Reflects activation state of the motion group / robot servos. The
+   * movement controls in the UI should only be enabled in the "active" state
+   */
+  activationState: "inactive" | "activating" | "deactivating" | "active" =
+    "inactive"
+
+  constructor(
+    readonly nova: NovaClient,
+    readonly controller: RobotControllerState,
+    readonly motionGroup: MotionGroupState,
+    readonly initialMotionState: MotionGroupState,
+    readonly motionStateSocket: AutoReconnectingWebsocket,
+    readonly isVirtual: boolean,
+    readonly tcps: RobotTcpLike[],
+    readonly description: MotionGroupDescription,
+    readonly initialControllerState: RobotControllerState,
+    readonly controllerStateSocket: AutoReconnectingWebsocket,
+  ) {
+    this.rapidlyChangingMotionState = initialMotionState
+    this.controllerState = initialControllerState
+
+    // Track controller state updates (e.g. safety state and operation mode)
+    controllerStateSocket.addEventListener("message", (event) => {
+      const data = tryParseJson(event.data)?.result
+
+      if (!data) {
+        return
+      }
+
+      runInAction(() => {
+        this.controllerState = data
+      })
+    })
+
+    motionStateSocket.addEventListener("message", (event) => {
+      const latestMotionState = tryParseJson(event.data)?.result as
+        | MotionGroupState
+        | undefined
+
+      if (!latestMotionState) {
+        throw new Error(
+          `Failed to get motion state for ${this.motionGroupId}: ${event.data}`,
+        )
+      }
+
+      // handle motionState message
+      if (
+        !jointValuesEqual(
+          this.rapidlyChangingMotionState.joint_position,
+          latestMotionState.joint_position,
+          MOTION_DELTA_THRESHOLD,
+        )
+      ) {
+        runInAction(() => {
+          this.rapidlyChangingMotionState = latestMotionState
+        })
+      }
+
+      // handle tcpPose message
+      if (
+        !tcpMotionEqual(
+          this.rapidlyChangingMotionState,
+          latestMotionState,
+          MOTION_DELTA_THRESHOLD,
+        )
+      ) {
+        runInAction(() => {
+          this.rapidlyChangingMotionState.tcp_pose = latestMotionState.tcp_pose
+        })
+      }
+
+      // handle standstill changes
+      if (
+        this.rapidlyChangingMotionState.standstill !==
+        latestMotionState.standstill
+      ) {
+        runInAction(() => {
+          this.rapidlyChangingMotionState.standstill =
+            latestMotionState.standstill
+        })
+      }
+    })
+    makeAutoObservable(this)
+  }
+
+  get motionGroupId() {
+    return this.motionGroup.motion_group
+  }
+
+  get controllerId() {
+    return this.controller.controller
+  }
+
+  get modelFromController() {
+    return this.description.motion_group_model
+  }
+
+  get wandelscriptIdentifier() {
+    const num = this.motionGroupId.split("@")[0]
+    return `${this.controllerId.replaceAll("-", "_")}_${num}`
+  }
+
+  /** Jogging velocity in radians for rotation and joint movement */
+  get joggingVelocityRads() {
+    return (this.joggingVelocity * Math.PI) / 180
+  }
+
+  get joints() {
+    return this.initialMotionState.joint_position.map((_, i) => {
+      return {
+        index: i,
+      }
+    })
+  }
+
+  get dhParameters() {
+    return this.description.dh_parameters
+  }
+
+  get safetyZones() {
+    return this.description.safety_zones
+  }
+
+  /** Gets the robot mounting position offset in 3D viz coordinates */
+  get mountingPosition(): [number, number, number] {
+    if (!this.description.mounting) {
+      return [0, 0, 0]
+    }
+
+    return [
+      (this.description.mounting.position?.[0] || 0) / 1000,
+      (this.description.mounting.position?.[1] || 0) / 1000,
+      (this.description.mounting.position?.[2] || 0) / 1000,
+    ]
+  }
+
+  /** Gets the robot mounting position rotation in 3D viz coordinates */
+  get mountingQuaternion() {
+    const rotationVector = new THREE.Vector3(
+      this.description.mounting?.orientation?.[0] || 0,
+      this.description.mounting?.orientation?.[1] || 0,
+      this.description.mounting?.orientation?.[2] || 0,
+    )
+
+    const magnitude = rotationVector.length()
+    const axis = rotationVector.normalize()
+
+    return new THREE.Quaternion().setFromAxisAngle(axis, magnitude)
+  }
+
+  /**
+   * Whether the controller is currently in a safety state
+   * corresponding to an emergency stop
+   */
+  get isEstopActive() {
+    const estopStates: SafetyStateType[] = [
+      "SAFETY_STATE_ROBOT_EMERGENCY_STOP",
+      "SAFETY_STATE_DEVICE_EMERGENCY_STOP",
+    ]
+
+    return estopStates.includes(this.controllerState.safety_state)
+  }
+
+  /**
+   * Whether the controller is in a safety state
+   * that may be non-functional for robot pad purposes
+   */
+  get isMoveableSafetyState() {
+    const goodSafetyStates: SafetyStateType[] = [
+      "SAFETY_STATE_NORMAL",
+      "SAFETY_STATE_REDUCED",
+    ]
+
+    return goodSafetyStates.includes(this.controllerState.safety_state)
+  }
+
+  /**
+   * Whether the controller is in an operation mode that allows movement
+   */
+  get isMoveableOperationMode() {
+    const goodOperationModes: OperationMode[] = [
+      "OPERATION_MODE_AUTO",
+      "OPERATION_MODE_MANUAL",
+      "OPERATION_MODE_MANUAL_T1",
+      "OPERATION_MODE_MANUAL_T2",
+    ]
+
+    return goodOperationModes.includes(this.controllerState.operation_mode)
+  }
+
+  /**
+   * Whether the robot is currently active and can be moved, based on the
+   * safety state, operation mode and servo toggle activation state.
+   */
+  get canBeMoved() {
+    return (
+      this.isMoveableSafetyState &&
+      this.isMoveableOperationMode &&
+      this.activationState === "active"
+    )
+  }
+
+  async deactivate() {
+    if (this.activationState !== "active") {
+      console.error("Tried to deactivate while already deactivating")
+      return
+    }
+
+    runInAction(() => {
+      this.activationState = "deactivating"
+    })
+
+    try {
+      await this.nova.api.controller.setDefaultMode(
+        this.controllerId,
+        "ROBOT_SYSTEM_MODE_MONITOR",
+      )
+
+      runInAction(() => {
+        this.activationState = "inactive"
+      })
+    } catch (err) {
+      runInAction(() => {
+        this.activationState = "active"
+      })
+      throw err
+    }
+  }
+
+  async activate() {
+    if (this.activationState !== "inactive") {
+      console.error("Tried to activate while already activating")
+      return
+    }
+
+    runInAction(() => {
+      this.activationState = "activating"
+    })
+
+    try {
+      await this.nova.api.controller.setDefaultMode(
+        this.controllerId,
+        "ROBOT_SYSTEM_MODE_CONTROL",
+      )
+
+      runInAction(() => {
+        this.activationState = "active"
+      })
+    } catch (err) {
+      runInAction(() => {
+        this.activationState = "inactive"
+      })
+      throw err
+    }
+  }
+
+  toggleActivation() {
+    if (this.activationState === "inactive") {
+      this.activate()
+    } else if (this.activationState === "active") {
+      this.deactivate()
+    }
+  }
+
+  dispose() {
+    this.motionStateSocket.close()
+    if (this.connectedJoggingSocket) this.connectedJoggingSocket.close()
+  }
+
+  setJoggingVelocity(velocity: number) {
+    this.joggingVelocity = velocity
+  }
+}