147 lines
4.9 KiB
Python
Executable File
147 lines
4.9 KiB
Python
Executable File
#!/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)
|
|
|