Import LinuxCNC source tree

tests/matrixkins/test-ui.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+
+import linuxcnc
+import hal
+
+import math
+import time
+import sys
+import subprocess
+import os
+import signal
+import glob
+import re
+
+def wait_for_linuxcnc_startup(status, timeout=10.0):
+
+    """Poll the Status buffer waiting for it to look initialized,
+    rather than just allocated (all-zero).  Returns on success, throws
+    RuntimeError on failure."""
+
+    start_time = time.time()
+    while time.time() - start_time < timeout:
+        status.poll()
+        if (status.angular_units == 0.0) \
+            or (status.axis_mask == 0) \
+            or (status.cycle_time == 0.0) \
+            or (status.exec_state != linuxcnc.EXEC_DONE) \
+            or (status.interp_state != linuxcnc.INTERP_IDLE) \
+            or (status.inpos == False) \
+            or (status.linear_units == 0.0) \
+            or (status.max_acceleration == 0.0) \
+            or (status.max_velocity == 0.0) \
+            or (status.program_units == 0.0) \
+            or (status.rapidrate == 0.0) \
+            or (status.state != linuxcnc.RCS_DONE) \
+            or (status.task_state != linuxcnc.STATE_ESTOP):
+            time.sleep(0.1)
+        else:
+            # looks good
+            return
+
+    # timeout, throw an exception
+    raise RuntimeError("Timeout")
+
+
+c = linuxcnc.command()
+s = linuxcnc.stat()
+e = linuxcnc.error_channel()
+h = hal.component("dummy")
+h.ready()
+
+# Wait for LinuxCNC to initialize itself so the Status buffer stabilizes.
+wait_for_linuxcnc_startup(s)
+
+# Because the kinematics is non-trivial, a homing is needed.
+# HOME_ABSOLUTE_ENCODER = 1 is used in the ini file.
+c.state(linuxcnc.STATE_ESTOP_RESET)
+c.state(linuxcnc.STATE_ON)
+c.home(0)
+c.home(1)
+c.home(2)
+c.wait_complete()
+
+def absdelta(a, b):
+    '''Maximum absolute difference between components of two coordinate points'''
+    return max(abs(a[i] - b[i]) for i in range(len(a)))
+
+def test_coordinate(target, expected):
+    '''Command LinuxCNC to go to 'target' position,
+    and check that actual joint positions given by inverse kinematics
+    match 'expected'. Also check that feedback position given by forward
+    kinematics matches the 'target' point.
+    '''
+    c.mode(linuxcnc.MODE_MDI)
+    c.mdi('G0 X%0.9f Y%0.9f Z%0.9f F1000' % target)
+    c.wait_complete()
+
+    # Read out resulting joint positions to verify kinematics is working correctly.
+    # The delay seems to be needed for trajectory to fully settle, otherwise there is
+    # about 1e-5 error between target and actual position.
+    time.sleep(0.05)
+    s.poll()
+    joints = s.joint_position[:3]
+    joints_feedback = s.joint_actual_position[:3]
+    coord_feedback = s.actual_position[:3]
+
+    print("Target location %s, joints %s, feedback %s" % (target, joints, coord_feedback))
+
+    # Accuracy limit is set to 1e-9 here.
+    # For practical purposes, a numerical accuracy of 1e-6 would be perfectly acceptable.
+    # Current implementation using doubles achieves about 1e-14 precision.
+    maxdelta = 1e-9
+
+    if absdelta(joints, expected) > maxdelta:
+        raise RuntimeError("Error in inverse kinematics: coordinates %s should give joint positions %s, got %s" %
+                           (target, expected, joints))
+
+    if absdelta(coord_feedback, target) > maxdelta:
+        raise RuntimeError("Error in forward kinematics: coordinates %s, feedback joints %s and coord %s" %
+                           (target, joints_feedback, coord_feedback))
+
+# Test the default identity transform
+test_coordinate((0, 0, 0), (0, 0, 0))
+test_coordinate((10, 20, 30), (10, 20, 30))
+test_coordinate((11, 12, 13), (11, 12, 13))
+
+# Test with axis scaling
+hal.set_p('matrixkins.C_xx', "0.99")
+hal.set_p('matrixkins.C_yy', "1.01")
+hal.set_p('matrixkins.C_zz', "1.05")
+test_coordinate((0, 0, 0), (0, 0, 0))
+test_coordinate((10, 20, 30), (10 * 0.99, 20 * 1.01, 30 * 1.05))
+test_coordinate((11, 12, 13), (11 * 0.99, 12 * 1.01, 13 * 1.05))
+hal.set_p('matrixkins.C_xx', "1.00")
+hal.set_p('matrixkins.C_yy', "1.00")
+hal.set_p('matrixkins.C_zz', "1.00")
+
+# Test with X axis skews
+hal.set_p('matrixkins.C_xy', "0.01")
+hal.set_p('matrixkins.C_xz', "0.03")
+test_coordinate((0, 0, 0), (0, 0, 0))
+test_coordinate((10, 20, 30), (10 + 20 * 0.01 + 30 * 0.03, 20, 30))
+test_coordinate((11, 12, 13), (11 + 12 * 0.01 + 13 * 0.03, 12, 13))
+hal.set_p('matrixkins.C_xy', "0.0")
+hal.set_p('matrixkins.C_xz', "0.0")
+
+# Test with Y axis skews
+hal.set_p('matrixkins.C_yx', "0.01")
+hal.set_p('matrixkins.C_yz', "0.03")
+test_coordinate((0, 0, 0), (0, 0, 0))
+test_coordinate((10, 20, 30), (10, 20 + 10 * 0.01 + 30 * 0.03, 30))
+test_coordinate((11, 12, 13), (11, 12 + 11 * 0.01 + 13 * 0.03, 13))
+hal.set_p('matrixkins.C_yx', "0.0")
+hal.set_p('matrixkins.C_yz', "0.0")
+
+# Test with Z axis skews
+hal.set_p('matrixkins.C_zx', "0.01")
+hal.set_p('matrixkins.C_zy', "0.03")
+test_coordinate((0, 0, 0), (0, 0, 0))
+test_coordinate((10, 20, 30), (10, 20, 30 + 0.01 * 10 + 0.03 * 20))
+test_coordinate((11, 12, 13), (11, 12, 13 + 0.01 * 11 + 0.03 * 12))
+hal.set_p('matrixkins.C_zx', "0.0")
+hal.set_p('matrixkins.C_zy', "0.0")
+
+sys.exit(0)
+