wangdequan/cnc_wams
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: verify run path with 50ms screenshots

Browse Source
This commit is contained in:
wangdequan
2026-07-03 17:04:33 -04:00
parent 7a30e5f0e9
commit ed9eb3ec17
3304 changed files with 237409 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

424
web-rtcp-5axis-xyzbc-trt-sim-plan/app/dist/src/state/store.js vendored
View File

@@ -487,6 +487,25 @@ export function createSimulationStore(seed = {}) {
});
};
const waitForStatePolling = (predicate, timeoutMs = 10000, intervalMs = 25) => {
if (predicate(state)) return Promise.resolve(state);
return new Promise((resolve, reject) => {
const startedAt = Date.now();
const tick = () => {
if (predicate(state)) {
resolve(state);
return;
}
if (Date.now() - startedAt > timeoutMs) {
reject(new Error("timed out waiting for store state"));
return;
}
setTimeout(tick, intervalMs);
};
setTimeout(tick, intervalMs);
});
};
const dispatch = (action) => {
switch (action.type) {
case "BOOT_READY":
@@ -1161,6 +1180,13 @@ export function createSimulationStore(seed = {}) {
});
});
break;
case "RUN_FROM_OPERATOR":
return operatorRunSequence().catch((error) => {
dispatch({
type: "TASK_HAL_COMMAND_FAILED",
error: error instanceof Error ? error.message : String(error),
});
});
case "SET_FRAME_SOURCE":
setState({
sourceMode: action.sourceMode,
@@ -1503,27 +1529,33 @@ export function createSimulationStore(seed = {}) {
break;
case "RUN_MDI":
{
const gate = gateLinuxCncTaskAction(state, action);
const command = normalizeMdiCommand(action.command ?? state.machine.mdiCommand);
const gate = gateLinuxCncTaskAction(state, { ...action, command });
if (!gate.allowed) {
setState({ operatorMessage: gate.operatorMessage });
break;
}
const preserveManualTouchOff = action.manualTouchOff === true && state.machine.mode === "manual";
if (state.taskHalRuntime?.loaded) {
const command = normalizeMdiCommand(action.command ?? state.machine.mdiCommand);
const mdiResult = executeMdiCommand(state, command);
setState(mdiResult.patch);
const mdiPatch = preserveManualTouchOff
? createManualTouchOffMdiPatch(state, mdiResult.patch, command)
: mdiResult.patch;
setState(mdiPatch);
runTaskHalCommandSequence([
{ type: "EMC_TASK_SET_MODE", mode: "MDI" },
{ type: "EMC_TASK_PLAN_EXECUTE", mdi: command },
], {
operatorMessage: `task/HAL MDI ${command}`,
preserveAxisPose: mdiResult.patch.axisPose,
preserveMachine: mdiResult.patch.machine,
preserveAxisPose: mdiPatch.axisPose,
preserveMachine: mdiPatch.machine,
}).catch(() => {});
break;
}
const mdiResult = executeMdiCommand(state, action.command ?? state.machine.mdiCommand);
setState(mdiResult.patch);
const mdiResult = executeMdiCommand(state, command);
setState(preserveManualTouchOff
? createManualTouchOffMdiPatch(state, mdiResult.patch, command)
: mdiResult.patch);
}
break;
case "LOAD_PROGRAM":
@@ -2480,16 +2512,121 @@ export function createSimulationStore(seed = {}) {
return state;
};
const operatorRunSequence = async () => {
if (!state.machineFileStaging?.selectedGcodeSourceRel) {
const sourceRel = defaultLinuxCncGcodeSourceForState(state)?.sourceRel;
if (sourceRel) {
dispatch({ type: "LOAD_LINUXCNC_GCODE_SOURCE", sourceRel });
await waitForStatePredicate((nextState) => nextState.machineFileStaging?.selectedGcodeSourceRel === sourceRel);
}
}
const tcpKinsType = tcpKinsTypeForProfile(state.profile);
if (state.taskHalRuntime?.loaded) {
if (
!Array.isArray(state.programExecution?.motion)
|| state.programExecution.motion.length === 0
|| state.interpreterExecutionPending
) {
if (!state.interpreterExecutionPending) {
dispatch({ type: "RUN_MACHINE_FILE_PROGRAM" });
}
await waitForStatePolling((nextState) => (
nextState.interpreterExecutionPending === false
&& Array.isArray(nextState.programExecution?.motion)
&& nextState.programExecution.motion.length > 0
), 15000);
}
if (tcpKinsType && state.kinsType !== tcpKinsType) {
dispatch({ type: "SET_KINS_TYPE", kinsType: tcpKinsType });
}
setState({
machine: {
...state.machine,
powerOn: true,
estopActive: false,
taskState: "on",
mode: "auto",
manualPanel: null,
allHomed: true,
interpState: "idle",
interpResumeState: "idle",
taskPaused: false,
},
runState: "idle",
operatorMessage: "RUN ready: motion plan source ready; preparing task/HAL session",
});
const expectedProgramPath = expectedTaskHalProgramPathForState(state);
if (!state.taskHalSession || (expectedProgramPath && state.taskHalSession.programPath !== expectedProgramPath)) {
setState({ operatorMessage: "RUN preparing: initializing task/HAL session" });
await initializeTaskHalSession({ openProgram: true });
}
setState({ operatorMessage: "RUN preparing: loading task/HAL motion plan" });
const loadedMotionPlan = await loadTaskHalMotionPlanWithSessionRetry();
if (!loadedMotionPlan) {
setState({ operatorMessage: "run blocked: task/HAL feed motion plan not loaded" });
return state;
}
const ready = validateRunPreconditions(state, { requireTaskHalSession: true });
if (!ready.ok) {
setState({ operatorMessage: ready.operatorMessage });
return state;
}
stopTaskHalStatusLoop("restarted", {
operatorMessage: "task/HAL status loop restarting",
});
setState({
activeLine: state.programStartLine || 1,
programExecutionMotionIndex: 0,
programExecutionSampleIndex: 0,
programRuntimeFeedback: null,
programLineExecution: {},
operatorMessage: "RUN executing: sending task/HAL PLAN_RUN",
});
const status = await runTaskHalCommandSequence([
{ type: "EMC_TASK_SET_STATE", state: "ON" },
{ type: "EMC_JOINT_HOME", joint: -1 },
{ type: "EMC_TASK_SET_MODE", mode: "AUTO" },
{ type: "EMC_TASK_PLAN_RUN", line: 0 },
], {
taskCycles: 5,
operatorMessage: `task/HAL program run ${ready.profileId} ${ready.kinematicsModuleId}`,
allowFixtureSession: false,
});
if (shouldContinueTaskHalStatusLoop(state, status)) {
startTaskHalStatusLoop({
profileId: ready.profileId,
iniPath: ready.iniPath,
kinematicsModuleId: ready.kinematicsModuleId,
operatorMessage: `task/HAL status loop running ${ready.profileId} ${ready.kinematicsModuleId}`,
});
}
return state;
}
const policy = createLinuxCncTaskPolicyStatus(state);
if (policy.taskMode !== "auto" || !policy.allHomed || policy.taskState !== "on") {
await runReadySequence();
}
dispatch({ type: "RUN" });
return state;
};
const loadTaskHalMotionPlanForSession = async (session = state.taskHalSession) => {
if (!state.taskHalRuntime?.loaded || typeof state.taskHalRuntime.loadProgramMotionPlan !== "function") {
return null;
}
setState({ operatorMessage: "RUN preparing: building task/HAL motion plan" });
const motion = state.programExecution?.motion || [];
const timing = state.programExecutionTiming || buildTimingForState(state, state.programExecution);
if (!session?.programPath || !Array.isArray(motion) || motion.length === 0 || !Array.isArray(timing?.segments) || timing.segments.length === 0) {
return null;
}
const plan = buildTaskHalProgramMotionPlan({
const previewPlan = buildTaskHalProgramMotionPlanFromPreviewPath({
programPath: session.programPath,
path: state.programAxisPreviewPath,
profile: state.profile,
linearUnits: timing.linearUnits || state.profile?.traj?.linearUnits || "mm",
});
const plan = previewPlan || buildTaskHalProgramMotionPlan({
programPath: session.programPath,
motion,
timing,
@@ -2499,10 +2636,27 @@ export function createSimulationStore(seed = {}) {
if (plan.segmentCount <= 0) {
return null;
}
await state.taskHalRuntime.loadProgramMotionPlan(plan);
setState({ operatorMessage: `RUN preparing: worker loading ${plan.segmentCount} task/HAL motion segments` });
await withTimeout(
state.taskHalRuntime.loadProgramMotionPlan(plan),
5000,
"task/HAL feed motion plan load timed out",
);
return plan;
};
const loadTaskHalMotionPlanWithSessionRetry = async () => {
let plan = null;
try {
plan = await loadTaskHalMotionPlanForSession(state.taskHalSession);
} catch (error) {
setState({ operatorMessage: `task/HAL motion plan reload: ${error instanceof Error ? error.message : String(error)}` });
}
if (plan) return plan;
await initializeTaskHalSession({ openProgram: true });
return loadTaskHalMotionPlanForSession(state.taskHalSession);
};
const startTaskHalStatusLoop = ({
profileId = state.machineProfile,
iniPath = state.profile?.iniPath || null,
@@ -3292,6 +3446,21 @@ function sourceBasename(path) {
return String(path || "").split("/").filter(Boolean).at(-1) || "";
}
function withTimeout(promise, timeoutMs, message) {
return new Promise((resolve, reject) => {
const timeoutId = setTimeout(() => reject(new Error(message)), timeoutMs);
Promise.resolve(promise)
.then((value) => {
clearTimeout(timeoutId);
resolve(value);
})
.catch((error) => {
clearTimeout(timeoutId);
reject(error);
});
});
}
function isAsyncKinematicsRuntime(runtime) {
return runtime?.executionContext === "worker";
}
@@ -3493,19 +3662,54 @@ function applyTaskHalStatusPatch(state, status, operatorMessage, {
? "jogging"
: "idle";
const runtimeFeedback = createTaskHalRuntimeFeedback(state, status, axisPose, activeLine);
const taskHalSampleIndex = resolveRuntimeSampleIndexForLine(state, activeLine, runtimeFeedback.sampleIndex);
const taskHalPlaybackPatch = applyProgramPlaybackUiPatch(state, {
activeLine,
axisPose,
kinsType,
rtcpState: rtcpStateFromKinsType(kinsType),
motionIndex: runtimeFeedback.motionIndex,
const shouldTrackProgramPlayback = runState === "running"
|| runState === "mdi"
|| (runState === "complete" && (state.runState === "running" || state.runState === "mdi" || state.runState === "complete"));
const taskHalSampleIndex = shouldTrackProgramPlayback
? resolveRuntimeSampleIndexForTaskHalPose(state, axisPose, activeLine, runtimeFeedback.sampleIndex)
: clampNumber(
state.programExecutionSampleIndex || 0,
0,
Math.max(Number(state.programAxisPreviewPath?.samples?.length || state.programAxisPreviewPath?.sampleCount || 1) - 1, 0),
);
const idleRuntimeFeedback = {
...runtimeFeedback,
sampleIndex: taskHalSampleIndex,
runtimeFeedback: {
...runtimeFeedback,
motionIndex: Number(state.programExecutionMotionIndex || runtimeFeedback.motionIndex || 0),
};
const taskHalPlaybackPatch = shouldTrackProgramPlayback
? applyProgramPlaybackUiPatch(state, {
activeLine,
axisPose,
kinsType,
rtcpState: rtcpStateFromKinsType(kinsType),
motionIndex: runtimeFeedback.motionIndex,
sampleIndex: taskHalSampleIndex,
},
});
runtimeFeedback: {
...runtimeFeedback,
sampleIndex: taskHalSampleIndex,
},
preferRuntimeAxisPose: true,
})
: {
activeLine: state.activeLine,
axisPose,
kinsType: kinsType || state.kinsType,
rtcpState: kinsType ? rtcpStateFromKinsType(kinsType) : state.rtcpState,
toolAxisVector: state.toolAxisVector,
programExecutionMotionIndex: Number(state.programExecutionMotionIndex || 0),
programExecutionSampleIndex: taskHalSampleIndex,
programRuntimeFeedback: idleRuntimeFeedback,
programUiExecution: state.programUiExecution
? createProgramUiExecution({
...state,
axisPose,
kinsType: kinsType || state.kinsType,
programExecutionSampleIndex: taskHalSampleIndex,
programRuntimeFeedback: idleRuntimeFeedback,
}, { preferRuntimePose: true })
: state.programUiExecution,
};
const loopActive = state.taskHalStatusLoop?.active === true
&& loopSequence !== null
&& Number(state.taskHalStatusLoop.sequence) === Number(loopSequence)
@@ -3720,6 +3924,84 @@ function createTaskHalRuntimeFeedback(state, status, axisPose, activeLine) {
};
}
function buildTaskHalProgramMotionPlanFromPreviewPath({
programPath = null,
path = null,
profile = defaultProfile,
linearUnits = "mm",
} = {}) {
const samples = path?.samples;
if (!Array.isArray(samples) || samples.length < 2) return null;
const samplePeriodSeconds = Math.max(Number(path.samplePeriodMs || 50) / 1000, 0.001);
const segments = [];
for (let index = 1; index < samples.length; index += 1) {
const startSample = samples[index - 1];
const endSample = samples[index];
const startAxes = taskHalPlanAxesFromPose(axisPoseFromProgramPathSample(startSample, initialAxisPose, profile));
const endAxes = taskHalPlanAxesFromPose(axisPoseFromProgramPathSample(endSample, startAxes, profile));
const startSeconds = Number.isFinite(Number(startSample?.timeMs))
? Number(startSample.timeMs) / 1000
: (index - 1) * samplePeriodSeconds;
const endSeconds = Number.isFinite(Number(endSample?.timeMs))
? Number(endSample.timeMs) / 1000
: index * samplePeriodSeconds;
const durationSeconds = Math.max(endSeconds - startSeconds, samplePeriodSeconds);
const velocityMmPerMin = estimatePreviewSegmentVelocityMmPerMin(startAxes, endAxes, durationSeconds);
const type = endSample?.motionType || startSample?.motionType || "STRAIGHT_FEED";
segments.push({
line: Number(endSample?.line || startSample?.line || 1),
type,
motionClass: String(type).includes("TRAVERSE") ? "rapid" : "feed",
feedMode: "units-per-minute",
startSeconds,
durationSeconds,
elapsedSeconds: startSeconds + durationSeconds,
feedRate: velocityMmPerMin,
linearUnits,
velocityMmPerMin,
requestedVelocityMmPerMin: velocityMmPerMin,
startAxes,
endAxes,
});
}
if (segments.length === 0) return null;
return {
apiName: "web-rtcp-5axis-task-hal-program-motion-plan",
semanticBoundary: "linuxcnc_task_hal_plan_from_expanded_axis_preview_samples",
programPath,
linearUnits,
totalSeconds: segments[segments.length - 1].elapsedSeconds,
segmentCount: segments.length,
segments,
};
}
function taskHalPlanAxesFromPose(pose = {}) {
return {
x: Number(pose.x || 0),
y: Number(pose.y || 0),
z: Number(pose.z || 0),
a: Number(pose.a || 0),
b: Number(pose.b || 0),
c: Number(pose.c || 0),
};
}
function estimatePreviewSegmentVelocityMmPerMin(startAxes = {}, endAxes = {}, durationSeconds = 0.05) {
const linearDistance = Math.hypot(
Number(endAxes.x || 0) - Number(startAxes.x || 0),
Number(endAxes.y || 0) - Number(startAxes.y || 0),
Number(endAxes.z || 0) - Number(startAxes.z || 0),
);
const rotaryDistance = Math.hypot(
Number(endAxes.a || 0) - Number(startAxes.a || 0),
Number(endAxes.b || 0) - Number(startAxes.b || 0),
Number(endAxes.c || 0) - Number(startAxes.c || 0),
);
const distance = Math.max(linearDistance, rotaryDistance * 0.1, 0.001);
return durationSeconds > 0 ? (distance / durationSeconds) * 60 : 1;
}
function resolveRuntimeSampleIndexForLine(state = {}, activeLine, fallbackSampleIndex = 0) {
const samples = state.programAxisPreviewPath?.samples;
if (!Array.isArray(samples) || samples.length === 0) {
@@ -3736,6 +4018,51 @@ function resolveRuntimeSampleIndexForLine(state = {}, activeLine, fallbackSample
return samples.length - 1;
}
function resolveRuntimeSampleIndexForTaskHalPose(state = {}, axisPose = {}, activeLine, fallbackSampleIndex = 0) {
const samples = state.programAxisPreviewPath?.samples;
if (!Array.isArray(samples) || samples.length === 0) {
return resolveRuntimeSampleIndexForLine(state, activeLine, fallbackSampleIndex);
}
const current = clampNumber(state.programExecutionSampleIndex || 0, 0, samples.length - 1);
const previousCycle = Number(state.programRuntimeFeedback?.cycle || 0);
const nextCycle = Number(fallbackSampleIndex || 0);
const cycleDelta = Number.isFinite(nextCycle) && Number.isFinite(previousCycle)
? Math.max(nextCycle - previousCycle, 0)
: 0;
const adaptiveWindow = Math.max(320, Math.min(samples.length - 1, Math.ceil(cycleDelta * 8) + 240));
const windowStart = current;
const windowEnd = Math.min(samples.length - 1, current + adaptiveWindow);
const line = Number(activeLine);
let bestIndex = current;
let bestScore = Number.POSITIVE_INFINITY;
for (let index = windowStart; index <= windowEnd; index += 1) {
const sample = samples[index];
const samplePose = axisPoseFromProgramPathSample(sample, axisPose, state.profile);
const linePenalty = Number.isFinite(line) && Number.isFinite(Number(sample?.line))
? Math.min(Math.abs(Number(sample.line) - line), 12) * 0.25
: 0;
const forwardPenalty = (index - current) * 0.000001;
const score = taskHalPoseDistanceScore(axisPose, samplePose) + linePenalty + forwardPenalty;
if (score < bestScore) {
bestScore = score;
bestIndex = index;
}
}
return Math.max(current, bestIndex);
}
function taskHalPoseDistanceScore(a = {}, b = {}) {
const linear = ["x", "y", "z"].reduce((sum, axis) => {
const delta = Number(a[axis] || 0) - Number(b[axis] || 0);
return sum + delta * delta;
}, 0);
const rotary = ["a", "b", "c"].reduce((sum, axis) => {
const delta = Number(a[axis] || 0) - Number(b[axis] || 0);
return sum + (delta * delta * 0.01);
}, 0);
return linear + rotary;
}
function applyProgramPlaybackUiPatch(state = {}, {
activeLine,
axisPose,
@@ -3744,9 +4071,13 @@ function applyProgramPlaybackUiPatch(state = {}, {
motionIndex,
sampleIndex,
runtimeFeedback,
preferRuntimeAxisPose = false,
} = {}) {
const sample = currentProgramPathSample(state, sampleIndex);
const sampleAxisPose = axisPoseFromProgramPathSample(sample, axisPose || state.axisPose, state.profile);
const runtimeAxisPose = clampAxisPoseToProfile(axisPose || runtimeFeedback?.axisPose || state.axisPose, state.profile);
const sampleAxisPose = preferRuntimeAxisPose
? runtimeAxisPose
: axisPoseFromProgramPathSample(sample, runtimeAxisPose, state.profile);
const sampleKinsType = sample?.activeKinematics
? normalizeSampleKinsType(state, sample.activeKinematics)
: kinsType;
@@ -3764,7 +4095,7 @@ function applyProgramPlaybackUiPatch(state = {}, {
axisPose: sampleAxisPose,
kinsType: sampleKinsType || state.kinsType,
toolAxisVector,
});
}, { preferRuntimeAxisPose });
const patchState = {
...state,
activeLine: sampleLine,
@@ -3785,11 +4116,13 @@ function applyProgramPlaybackUiPatch(state = {}, {
programExecutionMotionIndex: patchState.programExecutionMotionIndex,
programExecutionSampleIndex: patchState.programExecutionSampleIndex,
programRuntimeFeedback: enrichedFeedback,
programUiExecution: createProgramUiExecution(patchState),
programUiExecution: createProgramUiExecution(patchState, { preferRuntimePose: preferRuntimeAxisPose }),
};
}
function createProgramUiExecution(state = {}) {
function createProgramUiExecution(state = {}, {
preferRuntimePose = false,
} = {}) {
const sample = currentProgramPathSample(state, state.programExecutionSampleIndex);
const feedback = state.programRuntimeFeedback || {};
const sampleCount = Number(state.programAxisPreviewPath?.sampleCount || state.programAxisPreviewPath?.samples?.length || 0);
@@ -3829,8 +4162,12 @@ function createProgramUiExecution(state = {}) {
segmentIndex: sample?.segmentIndex ?? traceEntry?.segmentIndex ?? null,
motionType: sample?.motionType || traceEntry?.motionType || feedback.motionType || feedback.type || null,
activeKinematics: sample?.activeKinematics || traceEntry?.activeKinematicsAfter || state.kinsType,
joint: sample?.joint || pickExecutionAxes(state.axisPose || {}),
tcp: sample?.tcp || {
joint: preferRuntimePose
? pickExecutionAxes(feedback.axisPose || state.axisPose || {})
: sample?.joint || pickExecutionAxes(state.axisPose || {}),
tcp: preferRuntimePose && feedback.tcp
? feedback.tcp
: sample?.tcp || {
x: Number(state.tcpPose?.x || state.axisPose?.x || 0),
y: Number(state.tcpPose?.y || state.axisPose?.y || 0),
z: Number(state.tcpPose?.z || state.axisPose?.z || 0),
@@ -3886,13 +4223,22 @@ function toolAxisSampleFromVector(vector = {}) {
};
}
function enrichRuntimeFeedbackWithSample(feedback = {}, sample = null, state = {}) {
function enrichRuntimeFeedbackWithSample(feedback = {}, sample = null, state = {}, {
preferRuntimeAxisPose = false,
} = {}) {
if (!sample) return feedback;
const tcp = sample.tcp || {
const runtimeAxisPose = feedback?.axisPose || state.axisPose || {};
const tcp = preferRuntimeAxisPose
? {
x: Number(runtimeAxisPose.x ?? 0),
y: Number(runtimeAxisPose.y ?? 0),
z: Number(runtimeAxisPose.z ?? 0),
}
: sample.tcp || {
x: Number(state.tcpPose?.x ?? state.axisPose?.x ?? feedback?.axisPose?.x ?? 0),
y: Number(state.tcpPose?.y ?? state.axisPose?.y ?? feedback?.axisPose?.y ?? 0),
z: Number(state.tcpPose?.z ?? state.axisPose?.z ?? feedback?.axisPose?.z ?? 0),
};
};
return {
...(feedback || {}),
sampleIndex: Number(sample.sampleIndex || 0),
@@ -3902,7 +4248,9 @@ function enrichRuntimeFeedbackWithSample(feedback = {}, sample = null, state = {
statement: sample.statement || feedback?.statement || "",
motionType: sample.motionType || feedback?.motionType || null,
activeKinematics: sample.activeKinematics || feedback?.activeKinematics || state.kinsType,
axisPose: axisPoseFromProgramPathSample(sample, state.axisPose || feedback?.axisPose || {}, state.profile),
axisPose: preferRuntimeAxisPose
? runtimeAxisPose
: axisPoseFromProgramPathSample(sample, state.axisPose || feedback?.axisPose || {}, state.profile),
tcp,
toolAxis: sample.toolAxis || feedback?.toolAxis || null,
machineState: sample.machineState || feedback?.machineState || null,
@@ -4155,6 +4503,22 @@ function executeMdiCommand(state, rawCommand) {
return { patch };
}
function createManualTouchOffMdiPatch(state, patch, command) {
return {
...patch,
machine: {
...patch.machine,
mode: "manual",
manualPanel: state.machine.manualPanel || "manual",
interpState: "idle",
interpResumeState: "idle",
taskPaused: false,
},
runState: "idle",
operatorMessage: `manual touch off ${command}`,
};
}
function normalizeMdiCommand(command) {
return String(command || "")
.replace(/\([^)]*\)/g, " ")