diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index c66d62e13b2..da14de3f0ef 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -11,6 +11,7 @@ * Last change: ********************************************************************/ +#include #include #include @@ -1865,6 +1866,14 @@ double pmCircleEffectiveMinRadius(PmCircle const * circle) // Curvature of helix, assuming that helical motion is independent of plane motion double effective_radius = h2 / r_spiral + r_spiral; + // On a constant-radius helix, pitch is axial rise per radian. + // Keep the existing, tighter estimate for sweeps shorter than one radian. + if (circle->spiral == 0.0 && circle->angle > 1.0) { + double rise_per_radian = pmSqrt(h2) / circle->angle; + double helix_radius = circle->radius + pmSq(rise_per_radian) / circle->radius; + effective_radius = fmin(effective_radius, helix_radius); + } + return effective_radius; } - +EXPORT_SYMBOL(pmCircleEffectiveMinRadius); diff --git a/tests/tp/circle-curvature/curvature.hal b/tests/tp/circle-curvature/curvature.hal new file mode 100644 index 00000000000..12f6ef09042 --- /dev/null +++ b/tests/tp/circle-curvature/curvature.hal @@ -0,0 +1,2 @@ +loadrt tpmod +loadrt curvaturecheck diff --git a/tests/tp/circle-curvature/curvaturecheck.c b/tests/tp/circle-curvature/curvaturecheck.c new file mode 100644 index 00000000000..cee5855b888 --- /dev/null +++ b/tests/tp/circle-curvature/curvaturecheck.c @@ -0,0 +1,71 @@ +/* Check the production TP radius estimate after tpmod has been loaded. */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +extern double pmCircleEffectiveMinRadius(const PmCircle *circle); + +static int comp_id = -1; + +static int check_radius(const char *name, const PmCircle *circle, double expected) +{ + double actual = pmCircleEffectiveMinRadius(circle); + if (!isfinite(actual) || fabs(actual - expected) > 1e-9) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: radius %.12g, expected %.12g\n", + name, actual, expected); + return -1; + } + return 0; +} + +int rtapi_app_main(void) +{ + const double pi = 3.14159265358979323846; + PmCircle circle = {0}; + circle.radius = 10.0; + circle.angle = 2.0 * pi; + circle.rHelix.z = 20.0; + + comp_id = hal_init("curvaturecheck"); + if (comp_id < 0) { + return comp_id; + } + + /* R + (rise / sweep)^2 / R, independent of command subdivision. */ + double expected = 10.0 + (20.0 / (2.0 * pi)) * (20.0 / (2.0 * pi)) / 10.0; + if (check_radius("one turn", &circle, expected)) { + goto fail; + } + circle.angle = pi / 2.0; + circle.rHelix.z = 5.0; + if (check_radius("quarter turn", &circle, expected)) { + goto fail; + } + circle.rHelix.z = -5.0; + if (check_radius("reverse rise", &circle, expected)) { + goto fail; + } + + /* Below one radian, the old radius is the tighter limit. */ + circle.angle = 0.5; + circle.rHelix.z = 1.0; + if (check_radius("short sweep", &circle, 10.1)) { + goto fail; + } + + hal_ready(comp_id); + return 0; + +fail: + hal_exit(comp_id); + return -1; +} + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} diff --git a/tests/tp/circle-curvature/expected b/tests/tp/circle-curvature/expected new file mode 100644 index 00000000000..c9812d8eae5 --- /dev/null +++ b/tests/tp/circle-curvature/expected @@ -0,0 +1 @@ +circle curvature: OK diff --git a/tests/tp/circle-curvature/skip b/tests/tp/circle-curvature/skip new file mode 100755 index 00000000000..221f2cab512 --- /dev/null +++ b/tests/tp/circle-curvature/skip @@ -0,0 +1,2 @@ +#!/bin/sh +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/tp/circle-curvature/test.sh b/tests/tp/circle-curvature/test.sh new file mode 100755 index 00000000000..79e6a90edb8 --- /dev/null +++ b/tests/tp/circle-curvature/test.sh @@ -0,0 +1,7 @@ +#!/bin/sh +set -e + +# shellcheck disable=SC2086 +${SUDO} halcompile --install curvaturecheck.c >/dev/null +halrun curvature.hal +echo 'circle curvature: OK' diff --git a/tests/tp/helix-motion/.gitignore b/tests/tp/helix-motion/.gitignore new file mode 100644 index 00000000000..4ee8742667b --- /dev/null +++ b/tests/tp/helix-motion/.gitignore @@ -0,0 +1,2 @@ +positions.csv +acceleration.log diff --git a/tests/tp/helix-motion/capture.hal b/tests/tp/helix-motion/capture.hal new file mode 100644 index 00000000000..ba48c4483cd --- /dev/null +++ b/tests/tp/helix-motion/capture.hal @@ -0,0 +1,7 @@ +# Capture commanded XYZ acceleration on every servo cycle. +loadrt sampler depth=8192 cfg=fff +setp sampler.0.enable 0 +addf sampler.0 servo-thread +net Xacc => sampler.0.pin.0 +net Yacc => sampler.0.pin.1 +net Zacc => sampler.0.pin.2 diff --git a/tests/tp/helix-motion/expected b/tests/tp/helix-motion/expected new file mode 100644 index 00000000000..d9390e32f4e --- /dev/null +++ b/tests/tp/helix-motion/expected @@ -0,0 +1 @@ +helix motion: OK diff --git a/tests/tp/helix-motion/test-ui.py b/tests/tp/helix-motion/test-ui.py new file mode 100755 index 00000000000..e62b22f3d26 --- /dev/null +++ b/tests/tp/helix-motion/test-ui.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +"""Headless motion smoke test""" +import csv +import hal +import math +from pathlib import Path +import subprocess +import time + +import linuxcnc + + +command = linuxcnc.command() +status = linuxcnc.stat() +errors = linuxcnc.error_channel() +Path("result.log").unlink(missing_ok=True) + + +def complete(): + result = command.wait_complete(10) + if result not in (linuxcnc.RCS_DONE,): + raise RuntimeError(f"command failed or timed out: {result}") + + +def check_errors(): + while True: + error = errors.poll() + if error is None: + return + if error[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + raise RuntimeError(error[1]) + + +def move(gcode, endpoint, samples, writer): + command.mdi(gcode) + deadline = time.monotonic() + 20 + while True: + result = command.wait_complete(0.01) + status.poll() + check_errors() + xyz = status.position[:3] + if not all(math.isfinite(value) for value in xyz): + raise RuntimeError(f"nonfinite position: {xyz}") + writer.writerow((samples[0], time.monotonic(), *xyz)) + samples[0] += 1 + if result == linuxcnc.RCS_DONE: + break + if result == linuxcnc.RCS_ERROR or time.monotonic() > deadline: + raise RuntimeError(f"move failed or timed out: {gcode}") + if max(abs(a - b) for a, b in zip(xyz, endpoint)) > 1e-4: + raise RuntimeError(f"wrong endpoint for {gcode}: {xyz}, expected {endpoint}") + + +def checked_helix(samples, writer): + with open("acceleration.log", "w") as output: + sampler = subprocess.Popen(["halsampler", "-t"], stdout=output) + try: + hal.set_p("sampler.0.enable", "1") + move("G3 X10 Y0 Z20 I-10 J0 F6000", (10, 0, 20), samples, writer) + finally: + hal.set_p("sampler.0.enable", "0") + time.sleep(0.05) # allow halsampler to drain the realtime FIFO + sampler.terminate() + sampler.wait(timeout=5) + + peak = 0.0 + previous = None + count = 0 + for line in Path("acceleration.log").read_text().splitlines(): + if line == "overrun": + raise RuntimeError("acceleration samples overran the HAL FIFO") + fields = line.split() + if len(fields) != 4: + raise RuntimeError(f"invalid acceleration sample: {line}") + index = int(fields[0]) + if previous is not None and index != previous + 1: + raise RuntimeError("missing servo-cycle acceleration samples") + previous = index + acceleration = math.sqrt(sum(float(value) ** 2 for value in fields[1:])) + if not math.isfinite(acceleration): + raise RuntimeError("nonfinite commanded acceleration") + peak = max(peak, acceleration) + count += 1 + if count < 100 or peak > 120.0: + raise RuntimeError(f"helix acceleration: {peak:.3f} mm/s^2 over {count} samples") + + +try: + command.state(linuxcnc.STATE_ESTOP_RESET) + complete() + command.state(linuxcnc.STATE_ON) + complete() + command.mode(linuxcnc.MODE_MDI) + complete() + with open("positions.csv", "w", newline="") as output: + writer = csv.writer(output) + writer.writerow(("sample", "monotonic_seconds", "x", "y", "z")) + samples = [0] + move("G21 G90 G17 G61 F1200 G1 X10 Y0 Z0", (10, 0, 0), samples, writer) + move("G3 X10 Y0 I-10 J0", (10, 0, 0), samples, writer) + checked_helix(samples, writer) + move("G2 X10 Y0 Z0 I-10 J0", (10, 0, 0), samples, writer) + # Equivalent one-turn helix as four commands. + for block, endpoint in ( + ("G3 X0 Y10 Z5 I-10 J0", (0, 10, 5)), + ("G3 X-10 Y0 Z10 I0 J-10", (-10, 0, 10)), + ("G3 X0 Y-10 Z15 I10 J0", (0, -10, 15)), + ("G3 X10 Y0 Z20 I0 J10", (10, 0, 20)), + ): + move(block, endpoint, samples, writer) + # A partial helix with a sweep below one radian. + x, y = 10 * math.cos(0.5), 10 * math.sin(0.5) + move(f"G3 X{x:.12f} Y{y:.12f} Z21 I-10 J0", (x, y, 21), samples, writer) + if samples[0] < 20: + raise RuntimeError("insufficient motion samples") + Path("result.log").write_text("helix motion: OK\n") +finally: + command.abort() + command.wait_complete(5) + command.state(linuxcnc.STATE_OFF) + command.wait_complete(5) diff --git a/tests/tp/helix-motion/test.ini b/tests/tp/helix-motion/test.ini new file mode 100644 index 00000000000..79377fc57ca --- /dev/null +++ b/tests/tp/helix-motion/test.ini @@ -0,0 +1,87 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = LIB:core_sim.hal +HALFILE = capture.hal + +[TRAJ] +NO_FORCE_HOMING = 1 +COORDINATES = X Y Z +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10 +MAX_LINEAR_VELOCITY = 100 +MAX_LINEAR_ACCELERATION = 100 + +[EMCIO] +TOOL_TABLE = tools.tbl + +[KINS] +KINEMATICS = trivkins +JOINTS = 3 + +[AXIS_X] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 + +[AXIS_Y] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 + +[AXIS_Z] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +FERROR = 1 +MIN_FERROR = 0.1 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +FERROR = 1 +MIN_FERROR = 0.1 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 100 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +FERROR = 1 +MIN_FERROR = 0.1 diff --git a/tests/tp/helix-motion/test.sh b/tests/tp/helix-motion/test.sh new file mode 100755 index 00000000000..8908fb94bea --- /dev/null +++ b/tests/tp/helix-motion/test.sh @@ -0,0 +1,9 @@ +#!/bin/bash +set -e +rm -f result.log positions.csv acceleration.log +if ! linuxcnc -r test.ini > linuxcnc.log 2>&1; then + cat linuxcnc.log >&2 + exit 1 +fi +grep -qx 'helix motion: OK' result.log +echo 'helix motion: OK' diff --git a/tests/tp/helix-motion/tools.tbl b/tests/tp/helix-motion/tools.tbl new file mode 100644 index 00000000000..a9992dabd24 --- /dev/null +++ b/tests/tp/helix-motion/tools.tbl @@ -0,0 +1 @@ +T1 P1 D1 Z0 ; simulation only