tp: make helix curvature independent of arc subdivision

src/emc/tp/blendmath.c

@@ -11,6 +11,7 @@
 * Last change:
 ********************************************************************/
 
+#include <rtapi.h>
 #include <rtapi_math.h>
 #include <posemath.h>
 
@@ -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);

tests/tp/circle-curvature/curvature.hal

@@ -0,0 +1,2 @@
+loadrt tpmod
+loadrt curvaturecheck

tests/tp/circle-curvature/curvaturecheck.c

@@ -0,0 +1,71 @@
+/* Check the production TP radius estimate after tpmod has been loaded. */
+#include <rtapi.h>
+#include <rtapi_app.h>
+#include <rtapi_math.h>
+#include <hal.h>
+#include <posemath.h>
+
+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);
+}

tests/tp/circle-curvature/expected

@@ -0,0 +1 @@
+circle curvature: OK

tests/tp/circle-curvature/skip

@@ -0,0 +1,2 @@
+#!/bin/sh
+[ -z "$SYSTEM_BUILD" ]

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'

tests/tp/helix-motion/.gitignore

@@ -0,0 +1,2 @@
+positions.csv
+acceleration.log

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

tests/tp/helix-motion/expected

@@ -0,0 +1 @@
+helix motion: OK

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)

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

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'

tests/tp/helix-motion/tools.tbl

@@ -0,0 +1 @@
+T1 P1 D1 Z0 ; simulation only