@wandelbots/nova-js 2.1.4 → 3.0.0

package/src/lib/v2/ConnectedMotionGroup.ts

@@ -1,216 +0,0 @@
-import type {
-  Controller,
-  MotionGroupPhysical,
-  MotionGroupSpecification,
-  MotionGroupState,
-  RobotTcp,
-  SafetySetup,
-} from "@wandelbots/nova-api/v2"
-import { AxiosError } from "axios"
-import { makeAutoObservable, runInAction } from "mobx"
-import type { AutoReconnectingWebsocket } from "../AutoReconnectingWebsocket"
-import { tryParseJson } from "../converters"
-import { jointValuesEqual, tcpPoseEqual } from "./motionStateUpdate"
-import type { NovaClient } from "./NovaClient"
-
-const MOTION_DELTA_THRESHOLD = 0.0001
-
-export type MotionGroupOption = {
-  selectionId: string
-} & MotionGroupPhysical
-
-/**
- * Store representing the current state of a connected motion group.
- */
-export class ConnectedMotionGroup {
-  static async connect(
-    nova: NovaClient,
-    motionGroupId: string,
-    controllers: Controller[],
-  ) {
-    const [_motionGroupIndex, controllerId] = motionGroupId.split("@") as [
-      string,
-      string,
-    ]
-    const controller = controllers.find((c) => c.controller === 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(
-      `/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,
-    )
-
-    // This is used to determine if the robot is virtual or physical
-    let isVirtual = false
-    try {
-      const opMode =
-        await nova.api.virtualRobotMode.getOperationMode(controllerId)
-
-      if (opMode) isVirtual = true
-    } catch (err) {
-      if (err instanceof AxiosError) {
-        console.log(
-          `Received ${err.status} from getOperationMode, concluding that ${controllerId} is physical`,
-        )
-      } else {
-        throw err
-      }
-    }
-
-    // Find out what TCPs this motion group has (we need it for jogging)
-    const { tcps } = await nova.api.motionGroupInfos.listTcps(motionGroupId)
-
-    const motionGroupSpecification =
-      await nova.api.motionGroupInfos.getMotionGroupSpecification(motionGroupId)
-
-    const safetySetup =
-      await nova.api.motionGroupInfos.getSafetySetup(motionGroupId)
-
-    return new ConnectedMotionGroup(
-      nova,
-      controller,
-      motionGroup,
-      initialMotionState,
-      motionStateSocket,
-      isVirtual,
-      tcps!,
-      motionGroupSpecification,
-      safetySetup,
-    )
-  }
-
-  connectedJoggingCartesianSocket: WebSocket | null = null
-  connectedJoggingJointsSocket: WebSocket | null = null
-  planData: any | null // tmp
-  joggingVelocity: number = 10
-
-  // Not mobx-observable as this changes very fast; should be observed
-  // using animation frames
-  rapidlyChangingMotionState: MotionGroupState
-
-  constructor(
-    readonly nova: NovaClient,
-    readonly controller: Controller,
-    readonly motionGroup: MotionGroupPhysical,
-    readonly initialMotionState: MotionGroupState,
-    readonly motionStateSocket: AutoReconnectingWebsocket,
-    readonly isVirtual: boolean,
-    readonly tcps: RobotTcp[],
-    readonly motionGroupSpecification: MotionGroupSpecification,
-    readonly safetySetup: SafetySetup,
-  ) {
-    this.rapidlyChangingMotionState = initialMotionState
-
-    motionStateSocket.addEventListener("message", (event) => {
-      const motionStateResponse = tryParseJson(event.data)?.result as
-        | MotionGroupState
-        | undefined
-
-      if (!motionStateResponse) {
-        throw new Error(
-          `Failed to get motion state for ${this.motionGroupId}: ${event.data}`,
-        )
-      }
-
-      // handle motionState message
-      if (
-        !jointValuesEqual(
-          this.rapidlyChangingMotionState.joint_position.joints,
-          motionStateResponse.joint_position.joints,
-          MOTION_DELTA_THRESHOLD,
-        )
-      ) {
-        runInAction(() => {
-          this.rapidlyChangingMotionState = motionStateResponse
-        })
-      }
-
-      // handle tcpPose message
-      if (
-        !tcpPoseEqual(
-          this.rapidlyChangingMotionState.tcp_pose,
-          motionStateResponse.tcp_pose,
-          MOTION_DELTA_THRESHOLD,
-        )
-      ) {
-        runInAction(() => {
-          this.rapidlyChangingMotionState.tcp_pose =
-            motionStateResponse.tcp_pose
-        })
-      }
-    })
-    makeAutoObservable(this)
-  }
-
-  get motionGroupId() {
-    return this.motionGroup.motion_group
-  }
-
-  get controllerId() {
-    return this.controller.controller
-  }
-
-  get modelFromController() {
-    return this.motionGroup.model_from_controller
-  }
-
-  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.joints.map((_, i) => {
-      return {
-        index: i,
-      }
-    })
-  }
-
-  get dhParameters() {
-    return this.motionGroupSpecification.dh_parameters
-  }
-
-  get safetyZones() {
-    return this.safetySetup.safety_zones
-  }
-
-  dispose() {
-    this.motionStateSocket.close()
-    if (this.connectedJoggingCartesianSocket)
-      this.connectedJoggingCartesianSocket.close()
-    if (this.connectedJoggingJointsSocket)
-      this.connectedJoggingJointsSocket.close()
-  }
-
-  setJoggingVelocity(velocity: number) {
-    this.joggingVelocity = velocity
-  }
-}

package/src/lib/v2/JoggerConnection.ts

@@ -1,207 +0,0 @@
-import type {
-  InitializeJoggingRequest,
-  JoggingResponse,
-  JointVelocityRequest,
-  MotionGroupState,
-  TcpVelocityRequest,
-} from "@wandelbots/nova-api/v2"
-import type { AutoReconnectingWebsocket } from "../AutoReconnectingWebsocket"
-import { tryParseJson } from "../converters"
-import type { NovaClient } from "./NovaClient"
-import { axisToIndex } from "./vectorUtils"
-
-export type JoggerConnectionOpts = {
-  /**
-   * When an error message is received from the jogging websocket, it
-   * will be passed here. If this handler is not provided, the error will
-   * instead be thrown as an unhandled error.
-   */
-  onError?: (err: unknown) => void
-} & Omit<InitializeJoggingRequest, "message_type">
-
-export class JoggerConnection {
-  // Currently a separate websocket is needed for each mode, pester API people
-  // to merge these for simplicity
-  joggingWebsocket: AutoReconnectingWebsocket | null = null
-  lastVelocityRequest: JointVelocityRequest | TcpVelocityRequest | null = null
-  lastResponse: JoggingResponse | null = null
-
-  static async open(
-    nova: NovaClient,
-    cell: string,
-    motionGroupId: string,
-    opts: JoggerConnectionOpts,
-  ) {
-    const motionGroupState =
-      await nova.api.motionGroupInfos.getCurrentMotionGroupState(motionGroupId)
-
-    return new JoggerConnection(
-      nova,
-      cell,
-      motionGroupId,
-      motionGroupState,
-      opts,
-    )
-  }
-
-  constructor(
-    readonly nova: NovaClient,
-    readonly cell: string,
-    readonly motionGroupId: string,
-    readonly motionGroupState: MotionGroupState,
-    readonly opts: JoggerConnectionOpts,
-  ) {
-    this.joggingWebsocket = nova.openReconnectingWebsocket(
-      `/cells/${cell}/execution/jogging`,
-    )
-    this.joggingWebsocket.addEventListener("message", (ev: MessageEvent) => {
-      const data = tryParseJson(ev.data) as JoggingResponse
-      if (data && "error" in data) {
-        if (this.opts.onError) {
-          this.opts.onError(ev.data)
-        } else {
-          throw new Error(ev.data)
-        }
-      }
-    })
-
-    this.joggingWebsocket.addEventListener("message", (ev: MessageEvent) => {
-      const data = tryParseJson(ev.data)
-      if (data && "error" in data) {
-        if (this.opts.onError) {
-          this.opts.onError(ev.data)
-        } else {
-          throw new Error(ev.data)
-        }
-      }
-    })
-  }
-
-  get jointCount() {
-    return this.motionGroupState.joint_current?.joints.length
-  }
-
-  get activeWebsocket() {
-    return this.joggingWebsocket
-  }
-
-  async stop() {
-    // Why not call the stopJogging API endpoint?
-    // Because this results in the websocket closing and we
-    // would like to keep it open for now.
-    if (!this.joggingWebsocket) {
-      return
-    }
-
-    if (this.lastVelocityRequest?.message_type === "JointVelocityRequest") {
-      this.joggingWebsocket.sendJson({
-        message_type: "JointVelocityRequest",
-        velocity: {
-          joints: Array.from(new Array(this.jointCount).keys()).map(() => 0),
-        },
-      } as JointVelocityRequest)
-    }
-
-    if (this.lastVelocityRequest?.message_type === "TcpVelocityRequest") {
-      this.joggingWebsocket.sendJson({
-        message_type: "TcpVelocityRequest",
-        rotation: [0, 0, 0],
-        translation: [0, 0, 0],
-      } as TcpVelocityRequest)
-    }
-  }
-
-  dispose() {
-    if (this.joggingWebsocket) {
-      this.joggingWebsocket.dispose()
-    }
-  }
-
-  /**
-   * Start rotation of a single robot joint at the specified velocity
-   */
-  async startJointRotation({
-    joint,
-    velocityRadsPerSec,
-  }: {
-    /** Index of the joint to rotate */
-    joint: number
-    /** Speed of the rotation in radians per second */
-    velocityRadsPerSec: number
-  }) {
-    if (!this.joggingWebsocket) {
-      throw new Error(
-        "Joint jogging websocket not connected. Wait for reconnect or open new jogging connection",
-      )
-    }
-
-    const jointVelocities = new Array(this.jointCount).fill(0)
-    jointVelocities[joint] = velocityRadsPerSec
-
-    this.joggingWebsocket.sendJson({
-      message_type: "JointVelocityRequest",
-      velocity: {
-        joints: jointVelocities,
-      },
-    } as JointVelocityRequest)
-  }
-
-  /**
-   * Start the TCP moving along a specified axis at a given velocity
-   */
-  async startTCPTranslation({
-    axis,
-    velocityMmPerSec,
-    useToolCoordinateSystem,
-  }: {
-    axis: "x" | "y" | "z"
-    velocityMmPerSec: number
-    useToolCoordinateSystem: boolean
-  }) {
-    if (!this.joggingWebsocket) {
-      throw new Error(
-        "Cartesian jogging websocket not connected. Wait for reconnect or open new jogging connection",
-      )
-    }
-
-    const joggingVector = [0, 0, 0]
-    joggingVector[axisToIndex(axis)] = velocityMmPerSec
-
-    this.joggingWebsocket.sendJson({
-      message_type: "TcpVelocityRequest",
-
-      translation: joggingVector,
-      rotation: [0, 0, 0],
-      use_tool_coordinate_system: useToolCoordinateSystem,
-    } as TcpVelocityRequest)
-  }
-
-  /**
-   * Start the TCP rotating around a specified axis at a given velocity
-   */
-  async startTCPRotation({
-    axis,
-    velocityRadsPerSec,
-    useToolCoordinateSystem,
-  }: {
-    axis: "x" | "y" | "z"
-    velocityRadsPerSec: number
-    useToolCoordinateSystem: boolean
-  }) {
-    if (!this.joggingWebsocket) {
-      throw new Error(
-        "Cartesian jogging websocket not connected. Wait for reconnect or open new jogging connection",
-      )
-    }
-
-    const joggingVector = [0, 0, 0]
-    joggingVector[axisToIndex(axis)] = velocityRadsPerSec
-
-    this.joggingWebsocket.sendJson({
-      message_type: "TcpVelocityRequest",
-      rotation: joggingVector,
-      translation: [0, 0, 0],
-      use_tool_coordinate_system: useToolCoordinateSystem,
-    } as TcpVelocityRequest)
-  }
-}

package/src/lib/v2/MotionStreamConnection.ts

@@ -1,201 +0,0 @@
-import type {
-  Controller,
-  MotionGroupPhysical,
-  MotionGroupState,
-} 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, tcpPoseEqual } from "./motionStateUpdate"
-import type { NovaClient } from "./NovaClient"
-import { Vector3d, vector3ToArray } from "./vectorUtils"
-
-const MOTION_DELTA_THRESHOLD = 0.0001
-
-function unwrapRotationVector(
-  newRotationVectorApi: Vector3d,
-  currentRotationVectorApi: Vector3d,
-): Vector3d {
-  const currentRotationVector = new THREE.Vector3(
-    currentRotationVectorApi[0],
-    currentRotationVectorApi[1],
-    currentRotationVectorApi[2],
-  )
-
-  const newRotationVector = new THREE.Vector3(
-    newRotationVectorApi[0],
-    newRotationVectorApi[1],
-    newRotationVectorApi[2],
-  )
-
-  const currentAngle = currentRotationVector.length()
-  const currentAxis = currentRotationVector.normalize()
-
-  let newAngle = newRotationVector.length()
-  let newAxis = newRotationVector.normalize()
-
-  // Align rotation axes
-  if (newAxis.dot(currentAxis) < 0) {
-    newAngle = -newAngle
-    newAxis = newAxis.multiplyScalar(-1.0)
-  }
-
-  // Shift rotation angle close to previous one to extend domain of rotation angles beyond [0, pi]
-  // - this simplifies interpolation and prevents abruptly changing signs of the rotation angles
-  let angleDifference = newAngle - currentAngle
-  angleDifference -=
-    2.0 * Math.PI * Math.floor((angleDifference + Math.PI) / (2.0 * Math.PI))
-
-  newAngle = currentAngle + angleDifference
-
-  return vector3ToArray(newAxis.multiplyScalar(newAngle))
-}
-
-/**
- * Store representing the current state of a connected motion group.
- */
-export class MotionStreamConnection {
-  static async open(nova: NovaClient, motionGroupId: string) {
-    const { controllers: controllers } =
-      await nova.api.controller.listControllers()
-
-    const [_motionGroupIndex, controllerId] = motionGroupId.split("@") as [
-      string,
-      string,
-    ]
-    const controller = controllers.find((c) => c.controller === 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(
-      `/motion-groups/${motionGroupId}/state-stream`,
-    )
-
-    // Wait for the first message to get the initial state
-    const firstMessage = await motionStateSocket.firstMessage()
-    console.log("got first message", 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,
-    )
-
-    return new MotionStreamConnection(
-      nova,
-      controller,
-      motionGroup,
-      initialMotionState,
-      motionStateSocket,
-    )
-  }
-
-  // Not mobx-observable as this changes very fast; should be observed
-  // using animation frames
-  rapidlyChangingMotionState: MotionGroupState
-
-  constructor(
-    readonly nova: NovaClient,
-    readonly controller: Controller,
-    readonly motionGroup: MotionGroupPhysical,
-    readonly initialMotionState: MotionGroupState,
-    readonly motionStateSocket: AutoReconnectingWebsocket,
-  ) {
-    this.rapidlyChangingMotionState = initialMotionState
-
-    motionStateSocket.addEventListener("message", (event) => {
-      const motionState = tryParseJson(event.data)?.result as
-        | MotionGroupState
-        | undefined
-
-      if (!motionState) {
-        throw new Error(
-          `Failed to get motion state for ${this.motionGroupId}: ${event.data}`,
-        )
-      }
-
-      // handle motionState message
-      if (
-        !jointValuesEqual(
-          this.rapidlyChangingMotionState.joint_position.joints,
-          motionState.joint_position.joints,
-          MOTION_DELTA_THRESHOLD,
-        )
-      ) {
-        runInAction(() => {
-          this.rapidlyChangingMotionState = motionState
-        })
-      }
-
-      // handle tcpPose message
-      if (
-        !tcpPoseEqual(
-          this.rapidlyChangingMotionState.tcp_pose,
-          motionState.tcp_pose,
-          MOTION_DELTA_THRESHOLD,
-        )
-      ) {
-        runInAction(() => {
-          if (this.rapidlyChangingMotionState.tcp_pose == undefined) {
-            this.rapidlyChangingMotionState.tcp_pose = motionState.tcp_pose
-          } else {
-            this.rapidlyChangingMotionState.tcp_pose = {
-              position: motionState.tcp_pose!.position,
-              orientation: unwrapRotationVector(
-                motionState.tcp_pose!.orientation as Vector3d,
-                this.rapidlyChangingMotionState.tcp_pose!
-                  .orientation as Vector3d,
-              ),
-              tcp: motionState.tcp_pose!.tcp,
-              coordinate_system: motionState.tcp_pose!.coordinate_system,
-            }
-          }
-        })
-      }
-    })
-    makeAutoObservable(this)
-  }
-
-  get motionGroupId() {
-    return this.motionGroup.motion_group
-  }
-
-  get controllerId() {
-    return this.controller.controller
-  }
-
-  get modelFromController() {
-    return this.motionGroup.model_from_controller
-  }
-
-  get wandelscriptIdentifier() {
-    const num = this.motionGroupId.split("@")[0]
-    return `${this.controllerId.replaceAll("-", "_")}_${num}`
-  }
-
-  get joints() {
-    return this.initialMotionState.joint_position.joints.map((_, i) => {
-      return {
-        index: i,
-      }
-    })
-  }
-
-  dispose() {
-    this.motionStateSocket.close()
-  }
-}