@wandelbots/nova-js
Version:
Official JS client for the Wandelbots API
1,714 lines (1,705 loc) • 60.5 kB
JavaScript
const require_LoginWithAuth0 = require('./LoginWithAuth0-CBD9BXXz.cjs');
let axios = require("axios");
axios = require_LoginWithAuth0.__toESM(axios);
let url_join = require("url-join");
url_join = require_LoginWithAuth0.__toESM(url_join);
let mobx = require("mobx");
let three = require("three");
three = require_LoginWithAuth0.__toESM(three);
let three_src_math_Vector3_js = require("three/src/math/Vector3.js");
let __wandelbots_nova_api_v1 = require("@wandelbots/nova-api/v1");
let path_to_regexp = require("path-to-regexp");
path_to_regexp = require_LoginWithAuth0.__toESM(path_to_regexp);
//#region src/lib/converters.ts
/** Try to parse something as JSON; return undefined if we can't */
function tryParseJson(json) {
try {
return JSON.parse(json);
} catch {
return;
}
}
/** Try to turn something into JSON; return undefined if we can't */
function tryStringifyJson(json) {
try {
return JSON.stringify(json);
} catch {
return;
}
}
/**
* Converts object parameters to query string.
* e.g. { a: "1", b: "2" } => "?a=1&b=2"
* {} => ""
*/
function makeUrlQueryString(obj) {
const str = new URLSearchParams(obj).toString();
return str ? `?${str}` : "";
}
/** Convert radians to degrees */
function radiansToDegrees(radians) {
return radians * (180 / Math.PI);
}
/** Convert degrees to radians */
function degreesToRadians(degrees) {
return degrees * (Math.PI / 180);
}
/**
* Check for coordinate system id equivalence, accounting for the "world" default
* on empty/undefined values.
*/
function isSameCoordinateSystem(firstCoordSystem, secondCoordSystem) {
if (!firstCoordSystem) firstCoordSystem = "world";
if (!secondCoordSystem) secondCoordSystem = "world";
return firstCoordSystem === secondCoordSystem;
}
/**
* Helpful const for converting {x, y, z} to [x, y, z] and vice versa
*/
const XYZ_TO_VECTOR = {
x: 0,
y: 1,
z: 2
};
//#endregion
//#region src/lib/v1/wandelscriptUtils.ts
/**
* Convert a Pose object representing a motion group position
* into a string which represents that pose in Wandelscript.
*/
function poseToWandelscriptString(pose) {
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));
const rotationValues = orientation.map((v) => v.toFixed(4));
return `(${positionValues.concat(rotationValues).join(", ")})`;
}
//#endregion
//#region src/lib/v1/motionStateUpdate.ts
function jointValuesEqual(oldJointValues, newJointValues, changeDeltaThreshold) {
if (newJointValues.length !== oldJointValues.length) return true;
for (let jointIndex = 0; jointIndex < newJointValues.length; jointIndex++) if (Math.abs(newJointValues[jointIndex] - oldJointValues[jointIndex]) > changeDeltaThreshold) return false;
return true;
}
function tcpPoseEqual(oldTcp, newTcp, changeDeltaThreshold) {
if (oldTcp === void 0 && newTcp || oldTcp && newTcp === void 0) return false;
if (oldTcp === void 0 || newTcp === void 0) return true;
let changedDelta = 0;
changedDelta += Math.abs(oldTcp.orientation.x - newTcp.orientation.x);
changedDelta += Math.abs(oldTcp.orientation.y - newTcp.orientation.y);
changedDelta += Math.abs(oldTcp.orientation.z - newTcp.orientation.z);
changedDelta += Math.abs(oldTcp.position.x - newTcp.position.x);
changedDelta += Math.abs(oldTcp.position.y - newTcp.position.y);
changedDelta += Math.abs(oldTcp.position.z - newTcp.position.z);
if (changedDelta > changeDeltaThreshold) return false;
return oldTcp.coordinate_system === newTcp.coordinate_system && oldTcp.tcp === newTcp.tcp;
}
//#endregion
//#region src/lib/v1/ConnectedMotionGroup.ts
const MOTION_DELTA_THRESHOLD$1 = 1e-4;
/**
* Store representing the current state of a connected motion group.
*/
var ConnectedMotionGroup = class ConnectedMotionGroup {
static async connect(nova, motionGroupId, controllers) {
const [_motionGroupIndex, controllerId] = motionGroupId.split("@");
const controller = controllers.find((c) => c.controller === controllerId);
const motionGroup = controller?.physical_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`);
const firstMessage = await motionStateSocket.firstMessage();
const initialMotionState = tryParseJson(firstMessage.data)?.result;
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);
const isVirtual = (await nova.api.controller.getRobotController(controller.controller)).configuration.kind === "VirtualController";
const mounting = await (async () => {
try {
return await nova.api.motionGroupInfos.getMounting(motionGroup.motion_group);
} catch (err) {
console.error(`Error fetching mounting for ${motionGroup.motion_group}`, err);
return null;
}
})();
const controllerStateSocket = nova.openReconnectingWebsocket(`/controllers/${controller.controller}/state-stream?response_rate=1000`);
const firstControllerMessage = await controllerStateSocket.firstMessage();
const initialControllerState = tryParseJson(firstControllerMessage.data)?.result;
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);
const { tcps } = await nova.api.motionGroupInfos.listTcps(motionGroupId);
return new ConnectedMotionGroup(nova, controller, motionGroup, initialMotionState, motionStateSocket, isVirtual, tcps, await nova.api.motionGroupInfos.getMotionGroupSpecification(motionGroupId), await nova.api.motionGroupInfos.getSafetySetup(motionGroupId), mounting, initialControllerState, controllerStateSocket);
}
constructor(nova, controller, motionGroup, initialMotionState, motionStateSocket, isVirtual, tcps, motionGroupSpecification, safetySetup, mounting, initialControllerState, controllerStateSocket) {
this.nova = nova;
this.controller = controller;
this.motionGroup = motionGroup;
this.initialMotionState = initialMotionState;
this.motionStateSocket = motionStateSocket;
this.isVirtual = isVirtual;
this.tcps = tcps;
this.motionGroupSpecification = motionGroupSpecification;
this.safetySetup = safetySetup;
this.mounting = mounting;
this.initialControllerState = initialControllerState;
this.controllerStateSocket = controllerStateSocket;
this.connectedJoggingCartesianSocket = null;
this.connectedJoggingJointsSocket = null;
this.joggingVelocity = 10;
this.activationState = "inactive";
this.rapidlyChangingMotionState = initialMotionState;
this.controllerState = initialControllerState;
controllerStateSocket.addEventListener("message", (event) => {
const data = tryParseJson(event.data)?.result;
if (!data) return;
(0, mobx.runInAction)(() => {
this.controllerState = data;
});
});
motionStateSocket.addEventListener("message", (event) => {
const motionStateResponse = tryParseJson(event.data)?.result;
if (!motionStateResponse) throw new Error(`Failed to get motion state for ${this.motionGroupId}: ${event.data}`);
if (!jointValuesEqual(this.rapidlyChangingMotionState.state.joint_position.joints, motionStateResponse.state.joint_position.joints, MOTION_DELTA_THRESHOLD$1)) (0, mobx.runInAction)(() => {
this.rapidlyChangingMotionState.state = motionStateResponse.state;
});
if (!tcpPoseEqual(this.rapidlyChangingMotionState.tcp_pose, motionStateResponse.tcp_pose, MOTION_DELTA_THRESHOLD$1)) (0, mobx.runInAction)(() => {
this.rapidlyChangingMotionState.tcp_pose = motionStateResponse.tcp_pose;
});
});
(0, mobx.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.state.joint_position.joints.map((_, i) => {
return { index: i };
});
}
get dhParameters() {
return this.motionGroupSpecification.dh_parameters;
}
get safetyZones() {
return this.safetySetup.safety_zones;
}
/** Gets the robot mounting position offset in 3D viz coordinates */
get mountingPosition() {
if (!this.mounting) return [
0,
0,
0
];
return [
this.mounting.pose.position.x / 1e3,
this.mounting.pose.position.y / 1e3,
this.mounting.pose.position.z / 1e3
];
}
/** Gets the robot mounting position rotation in 3D viz coordinates */
get mountingQuaternion() {
const rotationVector = new three.Vector3(this.mounting?.pose.orientation?.x || 0, this.mounting?.pose.orientation?.y || 0, this.mounting?.pose.orientation?.z || 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() {
return ["SAFETY_STATE_ROBOT_EMERGENCY_STOP", "SAFETY_STATE_DEVICE_EMERGENCY_STOP"].includes(this.controllerState.safety_state);
}
/**
* Whether the controller is in a safety state
* that may be non-functional for robot pad purposes
*/
get isMoveableSafetyState() {
return ["SAFETY_STATE_NORMAL", "SAFETY_STATE_REDUCED"].includes(this.controllerState.safety_state);
}
/**
* Whether the controller is in an operation mode that allows movement
*/
get isMoveableOperationMode() {
return [
"OPERATION_MODE_AUTO",
"OPERATION_MODE_MANUAL",
"OPERATION_MODE_MANUAL_T1",
"OPERATION_MODE_MANUAL_T2"
].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;
}
(0, mobx.runInAction)(() => {
this.activationState = "deactivating";
});
try {
await this.nova.api.controller.setDefaultMode(this.controllerId, "MODE_MONITOR");
(0, mobx.runInAction)(() => {
this.activationState = "inactive";
});
} catch (err) {
(0, mobx.runInAction)(() => {
this.activationState = "active";
});
throw err;
}
}
async activate() {
if (this.activationState !== "inactive") {
console.error("Tried to activate while already activating");
return;
}
(0, mobx.runInAction)(() => {
this.activationState = "activating";
});
try {
await this.nova.api.controller.setDefaultMode(this.controllerId, "MODE_CONTROL");
(0, mobx.runInAction)(() => {
this.activationState = "active";
});
} catch (err) {
(0, mobx.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.connectedJoggingCartesianSocket) this.connectedJoggingCartesianSocket.close();
if (this.connectedJoggingJointsSocket) this.connectedJoggingJointsSocket.close();
}
setJoggingVelocity(velocity) {
this.joggingVelocity = velocity;
}
};
//#endregion
//#region src/lib/v1/JoggerConnection.ts
var JoggerConnection = class JoggerConnection {
static async open(nova, motionGroupId, opts = {}) {
return new JoggerConnection(await nova.connectMotionStream(motionGroupId), opts);
}
constructor(motionStream, opts = {}) {
this.motionStream = motionStream;
this.opts = opts;
this.cartesianWebsocket = null;
this.jointWebsocket = null;
this.cartesianJoggingOpts = {};
}
get motionGroupId() {
return this.motionStream.motionGroupId;
}
get nova() {
return this.motionStream.nova;
}
get numJoints() {
return this.motionStream.joints.length;
}
get activeJoggingMode() {
if (this.cartesianWebsocket) return "cartesian";
if (this.jointWebsocket) return "joint";
return "increment";
}
get activeWebsocket() {
return this.cartesianWebsocket || this.jointWebsocket;
}
async stop() {
if (this.cartesianWebsocket) this.cartesianWebsocket.sendJson({
motion_group: this.motionGroupId,
position_direction: {
x: 0,
y: 0,
z: 0
},
rotation_direction: {
x: 0,
y: 0,
z: 0
},
position_velocity: 0,
rotation_velocity: 0,
tcp: this.cartesianJoggingOpts.tcpId,
coordinate_system: this.cartesianJoggingOpts.coordSystemId
});
if (this.jointWebsocket) this.jointWebsocket.sendJson({
motion_group: this.motionGroupId,
joint_velocities: new Array(this.numJoints).fill(0)
});
}
dispose() {
if (this.cartesianWebsocket) this.cartesianWebsocket.dispose();
if (this.jointWebsocket) this.jointWebsocket.dispose();
}
setJoggingMode(mode, cartesianJoggingOpts) {
console.log("Setting jogging mode to", mode);
if (cartesianJoggingOpts) {
if (JSON.stringify(this.cartesianJoggingOpts) !== JSON.stringify(cartesianJoggingOpts)) {
if (this.cartesianWebsocket) {
this.cartesianWebsocket.dispose();
this.cartesianWebsocket = null;
}
}
this.cartesianJoggingOpts = cartesianJoggingOpts;
}
if (mode !== "cartesian" && this.cartesianWebsocket) {
this.cartesianWebsocket.dispose();
this.cartesianWebsocket = null;
}
if (mode !== "joint" && this.jointWebsocket) {
this.jointWebsocket.dispose();
this.jointWebsocket = null;
}
if (mode === "cartesian" && !this.cartesianWebsocket) {
this.cartesianWebsocket = this.nova.openReconnectingWebsocket(`/motion-groups/move-tcp`);
this.cartesianWebsocket.addEventListener("message", (ev) => {
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);
});
}
if (mode === "joint" && !this.jointWebsocket) {
this.jointWebsocket = this.nova.openReconnectingWebsocket(`/motion-groups/move-joint`);
this.jointWebsocket.addEventListener("message", (ev) => {
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);
});
}
}
/**
* Start rotation of a single robot joint at the specified velocity
*/
async startJointRotation({ joint, direction, velocityRadsPerSec }) {
if (!this.jointWebsocket) throw new Error("Joint jogging websocket not connected; call setJoggingMode first");
const jointVelocities = new Array(this.numJoints).fill(0);
jointVelocities[joint] = direction === "-" ? -velocityRadsPerSec : velocityRadsPerSec;
this.jointWebsocket.sendJson({
motion_group: this.motionGroupId,
joint_velocities: jointVelocities
});
}
/**
* Start the TCP moving along a specified axis at a given velocity
*/
async startTCPTranslation({ axis, direction, velocityMmPerSec }) {
if (!this.cartesianWebsocket) throw new Error("Cartesian jogging websocket not connected; call setJoggingMode first");
const zeroVector = {
x: 0,
y: 0,
z: 0
};
const joggingVector = Object.assign({}, zeroVector);
joggingVector[axis] = direction === "-" ? -1 : 1;
this.cartesianWebsocket.sendJson({
motion_group: this.motionGroupId,
position_direction: joggingVector,
rotation_direction: zeroVector,
position_velocity: velocityMmPerSec,
rotation_velocity: 0,
tcp: this.cartesianJoggingOpts.tcpId,
coordinate_system: this.cartesianJoggingOpts.coordSystemId
});
}
/**
* Start the TCP rotating around a specified axis at a given velocity
*/
async startTCPRotation({ axis, direction, velocityRadsPerSec }) {
if (!this.cartesianWebsocket) throw new Error("Cartesian jogging websocket not connected; call setJoggingMode first");
const zeroVector = {
x: 0,
y: 0,
z: 0
};
const joggingVector = Object.assign({}, zeroVector);
joggingVector[axis] = direction === "-" ? -1 : 1;
this.cartesianWebsocket.sendJson({
motion_group: this.motionGroupId,
position_direction: zeroVector,
rotation_direction: joggingVector,
position_velocity: 0,
rotation_velocity: velocityRadsPerSec,
tcp: this.cartesianJoggingOpts.tcpId,
coordinate_system: this.cartesianJoggingOpts.coordSystemId
});
}
/**
* Move the robot by a fixed distance in a single cartesian
* axis, either rotating or translating relative to the TCP.
* Promise resolves only after the motion has completed.
*/
async runIncrementalCartesianMotion({ currentTcpPose, currentJoints, coordSystemId, velocityInRelevantUnits, axis, direction, motion }) {
const commands = [];
if (!isSameCoordinateSystem(currentTcpPose.coordinate_system, coordSystemId)) throw new Error(`Current TCP pose coordinate system ${currentTcpPose.coordinate_system} does not match target coordinate system ${coordSystemId}`);
if (motion.type === "translate") {
const targetTcpPosition = Object.assign({}, currentTcpPose.position);
targetTcpPosition[axis] += motion.distanceMm * (direction === "-" ? -1 : 1);
commands.push({
settings: { limits_override: { tcp_velocity_limit: velocityInRelevantUnits } },
line: {
position: targetTcpPosition,
orientation: currentTcpPose.orientation,
coordinate_system: coordSystemId
}
});
} else if (motion.type === "rotate") {
const currentRotationVector = new three_src_math_Vector3_js.Vector3(currentTcpPose.orientation.x, currentTcpPose.orientation.y, currentTcpPose.orientation.z);
const currentRotationRad = currentRotationVector.length();
const currentRotationDirection = currentRotationVector.clone().normalize();
const differenceRotationRad = motion.distanceRads * (direction === "-" ? -1 : 1);
const differenceRotationDirection = new three_src_math_Vector3_js.Vector3(0, 0, 0);
differenceRotationDirection[axis] = 1;
const f1 = Math.cos(.5 * differenceRotationRad) * Math.cos(.5 * currentRotationRad);
const f2 = Math.sin(.5 * differenceRotationRad) * Math.sin(.5 * currentRotationRad);
const f3 = Math.sin(.5 * differenceRotationRad) * Math.cos(.5 * currentRotationRad);
const f4 = Math.cos(.5 * differenceRotationRad) * Math.sin(.5 * currentRotationRad);
const dotProduct = differenceRotationDirection.dot(currentRotationDirection);
const crossProduct = differenceRotationDirection.clone().cross(currentRotationDirection);
const newRotationRad = 2 * Math.acos(f1 - f2 * dotProduct);
const f5 = newRotationRad / Math.sin(.5 * newRotationRad);
const targetTcpOrientation = new three_src_math_Vector3_js.Vector3().addScaledVector(crossProduct, f2).addScaledVector(differenceRotationDirection, f3).addScaledVector(currentRotationDirection, f4).multiplyScalar(f5);
commands.push({
settings: { limits_override: { tcp_orientation_velocity_limit: velocityInRelevantUnits } },
line: {
position: currentTcpPose.position,
orientation: targetTcpOrientation,
coordinate_system: coordSystemId
}
});
}
const motionPlanRes = await this.nova.api.motion.planMotion({
motion_group: this.motionGroupId,
start_joint_position: currentJoints,
tcp: this.cartesianJoggingOpts.tcpId,
commands
});
const plannedMotion = motionPlanRes.plan_successful_response?.motion;
if (!plannedMotion) throw new Error(`Failed to plan jogging increment motion ${JSON.stringify(motionPlanRes)}`);
await this.nova.api.motion.streamMoveForward(plannedMotion, 100, void 0, void 0, void 0, { timeout: 1e3 * 60 });
}
/**
* Rotate a single robot joint by a fixed number of radians
* Promise resolves only after the motion has completed.
*/
async runIncrementalJointRotation({ joint, currentJoints, velocityRadsPerSec, direction, distanceRads }) {
const targetJoints = [...currentJoints.joints];
targetJoints[joint] += distanceRads * (direction === "-" ? -1 : 1);
const jointVelocityLimits = new Array(currentJoints.joints.length).fill(velocityRadsPerSec);
const motionPlanRes = await this.nova.api.motion.planMotion({
motion_group: this.motionGroupId,
start_joint_position: currentJoints,
commands: [{
settings: { limits_override: { joint_velocity_limits: { joints: jointVelocityLimits } } },
joint_ptp: { joints: targetJoints }
}]
});
const plannedMotion = motionPlanRes.plan_successful_response?.motion;
if (!plannedMotion) {
console.error("Failed to plan jogging increment motion", motionPlanRes);
return;
}
await this.nova.api.motion.streamMoveForward(plannedMotion, 100, void 0, void 0, void 0, { timeout: 1e3 * 60 });
}
};
//#endregion
//#region src/lib/v1/MotionStreamConnection.ts
const MOTION_DELTA_THRESHOLD = 1e-4;
function unwrapRotationVector(newRotationVectorApi, currentRotationVectorApi) {
const currentRotationVector = new three.Vector3(currentRotationVectorApi.x, currentRotationVectorApi.y, currentRotationVectorApi.z);
const newRotationVector = new three.Vector3(newRotationVectorApi.x, newRotationVectorApi.y, newRotationVectorApi.z);
const currentAngle = currentRotationVector.length();
const currentAxis = currentRotationVector.normalize();
let newAngle = newRotationVector.length();
let newAxis = newRotationVector.normalize();
if (newAxis.dot(currentAxis) < 0) {
newAngle = -newAngle;
newAxis = newAxis.multiplyScalar(-1);
}
let angleDifference = newAngle - currentAngle;
angleDifference -= 2 * Math.PI * Math.floor((angleDifference + Math.PI) / (2 * Math.PI));
newAngle = currentAngle + angleDifference;
return newAxis.multiplyScalar(newAngle);
}
/**
* Store representing the current state of a connected motion group.
*/
var MotionStreamConnection = class MotionStreamConnection {
static async open(nova, motionGroupId) {
const { instances: controllers } = await nova.api.controller.listControllers();
const [_motionGroupIndex, controllerId] = motionGroupId.split("@");
const controller = controllers.find((c) => c.controller === controllerId);
const motionGroup = controller?.physical_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`);
const firstMessage = await motionStateSocket.firstMessage();
const initialMotionState = tryParseJson(firstMessage.data)?.result;
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);
}
constructor(nova, controller, motionGroup, initialMotionState, motionStateSocket) {
this.nova = nova;
this.controller = controller;
this.motionGroup = motionGroup;
this.initialMotionState = initialMotionState;
this.motionStateSocket = motionStateSocket;
this.rapidlyChangingMotionState = initialMotionState;
motionStateSocket.addEventListener("message", (event) => {
const motionStateResponse = tryParseJson(event.data)?.result;
if (!motionStateResponse) throw new Error(`Failed to get motion state for ${this.motionGroupId}: ${event.data}`);
if (!jointValuesEqual(this.rapidlyChangingMotionState.state.joint_position.joints, motionStateResponse.state.joint_position.joints, MOTION_DELTA_THRESHOLD)) (0, mobx.runInAction)(() => {
this.rapidlyChangingMotionState.state = motionStateResponse.state;
});
if (!tcpPoseEqual(this.rapidlyChangingMotionState.tcp_pose, motionStateResponse.tcp_pose, MOTION_DELTA_THRESHOLD)) (0, mobx.runInAction)(() => {
if (this.rapidlyChangingMotionState.tcp_pose == null) this.rapidlyChangingMotionState.tcp_pose = motionStateResponse.tcp_pose;
else this.rapidlyChangingMotionState.tcp_pose = {
position: motionStateResponse.tcp_pose.position,
orientation: unwrapRotationVector(motionStateResponse.tcp_pose.orientation, this.rapidlyChangingMotionState.tcp_pose.orientation),
tcp: motionStateResponse.tcp_pose.tcp,
coordinate_system: motionStateResponse.tcp_pose.coordinate_system
};
});
});
(0, mobx.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.state.joint_position.joints.map((_, i) => {
return { index: i };
});
}
dispose() {
this.motionStateSocket.close();
}
};
//#endregion
//#region src/lib/v1/NovaCellAPIClient.ts
/**
* API client providing type-safe access to all the Nova API REST endpoints
* associated with a specific cell id.
*/
var NovaCellAPIClient = class {
constructor(cellId, opts) {
this.cellId = cellId;
this.opts = opts;
this.system = this.withUnwrappedResponsesOnly(__wandelbots_nova_api_v1.SystemApi);
this.cell = this.withUnwrappedResponsesOnly(__wandelbots_nova_api_v1.CellApi);
this.deviceConfig = this.withCellId(__wandelbots_nova_api_v1.DeviceConfigurationApi);
this.motionGroup = this.withCellId(__wandelbots_nova_api_v1.MotionGroupApi);
this.motionGroupInfos = this.withCellId(__wandelbots_nova_api_v1.MotionGroupInfosApi);
this.controller = this.withCellId(__wandelbots_nova_api_v1.ControllerApi);
this.program = this.withCellId(__wandelbots_nova_api_v1.ProgramApi);
this.programValues = this.withCellId(__wandelbots_nova_api_v1.ProgramValuesApi);
this.controllerIOs = this.withCellId(__wandelbots_nova_api_v1.ControllerIOsApi);
this.motionGroupKinematic = this.withCellId(__wandelbots_nova_api_v1.MotionGroupKinematicApi);
this.motion = this.withCellId(__wandelbots_nova_api_v1.MotionApi);
this.coordinateSystems = this.withCellId(__wandelbots_nova_api_v1.CoordinateSystemsApi);
this.application = this.withCellId(__wandelbots_nova_api_v1.ApplicationApi);
this.applicationGlobal = this.withUnwrappedResponsesOnly(__wandelbots_nova_api_v1.ApplicationApi);
this.motionGroupJogging = this.withCellId(__wandelbots_nova_api_v1.MotionGroupJoggingApi);
this.virtualRobot = this.withCellId(__wandelbots_nova_api_v1.VirtualRobotApi);
this.virtualRobotSetup = this.withCellId(__wandelbots_nova_api_v1.VirtualRobotSetupApi);
this.virtualRobotMode = this.withCellId(__wandelbots_nova_api_v1.VirtualRobotModeApi);
this.virtualRobotBehavior = this.withCellId(__wandelbots_nova_api_v1.VirtualRobotBehaviorApi);
this.libraryProgramMetadata = this.withCellId(__wandelbots_nova_api_v1.LibraryProgramMetadataApi);
this.libraryProgram = this.withCellId(__wandelbots_nova_api_v1.LibraryProgramApi);
this.libraryRecipeMetadata = this.withCellId(__wandelbots_nova_api_v1.LibraryRecipeMetadataApi);
this.libraryRecipe = this.withCellId(__wandelbots_nova_api_v1.LibraryRecipeApi);
this.storeObject = this.withCellId(__wandelbots_nova_api_v1.StoreObjectApi);
this.storeCollisionComponents = this.withCellId(__wandelbots_nova_api_v1.StoreCollisionComponentsApi);
this.storeCollisionScenes = this.withCellId(__wandelbots_nova_api_v1.StoreCollisionScenesApi);
}
/**
* Some TypeScript sorcery which alters the API class methods so you don't
* have to pass the cell id to every single one, and de-encapsulates the
* response data
*/
withCellId(ApiConstructor) {
const apiClient = new ApiConstructor({
...this.opts,
isJsonMime: (mime) => {
return mime === "application/json";
}
}, this.opts.basePath ?? "", this.opts.axiosInstance ?? axios.default.create());
for (const key of Reflect.ownKeys(Reflect.getPrototypeOf(apiClient))) if (key !== "constructor" && typeof apiClient[key] === "function") {
const originalFunction = apiClient[key];
apiClient[key] = (...args) => {
return originalFunction.apply(apiClient, [this.cellId, ...args]).then((res) => res.data);
};
}
return apiClient;
}
/**
* As withCellId, but only does the response unwrapping
*/
withUnwrappedResponsesOnly(ApiConstructor) {
const apiClient = new ApiConstructor({
...this.opts,
isJsonMime: (mime) => {
return mime === "application/json";
}
}, this.opts.basePath ?? "", this.opts.axiosInstance ?? axios.default.create());
for (const key of Reflect.ownKeys(Reflect.getPrototypeOf(apiClient))) if (key !== "constructor" && typeof apiClient[key] === "function") {
const originalFunction = apiClient[key];
apiClient[key] = (...args) => {
return originalFunction.apply(apiClient, args).then((res) => res.data);
};
}
return apiClient;
}
};
//#endregion
//#region src/lib/v1/mock/MockNovaInstance.ts
/**
* Ultra-simplified mock Nova server for testing stuff
*/
var MockNovaInstance = class {
constructor() {
this.connections = [];
}
async handleAPIRequest(config) {
const apiHandlers = [
{
method: "GET",
path: "/cells/:cellId/controllers",
handle() {
return { instances: [{
controller: "mock-ur5e",
model_name: "UniversalRobots::Controller",
host: "mock-ur5e",
allow_software_install_on_controller: true,
physical_motion_groups: [{
motion_group: "0@mock-ur5e",
name_from_controller: "UR5e",
active: false,
model_from_controller: "UniversalRobots_UR5e"
}],
has_error: false,
error_details: ""
}] };
}
},
{
method: "GET",
path: "/cells/:cellId/controllers/:controllerId",
handle() {
return {
configuration: {
kind: "VirtualController",
manufacturer: "universalrobots",
position: "[0,-1.571,-1.571,-1.571,1.571,-1.571,0]",
type: "universalrobots-ur5e"
},
name: "mock-ur5"
};
}
},
{
method: "GET",
path: "/cells/:cellId/motion-groups/:motionGroupId/specification",
handle() {
return {
dh_parameters: [
{
alpha: 1.5707963267948966,
theta: 0,
a: 0,
d: 162.25,
reverse_rotation_direction: false
},
{
alpha: 0,
theta: 0,
a: -425,
d: 0,
reverse_rotation_direction: false
},
{
alpha: 0,
theta: 0,
a: -392.2,
d: 0,
reverse_rotation_direction: false
},
{
alpha: 1.5707963267948966,
theta: 0,
a: 0,
d: 133.3,
reverse_rotation_direction: false
},
{
alpha: -1.5707963267948966,
theta: 0,
a: 0,
d: 99.7,
reverse_rotation_direction: false
},
{
alpha: 0,
theta: 0,
a: 0,
d: 99.6,
reverse_rotation_direction: false
}
],
mechanical_joint_limits: [
{
joint: "JOINTNAME_AXIS_1",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_2",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_3",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_4",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_5",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_6",
lower_limit: -6.335545063018799,
upper_limit: 6.335545063018799,
unlimited: false
}
]
};
}
},
{
method: "GET",
path: "/cells/:cellId/motion-groups/:motionGroupId/safety-setup",
handle() {
return {
safety_settings: [{
safety_state: "SAFETY_NORMAL",
settings: {
joint_position_limits: [
{
joint: "JOINTNAME_AXIS_1",
lower_limit: -2.96705961227417,
upper_limit: 2.96705961227417,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_2",
lower_limit: -1.7453292608261108,
upper_limit: 2.7925267219543457,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_3",
lower_limit: -3.3161256313323975,
upper_limit: .40142571926116943,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_4",
lower_limit: -3.4906585216522217,
upper_limit: 3.4906585216522217,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_5",
lower_limit: -2.4434609413146973,
upper_limit: 2.4434609413146973,
unlimited: false
},
{
joint: "JOINTNAME_AXIS_6",
lower_limit: -4.71238899230957,
upper_limit: 4.71238899230957,
unlimited: false
}
],
joint_velocity_limits: [
{
joint: "JOINTNAME_AXIS_1",
limit: 3.1415927410125732
},
{
joint: "JOINTNAME_AXIS_2",
limit: 3.1415927410125732
},
{
joint: "JOINTNAME_AXIS_3",
limit: 3.4906585216522217
},
{
joint: "JOINTNAME_AXIS_4",
limit: 6.108652591705322
},
{
joint: "JOINTNAME_AXIS_5",
limit: 6.108652591705322
},
{
joint: "JOINTNAME_AXIS_6",
limit: 6.981317043304443
}
],
joint_acceleration_limits: [],
joint_torque_limits: [],
tcp_velocity_limit: 1800
}
}],
safety_zones: [
{
id: 1,
priority: 0,
geometry: {
compound: { child_geometries: [
{
convex_hull: { vertices: [
{
x: -800,
y: -1330,
z: -1820
},
{
x: 1650,
y: -1330,
z: -1820
},
{
x: 1650,
y: 1330,
z: -1820
},
{
x: -800,
y: 1330,
z: -1820
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
},
{
convex_hull: { vertices: [
{
x: -800,
y: -1330,
z: -1820
},
{
x: 1650,
y: -1330,
z: -1820
},
{
x: 1650,
y: -1330,
z: 1500
},
{
x: -800,
y: -1330,
z: 1500
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
},
{
convex_hull: { vertices: [
{
x: -800,
y: -1330,
z: -1820
},
{
x: -800,
y: 1330,
z: -1820
},
{
x: -800,
y: 1330,
z: 1500
},
{
x: -800,
y: -1330,
z: 1500
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
},
{
convex_hull: { vertices: [
{
x: 1650,
y: 1330,
z: 1500
},
{
x: -800,
y: 1330,
z: 1500
},
{
x: -800,
y: -1330,
z: 1500
},
{
x: 1650,
y: -1330,
z: 1500
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
},
{
convex_hull: { vertices: [
{
x: 1650,
y: 1330,
z: 1500
},
{
x: -800,
y: 1330,
z: 1500
},
{
x: -800,
y: 1330,
z: -1820
},
{
x: 1650,
y: 1330,
z: -1820
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
},
{
convex_hull: { vertices: [
{
x: 1650,
y: 1330,
z: 1500
},
{
x: 1650,
y: -1330,
z: 1500
},
{
x: 1650,
y: -1330,
z: -1820
},
{
x: 1650,
y: 1330,
z: -1820
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "box"
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "Cell workzone"
},
motion_group_uid: 1
},
{
id: 2,
priority: 0,
geometry: {
convex_hull: { vertices: [
{
x: 1650,
y: 1330,
z: -1850
},
{
x: 865,
y: 1330,
z: -1850
},
{
x: 865,
y: -720,
z: -1850
},
{
x: 1650,
y: -720,
z: -1850
},
{
x: 1650,
y: 1330,
z: -920
},
{
x: 865,
y: 1330,
z: -920
},
{
x: 865,
y: -720,
z: -920
},
{
x: 1650,
y: -720,
z: -920
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "Transport"
},
motion_group_uid: 1
},
{
id: 3,
priority: 0,
geometry: {
convex_hull: { vertices: [
{
x: 1650,
y: 1330,
z: -600
},
{
x: 865,
y: 1330,
z: -600
},
{
x: 865,
y: 430,
z: -600
},
{
x: 1650,
y: 430,
z: -600
},
{
x: 1650,
y: 1330,
z: -1250
},
{
x: 865,
y: 1330,
z: -1250
},
{
x: 865,
y: 430,
z: -1250
},
{
x: 1650,
y: 430,
z: -1250
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "Tunel"
},
motion_group_uid: 1
},
{
id: 4,
priority: 0,
geometry: {
convex_hull: { vertices: [
{
x: 1650,
y: -760,
z: -440
},
{
x: 900,
y: -760,
z: -440
},
{
x: 900,
y: -1330,
z: -440
},
{
x: 1650,
y: -1330,
z: -440
},
{
x: 1650,
y: -760,
z: -1800
},
{
x: 900,
y: -760,
z: -1800
},
{
x: 900,
y: -1330,
z: -1800
},
{
x: 1650,
y: -1330,
z: -1800
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "Fanuc controller"
},
motion_group_uid: 1
},
{
id: 6,
priority: 0,
geometry: {
convex_hull: { vertices: [
{
x: -200,
y: -200,
z: -1900
},
{
x: 200,
y: -200,
z: -1900
},
{
x: 200,
y: 200,
z: -1900
},
{
x: -200,
y: 200,
z: -1900
},
{
x: -200,
y: -200,
z: -350
},
{
x: 200,
y: -200,
z: -350
},
{
x: 200,
y: 200,
z: -350
},
{
x: -200,
y: 200,
z: -350
}
] },
init_pose: {
position: {
x: 0,
y: 0,
z: 0
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "Robot base"
},
motion_group_uid: 1
}
],
robot_model_geometries: [
{
link_index: 1,
geometry: {
sphere: { radius: 270 },
init_pose: {
position: {
x: -70,
y: -70,
z: -50
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "link1_sphere"
}
},
{
link_index: 2,
geometry: {
capsule: {
radius: 160,
cylinder_height: 800
},
init_pose: {
position: {
x: -450,
y: 40,
z: 170
},
orientation: {
x: 0,
y: -Math.SQRT1_2,
z: 0,
w: Math.SQRT1_2
}
},
id: "link2_capsule"
}
},
{
link_index: 3,
geometry: {
sphere: { radius: 270 },
init_pose: {
position: {
x: -110,
y: 10,
z: -100
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "link3_sphere"
}
},
{
link_index: 4,
geometry: {
capsule: {
radius: 110,
cylinder_height: 600
},
init_pose: {
position: {
x: 0,
y: 300,
z: 40
},
orientation: {
x: -Math.SQRT1_2,
y: 0,
z: 0,
w: Math.SQRT1_2
}
},
id: "link4_capsule"
}
},
{
link_index: 5,
geometry: {
sphere: { radius: 75 },
init_pose: {
position: {
x: 0,
y: 0,
z: -50
},
orientation: {
x: 0,
y: 0,
z: 0,
w: 1
}
},
id: "link5_sphere"
}
}
],
tool_geometries: []
};
}
},
{
method: "GET",
path: "/cells/:cellId/coordinate-systems",
handle() {
return { coordinatesystems: [{
coordinate_system: "",
name: "world",
reference_uid: "",
position: {
x: 0,
y: 0,
z: 0
},
rotation: {
angles: [
0,
0,
0
],
type: "ROTATION_VECTOR"
}
}] };
}
},
{
method: "GET",
path: "/cells/:cellId/motion-groups/:motionGroupId/tcps",
handle() {
return { tcps: [{
id: "Flange",
readable_name: "Default-Flange",
position: {
x: 0,
y: 0,
z: 0
},
rotation: {
angles: [
0,
0,
0,
0
],
type: "ROTATION_VECTOR"
}
}, {
id: "complex-tcp-position",
readable_name: "Complex TCP Position",
position: {
x: -200,
y: 300,
z: 150
},
rotation: {
angles: [
-.12139440409113832,
-.06356210998212003,
-.2023240068185639,
0
],
type: "ROTATION_VECTOR"
}
}] };
}
}
];
const method = config.method?.toUpperCase() || "GET";
const path = `/cells${config.url?.split("/cells")[1]?.split("?")[0]}`;
for (const handler of apiHandlers) {
const match = path_to_regexp.match(handler.path)(path || "");
if (method === handler.method && match) {
const json = handler.handle();
return {
status: 200,
statusText: "Success",
data: JSON.stringify(json),
headers: {},
config,
request: { responseURL: config.url }
};
}
}
throw new axios.AxiosError(`No mock handler matched this request: ${method} ${path}`, "404", config);
}
handleWebsocketConnection(socket) {
this.connections.push(socket);
setTimeout(() => {
socket.dispatchEvent(new Event("open"));
console.log("Websocket connection opened from", socket.url);
if (socket.url.includes("/state-stream")) socket.dispatchEvent(new MessageEvent("message", { data: JSON.stringify(defaultMotionState) }));
if (socket.url.includes("/move-joint")) socket.dispatchEvent(new MessageEvent("message", { data: JSON.stringify({ result: {
motion_group: "0@ur",
state: {
controller: "ur",
operation_mode: "OPERATION_MODE_AUTO",
safety_state: "SAFETY_STATE_NORMAL",
timestamp: "2024-09-18T12:48:26.096266444Z",
velocity_override: 100,
motion_groups: [{
motion_group: "0@ur",
controller: "ur",
joint_position: { joints: [
1.3492152690887451,
-1.5659207105636597,
1.6653711795806885,
-1.0991662740707397,
-1.829018235206604,
1.264623761177063
] },
joint_velocity: { joints: [
0,
0,
0,
0,
0,
0
] },
flange_pose: {
position: {
x: 6.437331889439328,
y: -628.4123774830913,
z: 577.0569957147832
},
orientation: {
x: -1.683333649797158,
y: -1.9783363827298732,
z: -.4928031860165713
},
coordinate_system: ""
},
tcp_pose: {
position: {
x: 6.437331889439328,
y: -628.4123774830913,
z: 577.0569957147832
},
orientation: {
x: -1.683333649797158,
y: -1.9783363827298732,
z: -.4928031860165713
},
coordinate_system: "",
tcp: "Flange"
},
velocity: {
linear: {
x: 0,
y: 0,
z: 0
},
angular: {
x: -0,
y: 0,
z: 0
},
coordinate_system: ""
},
force: {
force: {
x: 0,
y: 0,
z: 0
},
moment: {
x: 0,
y: 0,
z: 0
},
coordinate_system: ""
},
joint_limit_reached: { limit_reached: [
false,
false,
false,
false,
false,
false
] },
joint_current: { joints: [
0,