export type RobotJoint = { id: string; label: string; minimum: number; maximum: number; linkLength?: number }
export type RobotWaypoint = { id: string; values: number[] }
export type RobotPose = { step: number; values: number[] }
export type RobotSnapshot = { id: string; label: string; joints: RobotJoint[]; waypoints: RobotWaypoint[]; trajectory: RobotPose[]; status: 'draft' | 'generated' | 'invalid'; version: number }
export type RobotKinematics = { position: [number, number, number]; maxReach: number }
export type RobotApi = { snapshot(): RobotSnapshot; addJoint(joint: RobotJoint): RobotJoint; addWaypoint(waypoint: RobotWaypoint): RobotWaypoint; generateLinearTrajectory(stepsPerSegment: number): RobotSnapshot; validateTrajectory(): { valid: boolean; violations: number[] }; forwardKinematics(values: number[]): RobotKinematics; workspace(): { maxReach: number; jointCount: number }; collisions(obstacles: Array<{ id: string; min: [number, number, number]; max: [number, number, number] }>): Array<{ obstacleId: string; pose: number }>; exportController(): string; exportCsv(): string }
const finite = (value: number, label: string) => { if (!Number.isFinite(value)) throw new RangeError(`${label} must be finite.`) }
const clone = <T>(value: T): T => JSON.parse(JSON.stringify(value)) as T
export const createRobot = (id = 'robot', label = 'Robot'): RobotApi => { const joints = new Map<string, RobotJoint>(); const waypoints: RobotWaypoint[] = []; let trajectory: RobotPose[] = []; let status: RobotSnapshot['status'] = 'draft'; let version = 0; const snapshot = (): RobotSnapshot => ({ id, label, joints: [...joints.values()].map(clone), waypoints: clone(waypoints), trajectory: clone(trajectory), status, version }); const addJoint = (input: RobotJoint) => { if (!input.id.trim() || joints.has(input.id)) throw new RangeError(`Robot joint already exists: ${input.id}`); if (!input.label.trim()) throw new RangeError('Robot joint label is required.'); finite(input.minimum, 'Robot joint minimum'); finite(input.maximum, 'Robot joint maximum'); if (input.minimum >= input.maximum) throw new RangeError('Robot joint minimum must be less than maximum.'); if (input.linkLength !== undefined && (!Number.isFinite(input.linkLength) || input.linkLength <= 0)) throw new RangeError('Robot link length must be positive and finite.'); joints.set(input.id, clone(input)); status = 'draft'; version += 1; return clone(input) }; const addWaypoint = (input: RobotWaypoint) => { if (!input.id.trim() || waypoints.some((entry) => entry.id === input.id)) throw new RangeError(`Robot waypoint already exists: ${input.id}`); if (input.values.length !== joints.size || input.values.some((value) => !Number.isFinite(value))) throw new RangeError('Robot waypoint values must match joint count and be finite.'); const jointList = [...joints.values()]; input.values.forEach((value, index) => { if (value < jointList[index].minimum || value > jointList[index].maximum) throw new RangeError(`Robot waypoint exceeds joint limit: ${jointList[index].id}`) }); waypoints.push(clone(input)); status = 'draft'; version += 1; return clone(input) }; const validateTrajectory = () => { const violations: number[] = []; const jointList = [...joints.values()]; trajectory.forEach((pose, poseIndex) => pose.values.forEach((value, jointIndex) => { if (value < jointList[jointIndex].minimum || value > jointList[jointIndex].maximum) violations.push(poseIndex) })); return { valid: violations.length === 0, violations: [...new Set(violations)] } }; const generateLinearTrajectory = (stepsPerSegment: number) => { if (!Number.isSafeInteger(stepsPerSegment) || stepsPerSegment < 1 || stepsPerSegment > 1000) throw new RangeError('Robot trajectory steps must be a positive integer no greater than 1000.'); if (waypoints.length < 2) { status = 'invalid'; trajectory = []; version += 1; return snapshot() }; trajectory = []; let step = 0; for (let segment = 0; segment < waypoints.length - 1; segment += 1) { const first = waypoints[segment].values; const second = waypoints[segment + 1].values; for (let index = 0; index <= stepsPerSegment; index += 1) { if (segment > 0 && index === 0) continue; const t = index / stepsPerSegment; trajectory.push({ step: step++, values: first.map((value, jointIndex) => value + (second[jointIndex] - value) * t) }) } } const validation = validateTrajectory(); status = validation.valid ? 'generated' : 'invalid'; version += 1; return snapshot() }; const forwardKinematics = (values: number[]): RobotKinematics => { if (values.length !== joints.size || values.some((value) => !Number.isFinite(value))) throw new RangeError('Robot kinematics values must match joint count.'); let angle = 0; let x = 0; let y = 0; const jointList = [...joints.values()]; for (let index = 0; index < jointList.length; index += 1) { angle += values[index] * Math.PI / 180; const length = jointList[index].linkLength ?? 1; x += Math.cos(angle) * length; y += Math.sin(angle) * length } return { position: [x, y, 0], maxReach: jointList.reduce((sum, joint) => sum + (joint.linkLength ?? 1), 0) } }; const workspace = () => ({ maxReach: [...joints.values()].reduce((sum, joint) => sum + (joint.linkLength ?? 1), 0), jointCount: joints.size }); const collisions = (obstacles: Array<{ id: string; min: [number, number, number]; max: [number, number, number] }>) => { const output: Array<{ obstacleId: string; pose: number }> = []; for (const pose of trajectory) { const endpoint = forwardKinematics(pose.values).position; for (const obstacle of obstacles) if (endpoint.every((value, axisIndex) => value >= obstacle.min[axisIndex] && value <= obstacle.max[axisIndex])) output.push({ obstacleId: obstacle.id, pose: pose.step }) } return output }; const exportController = () => `${JSON.stringify({ schemaVersion: 1, robot: id, joints: [...joints.values()], waypoints, trajectory }, null, 2)}\n`; const exportCsv = () => [['step', ...[...joints.values()].map((joint) => joint.id)], ...trajectory.map((pose) => [String(pose.step), ...pose.values.map((value) => value.toFixed(6))])].map((row) => row.join(',')).join('\n') + (trajectory.length ? '\n' : ''); return { snapshot, addJoint, addWaypoint, generateLinearTrajectory, validateTrajectory, forwardKinematics, workspace, collisions, exportController, exportCsv } }
