From e7b7cda8363ccf94794c73dd602c84f787a18326 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 18:53:54 +1000 Subject: [PATCH 01/44] tests: what the limits have to mean on a kinematics that is not the identity Two tests that fail today and define item 6 of the multiaxis plan: travel is the joint limits, on every move and every kinematics type, and the [AXIS_L] box is the machine frame envelope, the carriage, not the tool tip. tests/kins-limits/swing: 5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, then G0 C180 with the tool tip held. The carriage draws a half circle of radius 400 to keep the tip still, and the planner runs C at C's own limits, so the slides are asked for 1396 mm/s^2 against their 800 and would be asked for 419 mm/s against their 200. The joints follow their commands through limit3 at 20 percent over each joint's own limits, the way a drive or a step generator follows only as fast as it can, so the command a joint cannot follow is a real following error: joint 1 trips it 84 cycles in. The test asserts no following error, the endpoint reached, and every joint's commanded velocity and acceleration within its own INI limit at every servo cycle, read from a sampler. The slowest joint has to set the pace; how fast the tip goes is not asserted. tests/kins-limits/box: joints reaching 1000, the box at 500, a 250 pivot and a 50 tool with the head at B90 so that the tip sits 300 from the carriage. Under the tool centre point kinematics a move with the tip past the box and the carriage inside it must be accepted, one with the tip inside and the carriage past the box refused, G53.5 to a carriage past the box refused and to one inside it accepted whatever the tip does; a carriage past the joint limit is refused whatever the box says. Today the box is checked on the world of the kinematics in force, the tip, and never on a joint-target move, so three of the five go the wrong way. --- tests/kins-limits/box/README | 12 +++ tests/kins-limits/box/checkresult | 3 + tests/kins-limits/box/sim.hal | 16 +++ tests/kins-limits/box/test-ui.py | 117 +++++++++++++++++++++ tests/kins-limits/box/test.ini | 134 ++++++++++++++++++++++++ tests/kins-limits/box/test.sh | 4 + tests/kins-limits/box/tool.tbl | 1 + tests/kins-limits/swing/README | 15 +++ tests/kins-limits/swing/checkresult | 3 + tests/kins-limits/swing/sim.hal | 58 +++++++++++ tests/kins-limits/swing/test-ui.py | 153 ++++++++++++++++++++++++++++ tests/kins-limits/swing/test.ini | 146 ++++++++++++++++++++++++++ tests/kins-limits/swing/test.sh | 4 + tests/kins-limits/swing/tool.tbl | 1 + 14 files changed, 667 insertions(+) create mode 100644 tests/kins-limits/box/README create mode 100755 tests/kins-limits/box/checkresult create mode 100644 tests/kins-limits/box/sim.hal create mode 100755 tests/kins-limits/box/test-ui.py create mode 100644 tests/kins-limits/box/test.ini create mode 100755 tests/kins-limits/box/test.sh create mode 100644 tests/kins-limits/box/tool.tbl create mode 100644 tests/kins-limits/swing/README create mode 100755 tests/kins-limits/swing/checkresult create mode 100644 tests/kins-limits/swing/sim.hal create mode 100755 tests/kins-limits/swing/test-ui.py create mode 100644 tests/kins-limits/swing/test.ini create mode 100755 tests/kins-limits/swing/test.sh create mode 100644 tests/kins-limits/swing/tool.tbl diff --git a/tests/kins-limits/box/README b/tests/kins-limits/box/README new file mode 100644 index 00000000000..e00646f7457 --- /dev/null +++ b/tests/kins-limits/box/README @@ -0,0 +1,12 @@ +The [AXIS_L] box is the machine frame envelope: the carriage, not the +tool tip. + +5axiskins with the joints reaching 1000 and the box at 500, a 250 pivot and +a 50 tool, the head laid over to B90 so that the tip sits 300 from the +carriage in the XY plane. Under the tool centre point kinematics: a move +whose tip is past the box with the carriage inside it is accepted; a move +whose tip is inside with the carriage past the box is refused; G53.5 to a +carriage position past the box is refused, and to one inside it with the +tip past the box is accepted. A carriage past the joint limit is refused +whatever the box says. Nothing is asserted about the identity kinematics, +where the box and the joints describe the same thing. diff --git a/tests/kins-limits/box/checkresult b/tests/kins-limits/box/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/box/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/box/sim.hal b/tests/kins-limits/box/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/box/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/box/test-ui.py b/tests/kins-limits/box/test-ui.py new file mode 100755 index 00000000000..7b80555ecf9 --- /dev/null +++ b/tests/kins-limits/box/test-ui.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +# The [AXIS_L] box is the machine frame envelope, the carriage and not the +# tool tip: see README. +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def accepted(cmd, carriage_x): + drain() + now = mdi(cmd) + said = [m for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[0] - carriage_x) > 1e-3: + error("%s left the carriage at X %.3f, not %.3f" % (cmd, now[0], carriage_x)) + else: + print("accepted as expected: %-24s carriage X %.3f" % (cmd, now[0])) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def refused(cmd, needle): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + settled() + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + settled() + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics with the tool on, the head laid over by +# a joint move: the tip is now 300 from the carriage along +X at C0 +mdi("G12.1 P0") +mdi("G43 H1") +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") +j = joints() +s.poll() +print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) +if abs(s.position[0] - j[0] - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) + +# the tip past the box, the carriage inside it: accepted +accepted("G0 X600", 300) +# the tip inside the box, the carriage past it: refused +refused("G0 X-300", "limit") +# the carriage past the joint limit: refused whatever the box says +refused("G0 X-800", "limit") +# G53.5 names the carriage: past the box refused, inside it accepted even +# with the tip past the box +refused("G53.5 G0 X-600", "limit") +accepted("G53.5 G0 X400", 400) + +mdi("G53.7 G0 J0=0 J3=0") +mdi("G49") +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/box/test.ini b/tests/kins-limits/box/test.ini new file mode 100644 index 00000000000..f3300215554 --- /dev/null +++ b/tests/kins-limits/box/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 2000 +MAX_LINEAR_VELOCITY = 3460 +MAX_LINEAR_ACCELERATION = 80000 +DEFAULT_LINEAR_ACCELERATION = 80000 +MAX_ANGULAR_VELOCITY = 3600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/test.sh b/tests/kins-limits/box/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-limits/box/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-limits/box/tool.tbl b/tests/kins-limits/box/tool.tbl new file mode 100644 index 00000000000..b558277601f --- /dev/null +++ b/tests/kins-limits/box/tool.tbl @@ -0,0 +1 @@ +T1 P1 D6.0 Z50 ; diff --git a/tests/kins-limits/swing/README b/tests/kins-limits/swing/README new file mode 100644 index 00000000000..a9d17309b6d --- /dev/null +++ b/tests/kins-limits/swing/README @@ -0,0 +1,15 @@ +A turn of the head under the tool centre point must not run a slide +past its own limits. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +then a plain G0 C180 with the tool tip held: the tip does not move, the +carriage swings through a half circle of radius 400 to keep it still. The +joints follow their commands through limit3, 20 percent over each joint's +own velocity and acceleration limits, the way a drive or a step generator +follows only as fast as it can; a command inside the limits is followed +exactly, one well over them falls behind and motion trips the following +error. The test asserts that the move ends on its endpoint with no +following error, and that no joint's commanded velocity or acceleration +went over the joint's INI limit, sampled every servo cycle. The slowest +joint has to set the pace of the move; how slow the tip goes is not +asserted. diff --git a/tests/kins-limits/swing/checkresult b/tests/kins-limits/swing/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/swing/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/swing/sim.hal b/tests/kins-limits/swing/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/swing/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py new file mode 100755 index 00000000000..fea6005fe38 --- /dev/null +++ b/tests/kins-limits/swing/test-ui.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +# A turn of the head under the tool centre point must not run a slide past +# its own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the half circle it is about to draw +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() +# the log is block buffered: wait until it has caught up with the machine +# at rest before marking where the swing starts in it +deadline = time.time() + 10 +while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], start)) < 2e-6: + break + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + break + time.sleep(0.02) +n0 = len(samples) + +# the swing: the tip holds, C turns half a revolution, the carriage follows +# a half circle of radius 400 to keep the tip where it is +c.mdi("G0 C180") +c.wait_complete(60) +end = settled() +said = drain() +time.sleep(0.5) +samples = log_samples()[n0:] + +for m in said: + print("channel:", m) +faults = [m for m in said if "following error" in m[1]] +if faults: + error("the swing tripped a following error: %s" % faults[0][1].strip()) +s.poll() +if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after the swing (task state %d)" % s.task_state) +if abs(end[4] - 180) > 1e-3: + error("C ended at %.4f, not 180" % end[4]) +s.poll() +after = list(s.position[:3]) +if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s" % (tip, after)) + +# the commanded velocity and acceleration of every joint against its own +# INI limit, every servo cycle; a joint over its limit is what the drive +# could not follow. A fault freezes the command in one cycle, which is +# not an acceleration the planner asked for: the samples stop at the last +# moving one +last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) +samples = samples[:last + 1] +print("%d samples through the swing" % len(samples)) +for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/swing/test.ini b/tests/kins-limits/swing/test.ini new file mode 100644 index 00000000000..13379eb18a0 --- /dev/null +++ b/tests/kins-limits/swing/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/swing/test.sh b/tests/kins-limits/swing/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/swing/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/swing/tool.tbl b/tests/kins-limits/swing/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/swing/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From ef8e4594cae5cc941afa1908ac24b0ba8bed1288 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 19:07:51 +1000 Subject: [PATCH 02/44] motion: read the [AXIS_L] box on the machine frame, whatever kinematics is in force The position limits in [AXIS_L] were checked on the world of the kinematics in force, so under a tool centre point kinematics they boxed the tool tip, which moves with the tool length and the head, and a joint-target move never saw them at all. A box on the tip refuses moves the machine can make and lets through moves it cannot, which is why switchkins.adoc told configurations to setp ini.L.min_limit at every switch and the puma sims shipped M128 to M130 to do it. The box is now the machine frame envelope: the carriage, or a robot's flange, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. kinematicsMachineFrame() runs the forward of the type flagged KINSTYPE_MACHINE on a set of joints without switching to it, and motion's inRange() takes every endpoint through it after the inverse, Cartesian and joint interpolated moves alike, so G0 and G53.5 to the same carriage position get the same answer. A module that declares no machine frame type keeps the box on the world of the type in force, as before; motion links the new entry point weakly so an older module still loads. The travel itself is the joint limits, checked on every move as they were. The puma M-codes keep swapping the velocity and acceleration limits per type and leave the box alone, and M128 restores max_velocity rather than a pin that never existed. tests/kins-limits/box goes green: under the tool centre point a move with the tip past the box and the carriage inside is accepted, one with the tip inside and the carriage past the box is refused, and G53.5 gets the box too. --- configs/sim/axis/vismach/puma/mcodes/M128 | 7 +- configs/sim/axis/vismach/puma/mcodes/M129 | 9 +- configs/sim/axis/vismach/puma/mcodes/M130 | 9 +- configs/sim/axis/vismach/puma/puma.ini | 2 +- configs/sim/axis/vismach/puma/puma560.ini | 2 +- configs/sim/axis/vismach/puma/puma560_uvw.ini | 2 +- configs/sim/axis/vismach/puma/puma_cube.ini | 2 +- docs/src/config/ini-config.adoc | 5 + docs/src/motion/switchkins.adoc | 37 +++++--- include/kinematics.h | 8 ++ src/emc/kinematics/kins_single.c | 9 ++ src/emc/kinematics/switchkins.c | 15 +++ src/emc/motion/command.c | 95 +++++++++++++------ 13 files changed, 143 insertions(+), 59 deletions(-) diff --git a/configs/sim/axis/vismach/puma/mcodes/M128 b/configs/sim/axis/vismach/puma/mcodes/M128 index 26375100367..145f76e0015 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M128 +++ b/configs/sim/axis/vismach/puma/mcodes/M128 @@ -9,12 +9,11 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# RESTORE INI axis limits +# RESTORE INI axis velocity and acceleration limits (the position +# limits are never changed: a box on the machine frame, the flange) foreach l $coords { set L [string toupper $l] - catch {hal setp ini.$l.min_limit [set ::AXIS_[set L](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::AXIS_[set L](MAX_LIMIT)]} - catch {hal setp ini.$l.min_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} + catch {hal setp ini.$l.max_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::AXIS_[set L](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M129 b/configs/sim/axis/vismach/puma/mcodes/M129 index f9f9a2d6b84..7fe3c4e7e19 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M129 +++ b/configs/sim/axis/vismach/puma/mcodes/M129 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits to equiv joint limits +# switch kinematics and change axis velocity limits to equiv joint limits set kinstype 1 ;# 1: IDENTITY kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M130 b/configs/sim/axis/vismach/puma/mcodes/M130 index 8051f2a2237..dcdede0fa04 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M130 +++ b/configs/sim/axis/vismach/puma/mcodes/M130 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits +# switch kinematics and change axis velocity limits set kinstype 2 ;# 2: TYPE 2 kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index 99c0fa33112..f40fb5849c9 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -33,7 +33,7 @@ RS274NGC_STARTUP_CODE = G21 G10L2P0 x450 y100 z-495 a-180 (debug, ini: startup o MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 046f5d118d5..3d181caff47 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -36,7 +36,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index a6b9f8544e9..b4d84d0e71c 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -35,7 +35,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 8397ce3ea7a..2742f6d1059 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -113,7 +113,7 @@ POSTGUI_HALFILE = puma_postgui.hal MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 37d63eaa701..941ab9fcfcc 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1051,6 +1051,11 @@ The __ specifies one of: X Y Z A B C U V W When this limit is exceeded, the controller aborts axis motion. The axis must be homed before MAX_LIMIT is in force. For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. ++ +`MIN_LIMIT` and `MAX_LIMIT` together are a box in the machine frame: the carriage, or the flange of a robot, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. +On a kinematics module that declares its machine frame type (see the <> chapter) every move is checked against the box where its joints put the machine frame, whatever kinematics type is in force: a tool centre point move whose tip lies past the box with the carriage inside it is accepted, and a 'G53.5' to a carriage position past the box is refused. +The travel itself is the `[JOINT_N]` limits; the box is a restriction the configuration adds inside it. +A module that declares no machine frame type keeps the box on the world of the kinematics in force. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. It is ignored on a `LINEAR` axis. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 137d8f6f5c1..69be89c0bc9 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -300,9 +300,21 @@ MAX_VELOCITY = MIN_ACCELERATION = ---- -The INI file limits specified apply to the type 0 default -kinematics type that is activated at startup. These limits may -*not* be applicable when switching to alternative kinematics. +The position limits, `MIN_LIMIT` and `MAX_LIMIT`, are a box in the +machine frame, the world of the kinstype the module declares +KINSTYPE_MACHINE (its identity kinstype where it declares none): +the carriage in machine coordinates, or a robot's flange in its base +frame. Every move is checked against that box where its joints put +the machine frame, whichever kinstype is in force, so the box means +the same thing after a switch as before it and needs no resetting. +The travel is the `[JOINT_N]` limits, checked on every move as well; +the box is a restriction inside it. A module that declares no +kinstype flags keeps the box on the world of the kinstype in force, +as before. + +The velocity and acceleration limits specified apply to the type 0 +default kinematics type that is activated at startup. These limits +may *not* be applicable when switching to alternative kinematics. However, since an interpreter-motion synchronization is required when switching kinematics, INI-HAL pins can be used to setup limits for a pending kinematics type. @@ -340,7 +352,7 @@ kinematics (trivkins). See the kins man page for more information ($ man kins). A user-provided M-code can alter any or all of the axis coordinate -limits prior to changing the motion.switchkins-type pin and +velocity and acceleration limits prior to changing the motion.switchkins-type pin and synchronizing the interpreter and motionparts of LinuxCNC. As an example, a bash script invoking halcmd can be 'hardcoded' to set any number of HAL pins: @@ -366,24 +378,25 @@ with a complex (non-identity) kinematics (type0) after homing. The system is configured so that it can be switched to identity kinematics (type1) in order to manipulate individual joints using the conventional letters from the set 'XYZABCUVW'. The INI file -settings ([AXIS_L]) are *not* applicable when operating with -identity (type1) kinematics. To address this use case, the user -M-code scripts can be designed as follows: +velocity and acceleration settings ([AXIS_L]) are *not* applicable +when operating with identity (type1) kinematics. To address this +use case, the user M-code scripts can be designed as follows: *M129* (Switch to identity type1) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the 'identity-referenced' joint number INI file - setting ([JOINT_N]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the 'identity-referenced' joint + number INI file setting ([JOINT_N]) . HAL: `setp motion.switchkins-type 1` . MDI: execute a syncing G-code (M66E0L0) *M128* (restore robot default kinematics type 0) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the appropriate INI file setting ([AXIS_L]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the appropriate INI file setting + ([AXIS_L]) . HAL: `setp motion.switchkins-type 0` . MDI: execute a syncing G-code (M66E0L0) diff --git a/include/kinematics.h b/include/kinematics.h index eb9f0c15241..34160ce3b11 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -143,6 +143,14 @@ extern KINEMATICS_TYPE kinematicsType(void); ** kinematics); G12.1 refuses a type that returns -1 */ extern int kinematicsTypeFlags(int ktype); +/* The joints placed in the machine frame: the forward of the type +** flagged KINSTYPE_MACHINE, whatever type is in force, so that a limit +** on the frame can be read for a move under any type. pos carries the +** caller's estimate in for an iterative forward. Returns 0, -1 where +** no type declares the machine frame (a consumer then falls back on the +** world of the type in force), or -2 where the forward fails. */ +extern int kinematicsMachineFrame(const double *joint, struct EmcPose *pos); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 839313ef965..6ff60667663 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -150,6 +150,14 @@ int kinematicsTypeFlags(int ktype) return -1; } +// and so no machine frame type either +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + (void)joint; + (void)pos; + return -1; +} + // The module's description, for a copy of it loaded outside RT. A module // with one type does not depend on its parameters for its shape, so this // is the table as declared. @@ -171,5 +179,6 @@ EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(kinsSingleInit); EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index ac16fc36c4e..e2f92709ebd 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -475,6 +475,20 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + int k; + + for (k = 0; k < kins_count; k++) { + int f = kinematicsTypeFlags(k); + if (f < 0 || !(f & KINSTYPE_MACHINE)) { continue; } + return call_forward(k, joint, pos, &fflags, &iflags) == 0 ? 0 : -2; + } + return -1; +} // kinematicsMachineFrame() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -583,6 +597,7 @@ EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b669000e664..33d9a149416 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -74,6 +74,8 @@ // module written before the call exports no such symbol, and the weak // reference leaves it NULL rather than refusing to load motion #pragma weak kinematicsSetTool +// old modules export no kinematicsMachineFrame; keep it optional +#pragma weak kinematicsMachineFrame #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -270,47 +272,66 @@ void apply_spindle_limits(spindle_status_t *s){ } -/* inRange() returns non-zero if the position lies within the joint - limits, or 0 if not. It also reports an error for each joint limit - violation. It's possible to get more than one violation per move. */ -STATIC int inRange(EmcPose pos, int id, char *move_type) +/* The [AXIS_L] box is the machine frame envelope: the carriage, or the + flange, in machine coordinates, the frame that does not move when the + head tilts or a tool is loaded. Where the module names its machine + frame type, the endpoint's joints go through that type's forward and + the box is read there, whatever type is in force; a module that names + none keeps the box on the world of the type in force, as before. + Returns non-zero when the endpoint is inside the box, and reports each + letter outside it. */ +STATIC int box_ok(const double *joint_pos, const EmcPose *pos, int id, const char *move_type) { - double joint_pos[EMCMOT_MAX_JOINTS]; - int joint_num, axis_num; - emcmot_joint_t *joint; - int in_range = 1; + EmcPose frame = *pos; /* the estimate in, and the fallback */ int failing_axes[EMCMOT_MAX_AXIS]; double targets[EMCMOT_MAX_AXIS]; const char axis_letters[] = "XYZABCUVW"; + int axis_num, in_box = 1; if (EMCMOT_MAX_AXIS != 9) { rtapi_print_msg(RTAPI_MSG_ERR, "BUG: %s(): invalid number of axes defined", __func__); - } else { - targets[0] = pos.tran.x; - targets[1] = pos.tran.y; - targets[2] = pos.tran.z; - targets[3] = pos.a; - targets[4] = pos.b; - targets[5] = pos.c; - targets[6] = pos.u; - targets[7] = pos.v; - targets[8] = pos.w; - axis_check_constraints(targets, failing_axes); - for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { - if (failing_axes[axis_num] == -1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("negative")); - in_range = 0; - } - if (failing_axes[axis_num] == 1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("positive")); - in_range = 0; - } + return 1; + } + if (kinematicsMachineFrame && kinematicsMachineFrame(joint_pos, &frame) == -2) { + reportError(_("%s move on line %d cannot be placed in the machine frame"), + move_type, id); + return 0; + } + targets[0] = frame.tran.x; + targets[1] = frame.tran.y; + targets[2] = frame.tran.z; + targets[3] = frame.a; + targets[4] = frame.b; + targets[5] = frame.c; + targets[6] = frame.u; + targets[7] = frame.v; + targets[8] = frame.w; + axis_check_constraints(targets, failing_axes); + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { + if (failing_axes[axis_num] == -1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("negative")); + in_box = 0; + } + if (failing_axes[axis_num] == 1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("positive")); + in_box = 0; } } + return in_box; +} - /* Now, check that the endpoint puts the joints within their limits too */ +/* inRange() returns non-zero if the position lies within the joint + limits and the [AXIS_L] box, or 0 if not. It also reports an error for + each limit violation. It's possible to get more than one violation per + move. */ +STATIC int inRange(EmcPose pos, int id, char *move_type) +{ + double joint_pos[EMCMOT_MAX_JOINTS]; + int joint_num; + emcmot_joint_t *joint; + int in_range = 1; /* start the inverse from where the queue leaves the joints */ queue_end_joints(joint_pos); @@ -324,6 +345,10 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) return 0; } + /* the box, read where the joints put the machine frame */ + if (!box_ok(joint_pos, &pos, id, move_type)) { in_range = 0; } + + /* and the joints within their limits */ for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { /* point to joint data */ joint = &joints[joint_num]; @@ -1239,6 +1264,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } + /* the joints named still have to keep the machine frame in the box */ + if (!box_ok(target, &end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } } else { if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { reportError(_("invalid params in joint interpolated move")); From cdb4730c8eae13c38c537762ff4c71d08f8ce31e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 21:08:24 +1000 Subject: [PATCH 03/44] canon: cap every segment at what its joints can follow A segment is planned in world coordinates against the [AXIS_L] velocity and acceleration limits, which say nothing about the joints once the kinematics is not the identity: a turn of a tilted head under the tool centre point swings the carriage through a circle the head's own limit never mentions, and the carriage trips its following error. Every segment is now also sampled through the kinematics module's Jacobian in canon, ahead of motion, on the kinematics type and the tool offset the program is under, and its velocity and acceleration are lowered to what the slowest joint can give within its own [JOINT_n] limits. motion_planning/segment_cap.cc does the reading: at each sample the rate of every joint per unit of path parameter along the local tangent; between samples how that rate changes, charged against the acceleration budget at the capped velocity (what a carriage on a circle feels as centripetal, at most half the budget, the velocity lowered where it would take more); from three samples in a row how much the rate can bulge between two of them, so a peak falling between samples is covered. Lines are sampled from canon.endPoint to the target, traverse, feed, probe, rigid tap and the chained segments of the naive cam alike, through getStraightVelocity() and getStraightAcceleration(); arcs are sampled on the posemath circle ARC_FEED builds, in ARC_FEED itself. Canon owns its non-RT context: the module and joint count from [KINS] in the INI, the type tracked through SELECT_KINS_TYPE and dropped to the machine's whenever the interpreter synchs, the tool from the offset canon applies, the joints at the start of a segment kept from the last one while they still explain the point, else the machine's, else the inverse iterated to a fixed point. The identity, as a module or as the type in force, loads nothing. A module that runs only in realtime caps nothing, as before. Where the module cannot answer at a sample, or a joint would have to move at any speed at all (a pole), an operator message says so and the segment goes through capped rather than dropped, since a dropped segment sends the next one along another path. tests/kins-limits/swing goes green, and gains a second phase: the tip draws a full circle at a feed it could keep on its own while C turns another revolution, so the carriage rides the small circle and the big swing together. kinslimits prints the same cap for a move after its own per-sample listing. emcJointGetMaxVelocity() and emcJointGetMaxAcceleration() read back what iniJoint() set. The switchkins chapter's limit section says what the velocity and acceleration limits now mean and when a switch still wants them reset; the [AXIS_L] entry in the INI chapter points to it. --- docs/src/config/ini-config.adoc | 4 + docs/src/motion/kinematics-conventions.adoc | 4 +- docs/src/motion/switchkins.adoc | 28 +- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/kinslimits.cc | 31 +- src/emc/motion_planning/segment_cap.cc | 175 ++++++++++ src/emc/motion_planning/segment_cap.hh | 75 +++++ src/emc/nml_intf/emc.hh | 2 + src/emc/task/Submakefile | 2 +- src/emc/task/emccanon.cc | 352 +++++++++++++++++++- src/emc/task/taskintf.cc | 18 + tests/kins-limits/swing/test-ui.py | 117 ++++--- 12 files changed, 748 insertions(+), 64 deletions(-) create mode 100644 src/emc/motion_planning/segment_cap.cc create mode 100644 src/emc/motion_planning/segment_cap.hh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 941ab9fcfcc..9dda969b95d 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1040,6 +1040,10 @@ The __ specifies one of: X Y Z A B C U V W The preview, the DROs, touch off and jogging of the GUIs follow the type too. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. ++ +`MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, so on a kinematics that is not the identity a move runs at the pace of its slowest joint whatever these say. +See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. When set to 0 (default), no per-axis jerk limiting is applied. diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index ef36858dfe4..8f99f221407 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -611,7 +611,9 @@ names. Its tool is the caller's where the caller gives one through `kinematicsUserSetTool()`, since a planner knows what a segment runs under better than the machine does; otherwise it is motion's, from motion's own offset pins, and the loader says once when the module's tool pin disagrees with -them. `kinslimits` is built on it. +them. `kinslimits` is built on it, and so is the cap canon puts on every +segment's velocity and acceleration from the joint limits through the Jacobian +(see the Switchable Kinematics chapter, INI file limit settings). A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 69be89c0bc9..976896aa1d1 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -312,12 +312,28 @@ the box is a restriction inside it. A module that declares no kinstype flags keeps the box on the world of the kinstype in force, as before. -The velocity and acceleration limits specified apply to the type 0 -default kinematics type that is activated at startup. These limits -may *not* be applicable when switching to alternative kinematics. -However, since an interpreter-motion synchronization is required -when switching kinematics, INI-HAL pins can be used to setup -limits for a pending kinematics type. +The velocity and acceleration limits, `MAX_VELOCITY` and +`MAX_ACCELERATION`, bound the coordinate letter in the world of the +kinstype in force. What the joints can follow is bound separately: +every segment is sampled through the module's Jacobian ahead of +motion, on the kinstype and the tool offset the program is under, and +its velocity and acceleration are lowered to what the slowest joint +can give within its own `[JOINT_N]` limits, the share a carriage on a +circle feels as centripetal included. A turn of a tilted head under +the tool centre point, which swings the carriage through a circle the +head's own limit never mentions, runs at the carriage's pace. A +module that can only be evaluated in realtime (one without the +parameter block form, see <>) +caps nothing; on it the `[AXIS_L]` limits are all there is. + +So a switch does not need the velocity and acceleration limits reset +to keep the joints within their limits. It may still want them reset +where a letter names a joint of another kind after the switch: a +robot's identity kinstype puts joint 0, a rotary, on X, and the +`[AXIS_X]` limit in mm/s says nothing useful about it. Since an +interpreter-motion synchronization is required when switching +kinematics, INI-HAL pins can be used to set up limits for a pending +kinematics type. [NOTE] INI-HAL pins are typically not recognized during a G-code program diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 54a9b384935..5ec4c036a9f 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -6,6 +6,7 @@ INCLUDES += emc/kinematics_userspace LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ jacobian.cc \ joint_limits.cc \ + segment_cap.cc \ ) # kins_util.c is the shared kinematics code the modules link; the loader @@ -45,7 +46,8 @@ USERSRCS += $(KINSLIMITS_SRCS) TARGETS += ../bin/kinslimits -MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh \ + emc/motion_planning/segment_cap.hh $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh cp $^ $@ HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc index 1b750be6776..04ca010defc 100644 --- a/src/emc/motion_planning/kinslimits.cc +++ b/src/emc/motion_planning/kinslimits.cc @@ -6,8 +6,8 @@ * * The tool attaches to a running HAL instance, loads the kinematics * module through the non-RT interface, samples the move, and reports - * the most restrictive cap found along it. The sampling loop here is - * the same one the trajectory planner uses to cap a segment. + * the most restrictive cap found along it, then the cap canon puts on + * the segment through segmentCap(), which is the one the planner sees. * * Example (in a terminal with a running config, or under halrun): * @@ -36,6 +36,7 @@ #include #include "jacobian.hh" #include "joint_limits.hh" +#include "segment_cap.hh" using namespace motion_planning; @@ -263,6 +264,32 @@ int main(int argc, char **argv) min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); printf("worst cond : %.3f\n", max_cond); + /* the cap canon reads for the same move, along the same tangent, with + the change of the joint rates along the move charged as well */ + { + struct { EmcPose start, end; } line = { start, end }; + SegmentCapLimits lim; + SegmentCap cap; + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + lim.joints = num_joints; + for (int j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + lim.vel[j] = j < num_joints ? vel_v[j] : SEGMENT_CAP_NONE; + lim.acc[j] = j < num_joints ? acc_v[j] : SEGMENT_CAP_NONE; + } + auto pose_at = [](double f, EmcPose *p, void *arg) { + const EmcPose *se = (const EmcPose *)arg; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(p, ax, pose_axis(se[0], ax) + f * (pose_axis(se[1], ax) - pose_axis(se[0], ax))); + } + }; + if (segmentCap(ctx, &lim, target, samples, pose_at, &line, joints, &cap) == 0) { + printf("canon's cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), %d of %d samples unanswered\n", + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, cap.samples); + } else { + printf("canon's cap : none, no sample could be evaluated\n"); + } + } + kinematicsUserFree(ctx); hal_exit(comp_id); return 0; diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc new file mode 100644 index 00000000000..5c14539226c --- /dev/null +++ b/src/emc/motion_planning/segment_cap.cc @@ -0,0 +1,175 @@ +/******************************************************************** + * Description: segment_cap.cc + * The velocity and acceleration cap a segment gets from the joint + * limits through the kinematics module's Jacobian. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ + +#include "segment_cap.hh" +#include + +namespace motion_planning { + +static double pose_axis(const EmcPose &p, int ax) +{ + switch (ax) { + case AXIS_X: return p.tran.x; + case AXIS_Y: return p.tran.y; + case AXIS_Z: return p.tran.z; + case AXIS_A: return p.a; + case AXIS_B: return p.b; + case AXIS_C: return p.c; + case AXIS_U: return p.u; + case AXIS_V: return p.v; + default: return p.w; + } +} + +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out) +{ + const double tiny = 1e-12; + /* the rate of every joint per unit of path parameter at each sample, + the most that rate changes per unit of parameter on either side of + it, and the most it can bulge past the samples on either side */ + static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double h[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double bulge[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double s[SEGMENT_CAP_MAX_SAMPLES]; + static bool ok[SEGMENT_CAP_MAX_SAMPLES]; + int K = samples < 2 ? 2 : samples; + int njoints; + int k, j, a, prev, answered = 0; + + out->vel = SEGMENT_CAP_NONE; + out->acc = SEGMENT_CAP_NONE; + out->vel_joint = -1; + out->acc_joint = -1; + out->vel_at = 0.0; + out->samples = 0; + out->unanswered = 0; + if (!ctx || !lim || !pose_at || !joints || length <= 0.0) { return -1; } + if (K > SEGMENT_CAP_MAX_SAMPLES) { K = SEGMENT_CAP_MAX_SAMPLES; } + njoints = lim->joints; + if (njoints > KINEMATICS_USER_MAX_JOINTS) { njoints = KINEMATICS_USER_MAX_JOINTS; } + out->samples = K; + + for (k = 0; k < K; k++) { + double f = (double)k / (double)(K - 1); + /* the tangent by a central difference of the geometry, exact for + a line and a small fraction of a sample step for an arc */ + double df = 1.0 / (8.0 * (K - 1)); + double fp = f + df > 1.0 ? 1.0 : f + df; + double fm = f - df < 0.0 ? 0.0 : f - df; + EmcPose p, pp, pm; + double t[AXIS_COUNT]; + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; + + s[k] = f * length; + ok[k] = false; + pose_at(f, &p, arg); + pose_at(fp, &pp, arg); + pose_at(fm, &pm, arg); + for (a = 0; a < AXIS_COUNT; a++) { + t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); + } + /* the joints at the sample, seeded from the sample before so the + whole segment stays on one solution branch, then the Jacobian + on that branch */ + if (kinematicsUserInverse(ctx, &p, joints) != 0 + || kinematicsUserJacobian(ctx, &p, J) != 0) { + out->unanswered++; + continue; + } + for (j = 0; j < njoints; j++) { + g[k][j] = 0.0; + h[k][j] = 0.0; + bulge[k][j] = 0.0; + for (a = 0; a < AXIS_COUNT; a++) { g[k][j] += J[j][a] * t[a]; } + } + ok[k] = true; + answered++; + } + if (!answered) { return -1; } + + /* how far the rate can rise between two samples: the parabola + through three in a row bulges by an eighth of its second + difference over its middle, and a peak between samples is + covered by charging that to all three */ + for (k = 1; k + 1 < K; k++) { + if (!ok[k - 1] || !ok[k] || !ok[k + 1]) { continue; } + for (j = 0; j < njoints; j++) { + double b = fabs(g[k + 1][j] - 2.0 * g[k][j] + g[k - 1][j]) / 8.0; + if (b > bulge[k][j]) { bulge[k][j] = b; } + if (b > bulge[k - 1][j]) { bulge[k - 1][j] = b; } + if (b > bulge[k + 1][j]) { bulge[k + 1][j] = b; } + } + } + + /* the joint velocity limits */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny && lim->vel[j] / rate < out->vel) { + out->vel = lim->vel[j] / rate; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + + /* how the rates change along the segment, read between neighbours + and charged to both */ + for (prev = -1, k = 0; k < K; k++) { + if (!ok[k]) { continue; } + if (prev >= 0) { + for (j = 0; j < njoints; j++) { + double dh = fabs(g[k][j] - g[prev][j]) / (s[k] - s[prev]); + if (dh > h[k][j]) { h[k][j] = dh; } + if (dh > h[prev][j]) { h[prev][j] = dh; } + } + } + prev = k; + } + + /* the changing rate at the capped velocity takes at most half a + joint's acceleration budget */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + if (h[k][j] > tiny) { + double cap = sqrt(lim->acc[j] / (2.0 * h[k][j])); + if (cap < out->vel) { + out->vel = cap; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + } + + /* the joint acceleration limits, less what the changing rate takes */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny) { + double cap = (lim->acc[j] - h[k][j] * out->vel * out->vel) / rate; + if (cap < out->acc) { + out->acc = cap; + out->acc_joint = j; + } + } + } + } + return 0; +} // segmentCap() + +} // namespace motion_planning diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh new file mode 100644 index 00000000000..80826bcb900 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.hh @@ -0,0 +1,75 @@ +/******************************************************************** + * Description: segment_cap.hh + * The velocity and acceleration a segment can be run at without + * asking any joint for more than its own limits, read off the + * kinematics module's Jacobian along the segment. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ +#ifndef SEGMENT_CAP_HH +#define SEGMENT_CAP_HH + +#include +#include + +namespace motion_planning { + +/* The joint limits the cap is read against, in the units motion commands + the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION. */ +struct SegmentCapLimits { + int joints; + double vel[KINEMATICS_USER_MAX_JOINTS]; + double acc[KINEMATICS_USER_MAX_JOINTS]; +}; + +/* No joint binds. */ +#define SEGMENT_CAP_NONE 1e9 + +/* The most a segment was sampled at. */ +#define SEGMENT_CAP_MAX_SAMPLES 129 + +/* What the joints allow along the segment: the fastest the path parameter + may advance, and the hardest it may accelerate, per second. */ +struct SegmentCap { + double vel; + double acc; + int vel_joint; /* the joint setting each, -1 for none */ + int acc_joint; + double vel_at; /* the fraction of the segment the velocity cap is read at */ + int samples; /* points the segment was sampled at */ + int unanswered; /* of them, the ones the module could not invert or differentiate */ +}; + +/* The pose at a fraction of the segment, 0 the start and 1 the end, in + the units the module takes, which are motion's. A line interpolates, + an arc walks its circle. */ +typedef void (*SegmentPoseFn)(double fraction, EmcPose *pose, void *arg); + +/* Sample the segment at `samples` points, the ends among them, and read + at each the rate every joint moves at per unit of path parameter: the + module's Jacobian along the local tangent. Between neighbouring samples + read how that rate changes with the parameter, and from three in a row + how much it can bulge between two of them, which is added to the rate so + that a peak falling between samples is covered. A joint's velocity + limit then caps the path velocity, and its acceleration limit caps the + path acceleration once the share the changing rate takes at the capped + velocity (what a carriage on a circle feels as centripetal) is set + aside: at most half the joint's budget goes to it, and the velocity is + lowered where it would take more. + + length is the segment's extent in the path parameter, in the units the + caps come back in. joints seeds the inverse at the start and comes back + holding the joints at the end, so the next segment starts on the same + solution branch. Returns 0, or -1 where no sample could be evaluated, + the caps then NONE. */ +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out); + +} // namespace motion_planning + +#endif // SEGMENT_CAP_HH diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2037aba3a62..18ae2bc78cb 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -314,6 +314,8 @@ extern int emcJointSetHomingParams(int joint, double home, double offset, double extern int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence); extern int emcJointSetMaxVelocity(int joint, double vel); extern int emcJointSetMaxAcceleration(int joint, double acc); +extern double emcJointGetMaxVelocity(int joint); +extern double emcJointGetMaxAcceleration(int joint); extern int emcJointInit(int joint); extern int emcJointHalt(int joint); diff --git a/src/emc/task/Submakefile b/src/emc/task/Submakefile index 9fdab7004d6..e775d36d7af 100644 --- a/src/emc/task/Submakefile +++ b/src/emc/task/Submakefile @@ -32,7 +32,7 @@ USERSRCS += $(MILLTASKSRCS) ../bin/milltask: $(call TOOBJS, $(MILLTASKSRCS)) ../lib/librs274.so.0 ../lib/liblinuxcnc.a \ ../lib/libnml.so.0 ../lib/liblinuxcncini.so.1 ../lib/libposemath.so.0 \ ../lib/liblinuxcnchal.so.0 ../lib/libpyplugin.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 $(ECHO) Linking $(notdir $@) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index c2d96933119..a7bff9e7d24 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -69,6 +69,11 @@ #include "tooldata/tooldata.hh" #include #include +#include // getpid() +#include +#include +#include +#include using namespace linuxcnc; @@ -486,6 +491,13 @@ static double toExtVel(double vel) { static double toExtAcc(double acc) { return toExtVel(acc); } +static double fromExtVel(double vel) { + if (!canon.cartesian_move && canon.angular_move) { + return FROM_EXT_ANG(vel); + } + return FROM_EXT_LEN(vel); +} + static void send_g5x_msg(int index) { flush_segments(); @@ -840,6 +852,306 @@ static double getStraightJerk(double x, double y, double z, return 0.0; // a move to nowhere } +//---------------------------------------------------------------------- +// The kinematics ahead of motion: what the joints allow a segment. +// +// A segment is planned in world coordinates against the [AXIS_L] +// velocity and acceleration limits, which say nothing about the joints +// once the kinematics is not the identity: a turn of a tilted head under +// the tool centre point swings a carriage through a circle the head's +// own limit never mentions. So every segment is also sampled through +// the module's Jacobian here, on the kinematics type and the tool offset +// the program is under (canon runs ahead of motion, so neither is what +// the machine has reached), and its velocity and acceleration are +// lowered to what the slowest joint can follow. The module is the one +// motion runs, loaded through the non-RT loader in kinematics_userspace/ +// the first time a segment asks. A module that runs only in realtime, +// and a kinematics type that is the identity, cap nothing. +//---------------------------------------------------------------------- + +static struct { + KinematicsUserContext *ctx; + int comp_id; + bool tried; // loading was attempted, once + int type; // the kinematics type the program is in, -1 for the machine's + double joints[EMCMOT_MAX_JOINTS]; // the joints at canon.endPoint, as far as canon knows + bool in_arc; // ARC_FEED caps the arc itself; the straight helpers stand back + bool debug; // EMCCANON_KINS_DEBUG in the environment: say what every segment got + motion_planning::SegmentCapLimits limits; + // the last straight segment's answer, since its velocity and its + // acceleration are asked for separately + struct { + bool valid; + EmcPose start, end; + bool capped; + double vel, acc; + } memo; +} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0 } }; + +// the machine's own module, once +static bool kins_load(void) +{ + char name[HAL_NAME_LEN + 1]; + int joints, j; + + if (kins.tried) { return kins.ctx != NULL; } + kins.tried = true; + linuxcnc::IniFile ini(emc_inifile); + if (!ini) { return false; } + auto module = ini.findString("KINEMATICS", "KINS"); + joints = ini.findIntV("JOINTS", "KINS", 0); + if (!module || joints < 1) { return false; } + snprintf(name, sizeof(name), "canon.%d", (int)getpid()); + kins.comp_id = hal_init(name); + if (kins.comp_id < 0) { return false; } + kins.ctx = kinematicsUserInitString(module->c_str(), joints, kins.comp_id, name); + hal_ready(kins.comp_id); + if (kins.ctx && kinematicsUserIsRtOnly(kins.ctx)) { + kinematicsUserFree(kins.ctx); + kins.ctx = NULL; + } + if (!kins.ctx) { + hal_exit(kins.comp_id); + kins.comp_id = -1; + return false; + } + // the joint limits as the INI gives them; one left out binds nothing + kins.limits.joints = kinematicsUserGetNumJoints(kins.ctx); + for (j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + double vel = j < kins.limits.joints ? emcJointGetMaxVelocity(j) : 0.0; + double acc = j < kins.limits.joints ? emcJointGetMaxAcceleration(j) : 0.0; + kins.limits.vel[j] = vel > 0.0 ? vel : SEGMENT_CAP_NONE; + kins.limits.acc[j] = acc > 0.0 ? acc : SEGMENT_CAP_NONE; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { kins.joints[j] = 0.0; } + kins.debug = getenv("EMCCANON_KINS_DEBUG") != NULL; + if (kins.debug) { + fprintf(stderr, "canon kins: %s, %d joints, limits", module->c_str(), kins.limits.joints); + for (j = 0; j < kins.limits.joints; j++) { + fprintf(stderr, " %g/%g", kins.limits.vel[j], kins.limits.acc[j]); + } + fprintf(stderr, "\n"); + } + return true; +} + +// the module on the type and the tool offset the program is under, or +// NULL where there is nothing to cap with +static KinematicsUserContext *kins_here(void) +{ + int type = kins.type >= 0 ? kins.type : GET_EXTERNAL_KINS_TYPE(); + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(type); + EmcPose tool; + + // the identity, whether the module as a whole is one or the type in + // force is, needs no module loaded to know the joints are the axes + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return NULL; } + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return NULL; } + if (!kins_load()) { return NULL; } + if (kinematicsUserSetType(kins.ctx, type) != 0) { return NULL; } + if (kinematicsUserIsIdentity(kins.ctx)) { return NULL; } + tool = to_ext_pose(canon.toolOffset.tran.x, canon.toolOffset.tran.y, canon.toolOffset.tran.z, + canon.toolOffset.a, canon.toolOffset.b, canon.toolOffset.c, + canon.toolOffset.u, canon.toolOffset.v, canon.toolOffset.w); + kinematicsUserSetTool(kins.ctx, &tool); + return kins.ctx; +} + +// whether two machine points are the same, to a hair either way +static bool kins_same(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-6; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints at the start of a segment, as far as canon can know ahead of +// motion: the ones it holds while they still explain the point, since a +// point does not name one joint set, else the joints the machine stands +// in if those do, else the inverse iterated to a fixed point +static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) +{ + double standing[EMCMOT_MAX_JOINTS] = {0}; + EmcPose seeded = *start; + int i, n, pass; + + if (kinematicsUserForward(ctx, kins.joints, &seeded) == 0 && kins_same(&seeded, start)) { return; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + if (n > 0) { + seeded = *start; + if (kinematicsUserForward(ctx, standing, &seeded) == 0 && kins_same(&seeded, start)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { kins.joints[i] = standing[i]; } + return; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = kins.joints[i]; } + if (kinematicsUserInverse(ctx, start, kins.joints) != 0) { return; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(kins.joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } +} + +// how many points to sample a segment at: enough that a turn of the +// rotaries, a stretch of travel on a machine whose Jacobian changes with +// position, or a sweep of arc is seen every few degrees or centimetres +static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) +{ + double rot = fmax(fabs(end->a - start->a), fmax(fabs(end->b - start->b), fabs(end->c - start->c))); + double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + + pow(end->tran.z - start->tran.z, 2)); + int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); + + return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; +} + +// what the joints allow along a segment, in the machine's units per second +// of the path parameter canon plans it with; false where nothing binds +static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const EmcPose *end, + double length, int samples, motion_planning::SegmentPoseFn pose_at, void *arg, + double *vel, double *acc) +{ + motion_planning::SegmentCap cap; + + kins_seed(ctx, start); + if (motion_planning::segmentCap(ctx, &kins.limits, length, samples, pose_at, arg, kins.joints, &cap) != 0) { + CANON_ERROR(fmt::format("the kinematics cannot reach the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f}, the joints are not checked along it", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c)); + return false; + } + if (cap.unanswered) { + CANON_ERROR(fmt::format("the kinematics cannot reach {} of {} points on the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f}, the joints are not checked there", + cap.unanswered, cap.samples, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c)); + } + // a pole: some joint would have to move at any speed at all for the + // path to advance, and the move crawls rather than being dropped, + // since a dropped segment would send the next one along another path + if (cap.vel_joint >= 0 && cap.vel < 1e-3) { + CANON_ERROR(fmt::format("joint {} cannot follow the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f} near {:.0f}% of its length: the kinematics is singular there and the move crawls", + cap.vel_joint, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, 100.0 * cap.vel_at)); + } + if (kins.debug) { + fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered; seed", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, length, cap.samples, + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered); + for (int j = 0; j < kins.limits.joints; j++) { fprintf(stderr, " %.3f", kins.joints[j]); } + fprintf(stderr, "\n"); + } + *vel = cap.vel; + *acc = cap.acc; + return cap.vel_joint >= 0 || cap.acc_joint >= 0; +} + +struct KinsLine { + EmcPose start, end; +}; + +static void kins_line_pose(double f, EmcPose *pose, void *arg) +{ + const KinsLine *line = (const KinsLine *)arg; + + pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); + pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); + pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); + pose->a = line->start.a + f * (line->end.a - line->start.a); + pose->b = line->start.b + f * (line->end.b - line->start.b); + pose->c = line->start.c + f * (line->end.c - line->start.c); + pose->u = line->start.u + f * (line->end.u - line->start.u); + pose->v = line->start.v + f * (line->end.v - line->start.v); + pose->w = line->start.w + f * (line->end.w - line->start.w); +} + +// the cap on a straight segment from canon.endPoint, in the machine's +// units per second of the parameter getStraightVelocity() plans it with, +// the length along the axes getStraightSpan() measures +static bool kins_straight_cap(double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double *vel, double *acc) +{ + KinematicsUserContext *ctx; + KinsLine line; + double length; + + if (kins.in_arc) { return false; } + line.start = to_ext_pose(canon.endPoint); + line.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + if (kins.memo.valid && kins_same(&kins.memo.start, &line.start) && kins_same(&kins.memo.end, &line.end)) { + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; + } + ctx = kins_here(); + if (!ctx) { return false; } + // the length getStraightVelocity() plans the segment along + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + if (!span.moving) { + return false; + } + length = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + kins.memo.valid = true; + kins.memo.start = line.start; + kins.memo.end = line.end; + kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, + kins_samples(&line.start, &line.end, 0.0), + kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc); + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; +} + +struct KinsArc { + PmCircle circle; // in canon's units, as ARC_FEED builds it + bool line; // no turn at all: the chord + EmcPose start, end; +}; + +static void kins_arc_pose(double f, EmcPose *pose, void *arg) +{ + const KinsArc *arc = (const KinsArc *)arg; + PmCartesian p; + + kins_line_pose(f, pose, (void *)&arc->start); + if (arc->line) { return; } + pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); + pose->tran.x = TO_EXT_LEN(p.x); + pose->tran.y = TO_EXT_LEN(p.y); + pose->tran.z = TO_EXT_LEN(p.z); +} + +// the cap on an arc from canon.endPoint, in the machine's units per +// second of the XYZ length ARC_FEED plans it with +static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, + const PM_CARTESIAN &end_xyz, const CANON_POSITION &endpt, + int rotation, double length, double full_angle, + double *vel, double *acc) +{ + KinematicsUserContext *ctx = kins_here(); + KinsArc arc; + + if (!ctx || length <= 0.0) { return false; } + arc.start = to_ext_pose(canon.endPoint); + arc.end = to_ext_pose(endpt); + arc.line = rotation == 0; + if (!arc.line) { + PmCartesian s = { canon.endPoint.x, canon.endPoint.y, canon.endPoint.z }; + PmCartesian e = { end_xyz.x, end_xyz.y, end_xyz.z }; + PmCartesian c = { center.x, center.y, center.z }; + PmCartesian n = { normal.x, normal.y, normal.z }; + if (pmCircleInit(&arc.circle, &s, &e, &c, &n, rotation > 0 ? rotation - 1 : rotation) != 0) { + return false; + } + } + return kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), + kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), + kins_arc_pose, &arc, vel, acc); +} + /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -869,6 +1181,14 @@ static AccelData getStraightAcceleration(double x, double y, double z, if (out.tmax > 0.0) { out.acc = out.dtot / out.tmax; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.acc > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kacc) < out.acc) { + out.acc = fromExtVel(kacc); + } + } if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; @@ -913,6 +1233,14 @@ static VelData getStraightVelocity(double x, double y, double z, } else { out.vel = canon.linearFeedRate; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.vel > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kvel) < out.vel) { + out.vel = fromExtVel(kvel); + } + } if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; @@ -1101,6 +1429,10 @@ void SELECT_KINS_TYPE(int switchkins_type) { flush_segments(); + // the segments from here on run under it + kins.type = switchkins_type; + kins.memo.valid = false; + auto selectKinsMsg = std::make_unique(); selectKinsMsg->switchkins_type = switchkins_type; @@ -2801,6 +3133,7 @@ void ARC_FEED(int line_number, // Find the equivalent maximum velocity for a linear displacement // This accounts for speed restrictions due to helical and other axes + kins.in_arc = true; VelData veldata = getStraightVelocity(endpt); // Compute spiral length, first by the minimum circular arc length @@ -2833,6 +3166,7 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) AccelData accdata = getStraightAcceleration(endpt); + kins.in_arc = false; double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; @@ -2842,6 +3176,16 @@ void ARC_FEED(int line_number, // circle plane and helical axis will still be within limits double a_max = total_xyz_length / tt_max; + // and what the joints allow along the arc, through the kinematics + { + double kvel, kacc; + if (kins_arc_cap(center_cart, normal_cart, end_cart, endpt, rotation, + total_xyz_length, full_angle, &kvel, &kacc)) { + v_max = std::min(v_max, FROM_EXT_LEN(kvel)); + a_max = std::min(a_max, FROM_EXT_LEN(kacc)); + } + } + // Limit velocity by maximum double vel = std::min(canon.linearFeedRate, v_max); canon_debug("current F = %f\n",canon.linearFeedRate); @@ -3059,6 +3403,7 @@ static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) canon.toolOffset.u = FROM_PROG_AX(AXIS_U, offset.u); canon.toolOffset.v = FROM_PROG_AX(AXIS_V, offset.v); canon.toolOffset.w = FROM_PROG_AX(AXIS_W, offset.w); + kins.memo.valid = false; /* append it to interp list so it gets updated at the right time, not at read-ahead time */ @@ -3543,6 +3888,8 @@ void INIT_CANON() canon.linearFeedRate = 0.0; canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + kins.type = -1; + kins.memo.valid = false; { std::string err; @@ -3857,7 +4204,10 @@ int GET_EXTERNAL_KINS_TYPE() // motion publishes the kinematics it is actually running, which is // not necessarily the one G-code last asked for: an abort can drop a // queued switch, and the motion.switchkins-type pin can select one - // without the interpreter seeing it + // without the interpreter seeing it. The interpreter asks when it + // synchs to the machine; canon's own idea of the type goes with it + kins.type = -1; + kins.memo.valid = false; return emcStatus->motion.traj.switchkins_type; } diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index c291547cf39..e886366f78c 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -552,6 +552,24 @@ int emcAxisSetLockingJoint(int axis, int joint) return retval; } +double emcJointGetMaxVelocity(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxVel; +} + +double emcJointGetMaxAcceleration(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxAccel; +} + double emcAxisGetMaxVelocity(int axis) { if (axis < 0 || axis >= EMCMOT_MAX_AXIS) { diff --git a/tests/kins-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py index fea6005fe38..3a84f139ab4 100755 --- a/tests/kins-limits/swing/test-ui.py +++ b/tests/kins-limits/swing/test-ui.py @@ -46,7 +46,7 @@ def settled(): if s.inpos and not s.queue and now == last: return now last = now - time.sleep(0.05) + time.sleep(0.01) error("timed out waiting for the move") return last @@ -87,62 +87,75 @@ def log_samples(): print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), " ".join("%.3f" % v for v in tip))) drain() -# the log is block buffered: wait until it has caught up with the machine -# at rest before marking where the swing starts in it -deadline = time.time() + 10 -while True: - samples = log_samples() - if samples and max(abs(a - b) for a, b in zip(samples[-1][0], start)) < 2e-6: - break - if time.time() > deadline: - error("the sampler log did not catch up with the machine") - break - time.sleep(0.02) -n0 = len(samples) +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def swing(cmd, n0): + c.mdi(cmd) + c.wait_complete(60) + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (cmd, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (cmd, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), cmd)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) # the swing: the tip holds, C turns half a revolution, the carriage follows # a half circle of radius 400 to keep the tip where it is -c.mdi("G0 C180") -c.wait_complete(60) -end = settled() -said = drain() -time.sleep(0.5) -samples = log_samples()[n0:] - -for m in said: - print("channel:", m) -faults = [m for m in said if "following error" in m[1]] -if faults: - error("the swing tripped a following error: %s" % faults[0][1].strip()) -s.poll() -if s.task_state != linuxcnc.STATE_ON: - error("the machine is not on after the swing (task state %d)" % s.task_state) +end = swing("G0 C180", catch_up(start)) if abs(end[4] - 180) > 1e-3: error("C ended at %.4f, not 180" % end[4]) -s.poll() -after = list(s.position[:3]) -if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: - error("the tip moved from %s to %s" % (tip, after)) - -# the commanded velocity and acceleration of every joint against its own -# INI limit, every servo cycle; a joint over its limit is what the drive -# could not follow. A fault freezes the command in one cycle, which is -# not an acceleration the planner asked for: the samples stop at the last -# moving one -last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) -samples = samples[:last + 1] -print("%d samples through the swing" % len(samples)) -for j in range(JOINTS): - vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 - apeak = 0.0 - for k in range(1, len(samples)): - apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) - print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" - % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) - if vpeak > VEL[j] * 1.001: - error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) - if apeak > ACC[j] * 1.01: - error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) +tip_held("G0 C180") + +# the same on an arc: the tip draws a full circle of radius 100 at a feed +# it could keep on its own while C turns another revolution, so the +# carriage rides the small circle and the big swing together +end = swing("G17 G2 X0 Y0 I100 J0 C540 F40000", catch_up(end)) +if abs(end[4] - 540) > 1e-3: + error("C ended at %.4f, not 540" % end[4]) +tip_held("the arc") overruns = int(hal.get_value("sampler.0.overruns")) if overruns: From cc0a8912893b1a25b72e9997289c39a1bd3b01e1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 21:39:56 +1000 Subject: [PATCH 04/44] motion: cap a world jog at what its joints can follow A world jog is planned per axis against the [AXIS_L] limits, which say nothing about the joints once the kinematics is not the identity: a jog of C under the tool centre point with the head laid over swings the carriage through a circle, and the carriage trips its following error a fraction of a degree in. The segment cap in canon cannot help, a jog never passes canon. Each servo cycle in teleop mode, before the axis planners run, teleop_joint_cap() reads the module's Jacobian at the joints the machine stands in (weak, so an old module without one keeps today's behaviour, and nothing is read on the identity) and, for the jog the planners were asked for, the rate every joint would move at per unit of it and how that rate has changed since the last cycle, what a carriage on a circle feels as centripetal. The planners are then capped so that no joint is asked for more than its own limits: the velocity by the joint velocity limits, read a stopping distance ahead since the rate keeps growing while the planners slow down, and by the acceleration budget the changing rate takes at that velocity, at most half of it; the acceleration by what the budget leaves. Every active axis scales by the same factor, so a jog of two axes keeps its direction. The jog's own request is kept apart from the planner's caps in the axis, teleop_vel_req and teleop_acc_req, and axis_teleop_request() and axis_teleop_cap() are the two calls the cap needs from axis.c. tests/kins-limits/jog is the swing test's machine in teleop mode: a continuous jog of C at the axis's full rate stopped past 150 degrees, an incremental jog back by 90, an incremental jog of X with the head still laid over, every joint following through limit3 at 20 percent over its limits; red before this (joint 1 asked for 1396 mm/s^2 against 800, following error at C 0.4), green with it, the carriage held at its limits and the jog of C running faster where the carriage rate is small and slowing as it grows. The switchkins and INI chapters say that a jog is bounded the same way as a move. --- docs/src/config/ini-config.adoc | 2 +- docs/src/motion/switchkins.adoc | 7 +- src/emc/motion/axis.c | 36 ++++++ src/emc/motion/axis.h | 2 + src/emc/motion/control.c | 117 ++++++++++++++++++ tests/kins-limits/jog/README | 17 +++ tests/kins-limits/jog/checkresult | 3 + tests/kins-limits/jog/sim.hal | 58 +++++++++ tests/kins-limits/jog/test-ui.py | 189 ++++++++++++++++++++++++++++++ tests/kins-limits/jog/test.ini | 146 +++++++++++++++++++++++ tests/kins-limits/jog/test.sh | 4 + tests/kins-limits/jog/tool.tbl | 1 + 12 files changed, 580 insertions(+), 2 deletions(-) create mode 100644 tests/kins-limits/jog/README create mode 100755 tests/kins-limits/jog/checkresult create mode 100644 tests/kins-limits/jog/sim.hal create mode 100755 tests/kins-limits/jog/test-ui.py create mode 100644 tests/kins-limits/jog/test.ini create mode 100755 tests/kins-limits/jog/test.sh create mode 100644 tests/kins-limits/jog/tool.tbl diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 9dda969b95d..477363b02a2 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1042,7 +1042,7 @@ The __ specifies one of: X Y Z A B C U V W * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. + `MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. -The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, so on a kinematics that is not the identity a move runs at the pace of its slowest joint whatever these say. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, and a world jog the same way every servo cycle, so on a kinematics that is not the identity a move or a jog runs at the pace of its slowest joint whatever these say. See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 976896aa1d1..970674847b9 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -324,7 +324,12 @@ the tool centre point, which swings the carriage through a circle the head's own limit never mentions, runs at the carriage's pace. A module that can only be evaluated in realtime (one without the parameter block form, see <>) -caps nothing; on it the `[AXIS_L]` limits are all there is. +caps nothing; on it the `[AXIS_L]` limits are all there is. A world +jog is bounded the same way, in motion, every servo cycle: the Jacobian +at the joints the machine stands in caps the jog's planners so that no +joint is asked for more than its limits, and a jog of C under the tool +centre point with the head laid over runs at the pace of the carriage +it swings. So a switch does not need the velocity and acceleration limits reset to keep the joints within their limits. It may still want them reset diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c index 4aa4a26ba13..6bbaf08ad16 100644 --- a/src/emc/motion/axis.c +++ b/src/emc/motion/axis.c @@ -26,6 +26,8 @@ typedef struct { double acc_limit; /* upper limit of axis accel */ double jerk_limit; /* upper limit of axis jerk */ simple_tp_t teleop_tp; /* planner for teleop mode motion */ + double teleop_vel_req; /* the speed the jog asked for */ + double teleop_acc_req; /* and the acceleration it may use */ rtapi_sint old_ajog_counts; /* prior value, used for deltas */ int kb_ajog_active; /* non-zero during a keyboard jog */ @@ -336,6 +338,8 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -359,6 +363,8 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -381,6 +387,8 @@ void axis_jog_abs(int axis_num, double offset, double vel) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -490,6 +498,8 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo axis->teleop_tp.pos_cmd = pos; axis->teleop_tp.max_vel = axis->vel_limit; axis->teleop_tp.max_acc = aaccel_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->wheel_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -746,6 +756,32 @@ static int update_teleop_with_check(int axis_num, simple_tp_t *the_tp, double se return 0; } +// Whether the axis's teleop planner has somewhere to go this cycle, and +// which way, how fast and how hard the jog asked it to: what the cap on +// the joints reads before the planners run +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + double togo = axis->teleop_tp.pos_cmd - axis->teleop_tp.curr_pos; + + if (!axis->teleop_tp.enable) { return 0; } + if (fabs(togo) < TINY_DP(axis->teleop_tp.max_acc, 0.001) && axis->teleop_tp.curr_vel == 0.0) { return 0; } + *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : axis->teleop_tp.curr_vel > 0.0 ? 1.0 : -1.0; + *vel = axis->teleop_vel_req; + *acc = axis->teleop_acc_req; + return 1; +} + +// The most the planner may do this cycle: what the jog asked, or less +// where the joints cannot follow that +void axis_teleop_cap(int axis_num, double vel, double acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + + axis->teleop_tp.max_vel = vel < axis->teleop_vel_req ? vel : axis->teleop_vel_req; + axis->teleop_tp.max_acc = acc < axis->teleop_acc_req ? acc : axis->teleop_acc_req; +} + int axis_calc_motion(double servo_period) { int axis_num; diff --git a/src/emc/motion/axis.h b/src/emc/motion/axis.h index 5d61f0b13f0..bc00504570d 100644 --- a/src/emc/motion/axis.h +++ b/src/emc/motion/axis.h @@ -51,6 +51,8 @@ void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc); +void axis_teleop_cap(int axis_num, double vel, double acc); int axis_calc_motion(double servo_period); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 22cbfdaf5a4..d7e6f3a9522 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -46,6 +46,10 @@ static int switchkins_type = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags = 0; +// old modules export no kinematicsJacobian; a jog on them is bounded by +// the axis limits alone, as before +#pragma weak kinematicsJacobian + /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ // #define WATCH_FLAGS 1 @@ -1344,6 +1348,118 @@ static void handle_jjogwheels(void) first_pass = 0; } +/* A world jog is planned per axis against the [AXIS_L] limits, which say + nothing about the joints once the kinematics is not the identity: a jog + of C under the tool centre point with the head laid over swings the + carriage through a circle. Each cycle, before the teleop planners run, + the module's Jacobian at the joints the machine stands in says how fast + every joint would move per unit of the jog the planners were asked for, + and how that rate has changed since the last cycle (what a carriage on a + circle feels as centripetal), and the planners are capped so that no + joint is asked for more than its own limits: the velocity by the joint + velocity limits, read a stopping distance ahead since the rate keeps + growing while the planners slow down, and by the acceleration budget + the changing rate takes at that velocity (at most half); the + acceleration by what the budget leaves. All active axes scale together + so the jog keeps its direction. Nothing is done on the identity, or on + a module without a Jacobian. */ +static void teleop_joint_cap(double period) +{ + static double jac_prev[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + static int have_prev = 0; + static double acc_scale_prev = 1.0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double joint_pos[EMCMOT_MAX_JOINTS]; + double dir[EMCMOT_MAX_AXIS], vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS]; + double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; + int active[EMCMOT_MAX_AXIS]; + double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; + double vel_scale, acc_scale; + const double tiny = 1e-12; + int flags, any = 0, a, j; + + if (!kinematicsJacobian) { have_prev = 0; return; } + flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; + if (flags >= 0 ? (flags & KINSTYPE_IDENTITY) != 0 + : emcmotConfig->kinType == KINEMATICS_IDENTITY) { + have_prev = 0; + return; + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + active[a] = axis_teleop_request(a, &dir[a], &vreq[a], &areq[a]); + if (active[a]) { + double v = axis_get_teleop_vel_cmd(a); + any = 1; + rnorm += vreq[a] * vreq[a]; + anorm += areq[a] * areq[a]; + speed += v * v; + } + } + if (!any) { have_prev = 0; return; } + rnorm = sqrt(rnorm); + anorm = sqrt(anorm); + speed = sqrt(speed); + if (rnorm < tiny || anorm < tiny) { have_prev = 0; return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsJacobian(joint_pos, &emcmotStatus->carte_pos_cmd, jac, &iflags) != 0) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vreq[a], areq[a]); } + } + have_prev = 0; + return; + } + /* the direction the jog was asked in, and the one the planners + accelerate in, each at its own limit */ + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rhat[a] = active[a] ? dir[a] * vreq[a] / rnorm : 0.0; + ahat[a] = active[a] ? dir[a] * areq[a] / anorm : 0.0; + } + /* the path the planners need to stop from the speed they are at, + at the acceleration they were left last cycle */ + ahead = speed * speed / (2.0 * anorm * acc_scale_prev); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, change = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rate += jac[j][a] * rhat[a]; + if (have_prev) { change += (jac[j][a] - jac_prev[j][a]) * rhat[a]; } + } + rate = fabs(rate); + /* how the rate changes per unit of path, from the last cycle */ + change = (have_prev && speed > tiny) ? fabs(change) / (speed * period) : 0.0; + if (rate > tiny) { + cap = joints[j].vel_limit / (rate + change * ahead); + if (cap < vcap) { vcap = cap; } + } + if (change > tiny) { + cap = sqrt(joints[j].acc_limit / (2.0 * change)); + if (cap < vcap) { vcap = cap; } + } + joint_pos[j] = change; /* kept for the acceleration pass */ + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double arate = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { arate += jac[j][a] * ahat[a]; } + arate = fabs(arate); + if (arate > tiny) { + cap = (joints[j].acc_limit - joint_pos[j] * vcap * vcap) / arate; + if (cap < acap) { acap = cap; } + } + } + vel_scale = vcap / rnorm; + acc_scale = acap / anorm; + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + if (acc_scale < 1e-6) { acc_scale = 1e-6; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vel_scale * vreq[a], acc_scale * areq[a]); } + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } + } + have_prev = 1; + acc_scale_prev = acc_scale; +} // teleop_joint_cap() + static void get_pos_cmds(long period) { int joint_num, result; @@ -1655,6 +1771,7 @@ static void get_pos_cmds(long period) break; case EMCMOT_MOTION_TELEOP: + teleop_joint_cap(servo_period); ext_offset_teleop_limit = axis_calc_motion(servo_period); if (!ext_offset_teleop_limit) { ext_offset_coord_limit = 0; //in case was set in prior coord motion diff --git a/tests/kins-limits/jog/README b/tests/kins-limits/jog/README new file mode 100644 index 00000000000..2f2e00310c9 --- /dev/null +++ b/tests/kins-limits/jog/README @@ -0,0 +1,17 @@ +A world jog under the tool centre point must not run a slide past its +own limits. + +The swing test's machine: 5axiskins with a 400 mm pivot, the head laid +over to B90 by a joint move, the joints following their commands through +limit3 at 20 percent over each joint's own limits, the way a drive or a +step generator follows only as fast as it can. Then, in teleop mode, a +continuous jog of C at the axis's full rate, stopped by the test past +150 degrees, an incremental jog of C back by 90, and an incremental jog +of X with the head still laid over. The tip holds through the jogs of +C while the carriage swings on a circle of radius 400; nothing about the +[AXIS_C] limits says how fast that is. The test asserts that no jog +trips a following error, that the incremental jogs land where they were +sent, and that no joint's commanded velocity or acceleration went over +the joint's INI limit, sampled every servo cycle. The pace of the jog +is not asserted: it is whatever the slowest joint allows, and it changes +along the swing. diff --git a/tests/kins-limits/jog/checkresult b/tests/kins-limits/jog/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog/sim.hal b/tests/kins-limits/jog/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/jog/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/jog/test-ui.py b/tests/kins-limits/jog/test-ui.py new file mode 100755 index 00000000000..e863759c0e8 --- /dev/null +++ b/tests/kins-limits/jog/test-ui.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point must not run a slide past its +# own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (what, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the circle a jog of C is about to swing it on +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() + +# world jogs: teleop mode +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C at the axis's full rate, the tip held: stopped by +# the test once C is past 150, so the planner ramps up, cruises, ramps down +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 480) +deadline = time.time() + 60 +while time.time() < deadline: + s.poll() + if s.position[C] > 150: + break + time.sleep(0.01) +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C", n0) +if end[4] < 150: + error("C stopped at %.3f, before 150" % end[4]) +tip_held("the continuous jog of C") + +# an incremental jog back by 90 at the same rate: the planner knows its +# endpoint, the cap still holds +n0 = catch_up(end) +back_from = end[4] +c.jog(linuxcnc.JOG_INCREMENT, 0, C, -480, 90) +end = checked("the incremental jog of C", n0) +if abs(end[4] - (back_from - 90)) > 1e-3: + error("C ended at %.3f, not %.3f" % (end[4], back_from - 90)) +tip_held("the incremental jog of C") + +# a jog of X with the head still laid over: the Jacobian does not change +# along it, the carriage simply runs at the jog rate +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 1600, 100) +end = checked("the incremental jog of X", n0) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog/test.ini b/tests/kins-limits/jog/test.ini new file mode 100644 index 00000000000..13379eb18a0 --- /dev/null +++ b/tests/kins-limits/jog/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog/test.sh b/tests/kins-limits/jog/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog/tool.tbl b/tests/kins-limits/jog/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 5cfacecba44053e94446fe7e56fd2fbd9f4aaac0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 22:31:30 +1000 Subject: [PATCH 05/44] docs, qtvcp, hal_glib: list every code the kinematics work added The quick reference, the qtvcp MDI help and the modal group tables of hal_glib knew none of G12.1, G13.1, G28.5, G30.5, G43.4, G43.5, G53.1 to G53.7, G68.2 to G68.4 and G69. One sweep for all of them, the same three places G28.2 was added to. docs/src/gcode.html.in gets a row per code, linked to its section; the point-to-point and orientation codes sit under Motion, the two tool length forms under Tool Length Offset, the plane, its cancel and the kinematics selection under Other Modal Codes, the machine frame forms of G28 and G30 next to them. lib/python/qtvcp/lib/mdi_text.py gets a title, a word list and a description for each, condensed from the G-code chapter, so the qtvcp G-code help lists and explains them. lib/python/common/hal_glib.py gets the codes in their modal groups and a gcode-group9-changed signal for the tilted work plane, which stat.gcodes reports in its own slot; group 8 also gains G43.2, which was missing. The allocated G-code tables of the remap chapter get the same codes, and row 33, which listed G30 and G30.1 in G33's place, now lists G33 and G33.1. The G28.5 heading and its cross-reference from G30 said slide position; it is the machine frame position, as the section itself says. --- docs/src/gcode.html.in | 12 ++ docs/src/gcode/g-code.adoc | 10 +- docs/src/remap/remap.adoc | 18 +-- lib/python/common/hal_glib.py | 15 +- lib/python/qtvcp/lib/mdi_text.py | 265 +++++++++++++++++++++++++++++++ 5 files changed, 301 insertions(+), 19 deletions(-) diff --git a/docs/src/gcode.html.in b/docs/src/gcode.html.in index 02353bbcf36..b573eafa3be 100644 --- a/docs/src/gcode.html.in +++ b/docs/src/gcode.html.in @@ -65,6 +65,11 @@ tr.head td, tr.head th { background: black; color: white; } G33K ($)Spindle Synchronized Motion G33.1K ($)Rigid Tapping G80 Cancel Canned Cycle + G53.4Point-to-Point Move, Program Coordinates + G53.5Point-to-Point Move, Machine Frame + G53.7J<n>=Point-to-Point Move, Joint Values + G53.1, G53.3, G53.6(P) (Q)Orient the Tool to the Work Plane + G53.2(P) (Q)Solve the Tool Orientation, No Motion Canned cycles(X Y Z or U V W apply to canned cycles, depending on active plane) G70Q (X) (Z) (D) (E) (P)Lathe finishing cycle @@ -100,6 +105,8 @@ tr.head td, tr.head th { background: black; color: white; } G43 H Tool Length Offset G43.1 Dynamic Tool Length Offset G43.2 H Apply additional Tool Length Offset + G43.4 (H) Tool Length Offset on Primary Kinematics + G43.5 (H) Tool Length Offset, Tool Axis as a Vector G49 Cancel Tool Length Compensation Stopping @@ -136,6 +143,10 @@ tr.head td, tr.head th { background: black; color: white; } M52P0 (off) or P1 (on)Adaptive Feed Control M53P0 (off) or P1 (on)Feed Stop Control G54-G59.3Select Coordinate System + G68.2, G68.4P Q I J K RTilted Work Plane + G68.3(R)Tilted Work Plane from the Tool + G69Cancel Tilted Work Plane + G12.1, G13.1PSelect Kinematics Flow-control Codes o subSubroutines, sub/endsub call @@ -167,6 +178,7 @@ tr.head td, tr.head th { background: black; color: white; } G28, G28.1Go/Set Predefined Position G28.2PHome from G-code G30, G30.1Go/Set Predefined Position + G28.5, G30.5Go to Predefined Machine Frame Position G53Move in Machine Coordinates G52, G92Coordinate System Offset G92.1, G92.2Reset G92 Offsets diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 20007307b18..7469bbc6c2a 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,7 +74,7 @@ as the 'L number', and so on for any other letter. |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position -|<> |Go to Predefined Slide Position +|<> |Go to Predefined Machine Frame Position |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -1199,16 +1199,16 @@ G30 Z2.5 (rapid to Z2.5 then to the Z location specified in #5183) The 'absolute' position is the machine coordinate system of the kinematics in force, see <>; <> is the form that moves -the slides, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and 'G30.1' are -'G30.5' and its store whenever the kinematics in force is not the machine frame -kinematics. +in the machine frame, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and +'G30.1' are 'G30.5' and its store whenever the kinematics in force is not the +machine frame kinematics. It is an error if : * Cutter Compensation is turned on [[gcode:g28.5]] -== G28.5, G30.5 Go to Predefined Slide Position(((G28.5 Go to Predefined Slide Position))) +== G28.5, G30.5 Go to Predefined Machine Frame Position(((G28.5 Go to Predefined Machine Frame Position))) [source,ngc] ---- diff --git a/docs/src/remap/remap.adoc b/docs/src/remap/remap.adoc index 75e2a0980fa..02c225dda19 100644 --- a/docs/src/remap/remap.adoc +++ b/docs/src/remap/remap.adoc @@ -1829,8 +1829,8 @@ All the listed G-codes are already defined in the current implementation of Linu |# |Gxx |Gxx.1 |Gxx.2 |Gxx.3 |Gxx.4 |Gxx.5 |Gxx.6 |Gxx.7 |Gxx.8 |Gxx.9 |10 |G10 | | | | | | | | | |11 | | | | | | | | | | -|12 | | | | | | | | | | -|13 | | | | | | | | | | +|12 | |G12.1 | | | | | | | | +|13 | |G13.1 | | | | | | | | |14 | | | | | | | | | | |15 | | | | | | | | | | |16 | | | | | | | | | | @@ -1851,7 +1851,7 @@ All the listed G-codes are already defined in the current implementation of Linu |25 | | | | | | | | | | |26 | | | | | | | | | | |27 | | | | | | | | | | -|28 |G28 |G28.1 |G28.2 | | | | | | | +|28 |G28 |G28.1 |G28.2 | | |G28.5 | | | | |29 | | | | | | | | | | |=== @@ -1859,10 +1859,10 @@ All the listed G-codes are already defined in the current implementation of Linu [width="90%",align="center",options="header,strong,unbreakable",cols="1*2^em,10*1. +""" + +G13_1 = """G13.1 Select Machine Frame Kinematics +Cancels back to the kinematics the module declares +the machine frame: the identity on a machine whose +slides line up with its frame, the arm kinematics +with no tool on a robot. Which number that is, the +module declares, so G13.1 means the same whatever +order the module lists its types in. A queue +synchronisation point like G12.1. +""" + G17 = """G17 Plane Select G17 = XY Plane """ @@ -492,6 +564,28 @@ def gcode_descriptions(gcode): or if Pn names a joint the machine does not have. """ +G28_5 = """G28.5 Go to Predefined Machine Frame Position +G28.5 + +G28.5 is to G28 what G53.5 is to G53: the position +stored in #5161-#5169 is read in the machine frame +with the orientation left out, the carriage or the +flange in machine coordinates, and the move is a +point-to-point move there, rapid, with no offset, +tool length, rotation or work plane applied. With +axes, those letters go to the given machine frame +position first, then to their stored positions. +Store the position with G28.1 while the machine +frame kinematics is in force (G13.1). +""" + +G30_5 = """G30.5 Go to Predefined Machine Frame Position +G30.5 + +As G28.5, with the position stored in #5181-#5189 +by G30.1. +""" + G30 = """G30 Go to Predefined Position G30 uses the values stored in parameters 5181-5189 as the X Y Z A B C U V W final point to move to. @@ -698,6 +792,37 @@ def gcode_descriptions(gcode): location. """ +G43_4 = """G43.4 Tool Length Offset on Primary Kinematics +G43.4 Hn +n = tool number, Hn is optional + +G43 together with a switch to the kinematics the +module declares its working transform, tool centre +point control, so the program runs with tool length +compensation in that kinematics. The switch happens +first, the offset applies after it. G49 cancels the +offset and switches back to the machine frame +kinematics. Rejected on a switchable module that +declares no primary kinematics; a plain G43 on a +machine that does not switch. +""" + +G43_5 = """G43.5 Tool Length Offset with the Tool Axis as a Vector +G43.5 Hn +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- + +G43.4 with one thing more: while it is in effect a +G0 or G1 line may give the tool axis direction as a +vector I J K, from the tip towards the holder, in +place of rotary words, and the interpreter works +out where the rotaries have to go. The pose nearest +where the rotaries stand is taken. G2 and G3 keep +I J K as the arc centre. It is an error to give +I J K with a rotary word, or a zero vector, or on +the identity kinematics. +""" + G49 = """G49 Cancel Tool Length Compensation """ @@ -719,6 +844,101 @@ def gcode_descriptions(gcode): line if one is currently active. """ +G53_1 = """G53.1 Orient the Tool to the Work Plane +G53.1 + +Turns the rotaries until the tool axis is normal to +the active tilted work plane (G68.2). The linear +joints stay where they are and the tip swings with +them: a point-to-point move. P picks among the +poses that reach the plane, nearest first: P0 (the +default) the nearest, P1 and P2 the secondary +rotary positive or negative. Q0 (the default) holds +the joints that carry the work, Q1 frees them. An +error with no plane active, or on the identity +kinematics. +""" + +G53_2 = """G53.2 Solve the Tool Orientation without Moving +G53.2 + +Solves the same pose as G53.1 and moves nothing. +The pose is published on #<_orient_x> to +#<_orient_c> and on #5071 to #5079, in program +units in the plane, with #<_orient_valid> (#5080) +set to 1, so the program can reach it with a move +of its own, for instance a single G0 naming X Y Z +and the rotary words together. P and Q as for +G53.1. +""" + +G53_3 = """G53.3 Orient the Tool and Move in the Work Plane +G53.3 X- Y- Z- + +Turns the rotaries as G53.1 does and takes the tool +to X Y Z, given in the plane, in one point-to-point +move. A word left out keeps the present value. P +and Q as for G53.1. +""" + +G53_4 = """G53.4 Point-to-Point Move in Program Coordinates +G53.4 G0 +G53.4 G1 F- + +A point-to-point move defines its two ends and +leaves the path to the joints: the inverse +kinematics runs once at the destination and every +joint travels from where it is to where it must be, +all together, the slowest setting the pace. The +tool does not follow a straight line. G53.4 takes +the destination in program coordinates, through +the offsets and the work plane like any other +move: the move to cross a singularity or turn a +head right round without leaving the coordinate +system. Non-modal, on a line with G0 or G1 in +force; with G1 it takes the time the straight move +would at F. +""" + +G53_5 = """G53.5 Point-to-Point Move in the Machine Frame +G53.5 G0 +G53.5 G1 F- + +A point-to-point move (see G53.4) to a position in +the machine frame with the tool left out: X Y Z the +point the tool hangs from, the pivot of a head or +the flange of a robot, in machine coordinates, the +rotary letters the orientation as the machine frame +kinematics reports it, on a mill the rotary joints. +Program units, no offset, tool length, rotation or +work plane. A letter not given holds its machine +frame coordinate. The form for parking and tool +change positions, whatever the head is doing. +Absolute distance mode only. +""" + +G53_6 = """G53.6 Orient the Tool, Holding the Tool Centre Point +G53.6 + +Turns the rotaries until the tool is normal to the +active work plane, as G53.1, keeping the tool +centre point where it is: a Cartesian move of the +rotary words, the kinematics compensating the +linear joints all along. P and Q as for G53.1. +""" + +G53_7 = """G53.7 Point-to-Point Move to Joint Values +G53.7 G0 J= ... +G53.7 G1 F- J= ... + +A point-to-point move (see G53.4) to joint values, +one word per joint: J2=-5 sends joint 2 to -5 in +the joint's own units, nothing converted, no offset +of any kind. A joint not named holds. The form that +works on every machine, a robot included. Absolute +distance mode only; the J= word is read only here. +""" + G54 = """G54 Select Coordinate System G54 = select coordinate system 1 """ @@ -788,6 +1008,51 @@ def gcode_descriptions(gcode): linear move. """ +G68_2 = """G68.2 Tilted Work Plane +G68.2 X- Y- Z- I- J- K- (Euler angles) +G68.2 P1 X- Y- Z- I- J- K- (angles about fixed axes) +G68.2 P2 Q0..Q3 X- Y- Z- (three points, one per block) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: origin and +X,) +G68.2 P3 Q2 I- J- K- (then +Z, the normal) + +Defines a plane on top of the active coordinate +system, with its own cancel (G69), so the blocks +that follow are programmed in the tilted plane +while G54 itself is untouched. X Y Z is the plane's +origin, the rest of the words its rotation, both in +the coordinate system active when the plane is +defined. P selects the form, numbered as Fanuc +numbers them; Q names the axis order of the angle +forms (Q313 and Q123 the defaults) or the block of +the point and vector forms. R turns the plane about +its own Z after everything else. The plane does not +move the tool: G53.1, G53.3 or G53.6 orient it. +""" + +G68_3 = """G68.3 Tilted Work Plane from the Tool Direction +G68.3 X- Y- Z- + +Defines a plane whose Z is the tool axis as the +rotary joints stand now, with the origin at X Y Z +and R turning it about that Z. An error where the +kinematics cannot be evaluated by the interpreter, +is the identity, or reports no tool frame. +""" + +G68_4 = """G68.4 Tilted Work Plane on the Active Plane +G68.4 (any G68.2 form) + +Takes any G68.2 form and composes it onto the plane +already active, so the new plane is given in the +coordinates of the old one. An error with no plane +active. +""" + +G69 = """G69 Cancel Tilted Work Plane +Cancels the plane. So does the end of the program, +M2 or M30, and an abort. +""" + G73 = """G73 Drilling Cycle with Chip Breaking G73 X Y Z R Q L R = retract position along the Z axis From dfc7402211676c4bd9b4d8450efdd75c5d090142 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:39:07 +1000 Subject: [PATCH 06/44] kinematics_user: a Jacobian entry at joints the caller holds kinematicsUserJacobian() runs the module's inverse at the pose before differentiating, to be on the caller's solution branch. The segment sampler in canon has just run that inverse itself at every sample, so each sample paid for it twice. kinematicsUserJacobianAt() takes the joints the caller holds and differentiates there; the old entry inverts and then calls it. segmentCap() uses the new one, and its answers are unchanged. --- include/kinematics_user.h | 13 ++++++++++++ .../kinematics_userspace/kinematics_user.c | 20 +++++++++++++++++-- src/emc/motion_planning/segment_cap.cc | 2 +- 3 files changed, 32 insertions(+), 3 deletions(-) diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 8ee1891c065..6052f9e51ff 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -144,6 +144,19 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, const EmcPose* world, double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); +/** + * The same at joints the caller already holds for the pose, from its own + * kinematicsUserInverse(): the derivative is taken there and no inverse is + * run first. What a caller sampling a path wants, having just inverted + * each sample. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + /** * The parameter block as it stands, refreshed from HAL first. For * reporting; the block belongs to the context. diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 24e37b47d52..a734fbcc467 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -532,9 +532,7 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, { KINEMATICS_INVERSE_FLAGS iflags = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; double j[EMCMOT_MAX_JOINTS]; - int r, a; if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; @@ -546,6 +544,24 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, world, j, &iflags, &fflags) != 0) { return -1; } + return kinematicsUserJacobianAt(ctx, j, world, J); +} + +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS] = {0}; + int r, a; + + if (!ctx || !ctx->initialized || !joints || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) j[r] = joints[r]; if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, jac, &iflags) != 0) { return -1; diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc index 5c14539226c..ec4cf00faf0 100644 --- a/src/emc/motion_planning/segment_cap.cc +++ b/src/emc/motion_planning/segment_cap.cc @@ -83,7 +83,7 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, whole segment stays on one solution branch, then the Jacobian on that branch */ if (kinematicsUserInverse(ctx, &p, joints) != 0 - || kinematicsUserJacobian(ctx, &p, J) != 0) { + || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { out->unanswered++; continue; } From bbf4e4a85b482d55b748655368ed149e4060b962 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 29 Oct 2026 13:30:57 +1100 Subject: [PATCH 07/44] maxkins: read the [AXIS_L] box on the carriage The box is the machine frame envelope, read on the forward of the type a module flags as its machine frame. A module with one type, built on kins_single.c, flags none, and kept the box on the world of the type in force. On maxkins that is the tool tip, so the box moved with the tool length and the head: with the head laid over a move whose carriage stayed inside the box was refused, and one that drove the carriage past it was accepted, the case the box moved to the machine frame for. A type may now say its joints are its slides, each in the letter the joint map gives it, and kins_single.c then answers the machine frame with the joints read through the map. maxkins says so. The other single-type modules keep the world of the type in force: a delta's or a tripod's joints are not slides along X Y Z, and corexy's are motors that drive them together. tests/kins-limits/box runs maxkins as well, with a 250 pivot, a 50 tool and the head laid over to B90: the tip past the box with the carriage inside it is accepted, the tip inside with the carriage past it refused, G53.5 boxed on the carriage. Without the flag five of the six checks fail. --- docs/src/config/ini-config.adoc | 2 +- include/kinematics.h | 8 +- include/kins_module.h | 7 +- src/emc/kinematics/kins_single.c | 9 +- src/emc/kinematics/maxkins.c | 1 + tests/kins-limits/box/README | 5 + tests/kins-limits/box/checkresult | 4 +- tests/kins-limits/box/maxkins.hal | 19 ++++ tests/kins-limits/box/maxkins.ini | 180 ++++++++++++++++++++++++++++++ tests/kins-limits/box/test-ui.py | 69 ++++++++---- tests/kins-limits/box/test.sh | 9 +- 11 files changed, 277 insertions(+), 36 deletions(-) create mode 100644 tests/kins-limits/box/maxkins.hal create mode 100644 tests/kins-limits/box/maxkins.ini diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 477363b02a2..e8aabf083d6 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1059,7 +1059,7 @@ See the <> chapter, INI file li `MIN_LIMIT` and `MAX_LIMIT` together are a box in the machine frame: the carriage, or the flange of a robot, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. On a kinematics module that declares its machine frame type (see the <> chapter) every move is checked against the box where its joints put the machine frame, whatever kinematics type is in force: a tool centre point move whose tip lies past the box with the carriage inside it is accepted, and a 'G53.5' to a carriage position past the box is refused. The travel itself is the `[JOINT_N]` limits; the box is a restriction the configuration adds inside it. -A module that declares no machine frame type keeps the box on the world of the kinematics in force. +A module with one kinematics type whose joints are its slides, maxkins, reads the box on its joints, each in its letter. A module that declares no machine frame type and whose joints are not its slides keeps the box on the world of the kinematics in force. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. It is ignored on a `LINEAR` axis. diff --git a/include/kinematics.h b/include/kinematics.h index 34160ce3b11..850ada96802 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -146,9 +146,11 @@ extern int kinematicsTypeFlags(int ktype); /* The joints placed in the machine frame: the forward of the type ** flagged KINSTYPE_MACHINE, whatever type is in force, so that a limit ** on the frame can be read for a move under any type. pos carries the -** caller's estimate in for an iterative forward. Returns 0, -1 where -** no type declares the machine frame (a consumer then falls back on the -** world of the type in force), or -2 where the forward fails. */ +** caller's estimate in for an iterative forward. A module with one type +** whose joints are its slides answers the joints through its map. +** Returns 0, -1 where the module has no machine frame (a consumer then +** falls back on the world of the type in force), or -2 where the forward +** fails. */ extern int kinematicsMachineFrame(const double *joint, struct EmcPose *pos); /* These two give the orientation of the tool and of the workpiece for a set diff --git a/include/kins_module.h b/include/kins_module.h index 63b4f04ad8b..8443d2e621a 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -346,7 +346,11 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, module that leaves it unset on every type has its identity type stand in. A machine frame type does not read the tool offset in the parameter block, so a working transform that leaves the tool out anyway, a robot's flange, - carries primary and machine both. */ + carries primary and machine both. slides says the joints are the + machine's slides and rotaries, each in the letter the joint map gives + it, so that the carriage is the joints read through the map: the + machine frame of a module with one type, which has no machine frame + type to name. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -358,6 +362,7 @@ typedef struct kins_ops { int identity; /* joints are axes */ int primary; /* the working transform */ int machine; /* the machine frame */ + int slides; /* joints are the slides, letters by the map */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 6ff60667663..fb08407b951 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -150,11 +150,14 @@ int kinematicsTypeFlags(int ktype) return -1; } -// and so no machine frame type either +// and so no machine frame type either: the carriage is the joints, where +// they are the slides, and otherwise there is no machine frame to read int kinematicsMachineFrame(const double *joint, EmcPose *pos) { - (void)joint; - (void)pos; + if (!inited || !joint || !pos) { return -1; } + if (ops()->slides) { + return kinsMappedJointsToPose(&rt_params, joint, pos) == 0 ? 0 : -2; + } return -1; } diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 186873a014a..cb0cd653d13 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -214,6 +214,7 @@ static const kins_ops max_ops = { .tool = max_tool_frame, .native = &TOOL_FRAME_SPINDLE, .jacobian = max_jacobian, + .slides = 1, }; // joints 0..8 are X..W in order, always; the entry points come from diff --git a/tests/kins-limits/box/README b/tests/kins-limits/box/README index e00646f7457..91b37563b61 100644 --- a/tests/kins-limits/box/README +++ b/tests/kins-limits/box/README @@ -10,3 +10,8 @@ carriage position past the box is refused, and to one inside it with the tip past the box is accepted. A carriage past the joint limit is refused whatever the box says. Nothing is asserted about the identity kinematics, where the box and the joints describe the same thing. + +maxkins, one type, its joints the slides X to W: the same with a 250 pivot +and a 50 tool, the head laid over to B90 by a move that keeps the tip. The +module has no machine frame type to name, and reads the box on its joints +through the map. diff --git a/tests/kins-limits/box/checkresult b/tests/kins-limits/box/checkresult index 9d48d3f180e..8f4f3f58ba7 100755 --- a/tests/kins-limits/box/checkresult +++ b/tests/kins-limits/box/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -# the test script counts its own failures -grep -q "^Exiting with 0 errors" "$1" +# the test script counts its own failures, once per machine +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/kins-limits/box/maxkins.hal b/tests/kins-limits/box/maxkins.hal new file mode 100644 index 00000000000..551dc8d422b --- /dev/null +++ b/tests/kins-limits/box/maxkins.hal @@ -0,0 +1,19 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb +net J6 joint.6.motor-pos-cmd => joint.6.motor-pos-fb +net J7 joint.7.motor-pos-cmd => joint.7.motor-pos-fb +net J8 joint.8.motor-pos-cmd => joint.8.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/box/maxkins.ini b/tests/kins-limits/box/maxkins.ini new file mode 100644 index 00000000000..04ce1e06a02 --- /dev/null +++ b/tests/kins-limits/box/maxkins.ini @@ -0,0 +1,180 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# one type, the tool centre point, its joints the slides X to W in order; +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = maxkins +JOINTS = 9 + +[HAL] +HALFILE = maxkins.hal +HALCMD = setp maxkins.pivot-length 250 + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZABCUVW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 2000 +MAX_LINEAR_VELOCITY = 3460 +MAX_LINEAR_ACCELERATION = 80000 +DEFAULT_LINEAR_ACCELERATION = 80000 +MAX_ANGULAR_VELOCITY = 3600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_A] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 + +[AXIS_U] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_V] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 600 +MAX_ACCELERATION = 20000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_6] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_7] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_8] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 80000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/test-ui.py b/tests/kins-limits/box/test-ui.py index 7b80555ecf9..6fb0e193b51 100755 --- a/tests/kins-limits/box/test-ui.py +++ b/tests/kins-limits/box/test-ui.py @@ -5,7 +5,9 @@ import sys import time -JOINTS = 6 +ini = linuxcnc.ini(sys.argv[2]) +MAX = ini.find("KINS", "KINEMATICS") == "maxkins" +JOINTS = int(ini.find("KINS", "JOINTS")) c = linuxcnc.command() s = linuxcnc.stat() @@ -89,29 +91,50 @@ def refused(cmd, needle): c.wait_complete(30) drain() -# the tool centre point kinematics with the tool on, the head laid over by -# a joint move: the tip is now 300 from the carriage along +X at C0 -mdi("G12.1 P0") -mdi("G43 H1") -mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") -j = joints() -s.poll() -print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) -if abs(s.position[0] - j[0] - 300) > 1e-3: - error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) +if MAX: + # maxkins has one type, the tool centre point, its joints the slides: + # with the tool on, the head laid over to B90 by a move that keeps the + # tip, which leaves the carriage 300 from the tip along X + mdi("G43 H1") + mdi("G0 X0 Y0 Z0 B90") + j = joints() + s.poll() + d = s.position[0] - j[0] + print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) + if abs(abs(d) - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % d) + side = 1 if d > 0 else -1 + accepted("G0 X%g" % (700 * side), 400 * side) + refused("G0 X%g" % (-300 * side), "limit") + refused("G0 X%g" % (-800 * side), "limit") + refused("G53.5 G0 X%g" % (-600 * side), "limit") + accepted("G53.5 G0 X%g" % (400 * side), 400 * side) + mdi("G0 X0 B0") + mdi("G49") +else: + # the tool centre point kinematics with the tool on, the head laid over by + # a joint move: the tip is now 300 from the carriage along +X at C0 + mdi("G12.1 P0") + mdi("G43 H1") + mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") + j = joints() + s.poll() + print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) + if abs(s.position[0] - j[0] - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) -# the tip past the box, the carriage inside it: accepted -accepted("G0 X600", 300) -# the tip inside the box, the carriage past it: refused -refused("G0 X-300", "limit") -# the carriage past the joint limit: refused whatever the box says -refused("G0 X-800", "limit") -# G53.5 names the carriage: past the box refused, inside it accepted even -# with the tip past the box -refused("G53.5 G0 X-600", "limit") -accepted("G53.5 G0 X400", 400) + # the tip past the box, the carriage inside it: accepted + accepted("G0 X600", 300) + # the tip inside the box, the carriage past it: refused + refused("G0 X-300", "limit") + # the carriage past the joint limit: refused whatever the box says + refused("G0 X-800", "limit") + # G53.5 names the carriage: past the box refused, inside it accepted even + # with the tip past the box + refused("G53.5 G0 X-600", "limit") + accepted("G53.5 G0 X400", 400) -mdi("G53.7 G0 J0=0 J3=0") -mdi("G49") + mdi("G53.7 G0 J0=0 J3=0") + mdi("G49") print("Exiting with %d errors" % errors) sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/box/test.sh b/tests/kins-limits/box/test.sh index 765cf14fed6..e74944010c9 100755 --- a/tests/kins-limits/box/test.sh +++ b/tests/kins-limits/box/test.sh @@ -1,4 +1,7 @@ #!/bin/bash -e -# a failed run leaves the var file behind, and it carries offsets -rm -f sim.var sim.var.bak -linuxcnc -r test.ini +for ini in test.ini maxkins.ini; do + # a failed run leaves the var file behind, and it carries offsets + rm -f sim.var sim.var.bak + echo "=== $ini" + linuxcnc -r $ini +done From 622b811b2922a4fdbfd8d1416a67fffa5cedc082 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 9 Nov 2026 18:18:23 +1100 Subject: [PATCH 08/44] motion: slow a world jog to rest where a joint reaches its soft limit A world jog is planned per axis against the [AXIS_L] limits. Under the tool centre point a jog of a rotary moves the slides through the inverse, and nothing looked at their travel until a joint had crossed it: motion then reported the soft limit, stopped the jog hard and left the machine on the limit, to be jogged off in joint mode (reported by Sigma1912). teleop_joint_cap() already reads, every servo cycle, how fast each joint moves per unit of the jog and caps the planners at the joints' velocity and acceleration limits. It now also caps them at what lets a joint heading for a soft limit stop at it, at half the acceleration the planners are left, and go no further than it in one cycle. The room is read from where the planners have taken the joint, the latest inverse, not from the interpolated command, which follows a few cycles behind and let the carriage run on past the limit. The jog stops a ten-millionth of the joint's range short, for the curve of the path over the last step, and a jog that has come to rest there ends, as a jog of a slide ends at its axis limit, with no error; a jog the other way moves off it. Siemens behaves the same way in JOG: the axis stops at its software limit switch (alarm 10621, axis rests on software limit switch) and is jogged off without a mode change, and under a transformation the axis that stops can be another than the one jogged. tests/kins-limits/jog-travel is the jog test's machine with joint 1 given 300 mm each way, so a jog of C with the tip held swings the carriage out of its travel 48.6 degrees round: a continuous jog held well past that, an incremental jog away, one of 60 degrees back towards the limit and one away again. Before this the carriage ran 14.5 mm past its limit, the soft limit error stopped the jog, the hard stop asked joint 0 for 93000 mm/s^2 and the machine went off; with it each jog into the limit rests within 1 micron inside it, says nothing, and keeps every joint within its velocity and acceleration limits, and the jogs away land where they were sent. --- docs/src/motion/switchkins.adoc | 5 +- src/emc/motion/control.c | 35 +++- tests/kins-limits/jog-travel/README | 15 ++ tests/kins-limits/jog-travel/checkresult | 3 + tests/kins-limits/jog-travel/sim.hal | 58 ++++++ tests/kins-limits/jog-travel/test-ui.py | 226 +++++++++++++++++++++++ tests/kins-limits/jog-travel/test.ini | 146 +++++++++++++++ tests/kins-limits/jog-travel/test.sh | 4 + tests/kins-limits/jog-travel/tool.tbl | 1 + 9 files changed, 490 insertions(+), 3 deletions(-) create mode 100644 tests/kins-limits/jog-travel/README create mode 100755 tests/kins-limits/jog-travel/checkresult create mode 100644 tests/kins-limits/jog-travel/sim.hal create mode 100755 tests/kins-limits/jog-travel/test-ui.py create mode 100644 tests/kins-limits/jog-travel/test.ini create mode 100755 tests/kins-limits/jog-travel/test.sh create mode 100644 tests/kins-limits/jog-travel/tool.tbl diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 970674847b9..f12c25b527b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -329,7 +329,10 @@ jog is bounded the same way, in motion, every servo cycle: the Jacobian at the joints the machine stands in caps the jog's planners so that no joint is asked for more than its limits, and a jog of C under the tool centre point with the head laid over runs at the pace of the carriage -it swings. +it swings. The cap reads the joints' travel too: where the carriage +would run onto its `[JOINT_N]` soft limit the jog slows and comes to rest +against it, as a jog of a slide does at its axis limit, with no error, +and a jog the other way moves off it. So a switch does not need the velocity and acceleration limits reset to keep the joints within their limits. It may still want them reset diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index d7e6f3a9522..b7403cd8345 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1374,9 +1374,9 @@ static void teleop_joint_cap(double period) double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; int active[EMCMOT_MAX_AXIS]; double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; - double vel_scale, acc_scale; + double vel_scale, acc_scale, stop_acc; const double tiny = 1e-12; - int flags, any = 0, a, j; + int flags, any = 0, a, j, pinned = 0; if (!kinematicsJacobian) { have_prev = 0; return; } flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; @@ -1445,6 +1445,36 @@ static void teleop_joint_cap(double period) if (cap < acap) { acap = cap; } } } + /* the soft limits: a joint heading for one must still be able to stop + at it, at half the acceleration the planners are left, and gets no + further than it in one cycle. A jog that has come to rest against + one ends there, as a jog of a slide ends at its axis limit, and a + jog away from it is free. The room is read from where the planners + have taken the joint, not from the interpolated command that follows + it a few cycles behind, and a ten-millionth of the joint's range + short, for the curve of the path over the last step; within one + more the jog is at rest there */ + stop_acc = 0.5 * (acap < anorm ? acap : anorm); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, room, margin, cap; + if (!GET_JOINT_ACTIVE_FLAG(&joints[j]) || !get_homed(j)) { continue; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { rate += jac[j][a] * rhat[a]; } + if (fabs(rate) < tiny) { continue; } + room = rate > 0.0 ? joints[j].max_pos_limit - joints[j].coarse_pos + : joints[j].coarse_pos - joints[j].min_pos_limit; + margin = 1e-7 * (joints[j].max_pos_limit - joints[j].min_pos_limit); + room -= margin; + if (room <= margin) { + cap = 0.0; + } else { + cap = sqrt(2.0 * stop_acc * room / fabs(rate)); + if (room / (fabs(rate) * period) < cap) { cap = room / (fabs(rate) * period); } + } + if (cap < vcap) { + vcap = cap; + pinned = cap < tiny; + } + } vel_scale = vcap / rnorm; acc_scale = acap / anorm; if (vel_scale > 1.0) { vel_scale = 1.0; } @@ -1453,6 +1483,7 @@ static void teleop_joint_cap(double period) for (a = 0; a < EMCMOT_MAX_AXIS; a++) { if (active[a]) { axis_teleop_cap(a, vel_scale * vreq[a], acc_scale * areq[a]); } } + if (pinned && speed < tiny) { axis_jog_abort_all(0); } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } } diff --git a/tests/kins-limits/jog-travel/README b/tests/kins-limits/jog-travel/README new file mode 100644 index 00000000000..4a79b847ba6 --- /dev/null +++ b/tests/kins-limits/jog-travel/README @@ -0,0 +1,15 @@ +A world jog under the tool centre point slows to a stop where a joint +reaches its soft limit, as a jog of a slide does at its axis limit, and a +jog away from the limit is free. + +The jog test's machine (5axiskins, 400 mm pivot, the head laid over to +B90 by a joint move, the joints following through limit3) with joint 1 +given 300 mm of travel each way, so a jog of C with the tip held swings +the carriage out of it about 48.6 degrees round. In teleop mode: a +continuous jog of C held well past that, an incremental jog of 20 degrees +away, an incremental jog of 60 back towards the limit, and one away again. +The test asserts that each jog into the limit comes to rest with joint 1 +within 1 micron inside its limit and never past it, that the jog has +ended there and says nothing (no soft limit error, no following error), +that the jogs away land where they were sent, and that no joint's +commanded velocity or acceleration went over its INI limit. diff --git a/tests/kins-limits/jog-travel/checkresult b/tests/kins-limits/jog-travel/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog-travel/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog-travel/sim.hal b/tests/kins-limits/jog-travel/sim.hal new file mode 100644 index 00000000000..7c4e3727d8b --- /dev/null +++ b/tests/kins-limits/jog-travel/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 1920 +setp limit3.0.maxa 61440 +setp limit3.1.maxv 1920 +setp limit3.1.maxa 61440 +setp limit3.2.maxv 1920 +setp limit3.2.maxa 61440 +setp limit3.3.maxv 576 +setp limit3.3.maxa 15360 +setp limit3.4.maxv 576 +setp limit3.4.maxa 15360 +setp limit3.5.maxv 1920 +setp limit3.5.maxa 61440 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/jog-travel/test-ui.py b/tests/kins-limits/jog-travel/test-ui.py new file mode 100755 index 00000000000..1d186a69eae --- /dev/null +++ b/tests/kins-limits/jog-travel/test-ui.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point slows to a stop where a joint +# reaches its soft limit, and a jog away from it is free: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + catch_up(end) + said = drain() + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + # a jog that stops at a joint's limit says nothing: no soft limit + # error, no following error + if said: + error("%s said: %s" % (what, said[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + + +Y = 1 +YMAX = float(ini.find("JOINT_1", "MAX_LIMIT")) +YMIN = float(ini.find("JOINT_1", "MIN_LIMIT")) + +def at_limit(what, end): + # the carriage came to rest against its travel, not short of it and + # not past it + room = min(YMAX - end[Y], end[Y] - YMIN) + print("%s: joint 1 at %.6f, %.6f from its limit, C %.3f" % (what, end[Y], room, end[4])) + if room < 0: + error("%s: joint 1 at %.6f is past its limit" % (what, end[Y])) + elif room > 1e-3: + error("%s: joint 1 stopped %.6f short of its limit" % (what, room)) + +def jog_ended(what): + s.poll() + if any(s.axis[a]["velocity"] != 0 for a in range(9)): + error("%s: an axis is still moving" % what) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move; +# the carriage at X-400 puts the tip at X0, the centre of the circle a jog +# of C swings the carriage on. Joint 1 has 300 mm of travel each way, so +# the circle leaves it about 48.6 degrees round +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +drain() +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C held well past where joint 1 runs out: the jog +# slows and comes to rest with the carriage at its limit, and ends +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 480) +deadline = time.time() + 60 +moved = False +while time.time() < deadline: + s.poll() + if abs(s.position[C]) > 10: + moved = True + # held until the jog has come to rest with C well round, or a limit + # error has stopped it + if moved and s.axis[C]["velocity"] == 0: + break + if s.task_state != linuxcnc.STATE_ON: + break + time.sleep(0.01) +else: + error("the continuous jog of C did not come to rest") +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C into the limit", n0) +at_limit("the continuous jog of C", end) +tip_held("the continuous jog of C") +jog_ended("the continuous jog of C") +stop_c = end[4] + +# away from the limit by 20 degrees: free, and it lands where it was sent +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, -20 if stop_c > 0 else 20) +end = checked("the incremental jog of C away", n0) +want = stop_c - 20 if stop_c > 0 else stop_c + 20 +if abs(end[4] - want) > 1e-3: + error("C ended at %.3f, not %.3f, jogging away from the limit" % (end[4], want)) + +# an incremental jog of 60 degrees back towards it: it stops at the +# limit, short of where it was sent, and ends there +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, 60 if stop_c > 0 else -60) +end = checked("the incremental jog of C into the limit", n0) +at_limit("the incremental jog of C", end) +tip_held("the incremental jog of C") +jog_ended("the incremental jog of C") + +# and a jog away works straight after it +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, C, 480, -10 if stop_c > 0 else 10) +again = checked("the jog away after the incremental one", n0) +want = end[4] - 10 if stop_c > 0 else end[4] + 10 +if abs(again[4] - want) > 1e-3: + error("C ended at %.3f, not %.3f, jogging away again" % (again[4], want)) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog-travel/test.ini b/tests/kins-limits/jog-travel/test.ini new file mode 100644 index 00000000000..3a8e4293456 --- /dev/null +++ b/tests/kins-limits/jog-travel/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# every rate is eight times a real machine's, the accelerations 64 times, +# so the same moves take an eighth of the time +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 1600 +MAX_LINEAR_VELOCITY = 2768 +MAX_LINEAR_ACCELERATION = 51200 +DEFAULT_LINEAR_ACCELERATION = 51200 +MAX_ANGULAR_VELOCITY = 2880 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -300 +MAX_LIMIT = 300 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 480 +MAX_ACCELERATION = 12800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 1600 +MAX_ACCELERATION = 51200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog-travel/test.sh b/tests/kins-limits/jog-travel/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog-travel/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog-travel/tool.tbl b/tests/kins-limits/jog-travel/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog-travel/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 37a12f175523f4508d138b232b4d1e3fc71bf788 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:32:53 +1000 Subject: [PATCH 09/44] The stack's own conversions follow [AXIS_] TYPE The code this stack adds on top of the interpreter and canon still took A B C for angles and X Y Z U V W for lengths: the machine pose and back (interp_workplane.cc), the tool offset handed to the kinematics, the point G43.4 and G49 re-anchor on, the words of G53.5, G53.7, G28.5 and G30.5, and the check that a letter on the identity kinematics names a joint of its own kind. They go by type now, as the rest does since "Honor [AXIS_] TYPE in the interpreter and canon". Canon's segment sampling counts the turn of every angular axis, not of A B C. --- src/emc/rs274ngc/interp_convert.cc | 12 +++---- src/emc/rs274ngc/interp_workplane.cc | 48 +++++++++++++--------------- src/emc/task/emccanon.cc | 7 +++- 3 files changed, 34 insertions(+), 33 deletions(-) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index f3a00c02e6a..582e51a5538 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6844,12 +6844,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu in_program.tran.x = USER_TO_PROGRAM_LEN(point.tran.x); in_program.tran.y = USER_TO_PROGRAM_LEN(point.tran.y); in_program.tran.z = USER_TO_PROGRAM_LEN(point.tran.z); - in_program.a = USER_TO_PROGRAM_ANG(point.a); - in_program.b = USER_TO_PROGRAM_ANG(point.b); - in_program.c = USER_TO_PROGRAM_ANG(point.c); - in_program.u = USER_TO_PROGRAM_LEN(point.u); - in_program.v = USER_TO_PROGRAM_LEN(point.v); - in_program.w = USER_TO_PROGRAM_LEN(point.w); + in_program.a = USER_TO_PROGRAM_AX(AXIS_A, point.a); + in_program.b = USER_TO_PROGRAM_AX(AXIS_B, point.b); + in_program.c = USER_TO_PROGRAM_AX(AXIS_C, point.c); + in_program.u = USER_TO_PROGRAM_AX(AXIS_U, point.u); + in_program.v = USER_TO_PROGRAM_AX(AXIS_V, point.v); + in_program.w = USER_TO_PROGRAM_AX(AXIS_W, point.w); USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); } else { double dx, dy, dz; diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index d1e626df12f..88b552a4749 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -637,12 +637,12 @@ void Interp::kins_set_tool(void *vctx, const EmcPose *offset) tool.tran.x = PROGRAM_TO_USER_LEN(offset->tran.x); tool.tran.y = PROGRAM_TO_USER_LEN(offset->tran.y); tool.tran.z = PROGRAM_TO_USER_LEN(offset->tran.z); - tool.a = PROGRAM_TO_USER_ANG(offset->a); - tool.b = PROGRAM_TO_USER_ANG(offset->b); - tool.c = PROGRAM_TO_USER_ANG(offset->c); - tool.u = PROGRAM_TO_USER_LEN(offset->u); - tool.v = PROGRAM_TO_USER_LEN(offset->v); - tool.w = PROGRAM_TO_USER_LEN(offset->w); + tool.a = PROGRAM_TO_USER_AX(AXIS_A, offset->a); + tool.b = PROGRAM_TO_USER_AX(AXIS_B, offset->b); + tool.c = PROGRAM_TO_USER_AX(AXIS_C, offset->c); + tool.u = PROGRAM_TO_USER_AX(AXIS_U, offset->u); + tool.v = PROGRAM_TO_USER_AX(AXIS_V, offset->v); + tool.w = PROGRAM_TO_USER_AX(AXIS_W, offset->w); kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); } @@ -668,12 +668,12 @@ void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) pose->tran.x = PROGRAM_TO_USER_LEN(abs_pos[0]); pose->tran.y = PROGRAM_TO_USER_LEN(abs_pos[1]); pose->tran.z = PROGRAM_TO_USER_LEN(abs_pos[2]); - pose->a = PROGRAM_TO_USER_ANG(abs_pos[3]); - pose->b = PROGRAM_TO_USER_ANG(abs_pos[4]); - pose->c = PROGRAM_TO_USER_ANG(abs_pos[5]); - pose->u = PROGRAM_TO_USER_LEN(abs_pos[6]); - pose->v = PROGRAM_TO_USER_LEN(abs_pos[7]); - pose->w = PROGRAM_TO_USER_LEN(abs_pos[8]); + pose->a = PROGRAM_TO_USER_AX(AXIS_A, abs_pos[3]); + pose->b = PROGRAM_TO_USER_AX(AXIS_B, abs_pos[4]); + pose->c = PROGRAM_TO_USER_AX(AXIS_C, abs_pos[5]); + pose->u = PROGRAM_TO_USER_AX(AXIS_U, abs_pos[6]); + pose->v = PROGRAM_TO_USER_AX(AXIS_V, abs_pos[7]); + pose->w = PROGRAM_TO_USER_AX(AXIS_W, abs_pos[8]); } // and back: a machine pose as program coordinates, through the chain @@ -684,12 +684,12 @@ void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, doubl USER_TO_PROGRAM_LEN(pose->tran.y), USER_TO_PROGRAM_LEN(pose->tran.z), &prog[0], &prog[1], &prog[2]); - prog[3] = USER_TO_PROGRAM_ANG(pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; - prog[4] = USER_TO_PROGRAM_ANG(pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; - prog[5] = USER_TO_PROGRAM_ANG(pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; - prog[6] = USER_TO_PROGRAM_LEN(pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; - prog[7] = USER_TO_PROGRAM_LEN(pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; - prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; + prog[3] = USER_TO_PROGRAM_AX(AXIS_A, pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_AX(AXIS_B, pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_AX(AXIS_C, pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_AX(AXIS_U, pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_AX(AXIS_V, pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_AX(AXIS_W, pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; } // whether two machine points are the same, to a hair either way @@ -1293,7 +1293,7 @@ int Interp::letters_name_joints(const char *name, setup_pointer s, const struct CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); + const int angular = axisKindsAngular(s->axis_kinds, a); for (j = 0; j < njoints; j++) { int turns; if (!(p->joints_of_axis[a] & (1 << j))) { continue; } @@ -1325,12 +1325,11 @@ int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const str return machine_frame_joints(name, s, ctx, flags, words, joints); } for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); double value; if (!flags[a]) { continue; } CHKS((p->joints_of_axis[a] == 0), _("%s: %c is not a joint of this kinematics"), name, letters[a]); - value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + value = PROGRAM_TO_USER_AX(a, words[a]); for (j = 0; j < njoints; j++) { if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } } @@ -1358,9 +1357,8 @@ int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, coord[3] = &pose.a; coord[4] = &pose.b; coord[5] = &pose.c; coord[6] = &pose.u; coord[7] = &pose.v; coord[8] = &pose.w; for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); if (!flags[a]) { continue; } - *coord[a] = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + *coord[a] = PROGRAM_TO_USER_AX(a, words[a]); } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; @@ -1505,9 +1503,7 @@ int Interp::convert_home_slides(int code, block_pointer block, setup_pointer s) CHP(current_joints(s, ctx, joints)); for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); - home[a] = angular ? USER_TO_PROGRAM_ANG(parameters[base + a]) - : USER_TO_PROGRAM_LEN(parameters[base + a]); + home[a] = USER_TO_PROGRAM_AX(a, parameters[base + a]); given += flags[a]; } diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index a7bff9e7d24..38da667a2e9 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1001,7 +1001,12 @@ static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) // position, or a sweep of arc is seen every few degrees or centimetres static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) { - double rot = fmax(fabs(end->a - start->a), fmax(fabs(end->b - start->b), fabs(end->c - start->c))); + const double turn[6] = {end->a - start->a, end->b - start->b, end->c - start->c, + end->u - start->u, end->v - start->v, end->w - start->w}; + double rot = 0.0; + for (int n = 0; n < 6; n++) { + if (AXIS_ANG(n + 3)) { rot = fmax(rot, fabs(turn[n])); } + } double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + pow(end->tran.z - start->tran.z, 2)); int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); From 499b114f3c687eb652ea91be0c6e0a72a80dca97 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:42:39 +1000 Subject: [PATCH 10/44] Kinematics types name the axes that orient the tool G43.5 wrote the pose it solved into A B C and refused A B C words beside I J K, whatever axes the machine orients with. A kinematics type now names them: kins_ops.orient gives the two letters, first rotation then second, and left NULL they are the letters the joint map gives the two joints that turn the frames, which is right for every module in the tree. kinematicsUserOrientAxes() answers for the active type, with head or table for each, from the frames: two heads or two tables in the order toolFrameOrientJoints() gives, a table and a head the table first. G43.5 writes the solution into those two letters and refuses a word of either beside I J K, and G53.3 and G53.6 turn those two letters where they turned A B C and left U V W behind; a type that orients otherwise, a robot's three pose angles, keeps A B C. The interpreter exposes them as #<_kins_orient_1>, whose type orients with an axis [AXIS_] TYPE makes LINEAR when it first loads it. --- docs/src/gcode/g-code.adoc | 9 +- docs/src/gcode/overview.adoc | 13 ++ docs/src/motion/kinematics-conventions.adoc | 8 +- include/kinematics_user.h | 13 ++ include/kins_module.h | 15 +- .../kinematics_userspace/kinematics_user.c | 67 +++++++++ src/emc/rs274ngc/interp_convert.cc | 3 +- src/emc/rs274ngc/interp_namedparams.cc | 28 ++++ src/emc/rs274ngc/interp_workplane.cc | 113 ++++++++++++--- src/emc/rs274ngc/rs274ngc_interp.hh | 4 +- tests/kins-orient-linear/README | 4 + tests/kins-orient-linear/checkresult | 3 + tests/kins-orient-linear/sim.hal | 16 +++ tests/kins-orient-linear/test-ui.py | 39 ++++++ tests/kins-orient-linear/test.ini | 131 ++++++++++++++++++ tests/kins-orient-linear/test.sh | 3 + tests/kins-orient-linear/tool.tbl | 1 + tests/kins-switch/test-ui.py | 46 ++++++ tests/remap/introspect/expected | 4 +- 19 files changed, 490 insertions(+), 30 deletions(-) create mode 100644 tests/kins-orient-linear/README create mode 100755 tests/kins-orient-linear/checkresult create mode 100644 tests/kins-orient-linear/sim.hal create mode 100755 tests/kins-orient-linear/test-ui.py create mode 100644 tests/kins-orient-linear/test.ini create mode 100755 tests/kins-orient-linear/test.sh create mode 100644 tests/kins-orient-linear/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 7469bbc6c2a..228d32701a2 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1927,8 +1927,10 @@ a rotary swings the long way round only where the short way runs out of travel. rotary, the tool vertical on a mill, one joint no longer matters to the direction and it stays where it is. -The pose the interpreter finds goes into the move as ordinary rotary -words, in program coordinates, so the rotary offsets of the coordinate +The pose the interpreter finds goes into the move as ordinary words of +the two axes the kinematics type orients the tool with, the ones +'#<_kins_orient_1>' and '#<_kins_orient_2>' name (A B C for a type that +orients with three angles, a robot's wrist), in program coordinates, so the rotary offsets of the coordinate system apply, and the move interpolates like any other move with rotary words: the rotaries turn together with the linear axes between the poses of consecutive lines. The vector itself is not interpolated @@ -1942,7 +1944,8 @@ offset and undoes the kinematics switch, as after 'G43.4'. It is an error if: -* 'I J K' are given together with a rotary word on the same line, +* 'I J K' are given together with a word of an orienting axis on the same line, +* the kinematics type orients the tool with an axis that is not rotary, * the vector is zero, * the kinematics selected is the identity one, or the module supplies no tool frame or cannot be evaluated by the interpreter, diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 887f3d48864..3a4b025571d 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -513,6 +513,19 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_kins_orient_1>', '#<_kins_orient_2>' - The axes the kinematics type + in force orients the tool with, as axis numbers, 0 for X to 8 for W: the + first rotation, the one whose axis stays put, then the second. A head + and a table give the table first. The type's module declares the letters, + or they are those its joint map gives the joints that turn. -1 where the + type does not orient with two rotaries, the identity type or a robot + among them, or the interpreter cannot evaluate the kinematics. An axis + named here must be 'ANGULAR' in the INI file; the interpreter refuses a + module whose type orients with a 'LINEAR' axis when it first loads it. + +* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>' - 1 where that axis + turns the head, 0 where it turns the table, -1 as above. + * '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last solved, in program units in the tilted work plane. '#<_orient_valid>' is diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 8f99f221407..922fe702a33 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -570,7 +570,13 @@ the no-transform type, joints are axes; `primary` the working transform `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable -Kinematics chapter). A module with +Kinematics chapter). A type may also name the two axis letters that orient +the tool, first rotation then second, in `orient`: "AC" for instance. Left +out, they are the letters the joint map gives the joints that turn the frames, +which is right for a module whose forward puts each joint in its own letter; +a module that writes an orienting joint into another letter declares them. +`G43.5` writes its solution into these letters, and the interpreter reads them +back as `#<_kins_orient_1>` and `#<_kins_orient_2>`. A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 6052f9e51ff..a0859a9e164 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -270,6 +270,19 @@ int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, int* joint); +/** + * The axes that orient the tool on the active type, as axis numbers, 0 X + * to 8 W: the first rotation then the second, and whether each turns the + * head (1) or the table (0). Two heads or two tables are ordered as + * kinematicsUserOrientJoints() orders them, the one whose axis stays put + * first; a table and a head, the table first. The letters are the type's + * declared orient, else those the joint map gives the joints. Returns 0, + * or -1 with all four set to -1 where the type turns other than two + * rotaries between tool and work, or a letter cannot be found. + */ +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[2], int head[2]); + /** * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the * HAL file hands it to loadrt: the module name first, then any of diff --git a/include/kins_module.h b/include/kins_module.h index 8443d2e621a..6bf95e358d5 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -346,11 +346,15 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, module that leaves it unset on every type has its identity type stand in. A machine frame type does not read the tool offset in the parameter block, so a working transform that leaves the tool out anyway, a robot's flange, - carries primary and machine both. slides says the joints are the - machine's slides and rotaries, each in the letter the joint map gives - it, so that the carriage is the joints read through the map: the - machine frame of a module with one type, which has no machine frame - type to name. */ + carries primary and machine both. orient names the two axis letters + that orient the tool, first rotation then second, "AC" for instance; left + NULL they are the letters the joint map gives the joints that turn the + frames, which is right for a module whose forward puts each joint in its + own letter, and a module that puts one elsewhere declares them. + slides says the joints are the machine's slides and rotaries, each in + the letter the joint map gives it, so that the carriage is the joints + read through the map: the machine frame of a module with one type, + which has no machine frame type to name. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -362,6 +366,7 @@ typedef struct kins_ops { int identity; /* joints are axes */ int primary; /* the working transform */ int machine; /* the machine frame */ + const char *orient; /* orienting letters, NULL: from the joint map */ int slides; /* joints are the slides, letters by the map */ } kins_ops; diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index a734fbcc467..029f64c4c19 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -25,6 +25,7 @@ ********************************************************************/ #include +#include #include #include #include @@ -767,6 +768,72 @@ int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, return r; } +// the letter of joint j: the principal one, else any that maps to it +static int joint_letter(const kins_params *p, int j) +{ + int a; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joint_of_axis[a] == j) { return a; } + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joints_of_axis[a] & (1u << j)) { return a; } + } + return -1; +} + +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[2], int head[2]) +{ + static const char letters[] = "XYZABCUVW"; + const kins_ops *ops; + double j[EMCMOT_MAX_JOINTS]; + unsigned int table_mask = 0, head_mask = 0; + int joints[2] = {-1, -1}; + int i, r = -1; + + if (!axes || !head) return -1; + axes[0] = axes[1] = head[0] = head[1] = -1; + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + ops = ctx->info.ops[ctx->ktype]; + if (!ops->tool || !ops->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + if (toolFrameWorkJoints(frame_work, ctx->num_joints, j, &table_mask) == 0 + && toolFrameWorkJoints(frame_tool, ctx->num_joints, j, &head_mask) == 0 + && !(table_mask & head_mask)) { + int tables = __builtin_popcount(table_mask), heads = __builtin_popcount(head_mask); + if (heads == 2 && tables == 0) { + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 1; + } else if (tables == 2 && heads == 0) { + r = toolFrameOrientJoints(frame_work, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 0; + } else if (tables == 1 && heads == 1) { + joints[0] = __builtin_ctz(table_mask); + joints[1] = __builtin_ctz(head_mask); + head[0] = 0; + head[1] = 1; + r = 0; + } + } + frame_ctx = NULL; + for (i = 0; i < 2 && r == 0; i++) { + if (ops->orient && ops->orient[0] && ops->orient[1]) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[i])); + axes[i] = at ? (int)(at - letters) : -1; + } else { + axes[i] = joint_letter(&ctx->params, joints[i]); + } + if (axes[i] < 0) { r = -1; } + } + if (r != 0) { + axes[0] = axes[1] = head[0] = head[1] = -1; + } + return r; +} + KinematicsUserContext* kinematicsUserInitString(const char* kinematics, int num_joints, int comp_id, diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 582e51a5538..a2525a95c04 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -5740,7 +5740,8 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); if (tool_vector) { - CHP(tool_vector_ends(block, settings, &AA_end, &BB_end, &CC_end)); + double *rotary_end[6] = {&AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end}; + CHP(tool_vector_ends(block, settings, rotary_end)); } if (move == G_1) { diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 01574b1fef5..48ff1f4d5f5 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,10 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_KINS_ORIENT_1, + NP_KINS_ORIENT_2, + NP_KINS_ORIENT_1_HEAD, + NP_KINS_ORIENT_2_HEAD, NP_ORIENT_VALID, NP_ORIENT_X, NP_ORIENT_Y, @@ -553,6 +557,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_KINS_ORIENT_1: // _kins_orient_1 and kin: the axes that orient the tool + case NP_KINS_ORIENT_2: + case NP_KINS_ORIENT_1_HEAD: + case NP_KINS_ORIENT_2_HEAD: + { + int axes[2], head[2]; + kins_orient(&_setup, axes, head); + switch (cmd) { + case NP_KINS_ORIENT_1: *value = axes[0]; break; + case NP_KINS_ORIENT_2: *value = axes[1]; break; + case NP_KINS_ORIENT_1_HEAD: *value = head[0]; break; + default: *value = head[1]; break; + } + } + break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose *value = _setup.orient_valid; break; @@ -946,6 +966,14 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the axes that orient the tool on that type, 0 X to 8 W, first rotation + // then second, and whether each turns the head (1) or the table (0); -1 + // where the type does not orient with two rotaries + init_readonly_param("_kins_orient_1", NP_KINS_ORIENT_1, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2", NP_KINS_ORIENT_2, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_1_head", NP_KINS_ORIENT_1_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2_head", NP_KINS_ORIENT_2_HEAD, PA_USE_LOOKUP); + // the pose G53.2 last solved: 1.0 once one has been, and its words init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 88b552a4749..32600f0d31a 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -605,10 +605,49 @@ int Interp::kins_load(setup_pointer s) CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); s->kins_ctx = ctx; for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + CHP(kins_check_orient(s)); } return INTERP_OK; } +// Every type the module evaluates here orients the tool with angular axes: +// a type whose rotaries are letters the INI file makes LINEAR would have +// G43.5 and G68.2 write degrees into a length. +int Interp::kins_check_orient(setup_pointer s) +{ + static const char letters[] = "XYZABCUVW"; + KinematicsUserContext *ctx = KINS_CTX(s); + double zero[EMCMOT_MAX_JOINTS] = {0}; + int t, i, axes[2], head[2]; + + for (t = 0; t < kinematicsUserGetNumTypes(ctx); t++) { + if (kinematicsUserSetType(ctx, t) != 0) { continue; } + if (kinematicsUserOrientAxes(ctx, zero, axes, head) != 0) { continue; } + for (i = 0; i < 2; i++) { + if (axisKindsAngular(s->axis_kinds, axes[i])) { continue; } + kins_release(s); + ERS(_("kinematics type %d of %s orients the tool with %c, which [AXIS_%c] TYPE makes LINEAR"), + t, s->kins_module, letters[axes[i]], letters[axes[i]]); + } + } + return INTERP_OK; +} + +// The axes that orient the tool on the kinematics type the program is in, +// at the joints it stands in: axis numbers 0 X to 8 W, first rotation then +// second, and 1 for a head, 0 for a table. All -1 where the type turns +// other than two rotaries, or no module can be evaluated here. +void Interp::kins_orient(setup_pointer s, int axes[2], int head[2]) +{ + void *vctx; + double now[EMCMOT_MAX_JOINTS]; + + axes[0] = axes[1] = head[0] = head[1] = -1; + if (kins_context(s, &vctx) != INTERP_OK) { return; } + if (current_joints(s, (KinematicsUserContext *)vctx, now) != INTERP_OK) { return; } + kinematicsUserOrientAxes((KinematicsUserContext *)vctx, now, axes, head); +} + // the loaded module, on the kinematics type the program is in int Interp::kins_context(setup_pointer s, void **out) { @@ -898,6 +937,22 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } +// The rotary letters a solution for the kinematics type in force is +// written into, as axis numbers: the two it orients the tool with, else the +// A B C of the pose, a robot's wrist. Returns how many. +static int orienting_letters(KinematicsUserContext *ctx, const double *now, int orienting[3]) +{ + int axes[2], head[2]; + + orienting[0] = 3; + orienting[1] = 4; + orienting[2] = 5; + if (kinematicsUserOrientAxes(ctx, now, axes, head) != 0) { return 3; } + orienting[0] = axes[0]; + orienting[1] = axes[1]; + return 2; +} + // G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 // turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, // a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the @@ -954,6 +1009,18 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) return INTERP_OK; } + // the rotaries the solution turns, in the letters the type orients + // with; every other axis stays where it is + double rot[6] = {s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current}; + { + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + for (i = 0; i < count; i++) { + if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } + } + } + write_canon_state_tag(block, s); if (code == G_53_1) { // the rotaries alone: the linear joints are where they are, since @@ -969,28 +1036,28 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } else if (code == G_53_6) { // the tool centre point stays: a Cartesian move of the rotaries STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); } else { double x = block->x_flag ? block->x_number : s->current_x; double y = block->y_flag ? block->y_number : s->current_y; double z = block->z_flag ? block->z_number : s->current_z; JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); s->current_x = x; s->current_y = y; s->current_z = z; } - s->AA_current = end_prog[3]; - s->BB_current = end_prog[4]; - s->CC_current = end_prog[5]; if (code == G_53_1) { - s->u_current = end_prog[6]; - s->v_current = end_prog[7]; - s->w_current = end_prog[8]; + // the joints went where the solver put them, every letter with them + for (i = 0; i < 6; i++) { rot[i] = end_prog[3 + i]; } } + s->AA_current = rot[0]; + s->BB_current = rot[1]; + s->CC_current = rot[2]; + s->u_current = rot[3]; + s->v_current = rot[4]; + s->w_current = rot[5]; return INTERP_OK; } @@ -1159,19 +1226,21 @@ int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, c // G43.5: I J K on a G0 or G1 line are the tool axis, tip towards holder, in // the coordinate system the line's X Y Z are in. The rotaries come from the // tool frame inverse, every orienting joint free, the nearest pose, as program -// rotary coordinates so a rotary offset is right by construction. -int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, double *b, double *c) +// coordinates of the axes the kinematics type orients with (A B C U V W +// ends, 0 A to 5 W), so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotary_end[6]) { + static const char letters[] = "XYZABCUVW"; void *vctx; KinematicsUserContext *ctx; double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; double v[3], prog[9]; + const bool rotary_flag[6] = {block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag}; PmCartesian axis; EmcPose pose; int i; - CHKS((block->a_flag || block->b_flag || block->c_flag), - _("G43.5: a tool vector and rotary words on one line give the orientation twice")); v[0] = block->i_flag ? block->i_number : 0.0; v[1] = block->j_flag ? block->j_number : 0.0; v[2] = block->k_flag ? block->k_number : 0.0; @@ -1181,6 +1250,16 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, do CHKS((kinematicsUserIsIdentity(ctx)), _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); CHP(current_joints(s, ctx, now)); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + CHKS((orienting[0] < 3 || orienting[1] < 3), + _("G43.5: kinematics type %d orients the tool with %c and %c, not rotary axes"), + s->kins_type, letters[orienting[0]], letters[orienting[1]]); + for (i = 0; i < count; i++) { + CHKS((rotary_flag[orienting[i] - 3]), + _("G43.5: a tool vector and a %c word on one line give the orientation twice"), + letters[orienting[i]]); + } if (block->g_modes[GM_MODAL_0] == G_53) { axis.x = v[0]; axis.y = v[1]; @@ -1197,9 +1276,9 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, do _("G43.5: the kinematics cannot place the orientation it found")); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } machine_pose_to_program(s, &pose, prog); - *a = prog[3]; - *b = prog[4]; - *c = prog[5]; + for (i = 0; i < count; i++) { + *rotary_end[orienting[i] - 3] = prog[orienting[i]]; + } return INTERP_OK; } diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 7e575fdbe96..a0e52691800 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -374,7 +374,9 @@ public: int convert_orient_tool(int code, block_pointer block, setup_pointer settings); int orient_solve(setup_pointer settings, void *ctx, const PmCartesian *axis, const PmCartesian *xdir, int p, int q, const double *now, double *joints, const char *name); - int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); + int tool_vector_ends(block_pointer block, setup_pointer settings, double *rotary_end[6]); + int kins_check_orient(setup_pointer s); + void kins_orient(setup_pointer s, int axes[2], int head[2]); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int convert_home_slides(int code, block_pointer block, setup_pointer settings); int convert_savehome_slides(int code, setup_pointer settings); diff --git a/tests/kins-orient-linear/README b/tests/kins-orient-linear/README new file mode 100644 index 00000000000..674867e3a64 --- /dev/null +++ b/tests/kins-orient-linear/README @@ -0,0 +1,4 @@ +A kinematics type that orients the tool with an axis the INI file makes +LINEAR is refused: 5axiskins turns the tool with B and C, and here +[AXIS_B] TYPE is LINEAR. The interpreter refuses when it first loads the +module, the G43.5 vector below, with a message naming the type and B. diff --git a/tests/kins-orient-linear/checkresult b/tests/kins-orient-linear/checkresult new file mode 100755 index 00000000000..42526aa5579 --- /dev/null +++ b/tests/kins-orient-linear/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# test-ui.py prints PASS once the refusal names B +grep -q "^PASS$" "$1" diff --git a/tests/kins-orient-linear/sim.hal b/tests/kins-orient-linear/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-orient-linear/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-orient-linear/test-ui.py b/tests/kins-orient-linear/test-ui.py new file mode 100755 index 00000000000..3d72ea20b34 --- /dev/null +++ b/tests/kins-orient-linear/test-ui.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +# 5axiskins orients with B and C; [AXIS_B] TYPE = LINEAR: see README. + +import linuxcnc +import os +import sys + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +said = [] +for cmd in ("G12.1 P0", "G43.5 H1", "G0 X0 K1"): + c.mdi(cmd) + c.wait_complete(30) + while True: + m = e.poll() + if not m: + break + said.append(m) + +errors = [m[1] for m in said if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] +print("errors: %s" % errors) +ok = any("orients the tool with B, which [AXIS_B] TYPE makes LINEAR" in m for m in errors) +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("PASS" if ok else "FAIL: no refusal naming B") +sys.exit(0 if ok else 1) diff --git a/tests/kins-orient-linear/test.ini b/tests/kins-orient-linear/test.ini new file mode 100644 index 00000000000..0ea12355fb5 --- /dev/null +++ b/tests/kins-orient-linear/test.ini @@ -0,0 +1,131 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, 1 is identity, 2 is the userk template +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +TYPE = LINEAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-orient-linear/test.sh b/tests/kins-orient-linear/test.sh new file mode 100755 index 00000000000..079fb121f31 --- /dev/null +++ b/tests/kins-orient-linear/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-orient-linear/tool.tbl b/tests/kins-orient-linear/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-orient-linear/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index d245113f5ab..3fce98819f2 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -369,6 +369,52 @@ def refused(cmd, needle): mdi("G49") refused("G0 X0 K1", "K word with no") +# ---- the axes that orient the tool --------------------------------------- +# +# 5axiskins turns the tool with a head: C with its axis fixed, B carried by +# it. The interpreter finds them from the tool frame and names them by the +# joint map, as axis numbers; the identity type orients with nothing. Under +# G43.5 a word of an axis that does not orient, W here, goes with a vector. + +def param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + done(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_1_head", "_kins_orient_2_head") +errors_before = errors +mdi("G12.1 P0") +got = [param(n) for n in ORIENT] +if got != [5, 4, 1, 1]: + error("on the 5-axis type the orienting axes read %s, not C then B, both heads" % (got,)) +mdi("G13.1") +got = [param(n) for n in ORIENT] +if got != [-1, -1, -1, -1]: + error("on the identity type the orienting axes read %s, not none" % (got,)) +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0 W0") +mdi("G43.5 H1") +with_w = mdi("G0 X0 Y0 Z0 K1 W2") +if abs(with_w[5] - 2) > 1e-6: + error("a vector with a W word left W at %.4f, not 2" % with_w[5]) +mdi("G0 W0") +mdi("G49") +said = drain() +if any(m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) for m in said): + error("the orienting axes section reported %s" % (said,)) +elif errors == errors_before: + print("the orienting axes are C then B, heads, and W goes along with a vector") + # ---- a negative kinematics number is refused ----------------------------- drain() diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 206bc4d96af..37173c3fa16 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) From a4fadd919502077540613c84cf59eb4fe454c30d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 23:46:58 +1000 Subject: [PATCH 11/44] switchkins: a module may declare several primary types A machine with two tool heads, a saw blade and a waterjet say, has a working transform per head and selects the head with G12.1. The loader now accepts any number of types flagged primary; identity and machine frame stay unique, since G13.1 and G49 resolve a single answer from them. G43.4 and G43.5 stay on the type in force when it is primary, and from any other type switch to the lowest numbered primary, which is the single primary on every module that declares one. tests/ptp-machine-frame gains a second head on slantkins (type 3, the tip again, primary) and checks both rules. --- docs/src/gcode/g-code.adoc | 5 ++++- docs/src/motion/kinematics-conventions.adoc | 5 +++-- docs/src/motion/switchkins.adoc | 10 +++++++--- include/kins_module.h | 3 ++- src/emc/kinematics/switchkins.c | 14 +++++--------- src/emc/rs274ngc/interp_convert.cc | 6 +++++- tests/ptp-machine-frame/README | 6 ++++-- tests/ptp-machine-frame/slantkins.comp | 11 +++++++++++ tests/ptp-machine-frame/test-ui.py | 11 +++++++++++ 9 files changed, 52 insertions(+), 19 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 228d32701a2..b3127430e86 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1858,7 +1858,10 @@ The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module (KINSTYPE_PRIMARY, see the <> chapter), the number is not the answer; a module that -declares none rejects 'G43.4' at read time. +declares none rejects 'G43.4' at read time. A module with one working +kinematics per tool head declares each one primary: 'G43.4' stays on the +one selected with 'G12.1' and, from any other kinematics, switches to the +lowest numbered. The switch is a queue synchronisation point like '<>': when 'G43.4' or 'G49' changes the diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 922fe702a33..45a6706d8db 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -565,8 +565,9 @@ the optional work and tool frames with the native rotation that relates the tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks -the no-transform type, joints are axes; `primary` the working transform -`G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and +the no-transform type, joints are axes; `primary` a working transform +`G43.4` and `G43.5` switch to, which a module with one per tool head sets +on each, `G12.1` choosing the head; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index f12c25b527b..0a28ae9db0c 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -631,7 +631,11 @@ are not in the conventional order still gets working spellings. With `MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the INI file, 'G53', 'G28' and 'G30' move in the machine frame kinstype's world too whenever another kinstype is in force; the five axis sims set -it. The +it. A +machine with two tool heads, a saw blade and a waterjet say, declares +one primary kinstype per head and selects the head with 'G12.1 P-': +'G43.4' stays on the kinstype in force when it is primary, and from any +other switches to the lowest numbered primary. The machine frame kinstype is the machine with the tool left out, XYZ the pivot or the flange in machine coordinates and the rotary letters the orientation as that kinstype reports it; on a machine whose slides line @@ -650,8 +654,8 @@ composite Y on a mill-turn, declares its machine frame kinstype separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since motion and the planner skip the maths on that flag alone. At most one -kinstype may be declared identity, at most one primary and at most one -machine frame, and declaring a kinstype the module does not provide +kinstype may be declared identity and at most one machine frame, any +number primary, and declaring a kinstype the module does not provide fails the module load. A module that declares nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' are an error, since the numbers of the identity and primary kinematics diff --git a/include/kins_module.h b/include/kins_module.h index 6bf95e358d5..58a0c4c9f77 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -340,7 +340,8 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, forward starts from the pose it is handed, so the shared code seeds it with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is - the module's working transform, the type G43.4 switches to. machine says + a working transform, the type G43.4 switches to; a module with one per + tool head flags each, G12.1 choosing the head. machine says this is the machine frame type, the tool left out, the pivot or the flange in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a module that leaves it unset on every type has its identity type stand in. diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index e2f92709ebd..81cf190e4d1 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -730,7 +730,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities, primaries, machines; + int identities, machines; int res = 0; char* emsg = "other"; @@ -758,13 +758,13 @@ int switchkinsInit(const int comp_id, } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } - // declarations must name provided types, and each flag is unique: - // G13.1 and G49 resolve the machine frame type from the flags and - // G43.4 the primary, so two answers is a load error. A module that + // declarations must name provided types, and identity and machine + // are unique: G13.1 and G49 resolve the machine frame type from the + // flags, so two answers is a load error. Several types may be + // primary, one per tool head, G12.1 choosing among them. A module that // names no machine frame type has its identity type stand in, which // is the truth on every machine whose slides line up with its frame. identities = 0; - primaries = 0; machines = 0; for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } @@ -781,7 +781,6 @@ int switchkinsInit(const int comp_id, identities++; if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } } - if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", @@ -790,9 +789,6 @@ int switchkinsInit(const int comp_id, if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } - if (primaries > 1) { - emsg = "more than one primary switchkins-type declared"; goto error; - } if (machines > 1) { emsg = "more than one machine frame switchkins-type declared"; goto error; } diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index a2525a95c04..81589efa21a 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6642,7 +6642,7 @@ static int kins_type_info_available() return 0; } -// the type carrying a KINSTYPE_ flag, or -1 when the module declares none; +// the lowest type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test int flagged_kins_type(int flag) @@ -6712,10 +6712,14 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu (_("Cannot change tool offset with cutter radius compensation on"))); if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); + int now = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With // no kinematics attached there is nothing to switch to. G43.5 is // the same, and the lines after it may give the tool axis as I J K. + // A module with a working transform per tool head flags each one + // primary: the one in force stays, from any other type the lowest. + if (now >= 0 && (now & KINSTYPE_PRIMARY)) { primary = settings->kins_type; } CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); if (primary >= 0 && primary != settings->kins_type) { // the switch keeps the joints and moves the point, so the point diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README index 08480a0f1e2..ef9d659211a 100644 --- a/tests/ptp-machine-frame/README +++ b/tests/ptp-machine-frame/README @@ -6,10 +6,12 @@ template builds: XYZBC on five joints, joint 1 a slide slanted 30 degrees in the XY plane, so machine X and Y are made by joints 0 and 1 together. Type 0 is the carriage in machine coordinates, declared the machine frame type and not an identity; type 1 the tool tip with the head tilted about Y -and the tool length from the tool table; type 2 a plain identity. +and the tool length from the tool table; type 2 a plain identity; type 3 +the tip again, a second head, declared primary as well. The test checks that G13.1 and G49 select the machine frame type rather -than the identity, that G53.5 puts the carriage at a machine frame point, +than the identity, that G43.4 stays on a primary type selected with G12.1 +and from any other goes to the lowest numbered primary, that G53.5 puts the carriage at a machine frame point, the letters not given holding their machine coordinate and not their joint, from the machine frame type and from under the tilted tip kinematics alike, that G28.5 goes to a position stored on the machine diff --git a/tests/ptp-machine-frame/slantkins.comp b/tests/ptp-machine-frame/slantkins.comp index 80febbadea4..62574995943 100644 --- a/tests/ptp-machine-frame/slantkins.comp +++ b/tests/ptp-machine-frame/slantkins.comp @@ -15,6 +15,9 @@ by B and the tool length from the tool table along the tool axis, folded so that tip and carriage agree at B zero. type2 is a plain identity, the joints as the axes, declared identity. + +type3 is type1 again, declared primary as well, the way a machine with a +second tool head declares a working transform for each. """; pin out si32 dummy=0 "one pin, which halcompile requires"; option period no; @@ -130,6 +133,13 @@ static const kins_ops tip_ops = { .primary = 1, }; +// a second head: G43.4 stays on it once G12.1 has selected it +static const kins_ops second_head_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -150,6 +160,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterOps(0, &frame_ops); switchkinsRegisterOps(1, &tip_ops); switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(3, &second_head_ops); return 0; } diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index 5110681625d..d9160a02baa 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -117,6 +117,17 @@ def expect_type(what, want): expect_type("G49", 0) mdi("G12.1 P1", "G13.1") expect_type("G12.1 P1 then G13.1", 0) + +# --- two primary types: G43.4 keeps the head G12.1 selected ------------------ +mdi("G12.1 P3", "G43.4 H1") +expect_type("G12.1 P3 then G43.4, the second head", 3) +mdi("G49") +expect_type("G49 from the second head", 0) +mdi("G12.1 P2", "G43.4 H1") +expect_type("G43.4 from the identity, the lowest", 1) +mdi("G12.1 P3", "G43.4 H1") +expect_type("G43.4 again after G12.1 P3", 3) +mdi("G49") drain() # --- G53.5 is the machine frame, not the slides ------------------------------ From b846035ae070796c39ba49d5330bdb5ff84c0039 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 00:11:33 +1000 Subject: [PATCH 12/44] G53.2 publishes the rotaries in the order the kinematics orients with way whatever letters the machine gives its rotaries, as Fanuc and Siemens number the rotaries first and second. #5071-#5076 carry X Y Z and the three rotaries, #5077-#5079 are written 0. kinematicsUserOrientAxes() answers up to three axes. A type with three rotaries between tool and work, xyzacb_trsrn's A table and B C head, gives all three, tables first, each group in joint order, or the three letters its orient declares; a robot arm, whose joints all turn the tool, gives the A B C of its pose. #<_kins_orient_3> and before. G43.5 and the LINEAR check cover the third axis. --- .../bridgemill/g532-fused-orient-move.ngc | 2 +- docs/src/gcode/g-code.adoc | 12 ++-- docs/src/gcode/overview.adoc | 42 ++++++++----- docs/src/motion/kinematics-conventions.adoc | 17 ++--- include/kinematics_user.h | 12 ++-- include/kins_module.h | 5 +- lib/python/qtvcp/lib/mdi_text.py | 6 +- .../kinematics_userspace/kinematics_user.c | 36 ++++++++++- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_internal.hh | 4 +- src/emc/rs274ngc/interp_namedparams.cc | 35 ++++++----- src/emc/rs274ngc/interp_workplane.cc | 63 ++++++++++--------- src/emc/rs274ngc/rs274ngc_interp.hh | 2 +- tests/kins-switch/test-ui.py | 7 ++- tests/ptp-robot/test-ui.py | 19 ++++++ tests/remap/introspect/expected | 4 +- tests/twp-native/test-ui.py | 34 +++++++--- 17 files changed, 200 insertions(+), 102 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc index bd193299758..3439a71fae7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -36,7 +36,7 @@ o210 else o210 endif g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c g53.2 ; solve the orientation, stay put - g0 x0 y0 z[#+#] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel + g0 x0 y0 z[#+#] b#<_orient_rot2> c#<_orient_rot1> ; one TCP turn and travel, C orients first g1 z[#+#] g0 z[#+#] # = [#+1] diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index b3127430e86..7e8b66821ad 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2061,10 +2061,14 @@ differ in what happens then: * 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the plane, in one point-to-point move. A word left out keeps the present value. * 'G53.2' moves nothing. It only publishes the solved pose on the named - parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', - '#<_orient_b>' and '#<_orient_c>', and on the numbered parameters - '#5071' to '#5079' ('X Y Z A B C U V W'), in program units in the plane, - and sets '#<_orient_valid>' and '#5080' to 1. The program can then reach + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_rot1>', '#<_orient_rot2>' and '#<_orient_rot3>', and on the + numbered parameters '#5071' to '#5076' in the same order, in program + units in the plane, and sets '#<_orient_valid>' and '#5080' to 1. The + rotaries come in the order the kinematics orients the tool with, the + axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, so a program reads + the angles the same way whatever letters the machine gives them; + '#<_orient_rot3>' is 0 on a machine that orients with two rotaries. The program can then reach the pose with a move of its own making, for instance a single 'G0' that names 'X Y Z' and the rotary words together, so the turn and the travel are one Cartesian move under the TCP kinematics. This is what Heidenhain diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 3a4b025571d..ae352b92262 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -305,9 +305,11 @@ example '##2' means the value of the parameter whose index is the which the G38 took place. Volatile. * '5070' - <> probe result: 1 if success, 0 if probe failed to close. Used with G38.3 and G38.5. Volatile. -* '5071-5079' - Pose last solved by <> for X, Y, Z, A, - B, C, U, V & W, in program units in the tilted work plane. Same values - as `#<_orient_x>` and kin. Read-only, volatile. +* '5071-5076' - Pose last solved by <> for X, Y, Z + and the first, second and third orienting rotary, in program units in + the tilted work plane. Same values as `#<_orient_x>` and kin. Read-only, + volatile. +* '5077-5079' - Written 0 by 'G53.2'. Read-only, volatile. * '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, @@ -513,24 +515,32 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. -* '#<_kins_orient_1>', '#<_kins_orient_2>' - The axes the kinematics type - in force orients the tool with, as axis numbers, 0 for X to 8 for W: the - first rotation, the one whose axis stays put, then the second. A head - and a table give the table first. The type's module declares the letters, - or they are those its joint map gives the joints that turn. -1 where the - type does not orient with two rotaries, the identity type or a robot - among them, or the interpreter cannot evaluate the kinematics. An axis +* '#<_kins_orient_1>', '#<_kins_orient_2>', '#<_kins_orient_3>' - The + axes the kinematics type in force orients the tool with, as axis numbers, + 0 for X to 8 for W: the first rotation, the one whose axis stays put, then + the second. A head and a table give the table first. The type's module + declares the letters, or they are those its joint map gives the joints + that turn. '#<_kins_orient_3>' is -1 on those. A type with three + rotaries, a table and a two axis head say, gives all three, the tables + first; a robot arm, whose joints all turn the tool, reads 3, 4 and 5, the + A B C of its pose. All three are -1 where the type orients otherwise, the identity + type among them, or the interpreter cannot evaluate the kinematics. An axis named here must be 'ANGULAR' in the INI file; the interpreter refuses a module whose type orients with a 'LINEAR' axis when it first loads it. -* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>' - 1 where that axis - turns the head, 0 where it turns the table, -1 as above. +* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>', + '#<_kins_orient_3_head>' - 1 where that axis turns the head, 0 where it + turns the table, -1 as above. * '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', - '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last - solved, in program units in the tilted work plane. '#<_orient_valid>' is - 1 once a 'G53.2' has run; reading the others before that is an error. The - same values are on the numbered parameters '#5071' to '#5080'. See + '#<_orient_rot1>', '#<_orient_rot2>', '#<_orient_rot3>' - The pose 'G53.2' + last solved, in program units in the tilted work plane: X Y Z, then the + angles of the axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, in + that order, 0 for a third the machine does not have. This is how Fanuc + and Siemens number the rotaries of a five axis machine, first and second, + whatever their letters. '#<_orient_valid>' is 1 once a 'G53.2' has run; + reading the others before that is an error. The same values are on the + numbered parameters '#5071' to '#5076' and '#5080'. See <>. * '#<_plane>' - returns the value designating the current plane: diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 45a6706d8db..dd109f87527 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -571,13 +571,16 @@ on each, `G12.1` choosing the head; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable -Kinematics chapter). A type may also name the two axis letters that orient -the tool, first rotation then second, in `orient`: "AC" for instance. Left -out, they are the letters the joint map gives the joints that turn the frames, -which is right for a module whose forward puts each joint in its own letter; -a module that writes an orienting joint into another letter declares them. -`G43.5` writes its solution into these letters, and the interpreter reads them -back as `#<_kins_orient_1>` and `#<_kins_orient_2>`. A module with +Kinematics chapter). A type may also name the axis letters that orient +the tool, first rotation then second, in `orient`: "AC" for instance, or three +letters, tables first, for a type with three rotaries. Left out, they are the +letters the joint map gives the joints that turn the frames, which is right +for a module whose forward puts each joint in its own letter; a module that +writes an orienting joint into another letter declares them. A robot arm, all +of whose joints turn the tool, orients with the A B C of its pose. `G43.5` +writes its solution into these letters, the interpreter reads them back as +`#<_kins_orient_1>` to `#<_kins_orient_3>`, and `G53.2` publishes the angles +it solves in the same order as `#<_orient_rot1>` to `#<_orient_rot3>`. A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/include/kinematics_user.h b/include/kinematics_user.h index a0859a9e164..59ff8dc046a 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -276,12 +276,16 @@ int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, * head (1) or the table (0). Two heads or two tables are ordered as * kinematicsUserOrientJoints() orders them, the one whose axis stays put * first; a table and a head, the table first. The letters are the type's - * declared orient, else those the joint map gives the joints. Returns 0, - * or -1 with all four set to -1 where the type turns other than two - * rotaries between tool and work, or a letter cannot be found. + * declared orient, else those the joint map gives the joints. Three + * rotaries come tables first, each group in joint order. A type whose + * tool is turned by more than three joints and no table, a robot arm, + * orients with the A B C of its pose, all three head. Returns how many + * axes, 2 or 3, the unused entries -1; or -1 with every entry -1 where the + * type turns other rotaries between tool and work, or a letter cannot be + * found. */ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, - int axes[2], int head[2]); + int axes[3], int head[3]); /** * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the diff --git a/include/kins_module.h b/include/kins_module.h index 58a0c4c9f77..d29bc143332 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -347,8 +347,9 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, module that leaves it unset on every type has its identity type stand in. A machine frame type does not read the tool offset in the parameter block, so a working transform that leaves the tool out anyway, a robot's flange, - carries primary and machine both. orient names the two axis letters - that orient the tool, first rotation then second, "AC" for instance; left + carries primary and machine both. orient names the axis letters that + orient the tool, first rotation then second, "AC" for instance, or three, + tables first, on a type with three rotaries; left NULL they are the letters the joint map gives the joints that turn the frames, which is right for a module whose forward puts each joint in its own letter, and a module that puts one elsewhere declares them. diff --git a/lib/python/qtvcp/lib/mdi_text.py b/lib/python/qtvcp/lib/mdi_text.py index 712eaf78d2c..d849229a90b 100644 --- a/lib/python/qtvcp/lib/mdi_text.py +++ b/lib/python/qtvcp/lib/mdi_text.py @@ -863,8 +863,10 @@ def gcode_descriptions(gcode): G53.2 Solves the same pose as G53.1 and moves nothing. -The pose is published on #<_orient_x> to -#<_orient_c> and on #5071 to #5079, in program +The pose is published on #<_orient_x> #<_orient_y> +#<_orient_z> and the rotaries in the order the +kinematics orients with, #<_orient_rot1> to +#<_orient_rot3>, and on #5071 to #5076, in program units in the plane, with #<_orient_valid> (#5080) set to 1, so the program can reach it with a move of its own, for instance a single G0 naming X Y Z diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 029f64c4c19..d0fb6dda9f4 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -783,7 +783,7 @@ static int joint_letter(const kins_params *p, int j) } int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, - int axes[2], int head[2]) + int axes[3], int head[3]) { static const char letters[] = "XYZABCUVW"; const kins_ops *ops; @@ -793,7 +793,7 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, int i, r = -1; if (!axes || !head) return -1; - axes[0] = axes[1] = head[0] = head[1] = -1; + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; ops = ctx->info.ops[ctx->ktype]; if (!ops->tool || !ops->work) return -1; @@ -816,6 +816,35 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, head[0] = 0; head[1] = 1; r = 0; + } else if (tables + heads == 3) { + // three rotaries: the tables first, each group in joint order + int n = 0, jn; + for (jn = 0; jn < ctx->num_joints; jn++) { + if (table_mask & (1u << jn)) { axes[n] = jn; head[n++] = 0; } + } + for (jn = 0; jn < ctx->num_joints; jn++) { + if (head_mask & (1u << jn)) { axes[n] = jn; head[n++] = 1; } + } + frame_ctx = NULL; + for (n = 0; n < 3; n++) { + if (ops->orient && strlen(ops->orient) >= 3) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[n])); + axes[n] = at ? (int)(at - letters) : -1; + } else { + axes[n] = joint_letter(&ctx->params, axes[n]); + } + } + if (axes[0] < 0 || axes[1] < 0 || axes[2] < 0) { + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + return -1; + } + return 3; + } else if (heads > 3 && tables == 0) { + // an arm: the wrist is one of many joints that turn the tool, + // and the pose gives the orientation as A B C + for (i = 0; i < 3; i++) { axes[i] = 3 + i; head[i] = 1; } + frame_ctx = NULL; + return 3; } } frame_ctx = NULL; @@ -830,8 +859,9 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, } if (r != 0) { axes[0] = axes[1] = head[0] = head[1] = -1; + return -1; } - return r; + return 2; } KinematicsUserContext* kinematicsUserInitString(const char* kinematics, diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index b9906756047..31ac55ecb39 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -229,7 +229,7 @@ const int Interp::required_parameters[] = { const int Interp::readonly_parameters[] = { 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W - 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y ... W, valid + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y Z, rotaries 1 2 3, 0 0 0, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 51516c7a2a4..18f00dc79a7 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -808,9 +808,9 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r - // the pose G53.2 last solved, in program words, for #<_orient_a> and kin + // the pose G53.2 last solved, in program words, for #<_orient_x> and kin bool orient_valid; - double orient_pose[6]; // x y z a b c + double orient_pose[6]; // x y z, the rotaries in orient order // the kinematics, for G68.3 and the orientation moves: loaded on first // use through the non-realtime loader, on a HAL component of our own void *kins_ctx; // KinematicsUserContext diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 48ff1f4d5f5..19574483586 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -61,15 +61,17 @@ enum predefined_named_parameters { NP_KINS_TYPE, NP_KINS_ORIENT_1, NP_KINS_ORIENT_2, + NP_KINS_ORIENT_3, NP_KINS_ORIENT_1_HEAD, NP_KINS_ORIENT_2_HEAD, + NP_KINS_ORIENT_3_HEAD, NP_ORIENT_VALID, NP_ORIENT_X, NP_ORIENT_Y, NP_ORIENT_Z, - NP_ORIENT_A, - NP_ORIENT_B, - NP_ORIENT_C, + NP_ORIENT_ROT1, + NP_ORIENT_ROT2, + NP_ORIENT_ROT3, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -559,16 +561,17 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_KINS_ORIENT_1: // _kins_orient_1 and kin: the axes that orient the tool case NP_KINS_ORIENT_2: + case NP_KINS_ORIENT_3: case NP_KINS_ORIENT_1_HEAD: case NP_KINS_ORIENT_2_HEAD: + case NP_KINS_ORIENT_3_HEAD: { - int axes[2], head[2]; + int axes[3], head[3]; kins_orient(&_setup, axes, head); - switch (cmd) { - case NP_KINS_ORIENT_1: *value = axes[0]; break; - case NP_KINS_ORIENT_2: *value = axes[1]; break; - case NP_KINS_ORIENT_1_HEAD: *value = head[0]; break; - default: *value = head[1]; break; + if (cmd <= NP_KINS_ORIENT_3) { + *value = axes[cmd - NP_KINS_ORIENT_1]; + } else { + *value = head[cmd - NP_KINS_ORIENT_1_HEAD]; } } break; @@ -580,9 +583,9 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_ORIENT_X: // _orient_x and kin: the pose G53.2 last solved case NP_ORIENT_Y: case NP_ORIENT_Z: - case NP_ORIENT_A: - case NP_ORIENT_B: - case NP_ORIENT_C: + case NP_ORIENT_ROT1: + case NP_ORIENT_ROT2: + case NP_ORIENT_ROT3: if (!_setup.orient_valid) { ERS(_("no G53.2 has solved an orientation yet")); } @@ -971,17 +974,19 @@ int Interp::init_named_parameters() // where the type does not orient with two rotaries init_readonly_param("_kins_orient_1", NP_KINS_ORIENT_1, PA_USE_LOOKUP); init_readonly_param("_kins_orient_2", NP_KINS_ORIENT_2, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3", NP_KINS_ORIENT_3, PA_USE_LOOKUP); init_readonly_param("_kins_orient_1_head", NP_KINS_ORIENT_1_HEAD, PA_USE_LOOKUP); init_readonly_param("_kins_orient_2_head", NP_KINS_ORIENT_2_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3_head", NP_KINS_ORIENT_3_HEAD, PA_USE_LOOKUP); // the pose G53.2 last solved: 1.0 once one has been, and its words init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); - init_readonly_param("_orient_a", NP_ORIENT_A, PA_USE_LOOKUP); - init_readonly_param("_orient_b", NP_ORIENT_B, PA_USE_LOOKUP); - init_readonly_param("_orient_c", NP_ORIENT_C, PA_USE_LOOKUP); + init_readonly_param("_orient_rot1", NP_ORIENT_ROT1, PA_USE_LOOKUP); + init_readonly_param("_orient_rot2", NP_ORIENT_ROT2, PA_USE_LOOKUP); + init_readonly_param("_orient_rot3", NP_ORIENT_ROT3, PA_USE_LOOKUP); // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 32600f0d31a..4eac723ba1e 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -618,12 +618,12 @@ int Interp::kins_check_orient(setup_pointer s) static const char letters[] = "XYZABCUVW"; KinematicsUserContext *ctx = KINS_CTX(s); double zero[EMCMOT_MAX_JOINTS] = {0}; - int t, i, axes[2], head[2]; + int t, i, n, axes[3], head[3]; for (t = 0; t < kinematicsUserGetNumTypes(ctx); t++) { if (kinematicsUserSetType(ctx, t) != 0) { continue; } - if (kinematicsUserOrientAxes(ctx, zero, axes, head) != 0) { continue; } - for (i = 0; i < 2; i++) { + n = kinematicsUserOrientAxes(ctx, zero, axes, head); + for (i = 0; i < n; i++) { if (axisKindsAngular(s->axis_kinds, axes[i])) { continue; } kins_release(s); ERS(_("kinematics type %d of %s orients the tool with %c, which [AXIS_%c] TYPE makes LINEAR"), @@ -635,14 +635,15 @@ int Interp::kins_check_orient(setup_pointer s) // The axes that orient the tool on the kinematics type the program is in, // at the joints it stands in: axis numbers 0 X to 8 W, first rotation then -// second, and 1 for a head, 0 for a table. All -1 where the type turns -// other than two rotaries, or no module can be evaluated here. -void Interp::kins_orient(setup_pointer s, int axes[2], int head[2]) +// second, a robot's wrist A B C, and 1 for a head, 0 for a table. All -1 +// where the type turns other rotaries, or no module can be evaluated here. +void Interp::kins_orient(setup_pointer s, int axes[3], int head[3]) { void *vctx; double now[EMCMOT_MAX_JOINTS]; + int i; - axes[0] = axes[1] = head[0] = head[1] = -1; + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } if (kins_context(s, &vctx) != INTERP_OK) { return; } if (current_joints(s, (KinematicsUserContext *)vctx, now) != INTERP_OK) { return; } kinematicsUserOrientAxes((KinematicsUserContext *)vctx, now, axes, head); @@ -938,19 +939,18 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) } // The rotary letters a solution for the kinematics type in force is -// written into, as axis numbers: the two it orients the tool with, else the -// A B C of the pose, a robot's wrist. Returns how many. +// written into, as axis numbers: those it orients the tool with, else the +// A B C of the pose. Returns how many. static int orienting_letters(KinematicsUserContext *ctx, const double *now, int orienting[3]) { - int axes[2], head[2]; + int head[3]; + int n = kinematicsUserOrientAxes(ctx, now, orienting, head); + if (n > 0) { return n; } orienting[0] = 3; orienting[1] = 4; orienting[2] = 5; - if (kinematicsUserOrientAxes(ctx, now, axes, head) != 0) { return 3; } - orienting[0] = axes[0]; - orienting[1] = axes[1]; - return 2; + return 3; } // G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 @@ -996,15 +996,22 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } machine_pose_to_program(s, &end_pose, end_prog); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + if (code == G_53_2) { - // STAY: solve only, nothing moves. The pose goes to the named - // parameters #<_orient_x> and kin and to #5071-#5080, for the - // program to use in a move of its own making, the way - // Heidenhain's STAY fills Q120-122. The machine state does not - // change. - for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + // STAY: solve only, nothing moves. X Y Z and the rotaries in the + // order the type orients with go to the named parameters + // #<_orient_x> and kin and to #5071-#5080, for the program to use + // in a move of its own making, the way Heidenhain's STAY fills + // Q120-122. The machine state does not change. + for (i = 0; i < 3; i++) { s->orient_pose[i] = end_prog[i]; } + for (i = 0; i < 3; i++) { + s->orient_pose[3 + i] = (i < count) ? end_prog[orienting[i]] : 0.0; + } s->orient_valid = true; - for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + for (i = 0; i < 6; i++) { s->parameters[5071 + i] = s->orient_pose[i]; } + for (i = 6; i < 9; i++) { s->parameters[5071 + i] = 0.0; } s->parameters[5080] = 1.0; return INTERP_OK; } @@ -1013,12 +1020,8 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) // with; every other axis stays where it is double rot[6] = {s->AA_current, s->BB_current, s->CC_current, s->u_current, s->v_current, s->w_current}; - { - int orienting[3]; - int count = orienting_letters(ctx, now, orienting); - for (i = 0; i < count; i++) { - if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } - } + for (i = 0; i < count; i++) { + if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } } write_canon_state_tag(block, s); @@ -1252,10 +1255,10 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotar CHP(current_joints(s, ctx, now)); int orienting[3]; int count = orienting_letters(ctx, now, orienting); - CHKS((orienting[0] < 3 || orienting[1] < 3), - _("G43.5: kinematics type %d orients the tool with %c and %c, not rotary axes"), - s->kins_type, letters[orienting[0]], letters[orienting[1]]); for (i = 0; i < count; i++) { + CHKS((orienting[i] < 3), + _("G43.5: kinematics type %d orients the tool with %c, not a rotary axis"), + s->kins_type, letters[orienting[i]]); CHKS((rotary_flag[orienting[i] - 3]), _("G43.5: a tool vector and a %c word on one line give the orientation twice"), letters[orienting[i]]); diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index a0e52691800..1ee24a796f6 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -376,7 +376,7 @@ public: int p, int q, const double *now, double *joints, const char *name); int tool_vector_ends(block_pointer block, setup_pointer settings, double *rotary_end[6]); int kins_check_orient(setup_pointer s); - void kins_orient(setup_pointer s, int axes[2], int head[2]); + void kins_orient(setup_pointer s, int axes[3], int head[3]); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int convert_home_slides(int code, block_pointer block, setup_pointer settings); int convert_savehome_slides(int code, setup_pointer settings); diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 3fce98819f2..05f8abf3c04 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -391,15 +391,16 @@ def param(name): error("#<%s> gave no value" % name) return None -ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_1_head", "_kins_orient_2_head") +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") errors_before = errors mdi("G12.1 P0") got = [param(n) for n in ORIENT] -if got != [5, 4, 1, 1]: +if got != [5, 4, -1, 1, 1, -1]: error("on the 5-axis type the orienting axes read %s, not C then B, both heads" % (got,)) mdi("G13.1") got = [param(n) for n in ORIENT] -if got != [-1, -1, -1, -1]: +if got != [-1, -1, -1, -1, -1, -1]: error("on the identity type the orienting axes read %s, not none" % (got,)) mdi("G12.1 P0") mdi("G0 X0 Y0 Z0 B0 C0 W0") diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index 93d77c1ea8a..1a2efe37716 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -185,5 +185,24 @@ def check_held(what, before, after, moved): % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) +# the wrist turns the tool with three joints: the arm kinematics orients +# with the A B C of its pose, all three head +mdi("G13.1") +drain() +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>)") +done(30) +said = None +deadline = time.time() + 5 +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +if said != [3, 4, 5, 1, 1, 1]: + error("the arm kinematics orients with %s, not A B C, heads" % (said,)) +print("orienting axes %s" % said) + print("Exiting with %d errors" % errors) sys.exit(1 if errors else 0) diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 37173c3fa16..f7dfe04cee5 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 85e4f2889d8..f494d962033 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -133,14 +133,15 @@ def in_plane(): drain() if not close(after2, stay, 1e-9): error("G53.2 moved the machine: %s became %s" % (stay, after2)) -# read the pose back through the parameters: a move to the published -# rotary words is a move to the present B and C, with the table held +# read the pose back through the parameters: the rotaries come in the +# order the type orients with, the A table, then B and C, and a move to +# them is a move to the present B and C, with the table held before3 = mdi("G0 B0 C0") -after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") -show("G0 to #<_orient_b/c>", after3) +after3, samples = sampled("G0 B#<_orient_rot2> C#<_orient_rot3>") +show("G0 to #<_orient_rot2/rot3>", after3) drain() if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: - error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + error("#<_orient_rot2> #<_orient_rot3> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): error("the tool axis at the pose G53.2 published is not the plane normal") @@ -158,11 +159,26 @@ def in_plane(): if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): error("writing #5075 was accepted; the G53.2 pose is not read-only") drain() -c.mdi("G0 A#<_orient_a>") +# the axes the type orients with, and the first of them, the table, where +# it stands; the numbered parameters past the rotaries read 0 +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>" + " #<_orient_rot1> #5074 #5077 #5078 #5079)") done(30) -m = e.poll() -if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): - error("#<_orient_a> after G53.2: %s" % m[1]) +deadline = time.time() + 5 +said = None +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading the orienting axes after G53.2: %s" % m[1]) + break + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +want = [3, 4, 5, 0, 1, 1, stay[TABLE], stay[TABLE], 0, 0, 0] +if said is None or len(said) != len(want) or not close(said, want, 1e-3): + error("the orienting axes, rot1, #5074 and #5077-#5079 read %s, not %s" % (said, want)) drain() # --- the orientation stays inside the rotary travel -------------------- From d7ddc04ca87cc7d47504ed904f3e5438528c2eaa Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 15:41:26 +1000 Subject: [PATCH 13/44] twinspindlekins: a mill-turn whose two spindles are two primary types A head on X Y Z tilts the tool on B between a main spindle turning its part on C and a sub spindle facing it along Z and turning its own part on W, an ANGULAR axis. Type 0 works in the frame of the main spindle's part and orients the tool with C and B, type 1 in the frame of the sub spindle's part, the main one turned half a turn about X so its Z too points at the tool, and orients it with W and B; both are primary, so G12.1 picks the spindle and a program reads the same on either part. Type 2 is the identity and the machine frame. Written as pure functions of the parameter block, with work and tool frames and the orienting letters declared, so the interpreter solves the tilted work plane on either spindle. The joints read the gauge line with B at 0 and the tool length goes along the tool, as on 5axiskins. tests/kins-params checks both working types against realtime. --- docs/src/gcode/g-code.adoc | 12 +- docs/src/man/man9/kins.9.adoc | 35 ++++ docs/src/motion/switchkins.adoc | 5 + src/Makefile | 10 + src/emc/kinematics/twinspindlekins.c | 278 +++++++++++++++++++++++++++ tests/kins-params/test.sh | 21 ++ 6 files changed, 355 insertions(+), 6 deletions(-) create mode 100644 src/emc/kinematics/twinspindlekins.c diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 7e8b66821ad..31b08ff6445 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1721,12 +1721,12 @@ change the tool length with the tool axis untilted, motion stops the program otherwise. Of those, the modules that also report where the tool points (5axiskins, -the trt modules, the trsrn modules) apply an X and Y of the offset along -the tool's own X and Y, as they apply the length along the tool axis. The -offset is given in the tool's axes, which are the machine's with the -rotaries at zero, so a tool held off the spindle axis keeps its offset as -the head or the table turns. Under the identity and the other modules, X -and Y apply along the work axes. +the trt modules, the trsrn modules, twinspindlekins) apply an X and Y of +the offset along the tool's own X and Y, as they apply the length along +the tool axis. The offset is given in the tool's axes, which are the +machine's with the rotaries at zero, so a tool held off the spindle axis +keeps its offset as the head or the table turns. Under the identity and +the other modules, X and Y apply along the work axes. [NOTE] 'G43 H0' is a little special. Its behavior is different on random diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 4b48ec336a7..16ebf850e4b 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -44,6 +44,8 @@ kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakin *loadrt 5axiskins* +*loadrt twinspindlekins* + == DESCRIPTION Rather than exporting HAL pins and functions, these components provide @@ -427,6 +429,39 @@ expected by it (XYZBCW `->` joints 0..5) one rather than replacing it. Put a given length in one column or the other, not both. +=== twinspindlekins - mill-turn with two spindles + +XYZBCW -- a head on X Y Z tilts the tool on B, about Y, between a main +spindle turning its part on C and a sub spindle facing it along Z and +turning its own part on W. Declare W `TYPE = ANGULAR` in its `[AXIS_W]` +section; C and W may be `WRAPPED_ROTARY`. Coordinates XYZBCW are required; +A, U and V may be added with the coordinates parameter and map one to one +to their joints. + +Type 0 works in the frame of the part in the main spindle, and orients the +tool with C and B. Type 1 works in the frame of the part in the sub spindle, +which is the main spindle's turned half a turn about X, so its Z too points +out of the face towards the tool, and orients the tool with W and B. Both +are primary: G12.1 selects the spindle, and a program runs the same on +either part. Type 2 is the identity and the machine frame. The rotary +directions are the conventional ones. + +With B at 0 the tool points along -Z and the slides read the gauge line; the +tool length is applied along the tool, as on 5axiskins. The sim config +configs/sim/axis/vismach/twinspindle drills one subroutine's holes into +both parts. + +*twinspindlekins.pivot-length*:: + Distance from the B rotation point to the tool holder gauge line. + +*twinspindlekins.tool-length*:: + Tool length, applied along the tool. Motion hands the module the offset + in effect (G43, G49), so the pin needs no connection. An X and Y of the + offset are applied along the tool's own X and Y. + +*twinspindlekins.spindle-distance*:: + Distance along Z from the main spindle face to the sub spindle face. + == SEE ALSO For additional information, see following subsections of the section 'Advanced Topics' of the LinuxCNC documentation: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 0a28ae9db0c..c6e7aaa8f43 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -50,6 +50,7 @@ The following kinematics modules support switchable kinematics: . *xyzab_tdr_kins* (type0:identity type1:tcp) . *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) . *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) +. *twinspindlekins* (type0:main spindle type1:sub spindle type2:identity) Every module listed above uses its own kinematics for type0 and identity kinematics for type1. Each accepts the module string @@ -72,6 +73,9 @@ type0 is then reached without running the failing kinematics again. The 'sparm' setting exchanges type0 and type1, so any G-code or HAL logic that selects a kinematics type by number must match. +twinspindlekins has no 'sparm': its type0 and type1 are both working +kinematics, one per spindle, and type2 is the identity. + Kinematics that solve the forward direction iteratively, genhexkins among them, need a pose to start from. While another type is running they take the estimate the caller supplies, which motion seeds from @@ -466,6 +470,7 @@ configs/sim/axis/vismach/ . . 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins) . 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn) . 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn) +. twinspindle/twinspindle.ini (twinspindlekins) == User kinematics provisions diff --git a/src/Makefile b/src/Makefile index 05715dd7985..3a3a94126e3 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1224,6 +1224,15 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) + +obj-m += twinspindlekins.o +twinspindlekins-objs := emc/kinematics/twinspindlekins.o +twinspindlekins-objs += libposemath/_posemath.o +twinspindlekins-objs += $(MATHSTUB) +twinspindlekins-objs += emc/kinematics/kins_util.o +twinspindlekins-objs += emc/kinematics/switchkins.o +twinspindlekins-objs += emc/kinematics/switchkins_main.o +twinspindlekins-objs += emc/kinematics/switchkins_setup.o #---------------------------------------------------------------- obj-$(CONFIG_MOTMOD) += motmod.o @@ -1390,6 +1399,7 @@ endif ../rtlib/5axiskins$(MODULE_EXT): $(addprefix objects/rt,$(5axiskins-objs)) ../rtlib/switchkins_core$(MODULE_EXT): $(addprefix objects/rt,$(switchkins_core-objs)) ../rtlib/kins_single_core$(MODULE_EXT): $(addprefix objects/rt,$(kins_single_core-objs)) +../rtlib/twinspindlekins$(MODULE_EXT): $(addprefix objects/rt,$(twinspindlekins-objs)) ../rtlib/maxkins$(MODULE_EXT): $(addprefix objects/rt,$(maxkins-objs)) ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs)) ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs)) diff --git a/src/emc/kinematics/twinspindlekins.c b/src/emc/kinematics/twinspindlekins.c new file mode 100644 index 00000000000..8592f2b2598 --- /dev/null +++ b/src/emc/kinematics/twinspindlekins.c @@ -0,0 +1,278 @@ +/******************************************************************** +* Description: twinspindlekins.c +* kinematics for a mill-turn with a tilting head and two spindles +* +* License: GPL Version 2 +* +* Notes: +* 1) The head carries the tool on X Y Z and tilts it on B, about Y, from +* a pivot pivot-length above the gauge line. With B at 0 the tool +* points along -Z, tool axis +Z, and the slides read the gauge line. +* The pose is the tip plus the tool length along the part's Z, which +* G43 takes off again, so a tilt moves the tip by the length along +* the tool axis less the length along Z the offset stands for. +* 2) The main spindle turns the work about machine Z on C, its face in +* the plane Z 0. The sub spindle faces it from spindle-distance along +* +Z and turns its own work on W, which the INI makes ANGULAR. +* 3) Type 0 works on the part in the main spindle and type 1 on the part +* in the sub spindle, each in that part's own frame: X Y Z of the part +* and B, C or W the orientation. Both are primary: G12.1 picks the +* spindle. Type 2 is the identity and stands in for the machine frame. +* 4) The sub spindle part frame is the main spindle's turned half a turn +* about X: X stays, Y and Z reverse, so Z still points out of the face +* towards the tool. Moved to the other spindle, a program reads the +* same. +* 5) The rotary directions are the conventional ones: C and W turn the +* work the way that turns the tool positively about the part's Z. +* 6) Coordinates XYZBCW are required, A U V may be added with the +* coordinates parameter and map one to one to their joints. +********************************************************************/ + +#define REQUIRED_COORDINATES "XYZBCW" + +#include +#include +#include +#include + +#include + +static const kins_param_desc twin_params[] = { + { "pivot-length", KINS_PARAM_REAL, KINS_IN, 0, { .r = 150.0 } }, + { "tool-length", KINS_PARAM_REAL, KINS_IN, 1, { .r = 0.0 } }, + { "spindle-distance", KINS_PARAM_REAL, KINS_IN, 0, { .r = 500.0 } }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH, P_SPINDLE_DISTANCE }; + +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the tool tip in the machine frame, from the slides and B (note 1) +static PmCartesian tip_from_joints(const kins_params *p, const double *joints) +{ + const double P = p->geometry[P_PIVOT_LENGTH]; + const double L = P + p->tool.tran.z; + const double b = joints[JB]*TO_RAD; + PmCartesian t; + + t.x = joints[JX] - L*sin(b); + t.y = joints[JY]; + t.z = joints[JZ] + P - L*cos(b); + return t; +} // tip_from_joints() + +// the part frame of each spindle, the machine seen from the part: machine +// = O + R part, with R = Rz(-C) for the main spindle and Rx(180) Rz(-W) for +// the sub spindle (notes 2, 4, 5) +static PmCartesian part_from_machine(const kins_params *p, int sub, + double angle, PmCartesian m) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, r; + + q = m; + if (sub) { + q.y = -m.y; + q.z = p->geometry[P_SPINDLE_DISTANCE] - m.z; + } + r.x = c*q.x - s*q.y; + r.y = s*q.x + c*q.y; + r.z = q.z; + return r; +} // part_from_machine() + +static PmCartesian machine_from_part(const kins_params *p, int sub, + double angle, PmCartesian r) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, m; + + q.x = c*r.x + s*r.y; + q.y = -s*r.x + c*r.y; + q.z = r.z; + m = q; + if (sub) { + m.y = -q.y; + m.z = p->geometry[P_SPINDLE_DISTANCE] - q.z; + } + return m; +} // machine_from_part() + +static int twin_forward(const kins_params *p, int sub, const double *joints, + EmcPose *pos) +{ + const double angle = sub ? joints[JW] : joints[JC]; + + pos->tran = part_from_machine(p, sub, angle, tip_from_joints(p, joints)); + pos->tran.z += p->tool.tran.z; + pos->b = joints[JB]; + pos->c = joints[JC]; + pos->w = joints[JW]; + + // optional letters (specify with coordinates module parameter) + pos->a = (JA != -1)? joints[JA] : 0; + pos->u = (JU != -1)? joints[JU] : 0; + pos->v = (JV != -1)? joints[JV] : 0; + + return 0; +} // twin_forward() + +static int twin_inverse(const kins_params *p, int sub, const EmcPose *pos, + double *joints) +{ + const double T = p->tool.tran.z; + const double L = p->geometry[P_PIVOT_LENGTH] + T; + const double b = pos->b*TO_RAD; + PmCartesian tip = pos->tran; + PmCartesian t; + EmcPose P; // computed position + + tip.z -= T; + t = machine_from_part(p, sub, sub ? pos->w : pos->c, tip); + P.tran.x = t.x + L*sin(b); + P.tran.y = t.y; + P.tran.z = t.z - p->geometry[P_PIVOT_LENGTH] + L*cos(b); + P.b = pos->b; + P.c = pos->c; + P.w = pos->w; + + // optional letters (specify with coordinates module parameter) + P.a = (JA != -1)? pos->a : 0; + P.u = (JU != -1)? pos->u : 0; + P.v = (JV != -1)? pos->v : 0; + + return kinsPoseToMappedJoints(p, &P, joints); +} // twin_inverse() + +static int main_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 0, joints, pos); +} + +static int main_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 0, pos, joints); +} + +static int sub_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 1, joints, pos); +} + +static int sub_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 1, pos, joints); +} + +// the tool frame, Ry(B), is the same on both spindles +static int twin_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + + rot->x.x = cb; rot->y.x = 0; rot->z.x = sb; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = -sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} // twin_tool_frame() + +// the work frames, the R of part_from_machine(), its columns the part axes +static int main_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JC]*TO_RAD), c = cos(joints[JC]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = -s; rot->y.y = c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; + + return 0; +} // main_work_frame() + +static int sub_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JW]*TO_RAD), c = cos(joints[JW]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = s; rot->y.y = -c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = -1; + + return 0; +} // sub_work_frame() + +// each spindle orients with its own rotary, the table first, and B; the +// joint map gives the same letters, declared so the table reads +static const kins_ops main_ops = { + .forward = main_forward, + .inverse = main_inverse, + .work = main_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "CB", +}; + +static const kins_ops sub_ops = { + .forward = sub_forward, + .inverse = sub_inverse, + .work = sub_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "WB", +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "twinspindlekins"; // !!! must agree with filename + kp->halprefix = "twinspindlekins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = twin_params; + kp->nparams = sizeof(twin_params)/sizeof(twin_params[0]); + + switchkinsRegisterOps(0, &main_ops); + switchkinsRegisterOps(1, &sub_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + + return 0; +} // switchkinsSetup() diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 7456b518774..a81a5f3aabf 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -104,6 +104,19 @@ run "maxkins" \ run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" 1 +# a table and a head, two primaries: one rotary turns the tool, so no +# pair, and that one names the pose +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3" +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3" 1 + run "xyzac-trt-kins coordinates=XYZAC" \ "setp xyzac-trt-kins.y-offset 3 setp xyzac-trt-kins.z-offset 11 @@ -207,6 +220,14 @@ setp xyzbca_trsrn_kins.y-offset 7 setp xyzbca_trsrn_kins.x-rot-axis 300 setp xyzbca_trsrn_kins.z-rot-axis 400" \ "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3 tool=4,3,50" 1 +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3 tool=4,3,30" +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3 tool=4,3,30" 1 # polar run "rosekins" "" "joints=3 jnt=10,5,30" From f0d4de611d0ce58812ab514a1f2a3a63f4019b21 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 15:41:26 +1000 Subject: [PATCH 14/44] sim: twinspindle, one subroutine drilling the parts in both spindles A vismach sim of twinspindlekins, the spindle line across the screen, with a demo program that drills a ring of tilted holes into the part in the main spindle and then into the part in the sub spindle through one subroutine. It names no rotary: G53.3 turns the ones the type in force orients with, and the report reads them as #<_kins_orient_1> and #<_kins_orient_2>, C and B on one spindle, W and B on the other, with their angles as #<_orient_rot1> and #<_orient_rot2>. tests/twinspindle checks the orienting axes of each type, that the same tilted hole solves and is reached on either spindle, the tip on the hole and the tool along its axis, and that the demo program runs. --- configs/sim/axis/vismach/twinspindle/README | 34 ++++ .../twinspindle/twin-spindle-holes.ngc | 64 ++++++ .../axis/vismach/twinspindle/twinspindle.hal | 15 ++ .../axis/vismach/twinspindle/twinspindle.ini | 160 +++++++++++++++ .../axis/vismach/twinspindle/twinspindle.tbl | 1 + .../vismach/twinspindle/twinspindlegui.py | 104 ++++++++++ tests/twinspindle/README | 12 ++ tests/twinspindle/checkresult | 3 + tests/twinspindle/sim.hal | 16 ++ tests/twinspindle/test-ui.py | 188 ++++++++++++++++++ tests/twinspindle/test.ini | 142 +++++++++++++ tests/twinspindle/test.sh | 4 + tests/twinspindle/tool.tbl | 1 + 13 files changed, 744 insertions(+) create mode 100644 configs/sim/axis/vismach/twinspindle/README create mode 100644 configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.hal create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.ini create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.tbl create mode 100755 configs/sim/axis/vismach/twinspindle/twinspindlegui.py create mode 100644 tests/twinspindle/README create mode 100755 tests/twinspindle/checkresult create mode 100644 tests/twinspindle/sim.hal create mode 100755 tests/twinspindle/test-ui.py create mode 100644 tests/twinspindle/test.ini create mode 100755 tests/twinspindle/test.sh create mode 100644 tests/twinspindle/tool.tbl diff --git a/configs/sim/axis/vismach/twinspindle/README b/configs/sim/axis/vismach/twinspindle/README new file mode 100644 index 00000000000..1ac1c9df910 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/README @@ -0,0 +1,34 @@ +A mill-turn with a tilting head between two spindles, on twinspindlekins. + +The head carries the tool on X Y Z and tilts it on B, about Y. The main +spindle turns its part on C; the sub spindle faces it along Z, 500 away, +and turns its own part on W. W is an angle, so [AXIS_W] declares it +TYPE = ANGULAR, and both spindles are WRAPPED_ROTARY. The vismach window +draws the spindle line across the screen as on a lathe: main spindle on +the left, Z to the right, X up. + +Kinematics type 0 works on the part in the main spindle and orients the +tool with C and B; type 1 works on the part in the sub spindle and orients +it with W and B. Each part's frame has its Z out of its own spindle's face, +so a program reads the same on either part. Type 2 is the identity, the +machine frame, which G13.1 selects. + +Demo program: + + twin-spindle-holes.ngc -- one subroutine drills a ring of six holes, + each tilted 20 degrees out from the part axis, + into the face of the part the kinematics type + in force works on; the program runs it on the + main spindle (G12.1 P0, G54) and then on the + sub spindle (G12.1 P1, G55). The subroutine + names no rotary: G53.3 turns the ones the type + orients with, and the report on the terminal + reads them by number, #<_kins_orient_1> and + #<_kins_orient_2>, 5 and 4 (C and B) on the + main spindle, 8 and 4 (W and B) on the sub + spindle, with their angles #<_orient_rot1> and + #<_orient_rot2>. + +The tool table provides tool 1, length 50. The drawing hands its pivot +length and spindle distance to the kinematics (twinspindle.hal), so the +two agree. diff --git a/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc new file mode 100644 index 00000000000..86b120350c1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc @@ -0,0 +1,64 @@ +; twin-spindle-holes.ngc - one subroutine drills the same ring of tilted +; holes into the part in the main spindle and into the part in the sub +; spindle. +; +; Kinematics type 0 works on the main spindle's part and orients the tool +; with C and B; type 1 works on the sub spindle's part and orients it with +; W and B. The subroutine names neither: G53.3 turns whatever rotaries the +; type in force orients with, and the report reads them by number, +; #<_kins_orient_1> and #<_kins_orient_2>, with their angles in the same +; order, #<_orient_rot1> and #<_orient_rot2>. Each part's frame has its Z +; out of its own spindle, so both faces are at Z60 and the offsets read +; the same. + +o sub + # = 6 ; holes + # = 30 ; hole circle radius on the face + # = 20 ; tilt of each hole out from the part axis + # = 0 + o101 while [# lt #] + # = [# * 360 / #] + ; the hole's plane: origin on the face, X along the hole circle, + ; Z the hole axis + g68.2 p3 q1 x[#*cos[#]] y[#*sin[#]] z0 i[-sin[#]] j[cos[#]] k0 + g68.2 p3 q2 i[sin[#]*cos[#]] j[sin[#]*sin[#]] k[cos[#]] + g53.2 ; solve the pose, report it + (print, hole #: axis #<_kins_orient_1> to #<_orient_rot1>, axis #<_kins_orient_2> to #<_orient_rot2>) + g53.3 x0 y0 z5 ; orient and go, 5 above the hole + g1 z-10 + g0 z5 + # = [# + 1] + o101 endwhile + g0 z40 + g69 +o endsub + +; from one part to the other in the machine frame, the identity type: +; up, clear of both parts, and the head pointing down +o sub + g13.1 + g53 g0 x300 + g53 g0 z50 b90 +o endsub + +g21 g90 g94 g17 g40 +t1 m6 +g43 +f300 + +g10 l2 p1 x0 y0 z60 c0 w0 ; G54, the main spindle's part +g10 l2 p2 x0 y0 z60 c0 w0 ; G55, the sub spindle's part + +o call +g12.1 p0 ; the main spindle's part +g54 +o call + +o call +g12.1 p1 ; the sub spindle's part +g55 +o call + +o call +g54 +m2 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.hal b/configs/sim/axis/vismach/twinspindle/twinspindle.hal new file mode 100644 index 00000000000..16044ff4de1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.hal @@ -0,0 +1,15 @@ +loadusr -W ./twinspindlegui.py + +net :jx joint.0.pos-fb twinspindlegui.jx +net :jy joint.1.pos-fb twinspindlegui.jy +net :jz joint.2.pos-fb twinspindlegui.jz +net :jb joint.3.pos-fb twinspindlegui.jb +net :jc joint.4.pos-fb twinspindlegui.jc +net :jw joint.5.pos-fb twinspindlegui.jw + +# the drawing and the kinematics share the geometry +net :pivot-len twinspindlegui.pivot_len twinspindlekins.pivot-length +net :spindle-distance twinspindlegui.spindle_distance twinspindlekins.spindle-distance + +net :tool-len motion.tooloffset.z twinspindlegui.tool_length +net :tool-diam halui.tool.diameter twinspindlegui.tool_diam diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.ini b/configs/sim/axis/vismach/twinspindle/twinspindle.ini new file mode 100644 index 00000000000..438e0a85fd4 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.ini @@ -0,0 +1,160 @@ +[EMC] +VERSION = 1.1 +MACHINE = Sim-twinspindlekins (XYZBCW mill-turn, two spindles) + DEBUG = 0 + +[DISPLAY] +# X Y Z are already the tip in the frame of the part the type works on, so +# the plot takes them as they are: a rotary in GEOMETRY would turn them +# again, and W, a translation there, would draw as a move along Z + GEOMETRY = XYZ + DISPLAY = axis + OPEN_FILE = ./twin-spindle-holes.ngc + INCREMENTS = 10 mm, 1 mm, .1 mm + JOG_AXES = XYZ + CYCLE_TIME = 0.100 + POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_FEED_OVERRIDE = 2.0 + PROGRAM_PREFIX = ../../nc_files/ + INTRO_GRAPHIC = linuxcnc.gif + INTRO_TIME = 2 + TOOL_EDITOR = tooledit z diam + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 + PARAMETER_FILE = twinspindle.var + +[EMCMOT] + EMCMOT = motmod +COMM_TIMEOUT = 1.0 +SERVO_PERIOD = 1000000 + +[TASK] + TASK = milltask +CYCLE_TIME = 0.010 + +[HAL] + HALUI = halui +HALFILE = LIB:basic_sim.tcl +HALFILE = twinspindle.hal + +[TRAJ] + COORDINATES = XYZBCW + LINEAR_UNITS = mm + ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 + MAX_LINEAR_VELOCITY = 200.0 +MAX_LINEAR_ACCELERATION = 800.0 +DEFAULT_LINEAR_ACCELERATION = 800.0 + MAX_ANGULAR_VELOCITY = 360.0 + +[EMCIO] +TOOL_TABLE = twinspindle.tbl + +[KINS] +# type 0: the part in the main spindle, C and B orient +# type 1: the part in the sub spindle, W and B orient +# type 2: identity, the machine frame +KINEMATICS = twinspindlekins + JOINTS = 6 + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.tbl b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill diff --git a/configs/sim/axis/vismach/twinspindle/twinspindlegui.py b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py new file mode 100755 index 00000000000..4d4f25178cb --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. + +# The twinspindlekins machine: a B tilting head on X Y Z between a main +# spindle (C) and a sub spindle (W) that face each other along Z. The model +# is built in machine coordinates and turned at the end so that Z, the +# spindle line, runs across the screen as on a lathe: main spindle on the +# left, Z to the right, X up. +# +# pivot_len and spindle_distance are handed to the kinematics by +# twinspindle.hal, so the drawing and the maths agree; they can be set at +# invocation, example: twinspindlegui.py pivot_len=200 + +from vismach import * +import hal +import sys + +pivot_len = 150 # gauge line to the B axis +spindle_distance = 500 # main spindle face to sub spindle face +lat, lon = -80, 0 # the starting view, from the front + +for setting in sys.argv[1:]: + exec(setting) + +c = hal.component("twinspindlegui") +for pin in ("jx", "jy", "jz", "jb", "jc", "jw", "tool_length", "tool_diam"): + c.newpin(pin, hal.Type.REAL, hal.Dir.IN) +c.newpin("pivot_len", hal.Type.REAL, hal.Dir.OUT) +c.newpin("spindle_distance", hal.Type.REAL, hal.Dir.OUT) +c["pivot_len"] = pivot_len +c["spindle_distance"] = spindle_distance +c.ready() + +class HalToolCylinder(CylinderZ): + def __init__(self, comp, *args): + CylinderZ.__init__(self, *args) + self.comp = comp + + def coords(self): + r = 5 # default if hal pin not set + if c.tool_diam > 0: r = c.tool_diam/2 + return -self.comp.tool_length, r, 0, r + +# a chuck and a part, face at z 0, the part standing on +z; the flat on +# the part shows the spindle turning +def spindle(work=None): + part = [CylinderZ(0, 50, 60, 50), + Color([1, 0.2, 0.2, 1], [Box(40, -8, 0, 52, 8, 60)])] + if work: + part.append(Translate([work], 0, 0, 60)) + return Collection([Color([0.6, 0.6, 0.7, 1], [CylinderZ(-100, 90, 0, 90)]), + Color([0.9, 0.8, 0.5, 1], part)]) + +# the main spindle turns the work by -C: its frame is Rz(-C) +work = Capture() +main_spindle = HalRotate([spindle(work)], c, "jc", -1, 0, 0, 1) +main_spindle = Collection([main_spindle, + Box(-160, -160, -400, 160, 160, -100)]) + +# the sub spindle is the same, turned by -W, then half a turn about X to +# face the main spindle from spindle_distance +sub_spindle = HalRotate([spindle()], c, "jw", -1, 0, 0, 1) +sub_spindle = Collection([sub_spindle, + Box(-160, -160, -400, 160, 160, -100)]) +sub_spindle = Rotate([sub_spindle], 180, 1, 0, 0) +sub_spindle = HalTranslate([sub_spindle], c, "spindle_distance", 0, 0, 1) + +# the head, gauge line at z 0: the tool below it, the spindle motor above +# up to the B axis at pivot_len, which is then put at the origin +tooltip = Capture() +head = Collection([HalTranslate([tooltip], c, "tool_length", 0, 0, -1), + Color([0.9, 0.9, 0.2, 1], [HalToolCylinder(c)]), + CylinderZ(0, 30, 40, 45), + CylinderZ(40, 45, pivot_len - 45, 45), + Translate([CylinderY(-55, 55, 55, 55)], 0, 0, pivot_len)]) +head = Translate([head], 0, 0, -pivot_len) +head = HalRotate([head], c, "jb", 1, 0, 1, 0) + +# the ram carries the B axis from behind, along -Y, clear of the head +# whichever way it tilts; the slides read the gauge line at B 0, pivot_len +# below the B axis +head = Collection([head, + Box(-50, -700, -50, 50, -60, 50)]) +head = Translate([head], 0, 0, pivot_len) +head = HalTranslate([head], c, "jx", 1, 0, 0) +head = HalTranslate([head], c, "jy", 0, 1, 0) +head = HalTranslate([head], c, "jz", 0, 0, 1) + +bed = Box(-420, -900, -450, -400, 300, 1000) + +model = Collection([bed, main_spindle, sub_spindle, head]) +# Z across the screen to the right, X up, as on a lathe +model = Rotate([model], -90, 0, 1, 0) +model = Rotate([model], 180, 0, 0, 1) + +main(model, tooltip, work, size=900, lat=lat, lon=lon) diff --git a/tests/twinspindle/README b/tests/twinspindle/README new file mode 100644 index 00000000000..c6e5486836b --- /dev/null +++ b/tests/twinspindle/README @@ -0,0 +1,12 @@ +twinspindlekins: a B tilting head between a main spindle on C and a sub +spindle on W, W ANGULAR, each spindle a primary kinematics type in the +frame of its own part, and an identity type. + +The test checks that each type names its own rotaries on +#<_kins_orient_1> to #<_kins_orient_3> and their head flags, C then B on +the main spindle, W then B on the sub spindle, nothing on the identity; +that G53.2 and G53.3 on the same tilted hole give the spindle angle and a +B on either side of 180 as #<_orient_rot1> and #<_orient_rot2>, and put +the tip on the hole with the tool along its axis, worked out from the +joints; and that the sim config's demo program drills both parts through +one subroutine and parks. diff --git a/tests/twinspindle/checkresult b/tests/twinspindle/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twinspindle/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twinspindle/sim.hal b/tests/twinspindle/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twinspindle/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twinspindle/test-ui.py b/tests/twinspindle/test-ui.py new file mode 100755 index 00000000000..3daa9642821 --- /dev/null +++ b/tests/twinspindle/test-ui.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +# twinspindlekins: two primary types, one per spindle, orienting with +# different rotaries: see README. + +import linuxcnc +import math +import os +import sys +import time + +JOINTS = 6 +PIVOT = 150.0 # twinspindlekins.pivot-length default +DISTANCE = 500.0 # twinspindlekins.spindle-distance default +TOOL = 50.0 # T1 in tool.tbl +FACE = 60.0 # G54 and G55 Z, the part faces +PROGRAM = os.path.abspath("../../configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc") + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + c.wait_complete(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +def turn(a, b): + # a and b the same angle, whole turns apart + return abs((a - b + 180.0) % 360.0 - 180.0) < 1e-6 + +# the module's maths, written out again: the tip and the tool axis, tip +# towards holder, in the frame of the part on spindle `sub`. The slides +# carry the B axis PIVOT above the gauge line, the tip hangs PIVOT + TOOL +# below it along the tool axis. +def part_pose(j, sub): + L = PIVOT + TOOL + b = math.radians(j[3]) + m = [j[0] - L*math.sin(b), j[1], j[2] + PIVOT - L*math.cos(b)] + u = [math.sin(b), 0.0, math.cos(b)] + if sub: + m = [m[0], -m[1], DISTANCE - m[2]] + u = [u[0], -u[1], -u[2]] + a = math.radians(j[5] if sub else j[4]) + rz = lambda v: [math.cos(a)*v[0] - math.sin(a)*v[1], + math.sin(a)*v[0] + math.cos(a)*v[1], v[2]] + return rz(m), rz(u) + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") + +mdi("T1 M6", "G43", + "G10 L2 P1 X0 Y0 Z%g C0 W0" % FACE, + "G10 L2 P2 X0 Y0 Z%g C0 W0" % FACE) + +# ---- each type names its own rotaries -------------------------------------- + +for ktype, want, what in ((0, [5, 4, -1, 0, 1, -1], "C then B"), + (1, [8, 4, -1, 0, 1, -1], "W then B"), + (2, [-1] * 6, "nothing")): + mdi("G12.1 P%d" % ktype) + got = [param(n) for n in ORIENT] + if got != want: + error("type %d orients with %s, not %s: table then head" % (ktype, got, what)) + +# ---- the same hole on either spindle --------------------------------------- +# +# A hole 30 out on the face at 60 degrees, tilted 20 degrees outwards. On +# the main spindle C turns it under the head and B leans the tool; on the +# sub spindle, whose part faces the other way, W turns it and B leans the +# tool past 180. + +A, T, R = 60.0, 20.0, 30.0 +n = [math.sin(math.radians(T))*math.cos(math.radians(A)), + math.sin(math.radians(T))*math.sin(math.radians(A)), + math.cos(math.radians(T))] +hole = [R*math.cos(math.radians(A)), R*math.sin(math.radians(A)), 0.0] +PLANE = ("G68.2 P3 Q1 X%.9f Y%.9f Z0 I%.9f J%.9f K0" + % (hole[0], hole[1], -math.sin(math.radians(A)), math.cos(math.radians(A))), + "G68.2 P3 Q2 I%.9f J%.9f K%.9f" % tuple(n)) + +for ktype, offset, joint, b in ((0, "G54", 4, T), (1, "G55", 5, 180.0 - T)): + sub = ktype == 1 + mdi("G13.1", "G53 G0 X300", "G53 G0 Z50 B90") + mdi("G12.1 P%d" % ktype, offset, *PLANE) + mdi("G53.2") + rot1, rot2 = param("_orient_rot1"), param("_orient_rot2") + print("type %d: rot1 %.6f rot2 %.6f" % (ktype, rot1, rot2)) + if not (turn(rot1, A) and abs(rot2 - b) < 1e-6): + error("type %d: G53.2 gave rot1 %s rot2 %s, not %s and %s" % (ktype, rot1, rot2, A, b)) + j = mdi("G53.3 X0 Y0 Z5") + if not (turn(j[joint], A) and abs(j[3] - b) < 1e-6): + error("type %d: G53.3 left joint %d at %s and B at %s" % (ktype, joint, j[joint], j[3])) + tip, axis = part_pose(j, sub) + want = [hole[i] + 5*n[i] + (FACE if i == 2 else 0.0) for i in range(3)] + if max(abs(tip[i] - want[i]) for i in range(3)) > 1e-6: + error("type %d: the tip is at %s in the part, not %s" % (ktype, tip, want)) + if max(abs(axis[i] - n[i]) for i in range(3)) > 1e-9: + error("type %d: the tool points along %s in the part, not %s" % (ktype, axis, n)) + mdi("G69") +drain() + +# ---- the demo program, both spindles, one subroutine ----------------------- + +mdi("G13.1", "G54") +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete() +c.feedrate(100) +c.program_open(PROGRAM) +c.auto(linuxcnc.AUTO_RUN, 0) +started = time.time() + 5 +while time.time() < started: + s.poll() + if s.interp_state != linuxcnc.INTERP_IDLE: + break + time.sleep(0.002) +deadline = time.time() + 300 +while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and s.queue == 0 and s.inpos: + break + time.sleep(0.01) +else: + error("the demo program did not finish") +drain() +j = settled() +if max(abs(j[i] - w) for i, w in ((0, 300.0), (2, 50.0), (3, 90.0))) > 1e-6: + error("the demo program ended at %s, not parked" % (j,)) + +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("Exiting with %d errors" % errors) +sys.exit(errors != 0) diff --git a/tests/twinspindle/test.ini b/tests/twinspindle/test.ini new file mode 100644 index 00000000000..0648bd7c100 --- /dev/null +++ b/tests/twinspindle/test.ini @@ -0,0 +1,142 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py +MAX_FEED_OVERRIDE = 100 + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = twinspindlekins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end; the feed override runs the drilling of the demo program fast +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 4000 +MAX_LINEAR_ACCELERATION = 80000 +DEFAULT_LINEAR_ACCELERATION = 80000 +MAX_ANGULAR_VELOCITY = 7200 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 1800 +MAX_ACCELERATION = 40000 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 3600 +MAX_ACCELERATION = 72000 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 3600 +MAX_ACCELERATION = 72000 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 4000 +MAX_ACCELERATION = 80000 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 1800 +MAX_ACCELERATION = 40000 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 3600 +MAX_ACCELERATION = 72000 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 3600 +MAX_ACCELERATION = 72000 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/tests/twinspindle/test.sh b/tests/twinspindle/test.sh new file mode 100755 index 00000000000..f494f0fc1fb --- /dev/null +++ b/tests/twinspindle/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries the offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/twinspindle/tool.tbl b/tests/twinspindle/tool.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/tests/twinspindle/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill From 61e8f3871b7edc8449510591e1e1526f9d1eb8e3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 18 Oct 2026 18:37:51 +1100 Subject: [PATCH 15/44] sim: twinspindle moves G53, G28 and G30 in the machine frame Like the five axis sims: the INI sets MACHINE_MOVES_IN_MACHINE_FRAME with a comment, and the README says so, so G53 puts the slides in the same place on either spindle's type whatever the tool and the head angle. The demo program and tests/twinspindle (its own test.ini) use no G53, G28 or G30 off the machine frame type. --- configs/sim/axis/vismach/twinspindle/README | 3 +++ configs/sim/axis/vismach/twinspindle/twinspindle.ini | 6 ++++++ 2 files changed, 9 insertions(+) diff --git a/configs/sim/axis/vismach/twinspindle/README b/configs/sim/axis/vismach/twinspindle/README index 1ac1c9df910..3b75fd5cfb6 100644 --- a/configs/sim/axis/vismach/twinspindle/README +++ b/configs/sim/axis/vismach/twinspindle/README @@ -13,6 +13,9 @@ it with W and B. Each part's frame has its Z out of its own spindle's face, so a program reads the same on either part. Type 2 is the identity, the machine frame, which G13.1 selects. +The INI sets MACHINE_MOVES_IN_MACHINE_FRAME, so G53, G28 and G30 move +the slides in that machine frame whichever spindle's type is in force. + Demo program: twin-spindle-holes.ngc -- one subroutine drills a ring of six holes, diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.ini b/configs/sim/axis/vismach/twinspindle/twinspindle.ini index 438e0a85fd4..d03f27002e7 100644 --- a/configs/sim/axis/vismach/twinspindle/twinspindle.ini +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.ini @@ -24,6 +24,12 @@ MAX_FEED_OVERRIDE = 2.0 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 PARAMETER_FILE = twinspindle.var +# Off the machine frame kinematics (on either spindle's type), G53, G28 and +# G30 move the slides in the machine frame, as G53.5, G28.5 and G30.5 do, +# wherever the tool and the head point, and G28.1 and G30.1 store that +# position. Unset, they move in the world of the spindle's type. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [EMCMOT] EMCMOT = motmod From e1046adc3e7474e73bdbe39d385ee5573b008624 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 26 Sep 2026 22:35:50 +1000 Subject: [PATCH 16/44] genserkins: build the Jacobian from the DH frames compute_jfwd() converts every link to a quaternion pose twice and builds the Jacobian through generic matrix calls: 0.9 us of the 1.2 us that genser_jacobian() takes. tpnext asks for up to about 60 Jacobians per move near a singular pose, and genser_inverse() takes one per iteration. genser_jfwd_frames() chains the DH frames as rotations and origins and takes each column as z_i x (o_n - o_i), z_i. It gives what compute_jfwd() gives to within 3e-10 relative over 20000 random poses, in 0.12 us. genser_jacobian() and genser_inverse() use it; genser_kin_jac_inv() and genser_kin_jac_fwd() keep compute_jfwd(). With the puma560 genser config, adding a move near the wrist singular pose falls from 153 to 71 us on average and from 207 to 93 us at most; other moves fall from 36 to 22 us. --- src/emc/kinematics/genserfuncs.c | 76 +++++++++++++++++++++++++------- 1 file changed, 61 insertions(+), 15 deletions(-) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 612a72d2b2c..783a68dc9a5 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -507,6 +507,65 @@ int compute_jinv(const genser_matrix * Jfwd, genser_matrix * Jinv) return GENSER_OK; } +/* What compute_jfwd() gives, straight from the DH frames in the base + frame: joint i moves along or turns about z_i, the axis of frame i + through its origin o_i, so the last frame's origin moves z_i or + z_i x (o_n - o_i) and turns 0 or z_i. The planner asks for many + Jacobians along each move, and this one skips the quaternions. */ +static void genser_jfwd_frames(const genser_struct *genser, const double *jest, + genser_matrix *Jfwd) +{ + double R[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + double o[3] = {0.0, 0.0, 0.0}; + double z[GENSER_MAX_JOINTS][3], oi[GENSER_MAX_JOINTS][3]; + const int n = genser->link_num; + + for (int i = 0; i < n; i++) { + const genser_link *link = &genser->links[i]; + const int prismatic = GENSER_QUANTITY_LENGTH == link->quantity; + const double th = prismatic ? link->dh.theta : jest[i]; + const double d = prismatic ? jest[i] : link->dh.d; + const double sal = sin(link->dh.alpha), cal = cos(link->dh.alpha); + const double sth = sin(th), cth = cos(th); + // as link_pose_build(): Rx(alpha) Tx(a) Rz(theta) Tz(d) + const double L[3][3] = {{cth, -sth, 0.0}, + {sth * cal, cth * cal, -sal}, + {sth * sal, cth * sal, cal}}; + const double t[3] = {link->dh.a, -sal * d, cal * d}; + double N[3][3]; + + for (int r = 0; r < 3; r++) { + o[r] += R[r][0] * t[0] + R[r][1] * t[1] + R[r][2] * t[2]; + } + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + N[r][c] = R[r][0] * L[0][c] + R[r][1] * L[1][c] + R[r][2] * L[2][c]; + } + } + memcpy(R, N, sizeof(R)); + for (int r = 0; r < 3; r++) { + z[i][r] = R[r][2]; + oi[i][r] = o[r]; + } + } + for (int i = 0; i < n; i++) { + const double v[3] = {o[0] - oi[i][0], o[1] - oi[i][1], o[2] - oi[i][2]}; + if (GENSER_QUANTITY_LENGTH == genser->links[i].quantity) { + for (int r = 0; r < 3; r++) { + Jfwd->el[r][i] = z[i][r]; + Jfwd->el[3 + r][i] = 0.0; + } + continue; + } + Jfwd->el[0][i] = z[i][1] * v[2] - z[i][2] * v[1]; + Jfwd->el[1][i] = z[i][2] * v[0] - z[i][0] * v[2]; + Jfwd->el[2][i] = z[i][0] * v[1] - z[i][1] * v[0]; + for (int r = 0; r < 3; r++) { + Jfwd->el[3 + r][i] = z[i][r]; + } + } +} + /* The Jacobian in the terms of kinematics.h: joints in degrees per pose word in EmcPose units, the derivative of genser_inverse(). @@ -529,8 +588,6 @@ static int genser_jacobian(const kins_params *p, const double *joint, genser_struct genser_stg; genser_struct *genser = &genser_stg; genser_matrix Jfwd, Jinv; - PmPose T_L_0; - genser_link linkout[GENSER_MAX_JOINTS] = {}; double jest[GENSER_MAX_JOINTS]; double E[3][3]; double sb, cb, sc, cc; @@ -552,12 +609,7 @@ static int genser_jacobian(const kins_params *p, const double *joint, matrix_init(&Jfwd, 6, genser->link_num); matrix_init(&Jinv, genser->link_num, 6); - for (link = 0; link < genser->link_num; link++) { - link_joint_set(&genser->links[link], jest[link], &linkout[link]); - } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GENSER_OK != retval) - return -1; + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GENSER_OK != retval) return -1; // singular: no finite joint rate follows the pose @@ -691,7 +743,6 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, genser_struct genser_stg; genser_struct *genser = &genser_stg; genser_matrix Jfwd, Jinv; - PmPose T_L_0; double dvw[6]; double jest[GENSER_MAX_JOINTS]; double dj[GENSER_MAX_JOINTS]; @@ -736,12 +787,7 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, for (link = 0; link < genser->link_num; link++) { link_joint_set(&genser->links[link], jest[link], &linkout[link]); } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GENSER_OK != retval) { - rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", - joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - return retval; - } + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GENSER_OK != retval) { rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", From c992f247501300c20faa165f363ca3da6505a96a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 27 Sep 2026 00:19:27 +1000 Subject: [PATCH 17/44] tp: hold the joints when an abort stops a joint interpolated segment The abort reset the planner onto its own position, which inside a joint interpolated segment is the chord between the world ends, and forgot the joints; the servo thread then inverted the chord point and the joints jumped there in one cycle (on 5axiskins, pivot 400, Y at 166670 mm/s). The joints the segment stopped on are now handed out as its end, and the planner takes the point the servo thread found from them, so the machine stays where it stopped. --- src/emc/tp/tp.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 33499c60bab..0c0eb89a2da 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3526,9 +3526,21 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { + /* stopped inside a joint interpolated segment: the machine is + where the joints put it, which the servo thread found from them + last cycle, not on the chord this planner reports; the joints + are handed out as an end so that they stay where they are */ + double stop_joints[EMCMOT_MAX_JOINTS] = {0}; + int joint_stop = tc->active && tcGetJointPos(tc, stop_joints) > 0; tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tpForgetJoints(tp); + if (joint_stop) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = stop_joints[i]; } + tp->joint_end_valid = 1; + tp->currentPos = emcmotStatus->carte_pos_cmd; + } tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; From d8751f3c26c3a4b182a62476071c5628283aeba7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 27 Sep 2026 00:40:59 +1000 Subject: [PATCH 18/44] tp: cap a joint interpolated segment by the max velocity slider The slider left joint interpolated segments alone, since they run in joint units. It now caps the tool's chord between the ends, measured as canon measures a straight move (X Y Z, else U V W, not at all for the rotaries alone): the segment takes at least as long as the chord would at the slider's speed. With the slider at 10 mm/s a G53.4 over a 100 mm chord takes 10.07 s instead of 1.04 s. --- src/emc/tp/tp.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 0c0eb89a2da..8c700ce339e 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2182,10 +2182,24 @@ int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, if (tc.target < TP_POS_EPSILON) { return TP_ERR_ZERO_LENGTH; } + // it runs in joint units: the max velocity slider caps the tool as canon + // measures a straight move, along the chord between the ends: X Y Z, + // else U V W, and not at all when only the rotaries move + { + double dx = world_end.tran.x - jl->world_start.tran.x; + double dy = world_end.tran.y - jl->world_start.tran.y; + double dz = world_end.tran.z - jl->world_start.tran.z; + double du = world_end.u - jl->world_start.u; + double dv = world_end.v - jl->world_start.v; + double dw = world_end.w - jl->world_start.w; + double chord = pmSqrt(dx * dx + dy * dy + dz * dz); + if (chord < TP_POS_EPSILON) { + chord = pmSqrt(du * du + dv * dv + dw * dw); + } + tc.vlimit_scale = chord < TP_POS_EPSILON ? 0.0 : tc.target / chord; + } tc.nominal_length = tc.target; tcClampVelocityByLength(&tc); - // it runs in joint units, so the max velocity slider does not cap it - tc.vlimit_scale = 0.0; tc.indexer_jnum = -1; tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); From 95fa99cc7546e7b1e61e2eedf36e2160b5ab3f62 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 5 Oct 2026 02:29:29 +1100 Subject: [PATCH 19/44] docs: the two poses differ in the first rotary from the tool The conventions chapter said the two poses that reach one tool axis differ in the sign of the secondary rotary. That holds for two head rotaries only; on two table rotaries it is the one carrying the other, and on a head and a table it is the head, as G53.1 P now uses. --- docs/src/motion/kinematics-conventions.adoc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index dd109f87527..3ce73233498 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -337,9 +337,11 @@ every block. It is the inverse of the tool frame: not where the tool points for these joints, but which joints point it there. It has more than one answer. A five-axis machine reaches a given tool axis two -ways, one with the secondary rotary positive and one with it negative, and -which of the two is wanted depends on the joint limits and on what the operator -asked for, shortest move or one direction of rotation only. So the module +ways, which differ in the sign of the first rotary met going from the tool to +the work: the secondary rotary of two in the head, the one carrying the other +of two in the table, the head rotary of one of each. Which of the two is wanted +depends on the joint limits and on what the operator asked for, shortest move +or one direction of rotation only. So the module reports every set of joint values that reaches the orientation and stops there. It does not apply the joint limits and it does not rank the answers, because neither is geometry: a module that picked for the caller would be picking with From 7815f2de354edca3d13fb194301518f9d260b016 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 5 Oct 2026 22:06:41 +1100 Subject: [PATCH 20/44] canon: refuse a move whose joints leave their travel along it Motion checks the joints against [JOINT_n] MIN_LIMIT and MAX_LIMIT at the end of a move, so a move whose ends are both inside the travel can still pass a limit between them: a turn of a tilted head under the tool centre point swings the carriage round the tip, and an arc bulges past the travel its ends stay inside. Motion then stops on the soft limit partway through the move, the joint past its limit, to be jogged off it in joint mode. Rimas2200 bounded the joints along a move module by module in the first version of #4619; this follows them on the sampling canon already does, for every module evaluated outside realtime. The identity samples nothing, as before: its joints are the axes, and its arcs are what #4619 bounds. motion_planning/segment_cap.cc follows each joint against its travel on the samples segmentCap() takes for the joint velocity and acceleration cap: a sample past a limit, or between two samples the peak of the cubic through their positions and rates, read again through the inverse at that peak. A peak near the limit that the inverse does not confirm splits the stretch there, a bounded number of times. Only a point the inverse itself puts past a limit refuses the move. A side of the travel the joint starts the segment past is not followed, the move may be bringing it back, and neither is anything beyond a sample the module cannot invert or a joint moving between two points further than its rates allow, as where the inverse changes branch. The limits are the ones motion holds, ini pins included, on the joints that are homed. Canon drops the refused move and every move after it until task has taken the refusal, since a move sent after a dropped one would start from where that one began. Task reads it after the interpreter reads or executes a line and fails the line the way an interpreter error does, saying which joint would pass which limit, by how much and where along the move. A line stepped over on the way to the start line of a run lets its refusal go with the rest of its output: its moves are thrown away, and were sampled from wherever the machine stands. RIGID_TAP flushes the chained segments before it is sampled, so that the tap is read from where it starts. kinslimits takes --min and --max and prints where a joint leaves its travel. tests/kins-limits/travel swings a carriage 0.05 past its Y travel at a peak between two samples, 0.05 inside it and well past it, asks for the refused swing twice and runs moves inside the travel after it, refuses a move for its end, runs an arc under the travel and refuses it over the top, fails a program on the line of the swing with no joint past its limit, and runs the same program from the line after the swing, which only steps over it. Without the reading between samples the 0.05 swing runs onto the soft limit; without the exemption for stepped over lines the run from that line fails. --- docs/src/config/ini-config.adoc | 7 +- docs/src/motion/switchkins.adoc | 15 ++ src/emc/motion_planning/kinslimits.cc | 19 +- src/emc/motion_planning/segment_cap.cc | 300 ++++++++++++++++++++++--- src/emc/motion_planning/segment_cap.hh | 25 ++- src/emc/nml_intf/canon.hh | 6 + src/emc/task/emccanon.cc | 133 ++++++++++- src/emc/task/emctaskmain.cc | 29 ++- tests/kins-limits/travel/README | 16 ++ tests/kins-limits/travel/checkresult | 3 + tests/kins-limits/travel/sim.hal | 16 ++ tests/kins-limits/travel/test-ui.py | 196 ++++++++++++++++ tests/kins-limits/travel/test.ini | 136 +++++++++++ tests/kins-limits/travel/test.sh | 4 + tests/kins-limits/travel/tool.tbl | 1 + tests/kins-limits/travel/travel.ngc | 8 + 16 files changed, 864 insertions(+), 50 deletions(-) create mode 100644 tests/kins-limits/travel/README create mode 100755 tests/kins-limits/travel/checkresult create mode 100644 tests/kins-limits/travel/sim.hal create mode 100755 tests/kins-limits/travel/test-ui.py create mode 100644 tests/kins-limits/travel/test.ini create mode 100755 tests/kins-limits/travel/test.sh create mode 100644 tests/kins-limits/travel/tool.tbl create mode 100644 tests/kins-limits/travel/travel.ngc diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index e8aabf083d6..2cb57345001 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1204,9 +1204,10 @@ These settings are verified at startup when the trivkins kinematics modules is s The `[JOINT_N]MIN_LIMIT`/`MAX_LIMIT` settings are enforced while jogging in joint mode prior to homing. After homing, `[AXIS_L]MIN_LIMIT`/`MAX_LIMIT` coordinate limits are used as constraints for axis (coordinate letter) jogging and by the trajectory planning used for G-code moves (programs and MDI commands). -The trajectory planner works in Cartesian space (XYZABCUVW) and has no information about the motion of joints implemented by *any* kinematics module. -It is possible for joint limit violations to occur for G-code that obeys trajectory planning position limits when non identity kinematics are used. -The motion module always detects joint position limit violations and faults if they occur during the execution of G-code commands. +The trajectory planner works in Cartesian space (XYZABCUVW). +On a kinematics that is not the identity, every move of a program or an MDI command is also followed through the kinematics module ahead of motion, and a move that would take a joint past `MIN_LIMIT` or `MAX_LIMIT` anywhere along it is refused before it runs, its line failing with the joint and the limit; see the <> chapter, INI file limit settings. +A kinematics module that can only be evaluated in realtime is checked at the ends of each move. +The motion module detects a joint position limit violation during the execution of G-code commands and faults, for what cannot be seen ahead of it. See also related https://github.com/LinuxCNC/linuxcnc/issues/97[GitHub issue #97]. ==== diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index c6e7aaa8f43..661870fd27d 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -316,6 +316,21 @@ the box is a restriction inside it. A module that declares no kinstype flags keeps the box on the world of the kinstype in force, as before. +The travel is followed along each move, not only at its end. Ahead +of motion every segment is sampled through the module, on the +kinstype and the tool offset the program is under, and a move that +would take a homed joint past its `MIN_LIMIT` or `MAX_LIMIT` anywhere +along it is refused before it is sent: a turn of a tilted head under +the tool centre point that swings the carriage past its travel and +back, with both ends of the move inside it, fails its line with the +joint, the limit and where along the move it would be passed, and +nothing of it runs. Between samples the joint is followed on the +cubic through their positions and rates, and a peak that comes near +the limit is read through the module itself; only a point the module +puts past the limit refuses a move. A module that can only be +evaluated in realtime is checked at the ends of each move; motion's +own check of the joints while they move stays behind both. + The velocity and acceleration limits, `MAX_VELOCITY` and `MAX_ACCELERATION`, bound the coordinate letter in the world of the kinstype in force. What the joints can follow is bound separately: diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc index 04ca010defc..31731fb22c4 100644 --- a/src/emc/motion_planning/kinslimits.cc +++ b/src/emc/motion_planning/kinslimits.cc @@ -92,11 +92,13 @@ static void usage(const char *argv0) "usage: %s --module NAME --joints N --coords LETTERS\n" " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--min m0,m1,... --max m0,m1,...]\n" " [--samples N] [--singularity COND]\n" "\n" "Prints the Jacobian and the world-space caps the joint limits imply\n" - "for a straight move from --start to --end. Requires a running HAL\n" - "instance with the kinematics module loaded.\n", argv0); + "for a straight move from --start to --end, and with --min and --max\n" + "the first place along it a joint leaves its travel. Requires a\n" + "running HAL instance with the kinematics module loaded.\n", argv0); } int main(int argc, char **argv) @@ -106,7 +108,7 @@ int main(int argc, char **argv) int num_joints = 0; int samples = 11; double singularity = 100.0; - std::vector start_v, end_v, vel_v, acc_v, jerk_v; + std::vector start_v, end_v, vel_v, acc_v, jerk_v, min_v, max_v; for (int i = 1; i < argc; i++) { const char *a = argv[i]; @@ -121,6 +123,8 @@ int main(int argc, char **argv) else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else if (!strcmp(a, "--min") && next) { min_v = parse_list(next); i++; } + else if (!strcmp(a, "--max") && next) { max_v = parse_list(next); i++; } else { usage(argv[0]); return 1; } } @@ -275,6 +279,8 @@ int main(int argc, char **argv) for (int j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { lim.vel[j] = j < num_joints ? vel_v[j] : SEGMENT_CAP_NONE; lim.acc[j] = j < num_joints ? acc_v[j] : SEGMENT_CAP_NONE; + lim.min[j] = j < (int)min_v.size() ? min_v[j] : -HUGE_VAL; + lim.max[j] = j < (int)max_v.size() ? max_v[j] : HUGE_VAL; } auto pose_at = [](double f, EmcPose *p, void *arg) { const EmcPose *se = (const EmcPose *)arg; @@ -285,6 +291,13 @@ int main(int argc, char **argv) if (segmentCap(ctx, &lim, target, samples, pose_at, &line, joints, &cap) == 0) { printf("canon's cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), %d of %d samples unanswered\n", cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, cap.samples); + if (cap.travel_joint >= 0) { + printf("travel : joint %d past its %s at s=%.3f, at %.6f\n", + cap.travel_joint, cap.travel_side > 0 ? "MAX_LIMIT" : "MIN_LIMIT", + cap.travel_at, cap.travel_pos); + } else if (!min_v.empty() || !max_v.empty()) { + printf("travel : every joint inside it\n"); + } } else { printf("canon's cap : none, no sample could be evaluated\n"); } diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc index ec4cf00faf0..761669e68c3 100644 --- a/src/emc/motion_planning/segment_cap.cc +++ b/src/emc/motion_planning/segment_cap.cc @@ -1,7 +1,8 @@ /******************************************************************** * Description: segment_cap.cc * The velocity and acceleration cap a segment gets from the joint - * limits through the kinematics module's Jacobian. + * limits through the kinematics module's Jacobian, and the first + * place along it a joint would leave its travel. * * Author: LinuxCNC * License: GPL Version 2 @@ -15,6 +16,13 @@ namespace motion_planning { +/* at each sample: the joints, the rate of every joint per unit of path + parameter, the path parameter, and whether the module answered there */ +static double q[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; +static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; +static double s[SEGMENT_CAP_MAX_SAMPLES]; +static bool ok[SEGMENT_CAP_MAX_SAMPLES]; + static double pose_axis(const EmcPose &p, int ax) { switch (ax) { @@ -30,22 +38,268 @@ static double pose_axis(const EmcPose &p, int ax) } } +/* The joints at fraction f of a segment sampled at K points, the inverse + seeded from what joints holds, and the rate of each there per unit of + path parameter: the module's Jacobian along the tangent, taken by a + central difference of the geometry, exact for a line and a small + fraction of a sample step for an arc. False where the module cannot + answer. */ +static bool sample_at(KinematicsUserContext *ctx, double length, int K, + SegmentPoseFn pose_at, void *arg, int njoints, + double f, double *joints, double *rate) +{ + double df = 1.0 / (8.0 * (K - 1)); + double fp = f + df > 1.0 ? 1.0 : f + df; + double fm = f - df < 0.0 ? 0.0 : f - df; + EmcPose p, pp, pm; + double t[AXIS_COUNT]; + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; + int j, a; + + pose_at(f, &p, arg); + pose_at(fp, &pp, arg); + pose_at(fm, &pm, arg); + for (a = 0; a < AXIS_COUNT; a++) { + t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); + } + if (kinematicsUserInverse(ctx, &p, joints) != 0 + || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { + return false; + } + for (j = 0; j < njoints; j++) { + rate[j] = 0.0; + for (a = 0; a < AXIS_COUNT; a++) { rate[j] += J[j][a] * t[a]; } + } + return true; +} + +//---------------------------------------------------------------------- +// The travel. Every joint is followed from sample to sample against its +// MIN_LIMIT and MAX_LIMIT, the samples themselves and, between two, the +// cubic through their positions and rates, which is where the joint goes +// between them as long as the samples are close enough to see its turns. +// The cubic only says where to look: a joint is reported past a limit +// where the inverse puts it there, never on the cubic's word. +//---------------------------------------------------------------------- + +/* a hair past a limit is the inverse rounding, not travel */ +static const double TRAVEL_HAIR = 1e-6; +/* the share of its bulge past both ends the cubic may be out by at its + peak; a peak that close to the limit is read through the inverse */ +static const double TRAVEL_DOUBT = 0.05; +/* how deep a stretch is split at its peaks, and how many points beyond + the samples a segment may be read at in all */ +enum { TRAVEL_DEPTH = 6, TRAVEL_READS = 32 }; + +enum { WATCH_MAX = 1, WATCH_MIN = 2 }; +enum { TRAVEL_CLEAR, TRAVEL_CROSSED, TRAVEL_UNSURE }; + +/* a point the joints are known at */ +struct TravelPoint { + double s; + double q[KINEMATICS_USER_MAX_JOINTS]; + double g[KINEMATICS_USER_MAX_JOINTS]; +}; + +static void travel_load(TravelPoint *p, int k, int njoints) +{ + p->s = s[k]; + for (int j = 0; j < njoints; j++) { + p->q[j] = q[k][j]; + p->g[j] = g[k][j]; + } +} + +/* whether some joint moves between two points further than its rates at + them account for: the inverse has changed branch, or the joint swings + through more between the two than either shows */ +static bool travel_jumped(const TravelPoint &a, const TravelPoint &b, int njoints) +{ + double ds = b.s - a.s; + + for (int j = 0; j < njoints; j++) { + double moved = b.q[j] - a.q[j]; + double trapezoid = 0.5 * (a.g[j] + b.g[j]) * ds; + double rate = fmax(fabs(a.g[j]), fabs(b.g[j])) * ds; + if (fabs(moved - trapezoid) > rate + TRAVEL_HAIR) { return true; } + } + return false; +} + +/* whether a followed joint is past its travel at a point, and if so the + first one, into out */ +static bool travel_past(const TravelPoint &p, const SegmentCapLimits *lim, + const int *watch, int njoints, double length, SegmentCap *out) +{ + for (int j = 0; j < njoints; j++) { + int side = 0; + if ((watch[j] & WATCH_MAX) && p.q[j] > lim->max[j] + TRAVEL_HAIR) { side = 1; } + if ((watch[j] & WATCH_MIN) && p.q[j] < lim->min[j] - TRAVEL_HAIR) { side = -1; } + if (side) { + out->travel_joint = j; + out->travel_side = side; + out->travel_at = p.s / length; + out->travel_pos = p.q[j]; + return true; + } + } + return false; +} + +/* The peak toward one side of the cubic through two points, the joint at + qa and qb, its rates there scaled to the stretch ma and mb: where it is, + u inside (0, 1), its value, and how far it bulges past both ends. False + where the cubic has no peak inside the stretch on that side. */ +static bool travel_peak(double qa, double ma, double qb, double mb, int side, + double *u, double *peak, double *bulge) +{ + /* p(t) = qa + ma t + c2 t^2 + c3 t^3, p'(t) = 3 c3 t^2 + 2 c2 t + ma */ + double c2 = 3.0 * (qb - qa) - 2.0 * ma - mb; + double c3 = 2.0 * (qa - qb) + ma + mb; + double roots[2]; + int i, n = 0; + bool found = false; + + if (c3 == 0.0) { + if (c2 != 0.0) { roots[n++] = -ma / (2.0 * c2); } + } else { + double disc = c2 * c2 - 3.0 * c3 * ma; + if (disc >= 0.0) { + /* the pair that keeps its digits */ + double r = c2 >= 0.0 ? -(c2 + sqrt(disc)) : -(c2 - sqrt(disc)); + roots[n++] = r / (3.0 * c3); + if (r != 0.0) { roots[n++] = ma / r; } + } + } + for (i = 0; i < n; i++) { + double t = roots[i], v; + if (!(t > 0.0 && t < 1.0)) { continue; } + v = qa + t * (ma + t * (c2 + t * c3)); + if (!found || side * (v - *peak) > 0.0) { + *u = t; + *peak = v; + found = true; + } + } + if (!found) { return false; } + *bulge = side * (*peak - (side > 0 ? fmax(qa, qb) : fmin(qa, qb))); + return *bulge > 0.0; +} + +/* What lies between two points: where the cubic between them peaks past a + limit, or close enough that it may be wrong, the inverse is read at the + peak; a joint past a limit there is CROSSED, else each half is looked at + the same way. UNSURE where the module cannot answer at a peak or a + joint jumps to it. */ +static int travel_between(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int K, SegmentPoseFn pose_at, void *arg, + int njoints, const int *watch, + const TravelPoint &a, const TravelPoint &b, + int *reads, SegmentCap *out) +{ + struct Stretch { + TravelPoint a, b; + int depth; + }; + /* split depth first, the half nearer the start on top */ + static Stretch stack[TRAVEL_DEPTH + 2]; + static TravelPoint m; + int n = 0, j, side; + + stack[n].a = a; + stack[n].b = b; + stack[n++].depth = 0; + while (n > 0) { + const Stretch st = stack[--n]; + double ds = st.b.s - st.a.s; + double worst = 0.0, at = -1.0; + + for (j = 0; j < njoints; j++) { + for (side = 1; side >= -1; side -= 2) { + double limit, u, peak, bulge, past; + if (!(watch[j] & (side > 0 ? WATCH_MAX : WATCH_MIN))) { continue; } + if (!travel_peak(st.a.q[j], st.a.g[j] * ds, st.b.q[j], st.b.g[j] * ds, + side, &u, &peak, &bulge)) { + continue; + } + /* how far past the limit the true peak can be */ + limit = side > 0 ? lim->max[j] : lim->min[j]; + past = side * (peak - limit) + TRAVEL_DOUBT * bulge - TRAVEL_HAIR; + if (past > worst) { + worst = past; + at = u; + } + } + } + if (at < 0.0) { continue; } + /* out of reads: what is left is let through */ + if (*reads <= 0) { return TRAVEL_CLEAR; } + (*reads)--; + m.s = st.a.s + at * ds; + for (j = 0; j < njoints; j++) { m.q[j] = st.a.q[j]; } + if (!sample_at(ctx, length, K, pose_at, arg, njoints, m.s / length, m.q, m.g) + || travel_jumped(st.a, m, njoints) || travel_jumped(m, st.b, njoints)) { + return TRAVEL_UNSURE; + } + if (travel_past(m, lim, watch, njoints, length, out)) { return TRAVEL_CROSSED; } + if (st.depth < TRAVEL_DEPTH) { + stack[n].a = m; + stack[n].b = st.b; + stack[n++].depth = st.depth + 1; + stack[n].a = st.a; + stack[n].b = m; + stack[n++].depth = st.depth + 1; + } + } + return TRAVEL_CLEAR; +} + +/* the first place along the sampled segment a joint leaves its travel */ +static void travel(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int K, SegmentPoseFn pose_at, void *arg, + int njoints, SegmentCap *out) +{ + static int watch[KINEMATICS_USER_MAX_JOINTS]; + static TravelPoint a, b; + int k, j, any = 0, reads = TRAVEL_READS; + + if (!ok[0]) { return; } + /* a side of the travel the joint starts past is not followed: the move + may be the one bringing it back */ + for (j = 0; j < njoints; j++) { + watch[j] = 0; + if (std::isfinite(lim->max[j]) && q[0][j] <= lim->max[j] + TRAVEL_HAIR) { watch[j] |= WATCH_MAX; } + if (std::isfinite(lim->min[j]) && q[0][j] >= lim->min[j] - TRAVEL_HAIR) { watch[j] |= WATCH_MIN; } + any |= watch[j]; + } + if (!any) { return; } + travel_load(&a, 0, njoints); + for (k = 1; k < K; k++) { + if (!ok[k]) { return; } + travel_load(&b, k, njoints); + if (travel_jumped(a, b, njoints)) { return; } + if (travel_between(ctx, lim, length, K, pose_at, arg, njoints, watch, + a, b, &reads, out) != TRAVEL_CLEAR) { + return; + } + if (travel_past(b, lim, watch, njoints, length, out)) { return; } + a = b; + } +} + int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, double length, int samples, SegmentPoseFn pose_at, void *arg, double *joints, SegmentCap *out) { const double tiny = 1e-12; - /* the rate of every joint per unit of path parameter at each sample, - the most that rate changes per unit of parameter on either side of - it, and the most it can bulge past the samples on either side */ - static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + /* the most the rate of every joint changes per unit of parameter on + either side of a sample, and the most it can bulge past the samples + on either side */ static double h[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; static double bulge[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; - static double s[SEGMENT_CAP_MAX_SAMPLES]; - static bool ok[SEGMENT_CAP_MAX_SAMPLES]; int K = samples < 2 ? 2 : samples; int njoints; - int k, j, a, prev, answered = 0; + int k, j, prev, answered = 0; out->vel = SEGMENT_CAP_NONE; out->acc = SEGMENT_CAP_NONE; @@ -54,6 +308,10 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, out->vel_at = 0.0; out->samples = 0; out->unanswered = 0; + out->travel_joint = -1; + out->travel_side = 0; + out->travel_at = 0.0; + out->travel_pos = 0.0; if (!ctx || !lim || !pose_at || !joints || length <= 0.0) { return -1; } if (K > SEGMENT_CAP_MAX_SAMPLES) { K = SEGMENT_CAP_MAX_SAMPLES; } njoints = lim->joints; @@ -62,38 +320,21 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, for (k = 0; k < K; k++) { double f = (double)k / (double)(K - 1); - /* the tangent by a central difference of the geometry, exact for - a line and a small fraction of a sample step for an arc */ - double df = 1.0 / (8.0 * (K - 1)); - double fp = f + df > 1.0 ? 1.0 : f + df; - double fm = f - df < 0.0 ? 0.0 : f - df; - EmcPose p, pp, pm; - double t[AXIS_COUNT]; - double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; s[k] = f * length; - ok[k] = false; - pose_at(f, &p, arg); - pose_at(fp, &pp, arg); - pose_at(fm, &pm, arg); - for (a = 0; a < AXIS_COUNT; a++) { - t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); - } /* the joints at the sample, seeded from the sample before so the whole segment stays on one solution branch, then the Jacobian on that branch */ - if (kinematicsUserInverse(ctx, &p, joints) != 0 - || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { + ok[k] = sample_at(ctx, length, K, pose_at, arg, njoints, f, joints, g[k]); + if (!ok[k]) { out->unanswered++; continue; } for (j = 0; j < njoints; j++) { - g[k][j] = 0.0; + q[k][j] = joints[j]; h[k][j] = 0.0; bulge[k][j] = 0.0; - for (a = 0; a < AXIS_COUNT; a++) { g[k][j] += J[j][a] * t[a]; } } - ok[k] = true; answered++; } if (!answered) { return -1; } @@ -169,6 +410,9 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, } } } + + /* and the travel, on the same samples */ + travel(ctx, lim, length, K, pose_at, arg, njoints, out); return 0; } // segmentCap() diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh index 80826bcb900..76407abd991 100644 --- a/src/emc/motion_planning/segment_cap.hh +++ b/src/emc/motion_planning/segment_cap.hh @@ -19,11 +19,15 @@ namespace motion_planning { /* The joint limits the cap is read against, in the units motion commands - the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION. */ + the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION, and the + travel between MIN_LIMIT and MAX_LIMIT, -HUGE_VAL and HUGE_VAL for a + joint whose travel is not to be followed. */ struct SegmentCapLimits { int joints; double vel[KINEMATICS_USER_MAX_JOINTS]; double acc[KINEMATICS_USER_MAX_JOINTS]; + double min[KINEMATICS_USER_MAX_JOINTS]; + double max[KINEMATICS_USER_MAX_JOINTS]; }; /* No joint binds. */ @@ -42,6 +46,10 @@ struct SegmentCap { double vel_at; /* the fraction of the segment the velocity cap is read at */ int samples; /* points the segment was sampled at */ int unanswered; /* of them, the ones the module could not invert or differentiate */ + int travel_joint; /* the first joint found leaving its travel along the segment, -1 for none */ + int travel_side; /* 1 past its MAX_LIMIT, -1 past its MIN_LIMIT */ + double travel_at; /* the fraction of the segment it is found at */ + double travel_pos; /* where the joint is there */ }; /* The pose at a fraction of the segment, 0 the start and 1 the end, in @@ -61,11 +69,24 @@ typedef void (*SegmentPoseFn)(double fraction, EmcPose *pose, void *arg); aside: at most half the joint's budget goes to it, and the velocity is lowered where it would take more. + The joints are followed against their travel along the same samples: a + sample past a limit, or, between two samples, the peak of the cubic + through their positions and rates, read again through the inverse at + the peak and found past the limit there, is reported, the first one + along the segment. Where the cubic puts a peak near the limit without + the inverse confirming it, the stretch is split at the peak and each + half looked at the same way, a bounded number of times. Only a point + the inverse itself puts past a limit is reported, and not on a side of + the travel the joint starts the segment past, nor beyond a sample the + module cannot invert or a joint moving between two points further than + its rates allow, as where the inverse changes branch: past those the + segment is not known well enough to be refused. + length is the segment's extent in the path parameter, in the units the caps come back in. joints seeds the inverse at the start and comes back holding the joints at the end, so the next segment starts on the same solution branch. Returns 0, or -1 where no sample could be evaluated, - the caps then NONE. */ + the caps then NONE and no joint found leaving its travel. */ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, double length, int samples, SegmentPoseFn pose_at, void *arg, double *joints, SegmentCap *out); diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index f32e3317348..4e2635db30a 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -277,6 +277,12 @@ extern void CANON_UPDATE_END_POINT(double x, double y, double z, /* Called from emctask to update the canon position during skipping through programs started with start-from-line > 0. */ +extern int CANON_MOVE_REFUSED(char *message, int max); +/* Called from emctask after the interpreter has read or executed a line: + 1 and the reason in message where canon refused a move since the last + call, because a joint would leave its travel along it, 0 otherwise. + The refused move and every move after it are dropped until then. */ + extern void USE_LENGTH_UNITS(CANON_UNITS u); /* Use the specified units for length. Conceptually, the units must diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 38da667a2e9..c7745947b6f 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -867,8 +867,33 @@ static double getStraightJerk(double x, double y, double z, // motion runs, loaded through the non-RT loader in kinematics_userspace/ // the first time a segment asks. A module that runs only in realtime, // and a kinematics type that is the identity, cap nothing. +// +// The same samples follow every joint against its travel. Motion checks +// the joints at the end of a move only, so a tilted head turning under +// the tool centre point can swing a carriage past its limit in the middle +// of a move whose ends are both inside; motion then stops on the soft +// limit partway through it, with the joint past the limit. Here such a +// move is refused before it is sent: it is dropped, every move after it +// is dropped too, since one sent after it would start from where the +// dropped one began, and task fails the line with the reason. //---------------------------------------------------------------------- +// where a joint would leave its travel along a segment, joint -1 for nowhere +struct KinsTravel { + int joint; + int side; // 1 past MAX_LIMIT, -1 past MIN_LIMIT + double limit; // the limit, as motion held it + double pos; // where the joint would be + double at; // the fraction of the segment it is found at +}; + +// the first move refused, until task takes it +static struct { + bool pending; + int line; // of the move dropped for it, -1 until then + KinsTravel travel; +} refusal = { false, -1, { -1, 0, 0.0, 0.0, 0.0 } }; + static struct { KinematicsUserContext *ctx; int comp_id; @@ -885,8 +910,9 @@ static struct { EmcPose start, end; bool capped; double vel, acc; + KinsTravel travel; } memo; -} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0 } }; +} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0, { -1, 0, 0.0, 0.0, 0.0 } } }; // the machine's own module, once static bool kins_load(void) @@ -1014,15 +1040,30 @@ static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_d return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; } +// the travel motion holds the joints to now, MIN_LIMIT and MAX_LIMIT or +// what the ini.N pins have made of them since, on the joints that are +// homed: the position of one that is not means nothing to its limits +static void kins_travel_limits(void) +{ + for (int j = 0; j < kins.limits.joints; j++) { + bool homed = emcStatus->motion.joint[j].homed; + kins.limits.min[j] = homed ? emcStatus->motion.joint[j].minPositionLimit : -HUGE_VAL; + kins.limits.max[j] = homed ? emcStatus->motion.joint[j].maxPositionLimit : HUGE_VAL; + } +} + // what the joints allow along a segment, in the machine's units per second -// of the path parameter canon plans it with; false where nothing binds +// of the path parameter canon plans it with, false where nothing binds, +// and where along it a joint would leave its travel static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const EmcPose *end, double length, int samples, motion_planning::SegmentPoseFn pose_at, void *arg, - double *vel, double *acc) + double *vel, double *acc, KinsTravel *travel) { motion_planning::SegmentCap cap; + travel->joint = -1; kins_seed(ctx, start); + kins_travel_limits(); if (motion_planning::segmentCap(ctx, &kins.limits, length, samples, pose_at, arg, kins.joints, &cap) != 0) { CANON_ERROR(fmt::format("the kinematics cannot reach the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f}, the joints are not checked along it", end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c)); @@ -1039,10 +1080,19 @@ static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const Emc CANON_ERROR(fmt::format("joint {} cannot follow the move to X{:.3f} Y{:.3f} Z{:.3f} A{:.3f} B{:.3f} C{:.3f} near {:.0f}% of its length: the kinematics is singular there and the move crawls", cap.vel_joint, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, 100.0 * cap.vel_at)); } + if (cap.travel_joint >= 0) { + travel->joint = cap.travel_joint; + travel->side = cap.travel_side; + travel->limit = cap.travel_side > 0 ? kins.limits.max[cap.travel_joint] + : kins.limits.min[cap.travel_joint]; + travel->pos = cap.travel_pos; + travel->at = cap.travel_at; + } if (kins.debug) { - fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered; seed", + fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered, travel left by joint %d at %.2f; seed", end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, length, cap.samples, - cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered); + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, + cap.travel_joint, cap.travel_at); for (int j = 0; j < kins.limits.joints; j++) { fprintf(stderr, " %.3f", kins.joints[j]); } fprintf(stderr, "\n"); } @@ -1051,6 +1101,16 @@ static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const Emc return cap.vel_joint >= 0 || cap.acc_joint >= 0; } +// a segment a joint would leave its travel along refuses the move, the +// first one only: the moves after it are dropped with it +static void kins_refuse(const KinsTravel *travel) +{ + if (travel->joint < 0 || refusal.pending) { return; } + refusal.pending = true; + refusal.line = -1; + refusal.travel = *travel; +} + struct KinsLine { EmcPose start, end; }; @@ -1086,6 +1146,7 @@ static bool kins_straight_cap(double x, double y, double z, line.start = to_ext_pose(canon.endPoint); line.end = to_ext_pose(x, y, z, a, b, c, u, v, w); if (kins.memo.valid && kins_same(&kins.memo.start, &line.start) && kins_same(&kins.memo.end, &line.end)) { + kins_refuse(&kins.memo.travel); *vel = kins.memo.vel; *acc = kins.memo.acc; return kins.memo.capped; @@ -1104,7 +1165,9 @@ static bool kins_straight_cap(double x, double y, double z, kins.memo.end = line.end; kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, kins_samples(&line.start, &line.end, 0.0), - kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc); + kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc, + &kins.memo.travel); + kins_refuse(&kins.memo.travel); *vel = kins.memo.vel; *acc = kins.memo.acc; return kins.memo.capped; @@ -1138,6 +1201,8 @@ static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, { KinematicsUserContext *ctx = kins_here(); KinsArc arc; + KinsTravel travel; + bool capped; if (!ctx || length <= 0.0) { return false; } arc.start = to_ext_pose(canon.endPoint); @@ -1152,9 +1217,42 @@ static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, return false; } } - return kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), - kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), - kins_arc_pose, &arc, vel, acc); + capped = kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), + kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), + kins_arc_pose, &arc, vel, acc, &travel); + kins_refuse(&travel); + return capped; +} + +// whether a move about to be sent is dropped: the one refused, or one +// after it, which would start from where the refused one began, until +// task has taken the refusal +static bool kins_dropped(int line_number) +{ + if (!refusal.pending) { return false; } + if (refusal.line < 0) { refusal.line = line_number; } + return true; +} + +int CANON_MOVE_REFUSED(char *message, int max) +{ + const KinsTravel &t = refusal.travel; + const char *limit = t.side > 0 ? "MAX_LIMIT" : "MIN_LIMIT"; + char move[32] = "the move"; + + if (!refusal.pending) { return 0; } + if (refusal.line > 0) { snprintf(move, sizeof(move), "the move on line %d", refusal.line); } + if (t.at > 1.0 - 1e-9) { + snprintf(message, max, "%s would end with joint %d past its limit, at %.4f against %s %.4f", + move, t.joint, t.pos, limit, t.limit); + } else { + snprintf(message, max, "%s would take joint %d past its limit, to %.4f against %s %.4f, %.0f%% of the way along", + move, t.joint, t.pos, limit, t.limit, 100.0 * t.at); + } + refusal.pending = false; + // the move may be asked for again, under other limits by then + kins.memo.valid = false; + return 1; } /** @@ -1295,6 +1393,10 @@ static void flush_segments(void) { #endif VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); + if (kins_dropped(line_no)) { + drop_segments(); + return; + } double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); double vel = linedata.vel; @@ -1428,6 +1530,7 @@ void FINISH() { void ON_RESET() { drop_segments(); + refusal.pending = false; } void SELECT_KINS_TYPE(int switchkins_type) @@ -1496,6 +1599,7 @@ static void joint_move(int line_number, const double *joints, int have_joints, auto msg = std::make_unique(); flush_segments(); + if (kins_dropped(line_number)) { return; } from_prog(x,y,z,a,b,c,u,v,w); rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); @@ -1587,6 +1691,7 @@ void STRAIGHT_TRAVERSE(int line_number, VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); + if (kins_dropped(line_number)) { return; } vel = veldata.vel; acc = accdata.acc; @@ -1633,6 +1738,10 @@ void RIGID_TAP(int line_number, double x, double y, double z, double scale) from_prog(x,y,z,unused,unused,unused,unused,unused,unused); rotate_and_offset_pos(x,y,z,unused,unused,unused,unused,unused,unused); + // the segments before it first, so that the tap is read from where it + // starts + flush_segments(); + VelData veldata = getStraightVelocity(x, y, z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); @@ -1656,7 +1765,7 @@ void RIGID_TAP(int line_number, double x, double y, double z, double scale) rigidTapMsg->ini_maxjerk = toExtVel(jerk); rigidTapMsg->acc = toExtAcc(acc); rigidTapMsg->scale = scale; - flush_segments(); + if (kins_dropped(line_number)) { return; } if(ini_maxvel && acc) { interp_list.set_line_number(line_number); @@ -1706,6 +1815,7 @@ void STRAIGHT_PROBE(int line_number, AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); acc = accdata.acc; + if (kins_dropped(line_number)) { return; } probeMsg->vel = toExtVel(vel); probeMsg->ini_maxvel = toExtVel(ini_maxvel); @@ -3190,6 +3300,7 @@ void ARC_FEED(int line_number, a_max = std::min(a_max, FROM_EXT_LEN(kacc)); } } + if (kins_dropped(line_number)) { return; } // Limit velocity by maximum double vel = std::min(canon.linearFeedRate, v_max); @@ -3495,6 +3606,7 @@ void CHANGE_TOOL() double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); vel = veldata.vel; acc = accdata.acc; + if (kins_dropped(_tag.fields[GM_FIELD_LINE_NUMBER])) { return; } auto linearMoveMsg = std::make_unique(); linearMoveMsg->feed_mode = canon.feed_mode; @@ -3853,6 +3965,7 @@ void INIT_CANON() double units; chained_points.clear(); + refusal.pending = false; // initialize locals to original values canon.xy_rotation = 0.0; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index c6304738767..4641c40a3d0 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -540,6 +540,27 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) return 0; } + +// whether the line the interpreter has just executed is one stepped over +// on the way to the line a run starts from, its output thrown away +static bool stepping_over(void) +{ + return programStartLine != 0 && emcTaskPlanLevel() == 0 + && (programStartLine < 0 || emcTaskPlanLine() <= programStartLine); +} + +// A move canon refused, because a joint would leave its travel along it, +// fails the line like an interpreter error: canon has dropped it and every +// move after it. On a line stepped over the refusal goes with the rest of +// its output, its moves read from wherever the machine stands. +static int canon_refusal(int retval, bool stepped_over) +{ + char text[LINELEN]; + + if (!CANON_MOVE_REFUSED(text, sizeof(text)) || stepped_over) { return retval; } + emcOperatorError("%s", text); + return INTERP_ERROR; +} extern int emcTaskMopup(); void readahead_reading(void) @@ -568,6 +589,7 @@ void readahead_reading(void) } } else { readRetval = emcTaskPlanRead(); + readRetval = canon_refusal(readRetval, stepping_over()); /*! \todo MGS FIXME This if() actually evaluates to if (readRetval != INTERP_OK)... *** Need to look at all calls to things that return INTERP_xxx values! *** @@ -593,6 +615,7 @@ void readahead_reading(void) command); // and execute it execRetval = emcTaskPlanExecute(NULL); + execRetval = canon_refusal(execRetval, stepping_over()); // line number may need update after // returns from subprograms in external // files @@ -630,10 +653,7 @@ void readahead_reading(void) // throw the results away if we're supposed to // read // through it - if ( programStartLine != 0 && - emcTaskPlanLevel() == 0 && - ( programStartLine < 0 || - emcTaskPlanLine() <= programStartLine )) { + if (stepping_over()) { // we're stepping over lines, so check them // for // limits, etc. and clear then out @@ -2391,6 +2411,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) } execRetval = emcTaskPlanExecute(command, 0); + execRetval = canon_refusal(execRetval, false); level = emcTaskPlanLevel(); diff --git a/tests/kins-limits/travel/README b/tests/kins-limits/travel/README new file mode 100644 index 00000000000..61cf454cb3e --- /dev/null +++ b/tests/kins-limits/travel/README @@ -0,0 +1,16 @@ +A move whose joints leave their travel between its ends is refused before +it is sent, not stopped on the soft limit partway through it. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +the tool tip held while C turns, so that the carriage swings round the tip +at the pivot length plus the tool length; C-90 puts it at Y+ that radius, +against joint 1's MAX_LIMIT of 400. A tool length of 0.05 makes the swing +from C-5 to C-175 peak 0.05 past the travel at C-90, between two samples +1.2 inside it: the move is refused, asked again it is refused again, and +moves inside the travel still go, turning the other way among them; a +move ending past the travel is refused for its end. At -0.05 the same +swing peaks 0.05 inside and runs; at 100 the samples themselves are past. +With the head upright an arc whose ends are inside the travel and whose +top is past it is refused, the same arc the other way round runs. A +program fails on the line of the swing with no joint past its limit, and +run from the line after it, which only steps over the swing, it runs. diff --git a/tests/kins-limits/travel/checkresult b/tests/kins-limits/travel/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/travel/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/travel/sim.hal b/tests/kins-limits/travel/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/travel/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/travel/test-ui.py b/tests/kins-limits/travel/test-ui.py new file mode 100755 index 00000000000..b9f600ee1f6 --- /dev/null +++ b/tests/kins-limits/travel/test-ui.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 +# A move whose joints leave their travel between its ends is refused before +# it is sent: see README. +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +PROGRAM = os.path.abspath("travel.ngc") +# the line of travel.ngc that swings the carriage past its Y travel +SWING_LINE = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 +# the furthest joint 1 was seen, every time the machine is polled +y_peak = -1e99 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + global y_peak + s.poll() + y_peak = max(y_peak, s.joint_position[1]) + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def complaints(said): + return [m for m in said if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def back_to_mdi(): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def accepted(cmd, joint, where): + drain() + now = mdi(cmd) + said = complaints(drain()) + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[joint] - where) > 1e-3: + error("%s left joint %d at %.4f, not %.4f" % (cmd, joint, now[joint], where)) + else: + print("accepted as expected: %-28s joint %d at %.4f" % (cmd, joint, now[joint])) + back_to_mdi() + +def refused(cmd, *needles): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + settled() + said = complaints(drain()) + if not said: + error("%s was accepted" % cmd) + elif not all(n in said[0][1] for n in needles): + error("%s said %r, not %r" % (cmd, said[0][1].strip(), needles)) + else: + print("refused as expected: %s" % said[0][1].strip()) + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + back_to_mdi() + +def run(start_line): + drain() + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete(30) + c.program_open(PROGRAM) + c.wait_complete(30) + c.auto(linuxcnc.AUTO_RUN, start_line) + c.wait_complete(30) + deadline = time.time() + 60 + while time.time() < deadline: + joints() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue and s.inpos: + break + time.sleep(0.01) + else: + error("the program did not finish") + now = settled() + said = complaints(drain()) + back_to_mdi() + return now, said + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +back_to_mdi() +drain() + +# the tool centre point kinematics, the head laid over by a joint move; the +# carriage at X-400 puts the tip at X0. A turn of C holding the tip swings +# the carriage round it at the pivot length plus the tool length, C-90 +# putting it at Y+ that radius, against joint 1's MAX_LIMIT of 400 +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") + +# a radius of 400.05: the swing from C-5 to C-175 peaks at C-90 between two +# samples, which are 1.2 inside the travel; the peak is 0.05 past it +mdi("G43.1 Z0.05") +accepted("G0 C-5", 4, -5) +refused("G0 C-175", "joint 1", "MAX_LIMIT", "50%") +# asked again, it is refused again, and a move inside the travel still goes +refused("G0 C-175", "joint 1", "MAX_LIMIT") +accepted("G0 C-60", 4, -60) +accepted("G0 C-5", 4, -5) +# turning the other way, C through +90, runs the carriage through +# Y-400.05, inside the travel +accepted("G0 C175", 4, 175) +accepted("G0 C-5", 4, -5) +# a move that ends past the travel is refused for its end +refused("G0 C-90", "joint 1", "end with", "MAX_LIMIT") + +# a radius of 399.95: the same swing peaks 0.05 inside the travel +mdi("G43.1 Z-0.05") +accepted("G0 C-175", 4, -175) +accepted("G0 C-5", 4, -5) + +# a radius of 500: the samples themselves are past the travel +mdi("G43.1 Z100") +refused("G0 C-175", "joint 1", "MAX_LIMIT") +mdi("G49") + +# an arc with the head upright, its ends inside the travel and its top +# 54.4 past it; the other way round it runs below +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +accepted("G0 X-100 Y380", 1, 380) +refused("G2 X100 Y380 I100 J-30 F30000", "joint 1", "MAX_LIMIT") +accepted("G3 X100 Y380 I100 J-30 F30000", 1, 380) +accepted("G0 X0 Y0", 1, 0) + +# a program: the line that swings past the travel fails, the moves before +# it may have run, and no joint has passed its limit +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +y_peak = -1e99 +now, said = run(0) +if not said: + error("the program ran through line %d" % SWING_LINE) +elif "line %d" % SWING_LINE not in said[0][1] or "joint 1" not in said[0][1]: + error("the program said %r" % said[0][1].strip()) +else: + print("refused as expected: %s" % said[0][1].strip()) +if any("soft limit" in m[1] for m in said): + error("a joint ran onto its soft limit") +if y_peak > 400 + 1e-6: + error("joint 1 reached %.4f, past its MAX_LIMIT" % y_peak) + +# run from the line after the swing: the line is only stepped over, its +# moves are thrown away, and the program runs +mdi("G49") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +now, said = run(SWING_LINE + 1) +if said: + error("run from line %d said %r" % (SWING_LINE + 1, said[0][1].strip())) +elif abs(now[4] + 5) > 1e-3: + error("run from line %d left C at %.4f, not -5" % (SWING_LINE + 1, now[4])) +else: + print("ran from line %d as expected, C at %.4f" % (SWING_LINE + 1, now[4])) + +mdi("G49") +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +print("joint 1 peaked at %.4f since the program started" % y_peak) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/travel/test.ini b/tests/kins-limits/travel/test.ini new file mode 100644 index 00000000000..50349e5995f --- /dev/null +++ b/tests/kins-limits/travel/test.ini @@ -0,0 +1,136 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# laid over to B90, the head swings the carriage round the tip at the +# pivot length plus the tool's +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +# the rates are high so the moves are short: the test checks only which +# moves are refused and where the others end +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 8000 +MAX_LINEAR_VELOCITY = 13840 +MAX_LINEAR_ACCELERATION = 1280000 +DEFAULT_LINEAR_ACCELERATION = 1280000 +MAX_ANGULAR_VELOCITY = 14400 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 + +[AXIS_Y] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 + +[AXIS_Z] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 2400 +MAX_ACCELERATION = 320000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 2400 +MAX_ACCELERATION = 320000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +# the travel the swings are measured against +MIN_LIMIT = -1000 +MAX_LIMIT = 400 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 2400 +MAX_ACCELERATION = 320000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 2400 +MAX_ACCELERATION = 320000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 8000 +MAX_ACCELERATION = 1280000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/travel/test.sh b/tests/kins-limits/travel/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-limits/travel/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-limits/travel/tool.tbl b/tests/kins-limits/travel/tool.tbl new file mode 100644 index 00000000000..80153f2ca05 --- /dev/null +++ b/tests/kins-limits/travel/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z0 ; diff --git a/tests/kins-limits/travel/travel.ngc b/tests/kins-limits/travel/travel.ngc new file mode 100644 index 00000000000..23b2a4b9949 --- /dev/null +++ b/tests/kins-limits/travel/travel.ngc @@ -0,0 +1,8 @@ +(the head laid over to B90, the tip held while C turns) +G21 G90 G94 +G12.1 P0 +G43.1 Z0.05 +G0 C-5 +G0 C-175 +G0 C-5 +M2 From 09b01431c47d04fa3c718d13a27c053cdedc832c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 10 Oct 2026 23:13:10 +1100 Subject: [PATCH 21/44] motion: report the tool frame in the work frame in status Motion evaluates the module's work and tool frames at the commanded joints every cycle and publishes transpose(work) * tool, the tool's axes seen from the work, as stat.tool_frame. An identity kinematics type turns neither frame whatever the rotaries do, so under one, the machine frame type G13.1 selects, motion takes the frames the primary type has at the same joints: switchkins gains kinematicsToolFrameOf() and kinematicsWorkFrameOf(), the frames of a type other than the one in force, and a display under G13.1 shows the tool against the part as the tool centre point kinematics has it. The frames are resolved weakly, as the Jacobian is, and with a module that exports none stat.tool_frame is None. This is where a GUI reads where the tool points instead of guessing it from [DISPLAY] GEOMETRY, and what a jog in the tool frame will need. --- docs/src/config/python-interface.adoc | 5 ++ include/kinematics.h | 14 ++++ src/emc/kinematics/switchkins.c | 60 +++++++++------ src/emc/motion/control.c | 76 +++++++++++++++++++ src/emc/motion/motion.h | 3 + src/emc/nml_intf/emc.cc | 2 + src/emc/nml_intf/emc_nml.hh | 2 + src/emc/nml_intf/emcops.cc | 2 + src/emc/task/taskintf.cc | 3 + src/emc/usr_intf/axis/extensions/emcmodule.cc | 11 +++ 10 files changed, 156 insertions(+), 22 deletions(-) diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index c924886b1d8..50a41203f25 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -374,6 +374,11 @@ see <> for an example. task main loop heartbeat counter. Increments every task cycle. Rate is determined by `[TASK]CYCLE_TIME` in the INI file. +*tool_frame*:: '(returns tuple of floats or None)' - + orientation of the tool in the work frame at the commanded joints, nine values row by row; its columns are the tool's X, Y and Z, Z pointing from the tip towards the holder. + Under an identity type of a switchable module the frames are the primary type's, the one that follows the part, so a display can draw the tool against the part there too. + None where no type reports tool and work frames. + *tool_from_pocket*:: '(returns integer)' - pocket number for the currently loaded tool (0 if no tool loaded). diff --git a/include/kinematics.h b/include/kinematics.h index 850ada96802..b90ed4fe7a9 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -207,6 +207,20 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* The same frames of a switchkins type other than the one in force, ktype + being the number G12.1 takes: under a type that turns neither frame, a + display still shows the tool against the part as the primary type has + them. Exported by modules built on switchkins.c alone. */ +extern int kinematicsToolFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int kinematicsWorkFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + /* The inverse of kinematicsToolFrame(): which joint values point the tool along a requested direction. This is the question a tilted work plane asks diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 81cf190e4d1..fe641c3c890 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -324,45 +324,59 @@ int kinematicsInverse(const EmcPose * pos, return r; } // kinematicsInverse() -int kinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +int kinematicsToolFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { int r; - if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } - if (kops[switchkins_type]) { - read_block(switchkins_type); - return kinsOpsToolFrame(kops[switchkins_type], &rt_params, - joint, rot, fflags); + if (ktype < 0 || ktype >= kins_count) { return -1; } + if (kops[ktype]) { + read_block(ktype); + return kinsOpsToolFrame(kops[ktype], &rt_params, joint, rot, fflags); } - if (!ktools[switchkins_type]) { + if (!ktools[ktype]) { return -1; // this type does not supply one; not an error } - r = ktools[switchkins_type](joint, rot, fflags); + r = ktools[ktype](joint, rot, fflags); if (r) { return r; } // the type answers in its own frame; put it in the convention here so // no module has to get the half turn right for itself - return toolFrameApplyNative(rot, &knative[switchkins_type]); -} // kinematicsToolFrame() + return toolFrameApplyNative(rot, &knative[ktype]); +} // kinematicsToolFrameOf() -int kinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +int kinematicsWorkFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { - if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } - if (kops[switchkins_type]) { - read_block(switchkins_type); - return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, - joint, rot, fflags); + if (ktype < 0 || ktype >= kins_count) { return -1; } + if (kops[ktype]) { + read_block(ktype); + return kinsOpsWorkFrame(kops[ktype], &rt_params, joint, rot, fflags); } - if (!kworks[switchkins_type]) { + if (!kworks[ktype]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the // wrong way, so there are not two conventions for it to be caught between - return kworks[switchkins_type](joint, rot, fflags); + return kworks[ktype](joint, rot, fflags); +} // kinematicsWorkFrameOf() + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinematicsToolFrameOf(switchkins_type, joint, rot, fflags); +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinematicsWorkFrameOf(switchkins_type, joint, rot, fflags); } // kinematicsWorkFrame() int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, @@ -589,6 +603,8 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrameOf); +EXPORT_SYMBOL(kinematicsWorkFrameOf); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsSetTool); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b7403cd8345..a10b327751d 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -49,6 +49,13 @@ KINEMATICS_INVERSE_FLAGS iflags = 0; // old modules export no kinematicsJacobian; a jog on them is bounded by // the axis limits alone, as before #pragma weak kinematicsJacobian +// nor the frames; the status then reports none and a GUI keeps its own +// idea of the tool orientation +#pragma weak kinematicsToolFrame +#pragma weak kinematicsWorkFrame +// and only modules built on switchkins.c those of another type +#pragma weak kinematicsToolFrameOf +#pragma weak kinematicsWorkFrameOf /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ @@ -2481,6 +2488,74 @@ static void output_to_hal(void) } +/* The type that follows the part, the module's working transform, or -1 + where the module declares none. */ +static int primary_kins_type(void) +{ + int k, f; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + f = emcmotStatus->switchkins_flags[k]; + if (f >= 0 && (f & KINSTYPE_PRIMARY)) { return k; } + } + return -1; +} + +/* The tool frame in the work frame, transpose(work) * tool as + toolFrameInWork() has it, at the commanded joints, for the status. */ +static void update_tool_frame(void) +{ + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + PmRotationMatrix work, tool; + const PmCartesian *w[3] = { &work.x, &work.y, &work.z }; + const PmCartesian *t[3] = { &tool.x, &tool.y, &tool.z }; + int j, a, b, f, primary; + + emcmotStatus->tool_frame_ok = 0; + if (!kinematicsToolFrame || !kinematicsWorkFrame) { return; } + if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + /* an identity type turns neither frame whatever the rotaries do, so it + says nothing about where the tool points. The primary type, the one + that follows the part, still knows: a display under the machine frame + type shows the tool against the part as it has them. */ + if (switchkins_type >= 0 && switchkins_type < SWITCHKINS_MAX_TYPES) { + f = emcmotStatus->switchkins_flags[switchkins_type]; + if (f >= 0 && (f & KINSTYPE_IDENTITY)) { + if (!kinematicsToolFrameOf || !kinematicsWorkFrameOf) { return; } + primary = primary_kins_type(); + if (primary < 0 || (emcmotStatus->switchkins_flags[primary] & KINSTYPE_IDENTITY)) { + return; + } + if (kinematicsWorkFrameOf(primary, joint_pos, &work, &fflags) != 0 + || kinematicsToolFrameOf(primary, joint_pos, &tool, &fflags) != 0) { + return; + } + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotStatus->tool_frame[3 * a + b] = w[a]->x * t[b]->x + + w[a]->y * t[b]->y + w[a]->z * t[b]->z; + } + } + emcmotStatus->tool_frame_ok = 1; + return; + } + } + if (kinematicsWorkFrame(joint_pos, &work, &fflags) != 0 + || kinematicsToolFrame(joint_pos, &tool, &fflags) != 0) { + return; + } + /* entry (a, b) is column a of the work frame dotted with column b of + the tool frame */ + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotStatus->tool_frame[3 * a + b] = w[a]->x * t[b]->x + + w[a]->y * t[b]->y + w[a]->z * t[b]->z; + } + } + emcmotStatus->tool_frame_ok = 1; +} + static void update_status(void) { int joint_num, axis_num, dio, aio, misc_error; @@ -2560,6 +2635,7 @@ static void update_status(void) emcmotStatus->eoffset_pose.w = axis_get_ext_offset_curr_pos(8); emcmotStatus->external_offsets_applied = hal_get_bool(emcmot_hal_data->eoffset_active); + update_tool_frame(); for (dio = 0; dio < emcmotConfig->numDIO; dio++) { emcmotStatus->synch_di[dio] = hal_get_bool(emcmot_hal_data->synch_di[dio]); diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 49cb1b00a94..448c4d6db62 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -620,6 +620,9 @@ Suggestion: Split this in to an Error and a Status flag register.. int carte_pos_cmd_ok; /* non-zero if command is valid */ EmcPose carte_pos_fb; /* actual Cartesian position */ int carte_pos_fb_ok; /* non-zero if feedback is valid */ + double tool_frame[9]; /* the tool frame in the work frame at the + commanded joints, row major */ + int tool_frame_ok; /* non-zero if the module reports its frames */ EmcPose world_home; /* cartesean coords of home position */ emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 58e80a70f92..15ec482cac9 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1717,6 +1717,8 @@ void EMC_TRAJ_STAT::update(CMS * cms) cms->update(rapid_scale); EmcPose_update(cms, &position); EmcPose_update(cms, &actualPosition); + cms->update(tool_frame, 9); + cms->update(tool_frame_ok); cms->update(velocity); cms->update(acceleration); cms->update(maxVelocity); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 576de74797e..8ff7c421bf3 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -1039,6 +1039,8 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { EmcPose position; // current commanded position EmcPose actualPosition; // current actual position, from forward kins + double tool_frame[9]; // tool frame in the work frame, row major + bool tool_frame_ok; // the kinematics reports its frames double velocity; // system velocity, for subsequent motions double acceleration; // system acceleration, for subsequent // motions diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index e40a1818cf9..31a5df214d9 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -84,6 +84,8 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() position{}, actualPosition{}, + tool_frame{1, 0, 0, 0, 1, 0, 0, 0, 1}, + tool_frame_ok(false), velocity(1.0), acceleration(1.0), diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index e886366f78c..75dc9ee109f 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1737,6 +1737,9 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) stat->actualPosition = emcmotStatus.carte_pos_fb; + for (int i = 0; i < 9; i++) { stat->tool_frame[i] = emcmotStatus.tool_frame[i]; } + stat->tool_frame_ok = emcmotStatus.tool_frame_ok; + stat->velocity = emcmotStatus.vel; stat->acceleration = emcmotStatus.acc; stat->maxAcceleration = TrajConfig.MaxAccel; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 6cfd70e0672..35a3ca6b40a 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1305,6 +1305,15 @@ static PyObject *Stat_kins_type_flags(pyStatChannel *s, void *) { return res; } +static PyObject *Stat_tool_frame(pyStatChannel *s, void *) { + if (!s->status.motion.traj.tool_frame_ok) { Py_RETURN_NONE; } + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.motion.traj.tool_frame[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1613,6 +1622,8 @@ static PyGetSetDef Stat_getsetlist[] = { (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, {(char*)"kins_type_flags", (getter)Stat_kins_type_flags, NULL, (char*)"What the module declares each kinematics type to be, one entry per type: KINSTYPE_ flags, or -1 for a type it does not provide.", NULL}, + {(char*)"tool_frame", (getter)Stat_tool_frame, NULL, + (char*)"Tool frame in the work frame at the commanded joints, nine values row by row, the columns being tool X, Y and Z; None where the kinematics reports no frames.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, From 26f96394a096398ba9a58886a0df861c731750a1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 10 Oct 2026 23:13:10 +1100 Subject: [PATCH 22/44] glcanon: point the tool marker along the tool frame the kinematics reports Where status reports a tool frame the preview draws the tool marker along it and leaves GEOMETRY aside, so a machine whose rotaries GEOMETRY cannot describe, a nutating table among them, shows the tool as it stands against the work. GEOMETRY stays the fallback under an identity type and for modules without frames. Requested by Sigma1912. --- docs/src/config/ini-config.adoc | 2 + lib/python/rs274/glcanon_scene.py | 17 +++++- tests/glcanon/test.sh | 7 +-- tests/glcanon/test_tool_frame.py | 86 +++++++++++++++++++++++++++++++ 4 files changed, 108 insertions(+), 4 deletions(-) create mode 100755 tests/glcanon/test_tool_frame.py diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 2cb57345001..541e5910a0f 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -436,6 +436,8 @@ See <> document for GMOCCAPY details. . The proper GEOMETRY string depends on the machine configuration and the kinematics used to control it. The order of the letters is important. For example, rotating around C then B is different than rotating around B then C. +. The tool marker does not use GEOMETRY where the kinematics type in force reports its tool and work frames: it is drawn along the tool as the kinematics places it against the work, the `tool_frame` the Python interface reports. + GEOMETRY orients it under an identity kinematics type and with a module that reports no frames. . Rotations are by default applied with respect to the machine origin. Example: `GEOMETRY=CXYZ` first translates the control point to X, Y, Z and then performs a C rotation about the Z axis centered at the machine origin. . UVW translation example: `GEOMETRY=XYZUVW` causes UVW to move in the coordinate system of the tool and XYZ to move in the coordinate system of the material. diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 66a491ed2bc..fbec0cc6ad0 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -1808,13 +1808,28 @@ def _draw_foam(self, ctx: FrameContext, pos: Sequence[float], def _draw_at_tool(self, ctx: FrameContext, pos: Sequence[float], rx: float, ry: float, rz: float) -> None: ctx.mv.translate(*pos) - self._apply_rotary(ctx, rx, ry, rz) + if not self._apply_tool_frame(ctx): + self._apply_rotary(ctx, rx, ry, rz) current_tool = ctx.current_tool() if current_tool is None or current_tool.diameter <= ctx.view_tool_min_dia: self._draw_cone(ctx) else: self.draw_solid(ctx, current_tool) + @staticmethod + def _apply_tool_frame(ctx: FrameContext) -> bool: + """Turn the marker into the tool frame the kinematics reports, in + the work frame, and say whether it did. A module that reports its + frames knows where the tool points better than GEOMETRY does, so + GEOMETRY is only the fallback for one that does not.""" + frame = getattr(ctx.stat, 'tool_frame', None) + if frame is None: + return False + m = glnav.identity_matrix() + m[:3, :3] = np.asarray(frame, dtype=np.float64).reshape(3, 3) + ctx.mv.mult(m) + return True + @staticmethod def _apply_rotary(ctx: FrameContext, rx: float, ry: float, rz: float) -> None: diff --git a/tests/glcanon/test.sh b/tests/glcanon/test.sh index cf53bc3c761..83e5e7e2713 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -4,9 +4,9 @@ # # None of them needs a GPU or an X display. The first two are GL-free # outright - they load rs274.glcanon_bake by path and exercise it as plain -# numpy. The third imports rs274.glcanon, which pulls PyOpenGL at import time +# numpy. The others import rs274.glcanon, which pulls PyOpenGL at import time # (it never calls into it here), and PyOpenGL is not installed in a headless -# build - so that one is conditional, the same rule tests/pyvcp/skip applies +# build - so those are conditional, the same rule tests/pyvcp/skip applies # to a whole directory. set -e @@ -16,8 +16,9 @@ set -e if python3 -c 'import OpenGL' 2>/dev/null; then ./test_workpiece.py >&2 ./test_workplane.py >&2 + ./test_tool_frame.py >&2 else - echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py, test_workplane.py and test_tool_frame.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_tool_frame.py b/tests/glcanon/test_tool_frame.py new file mode 100755 index 00000000000..abf549474f8 --- /dev/null +++ b/tests/glcanon/test_tool_frame.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +"""Which way the preview points the tool marker: along the tool frame the +kinematics reports through status when it reports one, by the GEOMETRY +string otherwise. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: the marker itself is stubbed out below. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import numpy as np + +from rs274 import glcanon_scene + + +class StatStub: + def __init__(self, tool_frame=None): + self.tool_frame = tool_frame + + +class CtxStub: + """What ToolPart reads to place the marker.""" + + def __init__(self, stat, geometry="XYZBC"): + self.stat = stat + self.geometry = geometry + self.mv = glcanon_scene.MatrixStack() + self.view_tool_min_dia = 0.0 + + @staticmethod + def current_tool(): + return None + + +class Marker(glcanon_scene.ToolPart): + """The tool part with the cone replaced by a record of where it went.""" + + def __init__(self): + self.at = None + + def _draw_cone(self, ctx): + self.at = ctx.mv.top().copy() + + +def place(stat, rotary=(0, 0, 0), geometry="XYZBC"): + ctx = CtxStub(stat, geometry) + marker = Marker() + marker._draw_at_tool(ctx, (1, 2, 3), *rotary) + return marker.at + + +def tilted_about_x(degrees): + """A tool frame turned about work X, nine values row by row.""" + c, s = math.cos(math.radians(degrees)), math.sin(math.radians(degrees)) + return (1, 0, 0, 0, c, -s, 0, s, c) + + +class ToolFrameTest(unittest.TestCase): + def test_the_marker_follows_the_reported_frame(self): + frame = tilted_about_x(30) + at = place(StatStub(frame)) + np.testing.assert_allclose(at[:3, :3], np.reshape(frame, (3, 3)), atol=1e-12) + np.testing.assert_allclose(at[:3, 3], (1, 2, 3)) + + def test_the_frame_overrides_geometry(self): + # B and C turned: GEOMETRY would tilt the marker, the frame says the + # tool is square with the work + at = place(StatStub((1, 0, 0, 0, 1, 0, 0, 0, 1)), rotary=(0, 40, 25)) + np.testing.assert_allclose(at[:3, :3], np.eye(3), atol=1e-12) + + def test_geometry_without_a_frame(self): + at = place(StatStub(None), rotary=(0, 40, 0)) + b = math.radians(40) + # the marker's axis, turned about Y by B + np.testing.assert_allclose(at[:3, 2], (math.sin(b), 0, math.cos(b)), atol=1e-12) + + def test_a_host_stat_without_the_field(self): + at = place(object(), rotary=(0, 40, 0)) + self.assertGreater(abs(at[0, 2]), 0.5) + + +if __name__ == '__main__': + unittest.main() From 6dc9fdcd4efb8131d3c0d32f2afdf9b52c2f3b2f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 11 Oct 2026 01:14:00 +1100 Subject: [PATCH 23/44] motion: jog X, Y and Z along the tilted work plane or the tool A world jog of X, Y or Z moves along one of three frames the operator selects: the machine's axes, the axes of the active tilted work plane, or the tool's own axes as the kinematics module reports them. A program never changes the frame. It is selected by the new NML message EMC_TRAJ_SET_JOG_FRAME, in Python by jog_frame(), and reported as stat.jog_frame and motion.jog-frame. The machine frame is the machine's own X, Y and Z, which do not turn when a table does. Under a tool centre point type whose work frame turns with a joint, a rotary table or a work spindle, X, Y and Z are the part's, so a machine jog there runs through the frame planners turned by the transpose of the work frame, and a jog of X moves the X slide alone whatever the rotaries stand at, as on Heidenhain (M-CS) and Siemens (MCS/BCS). Whether a type turns the work is worked out once per type, by turning each joint a little. The part's axes stay one selection away: the plane frame with no plane in effect is the X, Y and Z of the kinematics in force. Task hands the plane to motion as it executes the plane's message, in program order, turned by the XY rotation of the coordinate system it was defined in. Only a change is sent, so a program without a plane sends motion nothing. Motion plans a frame jog with a planner of its own per frame axis. Each cycle its step, turned into the world, is added to the X, Y and Z teleop planners, so those keep holding where the machine is and a machine jog after a frame jog starts from there. One frame axis moves at a time, and a jog of another waits until the one under way has stopped: two at once would move each world axis by their sum, past what the limits along either allow, and run into the travel along neither one's way. The frame is taken when a jog starts and kept until it ends, and it cannot change while a jog is under way or slowing down. Every world axis keeps to its own [AXIS_L] velocity, acceleration and travel, and the joint cap reads the jog in world coordinates. The travel of an axis not homed is one range either way of where it stands when the jog starts, as for a plain jog of it. The tool frame is the one the kinematics reports whatever plane is in effect: jogging along the plane's axes is what the plane frame is for. Under an identity kinematics type, the machine frame type G13.1 selects, it is the primary type's at the same joints, so a tool jog moves along the tool as the head stands, the move it makes under the primary type: Fanuc's tool axis direction feed (0i-F operator's manual, 1.1.1), Heidenhain's Manual Operation Tool Axis and Siemens' JOG after a swivel move along the tool without their tool centre point either. tests/twp-jog-frame checks the plane and the tool frames on 5axiskins against status, that a plane leaves the tool frame alone, the speed and travel limits along a turned axis, a second frame axis waiting its turn, and the refused change of frame, and samples every cycle for the world axes' speed and acceleration; on the head machine a tool jog under the identity type moves the joints as it does under the primary type from the same joints. tests/twp-jog-frame-scurve runs the same on the S-curve planner. tests/twp-jog-frame-table checks on trivkins not homed the travel a plane jog gets, then on a trunnion under the tool centre point with the table turned and tilted that a machine jog moves one slide and a plane jog with no plane the part's axis. Each starts LinuxCNC once or twice and takes under 10 s, with task on a 1 ms cycle. Requested by Sigma1912. --- docs/src/code/nml-messages.adoc | 1 + docs/src/config/python-interface.adoc | 13 + docs/src/gcode/g-code.adoc | 46 ++ docs/src/man/man9/motion.9.adoc | 6 + include/motion_types.h | 5 + src/emc/motion-logger/motion-logger.c | 14 + src/emc/motion/axis.c | 442 +++++++++++++++++- src/emc/motion/axis.h | 7 +- src/emc/motion/command.c | 36 ++ src/emc/motion/control.c | 124 ++++- src/emc/motion/mot_priv.h | 2 + src/emc/motion/motion.c | 5 + src/emc/motion/motion.h | 20 + src/emc/nml_intf/emc.cc | 13 + src/emc/nml_intf/emc.hh | 3 + src/emc/nml_intf/emc_nml.hh | 17 + src/emc/nml_intf/emcops.cc | 1 + src/emc/task/emctaskmain.cc | 42 +- src/emc/task/taskintf.cc | 48 +- src/emc/usr_intf/axis/extensions/emcmodule.cc | 23 + tests/twp-jog-frame-scurve/README | 3 + tests/twp-jog-frame-scurve/checkresult | 3 + tests/twp-jog-frame-scurve/sim.hal | 1 + tests/twp-jog-frame-scurve/test-ui.py | 1 + tests/twp-jog-frame-scurve/test.sh | 10 + tests/twp-jog-frame-scurve/tool.tbl | 1 + tests/twp-jog-frame-table/README | 13 + tests/twp-jog-frame-table/checkresult | 4 + tests/twp-jog-frame-table/table.hal | 19 + tests/twp-jog-frame-table/table.ini | 120 +++++ tests/twp-jog-frame-table/test-table.py | 157 +++++++ tests/twp-jog-frame-table/test-unhomed.py | 129 +++++ tests/twp-jog-frame-table/test.sh | 7 + tests/twp-jog-frame-table/tool.tbl | 1 + tests/twp-jog-frame-table/unhomed.hal | 13 + tests/twp-jog-frame-table/unhomed.ini | 83 ++++ tests/twp-jog-frame/README | 25 + tests/twp-jog-frame/checkresult | 3 + tests/twp-jog-frame/sim.hal | 31 ++ tests/twp-jog-frame/test-ui.py | 407 ++++++++++++++++ tests/twp-jog-frame/test.ini | 145 ++++++ tests/twp-jog-frame/test.sh | 4 + tests/twp-jog-frame/tool.tbl | 1 + 43 files changed, 1990 insertions(+), 59 deletions(-) create mode 100644 tests/twp-jog-frame-scurve/README create mode 100755 tests/twp-jog-frame-scurve/checkresult create mode 120000 tests/twp-jog-frame-scurve/sim.hal create mode 120000 tests/twp-jog-frame-scurve/test-ui.py create mode 100755 tests/twp-jog-frame-scurve/test.sh create mode 120000 tests/twp-jog-frame-scurve/tool.tbl create mode 100644 tests/twp-jog-frame-table/README create mode 100755 tests/twp-jog-frame-table/checkresult create mode 100644 tests/twp-jog-frame-table/table.hal create mode 100644 tests/twp-jog-frame-table/table.ini create mode 100755 tests/twp-jog-frame-table/test-table.py create mode 100755 tests/twp-jog-frame-table/test-unhomed.py create mode 100755 tests/twp-jog-frame-table/test.sh create mode 120000 tests/twp-jog-frame-table/tool.tbl create mode 100644 tests/twp-jog-frame-table/unhomed.hal create mode 100644 tests/twp-jog-frame-table/unhomed.ini create mode 100644 tests/twp-jog-frame/README create mode 100755 tests/twp-jog-frame/checkresult create mode 100644 tests/twp-jog-frame/sim.hal create mode 100755 tests/twp-jog-frame/test-ui.py create mode 100644 tests/twp-jog-frame/test.ini create mode 100755 tests/twp-jog-frame/test.sh create mode 100644 tests/twp-jog-frame/tool.tbl diff --git a/docs/src/code/nml-messages.adoc b/docs/src/code/nml-messages.adoc index 307066ea39d..5b1bf4a702d 100644 --- a/docs/src/code/nml-messages.adoc +++ b/docs/src/code/nml-messages.adoc @@ -92,6 +92,7 @@ EMC_TRAJ_SET_G92_TYPE EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE EMC_TRAJ_PROBE_TYPE EMC_TRAJ_SET_TELEOP_ENABLE_TYPE +EMC_TRAJ_SET_JOG_FRAME_TYPE EMC_TRAJ_SET_SPINDLESYNC_TYPE EMC_TRAJ_SET_SPINDLE_SCALE_TYPE EMC_TRAJ_SET_FO_ENABLE_TYPE diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 50a41203f25..f3e985606f8 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -224,6 +224,12 @@ see <> for an example. INTERP_FILE_NOT_OPEN, INTERP_ERROR. see src/emc/nml_intf/interp_return.hh +*jog_frame*:: '(returns integer)' - + the frame world jogs of X, Y and Z move along, set by `jog_frame()`. One of: + * JOG_FRAME_MACHINE + * JOG_FRAME_PLANE + * JOG_FRAME_TOOL + *joint*:: '(returns tuple of dicts)' - reflecting current joint values. See <>. @@ -715,6 +721,13 @@ c.tool_offset(toolno, z_offset, x_offset, diameter, frontangle, backangle, orie the known coordinate letters XYZABCUVW (x=>0,y=>1,z=>2,a=>3,b=>4,c=>5,u=>6,v=>7,w=>8) +`jog_frame(int)`:: + set the frame an axis jog of X, Y and Z moves along: the machine's axes, the axes of the active tilted work plane, or the tool's own axes; see <>. Refused while a jog is under way. The other axis letters jog along their own axes in every frame. + + Constants;; + JOG_FRAME_MACHINE + + JOG_FRAME_PLANE + + JOG_FRAME_TOOL + `load_tool_table()`:: reload the tool table. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 31b08ff6445..f48f7b04448 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2655,6 +2655,52 @@ It is an error if: * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. +[[sec:jog-frame]] +=== Jogging along the plane or the tool(((Jog frame))) + +In world mode a jog of X, Y or Z moves along one of three frames: + +* *machine*: the machine's own X, Y and Z, the slides on a machine whose + slides line up with its frame. The machine does not turn when its table + does: under a tool centre point kinematics, where X, Y and Z are the + part's and turn with a rotary table or a work spindle, a jog of X still + moves the X slide alone, whatever the rotaries stand at, and the + position the GUI shows changes along the part's axes it crosses. +* *plane*: the axes of the active plane, the ones a program under it moves + along, so a jog of Z backs the tool straight out of a hole drilled into + the plane. With no plane active it is the X, Y and Z of the kinematics in + force: under a tool centre point kinematics the part's axes, turned with + the table. +* *tool*: the tool's own axes at the current pose as the kinematics module + reports them (`tool_frame` in status), Z along the tool from the tip + towards the holder: for a spindle head set at an angle, or a tool that is + not normal to the plane. A plane in effect does not change it: to jog + along the plane's axes, select the plane frame. Under an identity + kinematics type, the machine frame kinematics G13.1 selects, the tool's + axes are the ones the module's tool centre point kinematics has at the + joints, so a tool jog moves the machine as it does under the tool centre + point kinematics, along the tool as the head stands: Fanuc's tool axis + direction feed and Heidenhain's Manual Operation Tool Axis move so + without the tool centre point too. With a module that reports no frames + it is the machine frame. + +The operator selects the frame and a program never changes it: Python +`jog_frame()`, with `jog_frame` in status. `motion.jog-frame` reports the +frame in HAL, so a pendant can, for instance, let only Z through in the tool +frame. The jog pins `axis.L.jog-*` jog the letter of the selected frame. A, B, +C, U, V and W jog along their own axes in every frame, and joint mode is not +affected. + +The frame is taken when a jog starts and kept until the jog ends, so a jog +in the tool frame does not bend while a rotary jogs with it, and the frame +cannot change while a jog is under way. One of X, Y and Z moves at a time in +a frame: a jog of another waits until the one under way has stopped. Every +world axis keeps to its own `[AXIS_L]` velocity, acceleration and travel: a +jog along a turned axis runs as fast as the slowest axis it moves allows, a +continuous jog stops where the turned axis meets the travel, and an +incremental jog that would end outside it is refused. The joint limits cap +it as they cap any world jog. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index d24119c1ff6..84bdbc08630 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -215,6 +215,11 @@ Note: feed-inhibit applies to G-code commands -- not jogs. will be stopped immediately. *motion.jog-is-active* OUT BOOL:: TRUE if any joint or axis is jogging. +*motion.jog-frame* OUT SINT:: + The frame a jog of X, Y and Z moves along in world mode: 0 the machine + axes, 1 the axes of the tilted work plane, 2 the axes of the tool. The + **axis.**_L_**.jog-*** pins of X, Y and Z jog the letter of that frame; + a HAL net can read the pin to let only some of them through in a frame. *motion.in-position* OUT BOOL:: TRUE if the machine is in position (i.e., not currently moving towards the commanded position). **motion.misc-error-**_NN_ IN BOOL:: @@ -355,6 +360,7 @@ These pins represent the *commanded geometric intent* from the interpreter and a Values greater than 1 or less than zero are ignored. **axis.**_L_**.jog-counts** IN SINT:: Connect to the "counts" pin of an external encoder to use a physical jog wheel. + For X, Y and Z the wheel jogs that letter of the frame *motion.jog-frame* reports. **axis.**_L_**.jog-enable** IN BOOL:: When TRUE (and in manual mode), any change to "jog-counts" will result in motion. When false, "jog-counts" is ignored. diff --git a/include/motion_types.h b/include/motion_types.h index 60cadaa05ed..32a6426b8ea 100644 --- a/include/motion_types.h +++ b/include/motion_types.h @@ -27,4 +27,9 @@ #define EMC_SO_OVERRIDE_ON 1 #define EMC_SO_OVERRIDE_LOCK 2 +/* the frame a world jog of X, Y and Z moves along, EMC_TRAJ_SET_JOG_FRAME */ +#define EMC_JOG_FRAME_MACHINE 0 +#define EMC_JOG_FRAME_PLANE 1 +#define EMC_JOG_FRAME_TOOL 2 + #endif diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 28978ae80e8..52e7b219159 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -700,6 +700,20 @@ int main(int argc, char* argv[]) { c->probe_home_err_inhibit); break; + case EMCMOT_SET_WORK_PLANE: + log_print( + "SET_WORK_PLANE active=%d, x=(%.6g %.6g %.6g), y=(%.6g %.6g %.6g), z=(%.6g %.6g %.6g)\n", + c->work_plane_active, + c->work_plane[0], c->work_plane[3], c->work_plane[6], + c->work_plane[1], c->work_plane[4], c->work_plane[7], + c->work_plane[2], c->work_plane[5], c->work_plane[8] + ); + break; + + case EMCMOT_SET_JOG_FRAME: + log_print("SET_JOG_FRAME %d\n", c->jog_frame); + break; + default: log_print("ERROR: unknown command %d\n", c->command); diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c index 6bbaf08ad16..4941e9c2fbc 100644 --- a/src/emc/motion/axis.c +++ b/src/emc/motion/axis.c @@ -1,6 +1,7 @@ #include #include +#include // memset #include // EMCMOT_MAX_AXIS #include "axis.h" @@ -71,6 +72,42 @@ typedef struct { static emcmot_axis_t axis_array[EMCMOT_MAX_AXIS]; static axis_hal_data_t *hal_data = NULL; +/* A world jog of X, Y and Z along the axes of a frame turned against the + world, the work plane or the tool. A planner per frame axis plans the + jog along it; each cycle its step is turned into the world and added to + the X, Y and Z teleop planners, position and command alike, so those + always hold where the machine is. One frame axis moves at a time, and a + jog of another waits until the one under way has stopped: two at once + would move each world axis by their sum, past what the limits along + either allow, and into the travel along neither one's way. The rotation + is taken when a frame jog starts and kept until the planners are at + rest: a tool frame turning with the rotaries would bend the jog under + way. A frame axis gets the limits of the world axes over the share of + the jog each takes. */ +typedef struct { + int selected; /* X Y Z jogs go to the frame */ + double rot_now[3][3]; /* the frame as last reported, columns + its axes in world coordinates */ + double rot[3][3]; /* the frame the planners run in */ + int latched; /* rot is in use */ + int owner; /* the planner that moves, -1 none */ + unsigned int asked; /* counts the jogs asked for */ + simple_tp_t tp[3]; /* planners along the frame's axes */ + unsigned int order[3]; /* when each was asked */ + int cont[3]; /* a continuous jog, to the travel */ + double vel_req[3]; /* the speed the jog asked for */ + double acc_req[3]; /* and the acceleration it may use */ + double vel_cap[3]; /* the most the joints allow of them */ + double acc_cap[3]; + double vel_limit[3]; /* the world axes' limits along the */ + double acc_limit[3]; /* frame axis */ + double jerk_limit[3]; + double min_lim[3]; /* the travel of X, Y and Z, taken */ + double max_lim[3]; /* with the frame */ +} jog_frame_t; + +static jog_frame_t frame; + // Mark strings for translation, but defer translation to userspace #define _(s) (s) @@ -82,6 +119,10 @@ void axis_init_all(void) emcmot_axis_t *axis = &axis_array[n]; axis->locking_joint = -1; } + for (n = 0; n < 3; n++) { + frame.rot_now[n][n] = 1.0; + frame.rot[n][n] = 1.0; + } } void axis_initialize_external_offsets(void) @@ -175,7 +216,8 @@ void axis_output_to_hal(double *pcmd_p[]) hal_set_real(axis_data->teleop_vel_cmd, axis->teleop_vel_cmd); hal_set_real(axis_data->teleop_pos_cmd, axis->teleop_tp.pos_cmd); hal_set_real(axis_data->teleop_vel_lim, axis->teleop_tp.max_vel); - hal_set_bool(axis_data->teleop_tp_enable, axis->teleop_tp.enable); + hal_set_bool(axis_data->teleop_tp_enable, axis->teleop_tp.enable + || (n < 3 && frame.tp[n].enable)); hal_set_bool(axis_data->kb_ajog_active, axis->kb_ajog_active); hal_set_bool(axis_data->wheel_ajog_active, axis->wheel_ajog_active); @@ -263,7 +305,7 @@ double axis_get_compound_velocity(void) for (n = 0; n < EMCMOT_MAX_AXIS; n++) { emcmot_axis_t *axis = &axis_array[n]; - if (axis->teleop_tp.active) { + if (axis->teleop_tp.active || (n < 3 && frame.latched)) { v2 += axis->teleop_vel_cmd * axis->teleop_vel_cmd; } } @@ -322,6 +364,299 @@ static void axis_jog_limits(int axis_num, double *min_lim, double *max_lim) } } +// The frame for jogs of X, Y and Z, its axes in world coordinates as the +// columns of rot, row major; selected 0 jogs them along the world axes +void axis_set_jog_frame(int selected, const double rot[9]) +{ + int i, k; + + frame.selected = selected; + for (i = 0; i < 3; i++) { + for (k = 0; k < 3; k++) { frame.rot_now[i][k] = rot[3 * i + k]; } + } +} + +// Whether frame planner k moves, or has been asked to +static int frame_busy(int k); + +// Whether any teleop planner moves, a frame jog's included +bool axis_jog_is_moving(void) +{ + int n; + + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + if (axis_array[n].teleop_tp.active || axis_array[n].teleop_tp.curr_vel != 0.0) { + return 1; + } + } + return frame.latched; +} + +// Whether a planner has somewhere to go this cycle, and which way +static int planner_request(const simple_tp_t *tp, double *dir) +{ + double togo = tp->pos_cmd - tp->curr_pos; + + if (!tp->enable) { return 0; } + if (fabs(togo) < TINY_DP(tp->max_acc, 0.001) && tp->curr_vel == 0.0) { return 0; } + *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : tp->curr_vel > 0.0 ? 1.0 : -1.0; + return 1; +} + +static int frame_busy(int k) +{ + double dir; + + return frame.tp[k].active || frame.tp[k].curr_vel != 0.0 + || planner_request(&frame.tp[k], &dir); +} + +// Where world axis n is commanded, external offset included +static double world_pos(int n) +{ + return axis_array[n].teleop_tp.curr_pos + axis_array[n].ext_offset_tp.curr_pos; +} + +// Take the frame for a jog about to start, unless one is under way +static void frame_latch(void) +{ + int i, k; + + if (frame.latched) { return; } + for (k = 0; k < 3; k++) { + double vel = 1e99, acc = 1e99, jerk = 1e99; + for (i = 0; i < 3; i++) { + double u = frame.rot_now[i][k]; + // a rounding error is no share of the jog, and would hold an + // axis standing on its limit + if (fabs(u) < 1e-12) { u = 0.0; } + frame.rot[i][k] = u; + u = fabs(u); + if (u == 0.0) { continue; } + if (axis_array[i].vel_limit / u < vel) { vel = axis_array[i].vel_limit / u; } + if (axis_array[i].acc_limit / u < acc) { acc = axis_array[i].acc_limit / u; } + if (axis_array[i].jerk_limit / u < jerk) { jerk = axis_array[i].jerk_limit / u; } + } + frame.vel_limit[k] = vel; + frame.acc_limit[k] = acc; + frame.jerk_limit[k] = jerk; + } + // an axis not homed jogs within one range of where it stands, as a + // plain jog of it does + for (i = 0; i < 3; i++) { axis_jog_limits(i, &frame.min_lim[i], &frame.max_lim[i]); } + memset(frame.tp, 0, sizeof(frame.tp)); + memset(frame.cont, 0, sizeof(frame.cont)); + frame.owner = -1; + frame.asked = 0; + frame.latched = 1; +} + +// Stop the frame planners where they are and let the frame go +static void frame_release(void) +{ + memset(frame.tp, 0, sizeof(frame.tp)); + memset(frame.cont, 0, sizeof(frame.cont)); + frame.owner = -1; + frame.latched = 0; +} + +// How far frame axis k may go, s the way, before the jog leaves the box +static double frame_reach(int k, double s) +{ + double reach = 1e99; + int i; + + for (i = 0; i < 3; i++) { + double u = s * frame.rot[i][k], room; + if (u == 0.0) { continue; } + room = ((u > 0.0 ? frame.max_lim[i] : frame.min_lim[i]) + - world_pos(i)) / u; + if (room < reach) { reach = room; } + } + return reach > 0.0 ? reach : 0.0; +} + +// The planner that moves: the one under way until it has stopped, then the +// one asked for first of those waiting. A continuous jog runs to the box +// from where it starts +static int frame_owner(void) +{ + int k, next = -1; + + if (frame.owner >= 0 && frame_busy(frame.owner)) { return frame.owner; } + for (k = 0; k < 3; k++) { + if (frame_busy(k) && (next < 0 || frame.order[k] < frame.order[next])) { next = k; } + } + if (next >= 0 && frame.cont[next]) { + simple_tp_t *tp = &frame.tp[next]; + double s = tp->pos_cmd > tp->curr_pos ? 1.0 : -1.0; + tp->pos_cmd = tp->curr_pos + s * frame_reach(next, s); + } + frame.owner = next; + return next; +} + +// Whether the box holds the frame jogs once frame axis k is sent to target, +// each jog waiting its turn: where each ends, in the order they were asked +// for, inside it or no further out than now. A continuous jog waiting is +// left out, it stops at the box from wherever it starts +static int frame_ends_inside(int k, double target) +{ + double now[3], end[3]; + int i, j, done[3] = {0, 0, 0}; + + for (i = 0; i < 3; i++) { now[i] = end[i] = world_pos(i); } + for (;;) { + int next = -1; + double to; + for (j = 0; j < 3; j++) { + if (done[j] || (j != k && (!frame_busy(j) || frame.cont[j]))) { continue; } + if (next < 0 || (j != k && (next == k || frame.order[j] < frame.order[next]))) { + next = j; + } + } + if (next < 0) { return 1; } + done[next] = 1; + to = next == k ? target : frame.tp[next].pos_cmd; + for (i = 0; i < 3; i++) { + end[i] += frame.rot[i][next] * (to - frame.tp[next].curr_pos); + if (end[i] > frame.max_lim[i] && end[i] > now[i]) { return 0; } + if (end[i] < frame.min_lim[i] && end[i] < now[i]) { return 0; } + } + } +} + +// A jog of frame axis k under way, or one waiting its turn, keeps its +// place; a new one goes after those asked for before it +static void frame_start(int k, int queued, double vel, double acc) +{ + simple_tp_t *tp = &frame.tp[k]; + + if (!queued) { frame.order[k] = ++frame.asked; } + frame.vel_req[k] = vel < frame.vel_limit[k] ? vel : frame.vel_limit[k]; + frame.acc_req[k] = acc; + frame.vel_cap[k] = frame.vel_req[k]; + frame.acc_cap[k] = frame.acc_req[k]; + tp->max_vel = frame.vel_req[k]; + tp->max_acc = frame.acc_req[k]; + tp->enable = 1; +} + +static void frame_jog_cont(int k, double vel) +{ + double s = vel > 0.0 ? 1.0 : -1.0; + simple_tp_t *tp = &frame.tp[k]; + int queued; + + frame_latch(); + queued = frame_busy(k); + frame.cont[k] = 1; + // how far is read from the box when the jog gets to move + tp->pos_cmd = tp->curr_pos + s * (frame.owner == k ? frame_reach(k, s) : 1.0); + frame_start(k, queued, fabs(vel), frame.acc_limit[k]); + axis_array[k].kb_ajog_active = 1; +} + +static void frame_jog_incr(int k, double offset, double vel) +{ + simple_tp_t *tp = &frame.tp[k]; + double target; + int queued; + + frame_latch(); + queued = frame_busy(k); + target = (frame.cont[k] ? tp->curr_pos : tp->pos_cmd) + (vel > 0.0 ? offset : -offset); + if (!frame_ends_inside(k, target)) { return; } + frame.cont[k] = 0; + tp->pos_cmd = target; + frame_start(k, queued, fabs(vel), frame.acc_limit[k]); + axis_array[k].kb_ajog_active = 1; +} + +// A jog wheel's counts on X, Y or Z, as axis_handle_jogwheels() takes them +// in the world +static int frame_jog_wheel(int k, double distance, double fraction, bool vel_mode) +{ + simple_tp_t *tp = &frame.tp[k]; + double pos, acc; + int queued; + + frame_latch(); + queued = frame_busy(k); + acc = fraction * frame.acc_limit[k]; + pos = (frame.cont[k] ? tp->curr_pos : tp->pos_cmd) + distance; + if (vel_mode) { + double v = frame.vel_limit[k]; + double stop_dist = v * v / (2 * acc); + if (pos > tp->curr_pos + stop_dist) { + pos = tp->curr_pos + stop_dist; + } else if (pos < tp->curr_pos - stop_dist) { + pos = tp->curr_pos - stop_dist; + } + } + if (!frame_ends_inside(k, pos)) { return 0; } + frame.cont[k] = 0; + tp->pos_cmd = pos; + frame_start(k, queued, frame.vel_limit[k], acc); + axis_array[k].wheel_ajog_active = 1; + return 1; +} + +// Run the frame planner whose turn it is and carry its step into the world +// planners; 1 if the step would have taken an axis out of its box and was +// held back +static int frame_step(double period) +{ + simple_tp_t *tp; + double was, step[3]; + int i, k, out = 0; + + if (!frame.latched) { return 0; } + k = frame_owner(); + if (k < 0) { + // all at rest: the next frame jog takes the frame afresh + frame_release(); + return 0; + } + tp = &frame.tp[k]; + tp->max_vel = frame.vel_cap[k] < frame.vel_req[k] ? frame.vel_cap[k] : frame.vel_req[k]; + tp->max_acc = frame.acc_cap[k] < frame.acc_req[k] ? frame.acc_cap[k] : frame.acc_req[k]; + tp->max_jerk = frame.jerk_limit[k]; + was = tp->curr_pos; + simple_tp_update(tp, period); + for (i = 0; i < 3; i++) { + double to; + step[i] = frame.rot[i][k] * (tp->curr_pos - was); + to = world_pos(i) + step[i]; + if ((step[i] > 0.0 && to >= frame.max_lim[i]) + || (step[i] < 0.0 && to <= frame.min_lim[i])) { + out = 1; + } + } + if (out) { + // the jog ends where it is + tp->curr_pos = tp->pos_cmd = was; + tp->curr_vel = tp->curr_acc = 0.0; + tp->active = 0; + return 1; + } + for (i = 0; i < 3; i++) { + axis_array[i].teleop_tp.curr_pos += step[i]; + axis_array[i].teleop_tp.pos_cmd += step[i]; + } + return 0; +} + +// The velocity the frame planners give world axis n +static double frame_vel(int n) +{ + double v = 0.0; + int k; + + if (n >= 3 || !frame.latched) { return 0.0; } + for (k = 0; k < 3; k++) { v += frame.rot[n][k] * frame.tp[k].curr_vel; } + return v; +} void axis_jog_cont(int axis_num, double vel, long servo_period) { @@ -329,6 +664,10 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) emcmot_axis_t *axis = &axis_array[axis_num]; double min_lim, max_lim; + if (frame.selected && axis_num < 3) { + frame_jog_cont(axis_num, vel); + return; + } axis_jog_limits(axis_num, &min_lim, &max_lim); if (vel > 0.0) { axis->teleop_tp.pos_cmd = max_lim; @@ -350,6 +689,10 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) emcmot_axis_t *axis = &axis_array[axis_num]; double tmp1, min_lim, max_lim; + if (frame.selected && axis_num < 3) { + frame_jog_incr(axis_num, offset, vel); + return; + } if (vel > 0.0) { tmp1 = axis->teleop_tp.pos_cmd + offset; } else { @@ -406,6 +749,16 @@ bool axis_jog_abort(int axis_num, bool immediate) if (immediate) { axis->teleop_tp.curr_vel = 0.0; } + if (axis_num < 3) { + simple_tp_t *tp = &frame.tp[axis_num]; + if (tp->enable) { + aborted = 1; + } + tp->enable = 0; + if (immediate) { + tp->curr_vel = 0.0; + } + } return aborted; } @@ -480,6 +833,17 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo } distance = delta * hal_get_real(axis_data->ajog_scale); + if (frame.selected && axis_num < 3) { + if ( (ajog_accel_fraction > 1) + || (ajog_accel_fraction < 0) ) { + ajog_accel_fraction = 1; + } + if (!frame_jog_wheel(axis_num, distance, ajog_accel_fraction, + hal_get_bool(axis_data->ajog_vel_mode))) { + break; + } + continue; + } pos = axis->teleop_tp.pos_cmd + distance; if ( hal_get_bool(axis_data->ajog_vel_mode) ) { double v = axis->vel_limit; @@ -527,6 +891,7 @@ void axis_sync_teleop_tp_to_carte_pos(int extfactor, double *pcmd_p[]) axis_array[n].teleop_tp.curr_vel = 0.0; axis_array[n].teleop_tp.curr_acc = 0.0; } + frame_release(); } void axis_sync_carte_pos_to_teleop_tp(int extfactor, double *pcmd_p[]) @@ -756,30 +1121,58 @@ static int update_teleop_with_check(int axis_num, simple_tp_t *the_tp, double se return 0; } -// Whether the axis's teleop planner has somewhere to go this cycle, and -// which way, how fast and how hard the jog asked it to: what the cap on -// the joints reads before the planners run -int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc) +// What the teleop planners that have somewhere to go this cycle ask of +// the world axes, the velocity and the acceleration each jog asked for +// along its way, and the velocity they are at: what the cap on the joints +// reads before the planners run. 0 if none has anywhere to go +int axis_teleop_request(double vel[], double acc[], double now[]) { - emcmot_axis_t *axis = &axis_array[axis_num]; - double togo = axis->teleop_tp.pos_cmd - axis->teleop_tp.curr_pos; + double dir; + int n, k, any = 0; - if (!axis->teleop_tp.enable) { return 0; } - if (fabs(togo) < TINY_DP(axis->teleop_tp.max_acc, 0.001) && axis->teleop_tp.curr_vel == 0.0) { return 0; } - *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : axis->teleop_tp.curr_vel > 0.0 ? 1.0 : -1.0; - *vel = axis->teleop_vel_req; - *acc = axis->teleop_acc_req; - return 1; + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + emcmot_axis_t *axis = &axis_array[n]; + vel[n] = acc[n] = now[n] = 0.0; + if (planner_request(&axis->teleop_tp, &dir)) { + vel[n] = dir * axis->teleop_vel_req; + acc[n] = dir * axis->teleop_acc_req; + now[n] = axis->teleop_tp.curr_vel; + any = 1; + } + } + // the frame planner whose turn it is, the others wait + k = frame.latched ? frame_owner() : -1; + if (k >= 0 && planner_request(&frame.tp[k], &dir)) { + for (n = 0; n < 3; n++) { + vel[n] += frame.rot[n][k] * dir * frame.vel_req[k]; + acc[n] += frame.rot[n][k] * dir * frame.acc_req[k]; + now[n] += frame.rot[n][k] * frame.tp[k].curr_vel; + } + any = 1; + } + return any; } -// The most the planner may do this cycle: what the jog asked, or less -// where the joints cannot follow that -void axis_teleop_cap(int axis_num, double vel, double acc) +// The most the planners with somewhere to go may do this cycle: the given +// share of what their jog asked, where the joints cannot follow all of it +void axis_teleop_cap(double vel_scale, double acc_scale) { - emcmot_axis_t *axis = &axis_array[axis_num]; + double dir; + int n, k; - axis->teleop_tp.max_vel = vel < axis->teleop_vel_req ? vel : axis->teleop_vel_req; - axis->teleop_tp.max_acc = acc < axis->teleop_acc_req ? acc : axis->teleop_acc_req; + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + emcmot_axis_t *axis = &axis_array[n]; + if (!planner_request(&axis->teleop_tp, &dir)) { continue; } + axis->teleop_tp.max_vel = vel_scale * axis->teleop_vel_req; + axis->teleop_tp.max_acc = acc_scale * axis->teleop_acc_req; + } + k = frame.latched ? frame.owner : -1; + if (k >= 0 && planner_request(&frame.tp[k], &dir)) { + frame.vel_cap[k] = vel_scale * frame.vel_req[k]; + frame.acc_cap[k] = acc_scale * frame.acc_req[k]; + } } int axis_calc_motion(double servo_period) @@ -788,6 +1181,11 @@ int axis_calc_motion(double servo_period) int violated_teleop_limit = 0; emcmot_axis_t *axis; + // the frame planner first, so the frame's state is this cycle's when + // the jog flags it shares with X, Y and Z are read below + if (frame_step(servo_period)) { + violated_teleop_limit = 1; + } for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { axis = &axis_array[axis_num]; @@ -799,10 +1197,10 @@ int axis_calc_motion(double servo_period) violated_teleop_limit = 1; } - axis->teleop_vel_cmd = axis->teleop_tp.curr_vel; + axis->teleop_vel_cmd = axis->teleop_tp.curr_vel + frame_vel(axis_num); axis->pos_cmd = axis->teleop_tp.curr_pos; - if (!axis->teleop_tp.active) { + if (!axis->teleop_tp.active && !(axis_num < 3 && frame.latched && frame_busy(axis_num))) { axis->kb_ajog_active = 0; axis->wheel_ajog_active = 0; } diff --git a/src/emc/motion/axis.h b/src/emc/motion/axis.h index bc00504570d..834d548e69a 100644 --- a/src/emc/motion/axis.h +++ b/src/emc/motion/axis.h @@ -51,10 +51,13 @@ void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); -int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc); -void axis_teleop_cap(int axis_num, double vel, double acc); +int axis_teleop_request(double vel[], double acc[], double now[]); +void axis_teleop_cap(double vel_scale, double acc_scale); int axis_calc_motion(double servo_period); +void axis_set_jog_frame(int selected, const double rot[9]); +bool axis_jog_is_moving(void); + #ifdef __cplusplus } diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 33d9a149416..35ba4578d5b 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2335,6 +2335,42 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotConfig->switchkins_seq++; break; + case EMCMOT_SET_WORK_PLANE: + /* in program order, after the moves before it; a frame jog under + way keeps the frame it started in */ + rtapi_print_msg(RTAPI_MSG_DBG, "SET_WORK_PLANE"); + for (n = 0; n < 9; n++) { + emcmotInternal->work_plane[n] = emcmotCommand->work_plane[n]; + } + emcmotInternal->work_plane_active = emcmotCommand->work_plane_active; + emcmotUpdateJogFrame(); + break; + + case EMCMOT_SET_JOG_FRAME: + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOG_FRAME"); + if (emcmotCommand->jog_frame < EMC_JOG_FRAME_MACHINE + || emcmotCommand->jog_frame > EMC_JOG_FRAME_TOOL) { + reportError(_("unknown jog frame %d"), emcmotCommand->jog_frame); + break; + } + if (emcmotCommand->jog_frame == emcmotStatus->jog_frame) { + break; + } + /* not while a jog is under way, nor while one slows down: a + jog in the new frame would move the same world axes at once. + Planners left moving by leaving teleop mode are put at rest + when teleop mode is entered again */ + if (axis_jog_is_active() + || (emcmotStatus->motion_state == EMCMOT_MOTION_TELEOP + && axis_jog_is_moving())) { + reportError(_("the jog frame cannot change while jogging")); + break; + } + emcmotStatus->jog_frame = emcmotCommand->jog_frame; + /* in force for a jog wheel count in this very cycle */ + emcmotUpdateJogFrame(); + break; + default: rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); reportError(_("unrecognized command %d"), emcmotCommand->command); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index a10b327751d..4157e5f6bfa 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1377,13 +1377,12 @@ static void teleop_joint_cap(double period) static double acc_scale_prev = 1.0; double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; double joint_pos[EMCMOT_MAX_JOINTS]; - double dir[EMCMOT_MAX_AXIS], vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS]; + double vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS], now[EMCMOT_MAX_AXIS]; double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; - int active[EMCMOT_MAX_AXIS]; double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; double vel_scale, acc_scale, stop_acc; const double tiny = 1e-12; - int flags, any = 0, a, j, pinned = 0; + int flags, a, j, pinned = 0; if (!kinematicsJacobian) { have_prev = 0; return; } flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; @@ -1392,34 +1391,28 @@ static void teleop_joint_cap(double period) have_prev = 0; return; } + /* the jog in world coordinates, a frame jog turned in already */ + if (!axis_teleop_request(vreq, areq, now)) { have_prev = 0; return; } for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - active[a] = axis_teleop_request(a, &dir[a], &vreq[a], &areq[a]); - if (active[a]) { - double v = axis_get_teleop_vel_cmd(a); - any = 1; - rnorm += vreq[a] * vreq[a]; - anorm += areq[a] * areq[a]; - speed += v * v; - } + rnorm += vreq[a] * vreq[a]; + anorm += areq[a] * areq[a]; + speed += now[a] * now[a]; } - if (!any) { have_prev = 0; return; } rnorm = sqrt(rnorm); anorm = sqrt(anorm); speed = sqrt(speed); if (rnorm < tiny || anorm < tiny) { have_prev = 0; return; } for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } if (kinematicsJacobian(joint_pos, &emcmotStatus->carte_pos_cmd, jac, &iflags) != 0) { - for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - if (active[a]) { axis_teleop_cap(a, vreq[a], areq[a]); } - } + axis_teleop_cap(1.0, 1.0); have_prev = 0; return; } /* the direction the jog was asked in, and the one the planners accelerate in, each at its own limit */ for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - rhat[a] = active[a] ? dir[a] * vreq[a] / rnorm : 0.0; - ahat[a] = active[a] ? dir[a] * areq[a] / anorm : 0.0; + rhat[a] = vreq[a] / rnorm; + ahat[a] = areq[a] / anorm; } /* the path the planners need to stop from the speed they are at, at the acceleration they were left last cycle */ @@ -1487,9 +1480,7 @@ static void teleop_joint_cap(double period) if (vel_scale > 1.0) { vel_scale = 1.0; } if (acc_scale > 1.0) { acc_scale = 1.0; } if (acc_scale < 1e-6) { acc_scale = 1e-6; } - for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - if (active[a]) { axis_teleop_cap(a, vel_scale * vreq[a], acc_scale * areq[a]); } - } + axis_teleop_cap(vel_scale, acc_scale); if (pinned && speed < tiny) { axis_jog_abort_all(0); } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } @@ -2485,7 +2476,38 @@ static void output_to_hal(void) axis_output_to_hal(pcmd_p); hal_set_bool(emcmot_hal_data->jog_is_active, axis_jog_is_active() || joint_jog_is_active()); + hal_set_sint(emcmot_hal_data->jog_frame, emcmotStatus->jog_frame); + +} +static double frame_entry(const PmCartesian *c, int k) +{ + return (k == 0) ? c->x : (k == 1) ? c->y : c->z; +} + +/* Whether the work frame of the type in force turns with any joint: a + rotary table, or a work spindle, under a tool centre point type. A + property of the type, so worked out once per type, by turning each + joint a little from where the machine is. */ +static int work_frame_turns(double *joint_pos, const PmRotationMatrix *work) +{ + PmRotationMatrix turned; + const PmCartesian *w[3] = { &work->x, &work->y, &work->z }; + const PmCartesian *u[3] = { &turned.x, &turned.y, &turned.z }; + int j, a, k, turns = 0; + + for (j = 0; j < NO_OF_KINS_JOINTS && !turns; j++) { + joint_pos[j] += 7.0; + if (kinematicsWorkFrame(joint_pos, &turned, &fflags) == 0) { + for (a = 0; a < 3; a++) { + for (k = 0; k < 3; k++) { + if (fabs(frame_entry(u[a], k) - frame_entry(w[a], k)) > 1e-9) { turns = 1; } + } + } + } + joint_pos[j] -= 7.0; + } + return turns; } /* The type that follows the part, the module's working transform, or -1 @@ -2502,9 +2524,12 @@ static int primary_kins_type(void) } /* The tool frame in the work frame, transpose(work) * tool as - toolFrameInWork() has it, at the commanded joints, for the status. */ + toolFrameInWork() has it, at the commanded joints, for the status; and + the machine axes in the work frame, transpose(work), for the machine + jog frame on a type that turns the work. */ static void update_tool_frame(void) { + static int turns_type = -1, turns = 0; double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; PmRotationMatrix work, tool; const PmCartesian *w[3] = { &work.x, &work.y, &work.z }; @@ -2512,13 +2537,18 @@ static void update_tool_frame(void) int j, a, b, f, primary; emcmotStatus->tool_frame_ok = 0; + emcmotInternal->work_frame_ok = 0; + emcmotInternal->tool_jog_ok = 0; if (!kinematicsToolFrame || !kinematicsWorkFrame) { return; } if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { return; } for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } /* an identity type turns neither frame whatever the rotaries do, so it says nothing about where the tool points. The primary type, the one that follows the part, still knows: a display under the machine frame - type shows the tool against the part as it has them. */ + type shows the tool against the part as it has them, and a tool jog + moves along the tool as the head stands, the move it makes under the + primary type, as Fanuc's tool axis direction feed and Heidenhain's + Manual Operation Tool Axis move without the tool centre point. */ if (switchkins_type >= 0 && switchkins_type < SWITCHKINS_MAX_TYPES) { f = emcmotStatus->switchkins_flags[switchkins_type]; if (f >= 0 && (f & KINSTYPE_IDENTITY)) { @@ -2535,16 +2565,31 @@ static void update_tool_frame(void) for (b = 0; b < 3; b++) { emcmotStatus->tool_frame[3 * a + b] = w[a]->x * t[b]->x + w[a]->y * t[b]->y + w[a]->z * t[b]->z; + /* entry (a, b) is component a of column b of the tool + frame, the machine's world being the machine frame */ + emcmotInternal->tool_jog[3 * a + b] = frame_entry(t[b], a); } } emcmotStatus->tool_frame_ok = 1; + emcmotInternal->tool_jog_ok = 1; return; } } - if (kinematicsWorkFrame(joint_pos, &work, &fflags) != 0 - || kinematicsToolFrame(joint_pos, &tool, &fflags) != 0) { - return; + if (kinematicsWorkFrame(joint_pos, &work, &fflags) != 0) { return; } + if (turns_type != switchkins_type) { + turns = work_frame_turns(joint_pos, &work); + turns_type = switchkins_type; } + if (turns) { + /* entry (a, b) is component b of column a of the work frame */ + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotInternal->work_frame[3 * a + b] = frame_entry(w[a], b); + } + } + emcmotInternal->work_frame_ok = 1; + } + if (kinematicsToolFrame(joint_pos, &tool, &fflags) != 0) { return; } /* entry (a, b) is column a of the work frame dotted with column b of the tool frame */ for (a = 0; a < 3; a++) { @@ -2554,6 +2599,34 @@ static void update_tool_frame(void) } } emcmotStatus->tool_frame_ok = 1; + for (a = 0; a < 9; a++) { emcmotInternal->tool_jog[a] = emcmotStatus->tool_frame[a]; } + emcmotInternal->tool_jog_ok = 1; +} + +/* The frame world jogs of X, Y and Z move along, for the teleop planners: + the machine axes, which a type turning the work sees turned (the + machine does not tilt when the table does); the tilted work plane, or + the world axes where there is none; or the tool frame as the kinematics + reports it, whatever plane is in effect, the world axes where there is + none. Every cycle, and when the frame or the plane changes. */ +void emcmotUpdateJogFrame(void) +{ + double r[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + const double *m = emcmotInternal->work_frame; + const double *p = emcmotInternal->work_plane; + const double *t = emcmotInternal->tool_jog; + int selected = (emcmotStatus->jog_frame != EMC_JOG_FRAME_MACHINE); + int i; + + if (emcmotStatus->jog_frame == EMC_JOG_FRAME_MACHINE && emcmotInternal->work_frame_ok) { + for (i = 0; i < 9; i++) { r[i] = m[i]; } + selected = 1; + } else if (emcmotStatus->jog_frame == EMC_JOG_FRAME_PLANE && emcmotInternal->work_plane_active) { + for (i = 0; i < 9; i++) { r[i] = p[i]; } + } else if (emcmotStatus->jog_frame == EMC_JOG_FRAME_TOOL && emcmotInternal->tool_jog_ok) { + for (i = 0; i < 9; i++) { r[i] = t[i]; } + } + axis_set_jog_frame(selected, r); } static void update_status(void) @@ -2636,6 +2709,7 @@ static void update_status(void) emcmotStatus->external_offsets_applied = hal_get_bool(emcmot_hal_data->eoffset_active); update_tool_frame(); + emcmotUpdateJogFrame(); for (dio = 0; dio < emcmotConfig->numDIO; dio++) { emcmotStatus->synch_di[dio] = hal_get_bool(emcmot_hal_data->synch_di[dio]); diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 3a0f8e5f697..b00547e3257 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -133,6 +133,7 @@ typedef struct { hal_bool_t jog_stop; /* RPI: set TRUE to stop jogging following accel values*/ hal_bool_t jog_stop_immediate; /* RPI: set TRUE to stop jogging immediately*/ hal_bool_t jog_is_active; /* RPI: TRUE if active jogging*/ + hal_sint_t jog_frame; /* RPI: the frame world jogs move along */ hal_bool_t tp_reverse; /* Set true if trajectory planner is running in reverse*/ hal_bool_t motion_enabled; /* RPI: motion enable for all joints */ hal_bool_t is_all_homed; /* RPI: TRUE if all active joints is homed */ @@ -273,6 +274,7 @@ extern void clearHomes(int joint_num); extern void emcmot_config_change(void); extern void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, const EmcPose *expected, int have_expected); +extern void emcmotUpdateJogFrame(void); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 4a096507d64..39bfd58f0b1 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -15,6 +15,7 @@ #include /* memset */ #include #include /* decls for HAL implementation */ +#include /* EMC_JOG_FRAME_MACHINE */ #include "../tp/tp.h" #include "../tp/sp_scurve.h" /* sp_scurve_cleanup() at module exit */ @@ -610,6 +611,7 @@ static int init_hal_io(void) CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &(emcmot_hal_data->distance_to_go), 0.0, "motion.distance-to-go")); CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->program_line), 0, "motion.program-line")); CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_OUT, &(emcmot_hal_data->jog_is_active), 0, "motion.jog-is-active")); + CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->jog_frame), 0, "motion.jog-frame")); /* Standard Interp State Pins */ CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->interp_line_number), 0, "motion.interp.line-number")); @@ -914,6 +916,9 @@ static int init_comm_buffers(void) emcmotStatus->activeDepth = 0; emcmotStatus->paused = 0; emcmotStatus->overrideLimitMask = 0; + emcmotStatus->jog_frame = EMC_JOG_FRAME_MACHINE; + emcmotInternal->work_plane_active = 0; + emcmotInternal->work_frame_ok = 0; SET_MOTION_INPOS_FLAG(1); SET_MOTION_ENABLE_FLAG(0); /* record the kinematics type of the machine */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 448c4d6db62..ed9cc8c1495 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -180,6 +180,8 @@ extern "C" { EMCMOT_SELECT_KINS_TYPE, /* select the switchkins type (G12.1) */ EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ + EMCMOT_SET_WORK_PLANE, /* the tilted work plane in effect */ + EMCMOT_SET_JOG_FRAME, /* the frame world jogs of X Y Z move along */ } cmd_code_t; @@ -290,6 +292,11 @@ extern "C" { /* SET_OFFSET: pos is where the interpreter expects the point to be once the offset is on, for motion to check its own answer against */ int have_point; + + /* SET_WORK_PLANE: the plane's axes in world coordinates, row major */ + double work_plane[9]; + int work_plane_active; + int jog_frame; /* SET_JOG_FRAME: EMC_JOG_FRAME_* */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -623,6 +630,8 @@ Suggestion: Split this in to an Error and a Status flag register.. double tool_frame[9]; /* the tool frame in the work frame at the commanded joints, row major */ int tool_frame_ok; /* non-zero if the module reports its frames */ + int jog_frame; /* the frame world jogs of X Y Z move along, + EMC_JOG_FRAME_* */ EmcPose world_home; /* cartesean coords of home position */ emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ @@ -808,6 +817,17 @@ typedef struct emcmot_internal_t { move while overriding limits */ TP_STRUCT coord_tp; /* coordinated mode planner */ int idForStep; /* status id while stepping */ + double work_plane[9]; /* the tilted work plane's axes in world + coordinates, row major */ + int work_plane_active; /* non-zero while a plane is in effect */ + double work_frame[9]; /* the machine axes in world coordinates, + transpose(work), row major */ + int work_frame_ok; /* non-zero on a type whose work frame turns + with a joint, a table under TCP */ + double tool_jog[9]; /* the tool's axes in world coordinates of + the type in force, row major: the frame + a tool jog moves along */ + int tool_jog_ok; /* non-zero while tool_jog is known */ } emcmot_internal_t; /* error ring buffer access functions */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 15ec482cac9..5616d4f4aa7 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -298,6 +298,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_JOINT_MOVE_TYPE: ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); break; + case EMC_TRAJ_SET_JOG_FRAME_TYPE: + ((EMC_TRAJ_SET_JOG_FRAME *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -515,6 +518,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_G68"; case EMC_TRAJ_JOINT_MOVE_TYPE: return "EMC_TRAJ_JOINT_MOVE"; + case EMC_TRAJ_SET_JOG_FRAME_TYPE: + return "EMC_TRAJ_SET_JOG_FRAME"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1667,6 +1672,13 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) cms->update(active); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_JOG_FRAME::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + cms->update(frame); +} + // cppcheck-suppress duplInheritedMember void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) { @@ -1719,6 +1731,7 @@ void EMC_TRAJ_STAT::update(CMS * cms) EmcPose_update(cms, &actualPosition); cms->update(tool_frame, 9); cms->update(tool_frame_ok); + cms->update(jog_frame); cms->update(velocity); cms->update(acceleration); cms->update(maxVelocity); diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 18ae2bc78cb..3ce852cf480 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -110,6 +110,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) #define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) #define EMC_TRAJ_JOINT_MOVE_TYPE ((NMLTYPE) 240) +#define EMC_TRAJ_SET_JOG_FRAME_TYPE ((NMLTYPE) 241) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -385,6 +386,8 @@ extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, c extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index, double angular_offset_degrees = 0.0); extern int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point); +extern int emcTrajSetWorkPlane(const double rotation[9], int active); +extern int emcTrajSetJogFrame(int frame); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 8ff7c421bf3..f8ad6691ca3 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -886,6 +886,22 @@ class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { int active; }; +// the frame world jogs of X, Y and Z move along, EMC_JOG_FRAME_* +class EMC_TRAJ_SET_JOG_FRAME:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SET_JOG_FRAME() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_JOG_FRAME_TYPE, sizeof(EMC_TRAJ_SET_JOG_FRAME)), + frame(0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + int frame; +}; + class EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG() @@ -1041,6 +1057,7 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { EmcPose actualPosition; // current actual position, from forward kins double tool_frame[9]; // tool frame in the work frame, row major bool tool_frame_ok; // the kinematics reports its frames + int jog_frame; // the frame world jogs move along, EMC_JOG_FRAME_* double velocity; // system velocity, for subsequent motions double acceleration; // system acceleration, for subsequent // motions diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 31a5df214d9..3be7199a484 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -86,6 +86,7 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() actualPosition{}, tool_frame{1, 0, 0, 0, 1, 0, 0, 0, 1}, tool_frame_ok(false), + jog_frame(0), velocity(1.0), acceleration(1.0), diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 4641c40a3d0..e134d2c6a32 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -57,6 +57,8 @@ #include // fork() #include // waitpid(), WNOHANG, WIFEXITED #include // isspace() +#include // cos(), sin() +#include // std::equal(), std::copy() #include #include #include "motion/usrmotintf.h" @@ -822,6 +824,7 @@ static bool allow_while_idle_type() { case EMC_JOG_STOP_TYPE: case EMC_JOG_ABS_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: return 1; break; } @@ -908,6 +911,7 @@ static int emcTaskPlan(void) case EMC_MOTION_SET_AOUT_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_SET_DEBUG_TYPE: retval = emcTaskIssueCommand(emcCommand); break; @@ -1032,6 +1036,7 @@ static int emcTaskPlan(void) case EMC_MOTION_ADAPTIVE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_SET_DEBUG_TYPE: retval = emcTaskIssueCommand(emcCommand); break; @@ -1708,6 +1713,34 @@ int emcTaskQueueCommand(std::unique_ptr &&cmd) return 0; } +// The tilted work plane's axes in world coordinates, for motion to jog +// along: the plane's rotation turned by the XY rotation of the coordinate +// system it was defined in, as canon applies the two. Motion hears only +// of a change, so a program without a plane sends it nothing +static int sendWorkPlane() +{ + static double sent[9]; + static int sent_active = 0; + const double *g = emcStatus->task.g68_rotation; + double t = emcStatus->task.rotation_xy * M_PI / 180.0, c = cos(t), s = sin(t), r[9]; + int active = emcStatus->task.g68_active, retval; + + for (int j = 0; j < 3; j++) { + r[j] = c * g[j] - s * g[3 + j]; + r[3 + j] = s * g[j] + c * g[3 + j]; + r[6 + j] = g[6 + j]; + } + if (active == sent_active && (!active || std::equal(r, r + 9, sent))) { + return 0; + } + retval = emcTrajSetWorkPlane(r, active); + if (retval == 0) { + sent_active = active; + std::copy(r, r + 9, sent); + } + return retval; +} + // issues command immediately static int emcTaskIssueCommand(NMLmsg * cmd) { @@ -2023,7 +2056,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) case EMC_TRAJ_SET_ROTATION_TYPE: emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; - retval = 0; + retval = sendWorkPlane(); break; case EMC_TRAJ_SET_G68_TYPE: { @@ -2031,7 +2064,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcStatus->task.g68_offset = g68->origin; for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } emcStatus->task.g68_active = g68->active; - retval = 0; + retval = sendWorkPlane(); break; } @@ -2100,6 +2133,10 @@ static int emcTaskIssueCommand(NMLmsg * cmd) } break; + case EMC_TRAJ_SET_JOG_FRAME_TYPE: + retval = emcTrajSetJogFrame((reinterpret_cast(cmd))->frame); + break; + case EMC_MOTION_SET_AOUT_TYPE: retval = emcMotionSetAout((reinterpret_cast(cmd))->index, (reinterpret_cast(cmd))->start, @@ -2629,6 +2666,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_TRAJ_SET_FO_ENABLE_TYPE: case EMC_TRAJ_SET_FH_ENABLE_TYPE: case EMC_TRAJ_SET_SO_ENABLE_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 75dc9ee109f..fa7b60677f3 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -15,6 +15,7 @@ #include "logutil.hh" #include #include +#include // std::max #include // DBL_MAX #include // memcpy() strncpy() #include // unlink() @@ -840,6 +841,17 @@ int emcJointUnhome(int joint) return usrmotWriteEmcmotCommand(&emcmotCommand); } +// The fastest a jog of axis nr may ask for: the axis's own MAX_VELOCITY, +// or in a jog frame, where X, Y and Z jog along turned axes, the fastest of +// the three; motion holds a frame jog to what each world axis allows +static double jogMaxVel(int nr) +{ + if (nr < 3 && emcmotStatus.jog_frame != EMC_JOG_FRAME_MACHINE) { + return std::max({AxisConfig[0].MaxVel, AxisConfig[1].MaxVel, AxisConfig[2].MaxVel}); + } + return AxisConfig[nr].MaxVel; +} + int emcJogCont(int nr, double vel, int jjogmode) { if (jjogmode) { @@ -853,10 +865,10 @@ int emcJogCont(int nr, double vel, int jjogmode) emcmotCommand.axis = -1; //NA } else { if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; } - if (vel > AxisConfig[nr].MaxVel) { - vel = AxisConfig[nr].MaxVel; - } else if (vel < -AxisConfig[nr].MaxVel) { - vel = -AxisConfig[nr].MaxVel; + if (vel > jogMaxVel(nr)) { + vel = jogMaxVel(nr); + } else if (vel < -jogMaxVel(nr)) { + vel = -jogMaxVel(nr); } emcmotCommand.joint = -1; //NA emcmotCommand.axis = nr; @@ -880,10 +892,10 @@ int emcJogIncr(int nr, double incr, double vel, int jjogmode) emcmotCommand.axis = -1; //NA } else { if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; } - if (vel > AxisConfig[nr].MaxVel) { - vel = AxisConfig[nr].MaxVel; - } else if (vel < -AxisConfig[nr].MaxVel) { - vel = -AxisConfig[nr].MaxVel; + if (vel > jogMaxVel(nr)) { + vel = jogMaxVel(nr); + } else if (vel < -jogMaxVel(nr)) { + vel = -jogMaxVel(nr); } emcmotCommand.joint = -1; //NA emcmotCommand.axis = nr; @@ -1522,6 +1534,25 @@ int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point) return usrmotWriteEmcmotCommand(&emcmotCommand); } +int emcTrajSetWorkPlane(const double rotation[9], int active) +{ + emcmotCommand.command = EMCMOT_SET_WORK_PLANE; + for (int i = 0; i < 9; i++) { emcmotCommand.work_plane[i] = rotation[i]; } + emcmotCommand.work_plane_active = active; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + +int emcTrajSetJogFrame(int frame) +{ + if (frame < EMC_JOG_FRAME_MACHINE || frame > EMC_JOG_FRAME_TOOL) { + emcOperatorError("unknown jog frame %d", frame); + return -1; + } + emcmotCommand.command = EMCMOT_SET_JOG_FRAME; + emcmotCommand.jog_frame = frame; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcTrajSetSpindleSync(int spindle, double fpr, bool wait_for_index, double angular_offset_degrees) { emcmotCommand.command = EMCMOT_SET_SPINDLESYNC; @@ -1739,6 +1770,7 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) for (int i = 0; i < 9; i++) { stat->tool_frame[i] = emcmotStatus.tool_frame[i]; } stat->tool_frame_ok = emcmotStatus.tool_frame_ok; + stat->jog_frame = emcmotStatus.jog_frame; stat->velocity = emcmotStatus.vel; stat->acceleration = emcmotStatus.acc; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 35a3ca6b40a..6386a145596 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1194,6 +1194,9 @@ static PyMemberDef Stat_members[] = { { "kins_type", T_INT, O(motion.traj.switchkins_type), READONLY, "The kinematics type motion runs, as G12.1 selects it and motion.kins-type\n" "reports it; 0 for a module that switches nothing." }, + { "jog_frame", T_INT, O(motion.traj.jog_frame), READONLY, + "The frame world jogs of X, Y and Z move along: JOG_FRAME_MACHINE,\n" + "JOG_FRAME_PLANE or JOG_FRAME_TOOL." }, { "motion_type", T_INT, O(motion.traj.motion_type), READONLY, "The type of the currently executing motion (one of MOTION_TYPE_TRAVERSE,\n" "MOTION_TYPE_FEED, MOTION_TYPE_ARC, MOTION_TYPE_TOOLCHANGE,\n" @@ -2235,6 +2238,17 @@ static PyObject *teleop(pyCommandChannel *s, PyObject *o) { return Py_None; } +static PyObject *jog_frame(pyCommandChannel *s, PyObject *o) { + EMC_TRAJ_SET_JOG_FRAME fr; + + if(!PyArg_ParseTuple(o, "i", &fr.frame)) return NULL; + + emcSendCommand(s, fr); + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject *set_traj_mode(pyCommandChannel *s, PyObject *o) { EMC_TRAJ_SET_MODE mo; @@ -2394,6 +2408,11 @@ static PyMemberDef Command_members[] = { static PyMethodDef Command_methods[] = { {"debug", (PyCFunction)debug, METH_VARARGS, NULL}, {"teleop_enable", (PyCFunction)teleop, METH_VARARGS, NULL}, + {"jog_frame", (PyCFunction)jog_frame, METH_VARARGS, + "jog_frame(FRAME) - Jog X, Y and Z along the axes of FRAME in world mode:\n" + "JOG_FRAME_MACHINE, JOG_FRAME_PLANE (the tilted work plane) or\n" + "JOG_FRAME_TOOL. Refused while a jog is under way.\n" + }, {"traj_mode", (PyCFunction)set_traj_mode, METH_VARARGS, NULL}, {"wait_complete", (PyCFunction)wait_complete, METH_VARARGS, NULL}, {"state", (PyCFunction)state, METH_VARARGS, @@ -3435,6 +3454,10 @@ PyMODINIT_FUNC PyInit_linuxcnc(void) PyModule_AddIntConstant(m, "TRAJ_MODE_COORD", (int)EMC_TRAJ_MODE::COORD); PyModule_AddIntConstant(m, "TRAJ_MODE_TELEOP", (int)EMC_TRAJ_MODE::TELEOP); + ENUMX(4, EMC_JOG_FRAME_MACHINE); + ENUMX(4, EMC_JOG_FRAME_PLANE); + ENUMX(4, EMC_JOG_FRAME_TOOL); + ENUMX(4, EMC_MOTION_TYPE_TRAVERSE); ENUMX(4, EMC_MOTION_TYPE_FEED); ENUMX(4, EMC_MOTION_TYPE_ARC); diff --git a/tests/twp-jog-frame-scurve/README b/tests/twp-jog-frame-scurve/README new file mode 100644 index 00000000000..bf40f310858 --- /dev/null +++ b/tests/twp-jog-frame-scurve/README @@ -0,0 +1,3 @@ +twp-jog-frame on the S-curve planner: its INI with PLANNER_TYPE = 1 and +each jerk limit 50 times the acceleration limit beside it, its HAL file +and its test script, see twp-jog-frame/README. diff --git a/tests/twp-jog-frame-scurve/checkresult b/tests/twp-jog-frame-scurve/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-jog-frame-scurve/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-jog-frame-scurve/sim.hal b/tests/twp-jog-frame-scurve/sim.hal new file mode 120000 index 00000000000..e84db0d42b9 --- /dev/null +++ b/tests/twp-jog-frame-scurve/sim.hal @@ -0,0 +1 @@ +../twp-jog-frame/sim.hal \ No newline at end of file diff --git a/tests/twp-jog-frame-scurve/test-ui.py b/tests/twp-jog-frame-scurve/test-ui.py new file mode 120000 index 00000000000..52e019c6f6c --- /dev/null +++ b/tests/twp-jog-frame-scurve/test-ui.py @@ -0,0 +1 @@ +../twp-jog-frame/test-ui.py \ No newline at end of file diff --git a/tests/twp-jog-frame-scurve/test.sh b/tests/twp-jog-frame-scurve/test.sh new file mode 100755 index 00000000000..9742250056c --- /dev/null +++ b/tests/twp-jog-frame-scurve/test.sh @@ -0,0 +1,10 @@ +#!/bin/bash -e +# twp-jog-frame's sim on the S-curve planner, each jerk limit 50 times +# the acceleration limit beside it +awk '{ print } + /^MAX_LINEAR_ACCELERATION = / { print "PLANNER_TYPE = 1"; print "MAX_LINEAR_JERK = " 50 * $3 } + /^MAX_ACCELERATION = / { print "MAX_JERK = " 50 * $3 }' ../twp-jog-frame/test.ini > scurve.ini +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r scurve.ini +rm -f scurve.ini diff --git a/tests/twp-jog-frame-scurve/tool.tbl b/tests/twp-jog-frame-scurve/tool.tbl new file mode 120000 index 00000000000..523377dc28e --- /dev/null +++ b/tests/twp-jog-frame-scurve/tool.tbl @@ -0,0 +1 @@ +../twp-jog-frame/tool.tbl \ No newline at end of file diff --git a/tests/twp-jog-frame-table/README b/tests/twp-jog-frame-table/README new file mode 100644 index 00000000000..8eca9ffbdaf --- /dev/null +++ b/tests/twp-jog-frame-table/README @@ -0,0 +1,13 @@ +The jog frames on two more machines, beside twp-jog-frame. + +trivkins, not homed, with a plane turned 45 degrees about Z: a frame jog +gets the travel a plain jog of an axis not homed gets, one range either +way of where it stands, not the [AXIS_] limits, so an incremental +jog past them goes and a continuous one runs to one range from its start. + +Then a trunnion, xyzac-trt-kins, on the tool centre point type with C at +90 degrees: a machine frame jog moves the slide of its letter alone, X, +Y and Z, and again with A tilted 30 degrees, since the machine does not +turn when the table does; the plane frame with no plane jogs the part's +X, turned with the table onto the Y slide; the tool frame jogs Z on the +Z slide; on the identity type a machine X jog moves X. diff --git a/tests/twp-jog-frame-table/checkresult b/tests/twp-jog-frame-table/checkresult new file mode 100755 index 00000000000..01286f38d16 --- /dev/null +++ b/tests/twp-jog-frame-table/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +# the test scripts count their own failures: the machine not homed, then +# the trunnion +test "$(grep -c "^Exiting with 0 errors" "$1")" = 2 diff --git a/tests/twp-jog-frame-table/table.hal b/tests/twp-jog-frame-table/table.hal new file mode 100644 index 00000000000..c615d9e1e9c --- /dev/null +++ b/tests/twp-jog-frame-table/table.hal @@ -0,0 +1,19 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +# the rotation centre off the machine origin, so a turn moves the work +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame-table/table.ini b/tests/twp-jog-frame-table/table.ini new file mode 100644 index 00000000000..fa6f12dd35b --- /dev/null +++ b/tests/twp-jog-frame-table/table.ini @@ -0,0 +1,120 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-table.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +# a trunnion: switchkins-type 0 is identity, 1 the tool centre point, +# which turns the work with A and C +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = table.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Y] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Z] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 + +[AXIS_C] +MIN_LIMIT = -400 +MAX_LIMIT = 400 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_3] +TYPE = ANGULAR +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 +MIN_LIMIT = -400 +MAX_LIMIT = 400 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_4] +TYPE = ANGULAR +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 +MIN_LIMIT = -400 +MAX_LIMIT = 400 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 diff --git a/tests/twp-jog-frame-table/test-table.py b/tests/twp-jog-frame-table/test-table.py new file mode 100755 index 00000000000..cfcc4ef9845 --- /dev/null +++ b/tests/twp-jog-frame-table/test-table.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +# Machine frame jogs on a trunnion under the tool centre point: see README. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1].strip()) + +def joints(): + s.poll() + return [s.joint[j]["output"] for j in range(5)] + +# what done() waits for, task done with the last command sent, +# looked at every 2 ms rather than every 10 +def done(timeout=60): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if s.echo_serial_number >= c.serial and s.state != linuxcnc.RCS_EXEC: + return + time.sleep(0.002) + error("timed out waiting for task") + +# the machine has stopped when task has nothing left to run, no jog is +# active and the position is the same over two status copies a servo cycle +# or more apart, the first written two task cycles after the command: two +# polls can read the same copy +def settled(jogging=False): + deadline = time.time() + 60 + s.poll() + beat = s.taskbeat + 2 + last = None + last_hb = None + while time.time() < deadline: + s.poll() + if s.taskbeat >= beat: + now = joints() + if ((jogging or (s.interp_state == linuxcnc.INTERP_IDLE and s.inpos and not s.queue + and not hal.get_value("motion.jog-is-active"))) + and now == last and s.heartbeat != last_hb): + return now + last = now + last_hb = s.heartbeat + time.sleep(0.002) + error("timed out waiting for the machine to stop") + return last + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + done() + for cmd in cmds: + c.mdi(cmd) + done() + settled() + drain() + c.mode(linuxcnc.MODE_MANUAL) + done() + c.teleop_enable(1) + done() + +def frame(f): + c.jog_frame(f) + done() + +# an incremental jog of 5 on one letter, and the joints it moved +def jog(what, axis, want): + before = joints() + c.jog(linuxcnc.JOG_INCREMENT, False, axis, 200, 5) + after = settled() + moved = [after[j] - before[j] for j in range(5)] + print("%s: joints moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved the joints %s, not %s" % (what, fmt(moved), fmt(want))) + drain() + return moved + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +done() +c.mode(linuxcnc.MODE_MANUAL) +done() +c.teleop_enable(0) +done() +c.home(-1) +done() +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if all(s.homed[:5]): + break + time.sleep(0.002) +else: + error("the joints did not home") + +# the tool centre point, the table turned a quarter turn +mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 C90") + +# the machine axes: the slides, however the table stands +frame(linuxcnc.JOG_FRAME_MACHINE) +jog("machine X at C90", 0, [5, 0, 0, 0, 0]) +jog("machine Y at C90", 1, [0, 5, 0, 0, 0]) +jog("machine Z at C90", 2, [0, 0, 5, 0, 0]) + +# the plane frame with no plane: the part's axes, turned with the table, +# so its X is a slide other than X +frame(linuxcnc.JOG_FRAME_PLANE) +before = joints() +c.jog(linuxcnc.JOG_INCREMENT, False, 0, 200, 5) +after = settled() +moved = [after[j] - before[j] for j in range(5)] +print("plane X at C90, no plane: joints moved %s" % fmt(moved)) +if abs(moved[0]) > 1e-5 or abs(abs(moved[1]) - 5) > 1e-5 or abs(moved[2]) > 1e-5: + error("plane X at C90 moved the joints %s, not Y alone" % fmt(moved)) +drain() + +# the tool frame: a trunnion's tool stands still, Z is the Z slide +frame(linuxcnc.JOG_FRAME_TOOL) +jog("tool Z at C90", 2, [0, 0, 5, 0, 0]) + +# the table tilted as well +mdi("G0 A30") +frame(linuxcnc.JOG_FRAME_MACHINE) +jog("machine X at A30 C90", 0, [5, 0, 0, 0, 0]) +jog("machine Z at A30 C90", 2, [0, 0, 5, 0, 0]) + +# the machine frame type: the slides too +mdi("G13.1") +jog("machine X on the identity type", 0, [5, 0, 0, 0, 0]) + +frame(linuxcnc.JOG_FRAME_MACHINE) +c.state(linuxcnc.STATE_OFF) +done() + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame-table/test-unhomed.py b/tests/twp-jog-frame-table/test-unhomed.py new file mode 100755 index 00000000000..f0f3c087abc --- /dev/null +++ b/tests/twp-jog-frame-table/test-unhomed.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# Plane frame jogs on a machine not homed: see README. +import hal +import linuxcnc +import math +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1].strip()) + +# what done() waits for, task done with the last command sent, +# looked at every 2 ms rather than every 10 +def done(timeout=60): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if s.echo_serial_number >= c.serial and s.state != linuxcnc.RCS_EXEC: + return + time.sleep(0.002) + error("timed out waiting for task") + +# the machine has stopped when task has nothing left to run, no jog is +# active and the position is the same over two status copies a servo cycle +# or more apart, the first written two task cycles after the command: two +# polls can read the same copy +def settled(jogging=False): + deadline = time.time() + 60 + s.poll() + beat = s.taskbeat + 2 + last = None + last_hb = None + while time.time() < deadline: + s.poll() + if s.taskbeat >= beat: + now = list(s.position) + if ((jogging or (s.interp_state == linuxcnc.INTERP_IDLE and s.inpos and not s.queue + and not hal.get_value("motion.jog-is-active"))) + and now == last and s.heartbeat != last_hb): + return now + last = now + last_hb = s.heartbeat + time.sleep(0.002) + error("timed out waiting for the machine to stop") + return last + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def check(what, before, after, want): + moved = [after[i] - before[i] for i in range(3)] + print("%s: moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved %s, not %s" % (what, fmt(moved), fmt(want))) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +done() +c.mode(linuxcnc.MODE_MDI) +done() +# the plane's X runs at 45 degrees between X and Y +c.mdi("G68.2 X0 Y0 Z0 I45 J0 K0") +done() +settled() +drain() + +c.mode(linuxcnc.MODE_MANUAL) +done() +c.teleop_enable(1) +done() +deadline = time.time() + 10 +while time.time() < deadline: + s.poll() + if s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP: + break + time.sleep(0.002) +if s.motion_mode != linuxcnc.TRAJ_MODE_TELEOP: + error("motion did not enter teleop mode") +if any(s.homed[:3]): + error("a joint is homed, the test is empty") +c.jog_frame(linuxcnc.JOG_FRAME_PLANE) +done() + +h = math.sqrt(0.5) +# [AXIS_X] and [AXIS_Y] are -10 to 10: homed, this jog would end outside +# them and be refused; not homed, an axis has one range, 20, either way +# of where it stands, as a plain jog of it does +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 200, 20) +after = settled() +check("an incremental plane X jog of 20", before, after, [20 * h, 20 * h, 0]) +drain() + +# a continuous jog runs to one range from where it starts, X and Y alike +before = after +c.jog(linuxcnc.JOG_CONTINUOUS, 0, 0, 200) +done() +# at the end of its range the jog stands, still active +settled(jogging=True) +c.jog(linuxcnc.JOG_STOP, 0, 0) +after = settled() +check("a continuous plane X jog", before, after, [20, 20, 0]) +drain() + +c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) +done() +c.state(linuxcnc.STATE_OFF) +done() + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame-table/test.sh b/tests/twp-jog-frame-table/test.sh new file mode 100755 index 00000000000..8100ac0367d --- /dev/null +++ b/tests/twp-jog-frame-table/test.sh @@ -0,0 +1,7 @@ +#!/bin/bash -e +# plane jogs on a machine not homed, identity kinematics +rm -f sim.var sim.var.bak +linuxcnc -r unhomed.ini +# machine frame jogs on a trunnion under the tool centre point +rm -f sim.var sim.var.bak +linuxcnc -r table.ini diff --git a/tests/twp-jog-frame-table/tool.tbl b/tests/twp-jog-frame-table/tool.tbl new file mode 120000 index 00000000000..523377dc28e --- /dev/null +++ b/tests/twp-jog-frame-table/tool.tbl @@ -0,0 +1 @@ +../twp-jog-frame/tool.tbl \ No newline at end of file diff --git a/tests/twp-jog-frame-table/unhomed.hal b/tests/twp-jog-frame-table/unhomed.hal new file mode 100644 index 00000000000..45563353e5e --- /dev/null +++ b/tests/twp-jog-frame-table/unhomed.hal @@ -0,0 +1,13 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame-table/unhomed.ini b/tests/twp-jog-frame-table/unhomed.ini new file mode 100644 index 00000000000..d8523cf7fce --- /dev/null +++ b/tests/twp-jog-frame-table/unhomed.ini @@ -0,0 +1,83 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-unhomed.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +# identity kinematics: the one kind that enters teleop with the joints +# not homed +[KINS] +KINEMATICS = trivkins coordinates=xyz +JOINTS = 3 + +[HAL] +HALFILE = unhomed.hal + +[TRAJ] +COORDINATES = XYZ +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Y] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Z] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 diff --git a/tests/twp-jog-frame/README b/tests/twp-jog-frame/README new file mode 100644 index 00000000000..ac9542861a6 --- /dev/null +++ b/tests/twp-jog-frame/README @@ -0,0 +1,25 @@ +World jogs of X, Y and Z along the tilted work plane and along the tool. + +5axiskins with a 100 mm pivot, the tool centre point type, Y slow and +short against X and Z. The plane frame: a plane defined under an XY +rotation, incremental jogs of each letter, each world move checked +against the plane's axes as status reports them turned by the XY +rotation, rotaries untouched; a machine jog afterwards starts where the +frame jogs left the machine. A plane whose X runs mostly along Y: the +jog along it runs at the speed Y allows. One frame axis moves at a +time: a jog of plane Y asked for while plane X runs waits until X has +stopped. A continuous jog stops where the turned axis meets the +[AXIS_Y] travel without a soft limit fault, plane Y asked for at once +too, and no further jog leaves it. The frame cannot change while a jog +is under way. The tool frame with the head laid over and turned: the +jogs follow the tool frame status reports, and a plane in effect leaves +that frame as the kinematics reports it. Under the identity type a tool +jog moves the joints as it does under the primary type, along the tool as +the head stands, and status has the tool frame the primary type has at +the same joints. Every servo +cycle of every jog is sampled: no world axis goes over its own speed or +acceleration. + +twp-jog-frame-scurve runs the same on the S-curve planner, and +twp-jog-frame-table the plane frame on a machine not homed and the frames +on a trunnion. diff --git a/tests/twp-jog-frame/checkresult b/tests/twp-jog-frame/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-jog-frame/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-jog-frame/sim.hal b/tests/twp-jog-frame/sim.hal new file mode 100644 index 00000000000..b6ab2858bfe --- /dev/null +++ b/tests/twp-jog-frame/sim.hal @@ -0,0 +1,31 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# the world velocity of X, Y and Z every servo cycle, for the speed and +# acceleration of every jog, and the servo thread's time, to tell when the +# log has caught up +loadrt sampler depth=4000 cfg=ffff +loadrt integ names=clock + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf clock servo-thread +addf sampler.0 servo-thread + +setp clock.in 1 +net VX axis.x.teleop-vel-cmd => sampler.0.pin.0 +net VY axis.y.teleop-vel-cmd => sampler.0.pin.1 +net VZ axis.z.teleop-vel-cmd => sampler.0.pin.2 +net T clock.out => sampler.0.pin.3 +loadusr halsampler -t samples.log + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame/test-ui.py b/tests/twp-jog-frame/test-ui.py new file mode 100755 index 00000000000..4878b3f79ac --- /dev/null +++ b/tests/twp-jog-frame/test-ui.py @@ -0,0 +1,407 @@ +#!/usr/bin/env python3 +# World jogs of X, Y and Z along the tilted work plane and along the tool: +# see README. +import hal +import linuxcnc +import math +import os +import sys +import time + +JOINTS = 6 +X, Y, Z = 0, 1, 2 +B, C = 4, 5 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) +AXIS_VEL = [float(ini.find("AXIS_%s" % l, "MAX_VELOCITY")) for l in "XYZ"] +AXIS_ACC = [float(ini.find("AXIS_%s" % l, "MAX_ACCELERATION")) for l in "XYZ"] +# Y is the slow axis +VEL_Y = AXIS_VEL[Y] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(expect=None): + # the messages since the last call; any error is a failure unless it + # carries the text expected + said = [] + while True: + m = e.poll() + if not m: + break + print("channel:", m) + said.append(m[1]) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) \ + and not (expect and expect in m[1]): + error("reported: %s" % m[1].strip()) + return said + +# what c.wait_complete() waits for, task done with the last command sent, +# looked at every 2 ms rather than every 10 +def done(timeout=60): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if s.echo_serial_number >= c.serial and s.state != linuxcnc.RCS_EXEC: + return + time.sleep(0.002) + error("timed out waiting for task") + +def wait_for(what, cond, timeout=10): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if cond(): + return True + time.sleep(0.002) + error("timed out waiting for %s" % what) + return False + +# the machine has stopped when task has nothing left to run, no jog is +# active and the position is the same over two status copies a servo cycle +# or more apart, the first written two task cycles after the command: two +# polls can read the same copy +def settled(): + deadline = time.time() + 60 + s.poll() + beat = s.taskbeat + 2 + last = None + last_hb = None + while time.time() < deadline: + s.poll() + if s.taskbeat >= beat: + # the joints too, which follow the world position through the + # interpolator a few servo cycles behind + now = list(s.position) + list(s.joint_position) + if (s.interp_state == linuxcnc.INTERP_IDLE and s.inpos and not s.queue + and now == last and s.heartbeat != last_hb + and not hal.get_value("motion.jog-is-active")): + return now[:len(s.position)] + last = now + last_hb = s.heartbeat + time.sleep(0.002) + error("timed out waiting for the machine to stop") + return last[:len(s.position)] if last else last + +# the world velocity once it holds over three reads of the servo thread's +# output with the joints moved between each: a jog at its cruise +def cruising(): + deadline = time.time() + 10 + first = last = None + while time.time() < deadline: + now = ([hal.get_value("axis.%s.teleop-vel-cmd" % l) for l in "xyz"], + [hal.get_value("joint.%d.pos-cmd" % j) for j in range(3)]) + if (first and now[0] == last[0] == first[0] and now[1] != last[1] != first[1] + and any(now[0])): + return now[0] + first, last = last, now + time.sleep(0.002) + error("timed out waiting for the jog to cruise") + return last[0] + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + done() + for cmd in cmds: + c.mdi(cmd) + done() + settled() + drain() + +def teleop(): + c.mode(linuxcnc.MODE_MANUAL) + done() + c.teleop_enable(1) + done() + wait_for("teleop mode", lambda: s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP) + +def frame(which): + c.jog_frame(which) + done() + wait_for("jog frame %d" % which, lambda: s.jog_frame == which, 2) + if s.jog_frame != which: + error("the jog frame is %d, not %d" % (s.jog_frame, which)) + if int(hal.get_value("motion.jog-frame")) != which: + error("motion.jog-frame is %s, not %d" % (hal.get_value("motion.jog-frame"), which)) + +def column(m, k): + return [m[3 * i + k] for i in range(3)] + +def rot_z(deg, v): + t = math.radians(deg) + return [math.cos(t) * v[0] - math.sin(t) * v[1], + math.sin(t) * v[0] + math.cos(t) * v[1], v[2]] + +def dot(a, b): + return sum(p * q for p, q in zip(a, b)) + +def unit(v): + n = math.sqrt(dot(v, v)) + return [p / n for p in v] + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def plane_axes(): + # the plane's axes in world coordinates, from status: the plane's own + # rotation turned by the XY rotation it was defined under + s.poll() + r = s.g68_rotation + return [rot_z(s.rotation_xy, column(r, k)) for k in range(3)] + +def tool_axes(): + # the tool frame's axes in world coordinates, from status + s.poll() + t = s.tool_frame + if t is None: + error("no tool frame in status") + return [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + return [column(t, k) for k in range(3)] + +def jog_incr(axis, dist, speed, want, what): + # an incremental jog of a world letter, and the world move it must make + s.poll() + before = list(s.position) + c.jog(linuxcnc.JOG_INCREMENT, 0, axis, speed if dist > 0 else -speed, abs(dist)) + after = settled() + moved = [after[i] - before[i] for i in range(3)] + print("%s: moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved %s, not %s" % (what, fmt(moved), fmt(want))) + for a in (B, C): + if abs(after[a] - before[a]) > 1e-9: + error("%s turned a rotary from %.6f to %.6f" % (what, before[a], after[a])) + drain() + return after + +def cruise(axes, speed, what): + # continuous jogs of the given letters at once, read in the cruise + s.poll() + before = list(s.position) + for a in axes: + c.jog(linuxcnc.JOG_CONTINUOUS, 0, a, speed) + vel = cruising() + for a in axes: + c.jog(linuxcnc.JOG_STOP, 0, a) + after = settled() + moved = [after[i] - before[i] for i in range(3)] + print("%s: velocity %s, moved %s" % (what, fmt(vel), fmt(moved))) + drain() + return vel, moved + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +done() +c.home(-1) +done() +settled() +drain() + +s.poll() +if s.jog_frame != linuxcnc.JOG_FRAME_MACHINE: + error("the jog frame starts at %d, not the machine's" % s.jog_frame) + +# the plane, defined under an XY rotation of 30 degrees: its axes turned by +# both are what the jog must follow +mdi("G12.1 P0", "G10 L2 P1 R30", "G54", "G68.2 X0 Y0 Z0 I0 J30 K0") +W = plane_axes() +print("plane axes in world: X %s, Y %s, Z %s" % tuple(fmt(v) for v in W)) +teleop() +frame(linuxcnc.JOG_FRAME_PLANE) +jog_incr(X, 10, 100, [10 * p for p in W[0]], "plane X +10") +jog_incr(Y, -5, 40, [-5 * p for p in W[1]], "plane Y -5") +jog_incr(Z, 3, 100, [3 * p for p in W[2]], "plane Z +3") + +# the world planners hold where the machine is: a machine jog after the +# frame jogs starts from there +frame(linuxcnc.JOG_FRAME_MACHINE) +jog_incr(X, 2, 100, [2, 0, 0], "machine X +2 after the plane jogs") + +# a plane whose X runs mostly along the slow Y: the jog along it goes no +# faster than Y allows, and two frame axes jogged at once keep every world +# axis inside its own speed +mdi("G69", "G10 L2 P1 R0", "G54", "G68.2 X0 Y0 Z0 I60 J0 K0") +W = plane_axes() +teleop() +frame(linuxcnc.JOG_FRAME_PLANE) +vel, moved = cruise([X], 200, "plane X at 200") +if abs(abs(vel[Y]) - VEL_Y) > 1e-6 or abs(vel[X] - VEL_Y * W[0][0] / W[0][1]) > 1e-6: + error("plane X ran at %s, not at the speed Y allows" % fmt(vel)) +if abs(dot(unit(moved), W[0]) - 1) > 1e-9: + error("plane X moved along %s, not %s" % (fmt(unit(moved)), fmt(W[0]))) + +# a jog of plane Y asked for while plane X runs waits until X has stopped, +# then runs on its own +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, X, 200) +cruising() +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, 200) +done() +vel = cruising() +print("plane X with plane Y waiting: velocity %s" % fmt(vel)) +if abs(dot(unit(vel), W[0]) - 1) > 1e-9: + error("plane X with plane Y asked for ran along %s, not %s" % (fmt(unit(vel)), fmt(W[0]))) +c.jog(linuxcnc.JOG_STOP, 0, X) +done() +vel = cruising() +print("plane Y after plane X stopped: velocity %s" % fmt(vel)) +if abs(dot(unit(vel), W[1]) - 1) > 1e-9 or abs(abs(vel[Y]) - VEL_Y) > 1e-6: + error("plane Y after plane X ran at %s, not along %s at the speed Y allows" % (fmt(vel), fmt(W[1]))) +c.jog(linuxcnc.JOG_STOP, 0, Y) +settled() +drain() + +# the jog stops where the turned axis meets the box, without tripping a +# soft limit, and goes no further out from there; plane Y asked for at +# once waits, and from the box has nowhere to go +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, X, 200) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, 200) +done() +at_box = settled() +c.jog(linuxcnc.JOG_STOP, 0, X) +c.jog(linuxcnc.JOG_STOP, 0, Y) +print("ran into the box at %s" % fmt(at_box[:3])) +s.poll() +if not 99.9 < at_box[Y] < 100 or s.task_state != linuxcnc.STATE_ON: + error("the jog along plane X ended at Y %.6f, task state %d" % (at_box[Y], s.task_state)) +moved = [at_box[i] - before[i] for i in range(3)] +if abs(dot(unit(moved), W[0]) - 1) > 1e-9: + error("the jog to the box went along %s, not %s" % (fmt(unit(moved)), fmt(W[0]))) +drain() +jog_incr(X, 1, 100, [0, 0, 0], "plane X +1 out of the box") +jog_incr(X, -1, 100, [-p for p in W[0]], "plane X -1 back in") + +# the frame stays while a jog is under way +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, -20) +cruising() +c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) +done() +said = [] +deadline = time.time() + 5 +while time.time() < deadline and not any("cannot change while jogging" in m for m in said): + said += drain("cannot change while jogging") + time.sleep(0.002) +s.poll() +if s.jog_frame != linuxcnc.JOG_FRAME_PLANE: + error("the jog frame changed to %d while jogging" % s.jog_frame) +if not any("cannot change while jogging" in m for m in said): + error("no word of the refused change") +c.jog(linuxcnc.JOG_STOP, 0, Y) +settled() +drain() + +# the tool frame, the head laid over and turned: X, Y and Z of the tool as +# the kinematics reports it. The carriage goes where the tip, swung out +# by the head, stays inside the short Y travel +mdi("G69", "G53.7 G0 J0=0 J1=-40 J2=0 J3=30 J4=45") +teleop() +frame(linuxcnc.JOG_FRAME_TOOL) +T = tool_axes() +print("tool axes in world: X %s, Y %s, Z %s" % tuple(fmt(v) for v in T)) +jog_incr(Z, 5, 100, [5 * p for p in T[2]], "tool Z +5") +jog_incr(X, 5, 100, [5 * p for p in T[0]], "tool X +5") +jog_incr(Y, -4, 40, [-4 * p for p in T[1]], "tool Y -4") + +# a plane in effect leaves the tool frame alone: the jogs still follow the +# frame the kinematics reports, not the plane's axes +before = tool_axes() +mdi("G68.2 X0 Y0 Z0 I60 J0 K0") +W = plane_axes() +teleop() +T = tool_axes() +print("tool axes under the plane: X %s, Y %s, Z %s" % tuple(fmt(v) for v in T)) +if any(abs(p - q) > 1e-9 for a, b in zip(T, before) for p, q in zip(a, b)): + error("the plane changed the tool frame in status") +if abs(dot(T[0], W[0])) > 1 - 1e-6: + error("the plane's X lies along the tool's X, which tells the frames apart nowhere") +jog_incr(X, 5, 100, [5 * p for p in T[0]], "tool X +5 under the plane") +jog_incr(Y, 4, 40, [4 * p for p in T[1]], "tool Y +4 under the plane") +jog_incr(Z, -5, 100, [-5 * p for p in T[2]], "tool Z -5 under the plane") + +# under the identity type a tool jog moves the joints as it does under the +# primary type, along the tool as the head stands, as Fanuc's tool axis +# direction feed and Heidenhain's Manual Operation Tool Axis move without +# the tool centre point; status has where the tool points against the +# part, the primary type's frame at the same joints, for a display +def joint_jog(axis, dist, what): + s.poll() + before = list(s.joint_position[:s.joints]) + c.jog(linuxcnc.JOG_INCREMENT, 0, axis, 100 if dist > 0 else -100, abs(dist)) + settled() + s.poll() + moved = [p - q for p, q in zip(s.joint_position[:s.joints], before)] + print("%s: joints moved %s" % (what, fmt(moved))) + drain() + return moved + +mdi("G69") +teleop() +on_primary = tool_axes() +primary_moves = [] +for axis, name in ((Z, "Z"), (X, "X")): + primary_moves.append(joint_jog(axis, 2, "tool %s +2 under the primary type" % name)) + joint_jog(axis, -2, "and back") +mdi("G12.1 P1") +teleop() +got = tool_axes() +if any(abs(p - q) > 1e-9 for a, b in zip(got, on_primary) for p, q in zip(a, b)): + error("under the identity type the tool frame is %s, the primary type's %s" + % (" ".join(fmt(v) for v in got), " ".join(fmt(v) for v in on_primary))) +for (axis, name), want in zip(((Z, "Z"), (X, "X")), primary_moves): + moved = joint_jog(axis, 2, "tool %s +2 under the identity type" % name) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("tool %s +2 under the identity type moved the joints %s, under the primary type %s" + % (name, fmt(moved), fmt(want))) +# with the head straight this would show nothing +if abs(primary_moves[0][2] - 2) < 1e-3: + error("the head stands straight, the tool Z jog moved the joints %s" % fmt(primary_moves[0])) + +# every servo cycle of every jog above: no world axis over its own speed or +# acceleration, a frame jog's stop at the box included. The log is block +# buffered: read it until it reaches the servo thread's time now, which it +# samples beside the velocities +now = hal.get_value("clock.out") +deadline = time.time() + 10 +while True: + with open(LOG) as f: + lines = f.read().split("\n") + samples = [[float(x) for x in v[1:]] for v in (line.split() for line in lines[:-1]) if len(v) == 5] + if samples and samples[-1][3] >= now or time.time() > deadline: + break + time.sleep(0.01) +if not samples or samples[-1][3] < now: + error("the sampler log did not catch up with the servo thread") +print("%d samples" % len(samples)) +for i, l in enumerate("XYZ"): + vpeak = max(abs(v[i]) for v in samples) if samples else 0.0 + apeak = max((abs(samples[k][i] - samples[k - 1][i]) / SERVO + for k in range(1, len(samples))), default=0.0) + print("%s: velocity peak %.3f of %.3f, acceleration peak %.2f of %.2f" + % (l, vpeak, AXIS_VEL[i], apeak, AXIS_ACC[i])) + if vpeak > AXIS_VEL[i] * 1.001: + error("%s ran at %.3f, over its limit of %.3f" % (l, vpeak, AXIS_VEL[i])) + if apeak > AXIS_ACC[i] * 1.001: + error("%s accelerated at %.2f, over its limit of %.2f" % (l, apeak, AXIS_ACC[i])) +if not samples: + error("no samples") +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame/test.ini b/tests/twp-jog-frame/test.ini new file mode 100644 index 00000000000..9de5a6627a3 --- /dev/null +++ b/tests/twp-jog-frame/test.ini @@ -0,0 +1,145 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 100 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +# Y is slow and its travel short: a jog along a turned axis must keep each +# world axis inside its own speed and its own travel +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 3600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/twp-jog-frame/test.sh b/tests/twp-jog-frame/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/twp-jog-frame/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/twp-jog-frame/tool.tbl b/tests/twp-jog-frame/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/twp-jog-frame/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 8368bff4dd892d7bb192900440b068cbb4e4adbf Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 11 Oct 2026 01:14:00 +1100 Subject: [PATCH 24/44] halui: select the jog frame halui.jog-frame.machine, halui.jog-frame.plane and halui.jog-frame.tool select the frame X, Y and Z jog along in world mode, for a pendant, and halui.jog-frame.is-machine, -plane and -tool report the frame in force. tests/twp-jog-frame selects each frame through the pins. --- docs/src/gcode/g-code.adoc | 12 +++++++----- docs/src/gui/halui.adoc | 11 +++++++++++ docs/src/man/man1/halui.1.adoc | 15 +++++++++++++++ src/emc/usr_intf/halui.cc | 35 ++++++++++++++++++++++++++++++++++ tests/twp-jog-frame/README | 5 +++-- tests/twp-jog-frame/test-ui.py | 17 +++++++++++++++++ tests/twp-jog-frame/test.ini | 1 + 7 files changed, 89 insertions(+), 7 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index f48f7b04448..1a77973a2e7 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2685,11 +2685,13 @@ In world mode a jog of X, Y or Z moves along one of three frames: it is the machine frame. The operator selects the frame and a program never changes it: Python -`jog_frame()`, with `jog_frame` in status. `motion.jog-frame` reports the -frame in HAL, so a pendant can, for instance, let only Z through in the tool -frame. The jog pins `axis.L.jog-*` jog the letter of the selected frame. A, B, -C, U, V and W jog along their own axes in every frame, and joint mode is not -affected. +`jog_frame()`, with `jog_frame` in status; the halui pins +`halui.jog-frame.machine`, `halui.jog-frame.plane` and `halui.jog-frame.tool`, +with `halui.jog-frame.is-machine`, `-plane` and `-tool` reporting it. +`motion.jog-frame` reports the frame in HAL, so a pendant can, for instance, +let only Z through in the tool frame. The jog pins `axis.L.jog-*` jog the +letter of the selected frame. A, B, C, U, V and W jog along their own axes in +every frame, and joint mode is not affected. The frame is taken when a jog starts and kept until the jog ends, so a jog in the tool frame does not bend while a rotary jogs with it, and the frame diff --git a/docs/src/gui/halui.adoc b/docs/src/gui/halui.adoc index e9c87f8c1ae..ba70bbaaf50 100644 --- a/docs/src/gui/halui.adoc +++ b/docs/src/gui/halui.adoc @@ -240,6 +240,17 @@ _L_ = axis letter (xyzabcuvw) * 'halui.axis.selected.minus' (bool in) - pin for jogging the selected axis in negative direction at the halui.axis.jog-speed velocity * 'halui.axis.selected.plus' (bool in) - for jogging the selected axis in positive direction at the halui.axis.jog-speed velocity +=== Jog Frame + +The frame X, Y and Z jog along in coordinated jog mode, see <>. + +* 'halui.jog-frame.is-machine' (bool, out) - indicates X, Y and Z jog along the machine axes +* 'halui.jog-frame.is-plane' (bool, out) - indicates X, Y and Z jog along the axes of the tilted work plane +* 'halui.jog-frame.is-tool' (bool, out) - indicates X, Y and Z jog along the axes of the tool +* 'halui.jog-frame.machine' (bool, in) - pin for requesting jogs along the machine axes +* 'halui.jog-frame.plane' (bool, in) - pin for requesting jogs along the axes of the tilted work plane +* 'halui.jog-frame.tool' (bool, in) - pin for requesting jogs along the axes of the tool + === Mode * 'halui.mode.auto' (bool, in) - pin for requesting auto mode diff --git a/docs/src/man/man1/halui.1.adoc b/docs/src/man/man1/halui.1.adoc index 861abcd04c0..f8e05f3a7d3 100644 --- a/docs/src/man/man1/halui.1.adoc +++ b/docs/src/man/man1/halui.1.adoc @@ -370,6 +370,21 @@ _L_ = axis letter (xyzabcuvw) *halui.axis.selected.plus* bool in:: pin for jogging the selected axis in positive direction at the halui.axis.jog-speed velocity +=== Jog frame + +*halui.jog-frame.is-machine* bool out:: + pin showing X, Y and Z jog along the machine axes +*halui.jog-frame.is-plane* bool out:: + pin showing X, Y and Z jog along the axes of the tilted work plane +*halui.jog-frame.is-tool* bool out:: + pin showing X, Y and Z jog along the axes of the tool +*halui.jog-frame.machine* bool in:: + pin for requesting jogs along the machine axes +*halui.jog-frame.plane* bool in:: + pin for requesting jogs along the axes of the tilted work plane +*halui.jog-frame.tool* bool in:: + pin for requesting jogs along the axes of the tool + === Flood coolant *halui.flood.is-on* bool out:: diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index c40e2831e73..bd47864a39c 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -70,6 +70,12 @@ static int axis_mask = 0; FIELD(bool,mode_is_teleop) /* pin for teleop mode is on */ \ FIELD(bool,mode_joint) /* pin for requesting joint mode */ \ FIELD(bool,mode_is_joint) /* pin for joint mode is on */ \ + FIELD(bool,jog_frame_machine) /* pin for jogging along the machine axes */ \ + FIELD(bool,jog_frame_is_machine) /* pin for the machine axes are jogged */ \ + FIELD(bool,jog_frame_plane) /* pin for jogging along the work plane */ \ + FIELD(bool,jog_frame_is_plane) /* pin for the work plane is jogged */ \ + FIELD(bool,jog_frame_tool) /* pin for jogging along the tool */ \ + FIELD(bool,jog_frame_is_tool) /* pin for the tool is jogged */ \ \ FIELD(bool,mist_on) /* pin for starting mist */ \ FIELD(bool,mist_off) /* pin for stopping mist */ \ @@ -568,6 +574,9 @@ int halui_hal_init(void) CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_mdi), "halui.mode.is-mdi")); CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_teleop), "halui.mode.is-teleop")); CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_joint), "halui.mode.is-joint")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_machine), "halui.jog-frame.is-machine")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_plane), "halui.jog-frame.is-plane")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_tool), "halui.jog-frame.is-tool")); CHK(halui_export_pin_OUT_bool(&(halui_data->mist_is_on), "halui.mist.is-on")); CHK(halui_export_pin_OUT_bool(&(halui_data->flood_is_on), "halui.flood.is-on")); CHK(halui_export_pin_OUT_bool(&(halui_data->program_is_idle), "halui.program.is-idle")); @@ -667,6 +676,9 @@ int halui_hal_init(void) CHK(halui_export_pin_IN_bool(&(halui_data->mode_mdi), "halui.mode.mdi")); CHK(halui_export_pin_IN_bool(&(halui_data->mode_teleop), "halui.mode.teleop")); CHK(halui_export_pin_IN_bool(&(halui_data->mode_joint), "halui.mode.joint")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_machine), "halui.jog-frame.machine")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_plane), "halui.jog-frame.plane")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_tool), "halui.jog-frame.tool")); CHK(halui_export_pin_IN_bool(&(halui_data->mist_on), "halui.mist.on")); CHK(halui_export_pin_IN_bool(&(halui_data->mist_off), "halui.mist.off")); CHK(halui_export_pin_IN_bool(&(halui_data->flood_on), "halui.flood.on")); @@ -899,6 +911,17 @@ static int sendJoint() return emcCommandWaitDone(); } +static int sendJogFrame(int frame) +{ + EMC_TRAJ_SET_JOG_FRAME emc_set_jog_frame_msg; + + emc_set_jog_frame_msg.frame = frame; + if (emcCommandSend(emc_set_jog_frame_msg)) { + return -1; + } + return emcCommandWaitDone(); +} + static int sendMistOn() { EMC_COOLANT_MIST_ON emc_coolant_mist_on_msg; @@ -1468,6 +1491,15 @@ static void check_hal_changes() if (check_bit_changed(new_halui_data.mode_joint, old_halui_data.mode_joint) != 0) sendJoint(); + if (check_bit_changed(new_halui_data.jog_frame_machine, old_halui_data.jog_frame_machine) != 0) + sendJogFrame(EMC_JOG_FRAME_MACHINE); + + if (check_bit_changed(new_halui_data.jog_frame_plane, old_halui_data.jog_frame_plane) != 0) + sendJogFrame(EMC_JOG_FRAME_PLANE); + + if (check_bit_changed(new_halui_data.jog_frame_tool, old_halui_data.jog_frame_tool) != 0) + sendJogFrame(EMC_JOG_FRAME_TOOL); + if (check_bit_changed(new_halui_data.mist_on, old_halui_data.mist_on) != 0) sendMistOn(); @@ -1927,6 +1959,9 @@ static void modify_hal_pins() hal_set_bool(halui_data->mode_is_mdi, emcStatus->task.mode == EMC_TASK_MODE::MDI); hal_set_bool(halui_data->mode_is_teleop, emcStatus->motion.traj.mode == EMC_TRAJ_MODE::TELEOP); hal_set_bool(halui_data->mode_is_joint, emcStatus->motion.traj.mode == EMC_TRAJ_MODE::FREE); + hal_set_bool(halui_data->jog_frame_is_machine, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_MACHINE); + hal_set_bool(halui_data->jog_frame_is_plane, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_PLANE); + hal_set_bool(halui_data->jog_frame_is_tool, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_TOOL); hal_set_bool(halui_data->program_is_paused, emcStatus->task.interpState == EMC_TASK_INTERP::PAUSED); hal_set_bool(halui_data->program_is_running, emcStatus->task.interpState == EMC_TASK_INTERP::READING || diff --git a/tests/twp-jog-frame/README b/tests/twp-jog-frame/README index ac9542861a6..3b33b204d32 100644 --- a/tests/twp-jog-frame/README +++ b/tests/twp-jog-frame/README @@ -16,8 +16,9 @@ jogs follow the tool frame status reports, and a plane in effect leaves that frame as the kinematics reports it. Under the identity type a tool jog moves the joints as it does under the primary type, along the tool as the head stands, and status has the tool frame the primary type has at -the same joints. Every servo -cycle of every jog is sampled: no world axis goes over its own speed or +the same joints. halui's +jog-frame pins select the frame and report it. Every servo cycle of +every jog is sampled: no world axis goes over its own speed or acceleration. twp-jog-frame-scurve runs the same on the S-curve planner, and diff --git a/tests/twp-jog-frame/test-ui.py b/tests/twp-jog-frame/test-ui.py index 4878b3f79ac..537a3fba3f9 100755 --- a/tests/twp-jog-frame/test-ui.py +++ b/tests/twp-jog-frame/test-ui.py @@ -369,6 +369,23 @@ def joint_jog(axis, dist, what): if abs(primary_moves[0][2] - 2) < 1e-3: error("the head stands straight, the tool Z jog moved the joints %s" % fmt(primary_moves[0])) +# halui selects the frame and says which is in force +for name, which in (("plane", linuxcnc.JOG_FRAME_PLANE), ("machine", linuxcnc.JOG_FRAME_MACHINE), + ("tool", linuxcnc.JOG_FRAME_TOOL)): + hal.set_p("halui.jog-frame.%s" % name, "1") + wait_for("halui.jog-frame.%s" % name, lambda: s.jog_frame == which, 5) + hal.set_p("halui.jog-frame.%s" % name, "0") + # halui shows the frame from its own status copy + deadline = time.time() + 5 + while time.time() < deadline: + shown = [n for n in ("machine", "plane", "tool") if hal.get_value("halui.jog-frame.is-%s" % n)] + if shown == [name]: + break + time.sleep(0.002) + if s.jog_frame != which or shown != [name]: + error("halui.jog-frame.%s gave frame %d, halui shows %s" % (name, s.jog_frame, shown)) +drain() + # every servo cycle of every jog above: no world axis over its own speed or # acceleration, a frame jog's stop at the box included. The log is block # buffered: read it until it reaches the servo thread's time now, which it diff --git a/tests/twp-jog-frame/test.ini b/tests/twp-jog-frame/test.ini index 9de5a6627a3..63e11993616 100644 --- a/tests/twp-jog-frame/test.ini +++ b/tests/twp-jog-frame/test.ini @@ -17,6 +17,7 @@ JOINTS = 6 [HAL] HALFILE = sim.hal HALCMD = setp 5axiskins.pivot-length 100 +HALUI = halui [TRAJ] COORDINATES = XYZBCW From 193b0fb8d9c5e520aa195ac70fec72467615a6e5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 11 Oct 2026 01:14:00 +1100 Subject: [PATCH 25/44] axis: select the jog frame from the View menu or with % The View menu lists the three jog frames after joint and world mode, % cycles through them as $ toggles the mode, and the status bar shows the frame next to the position type while it is not the machine's. --- docs/src/gcode/g-code.adoc | 10 +++---- docs/src/gui/axis.adoc | 8 +++++ share/axis/tcl/axis.tcl | 43 +++++++++++++++++++++++++++ src/emc/usr_intf/axis/scripts/axis.py | 14 +++++++++ 4 files changed, 70 insertions(+), 5 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 1a77973a2e7..4bf5795a850 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2687,11 +2687,11 @@ In world mode a jog of X, Y or Z moves along one of three frames: The operator selects the frame and a program never changes it: Python `jog_frame()`, with `jog_frame` in status; the halui pins `halui.jog-frame.machine`, `halui.jog-frame.plane` and `halui.jog-frame.tool`, -with `halui.jog-frame.is-machine`, `-plane` and `-tool` reporting it. -`motion.jog-frame` reports the frame in HAL, so a pendant can, for instance, -let only Z through in the tool frame. The jog pins `axis.L.jog-*` jog the -letter of the selected frame. A, B, C, U, V and W jog along their own axes in -every frame, and joint mode is not affected. +with `halui.jog-frame.is-machine`, `-plane` and `-tool` reporting it; in AXIS +the View menu or the `%` key. `motion.jog-frame` reports the frame in HAL, so +a pendant can, for instance, let only Z through in the tool frame. The jog +pins `axis.L.jog-*` jog the letter of the selected frame. A, B, C, U, V and W +jog along their own axes in every frame, and joint mode is not affected. The frame is taken when a jog starts and kept until the jog ends, so a jog in the tool frame does not bend while a rotary jogs with it, and the frame diff --git a/docs/src/gui/axis.adoc b/docs/src/gui/axis.adoc index 341ea500526..64bb0bd517d 100644 --- a/docs/src/gui/axis.adoc +++ b/docs/src/gui/axis.adoc @@ -277,6 +277,12 @@ So be familiar with your machine's design and interpret the display as needed. tuning, physical constraints, or position quantization. * 'Show Machine Position' - This is the position in unoffset coordinates, as established by Homing. * 'Show Relative Position' - This is the Machine Position modified by 'G5x', 'G92', and 'G43' offsets. +* 'Jog in machine frame', 'Jog in work plane frame', 'Jog in tool frame' - The + frame a jog of X, Y and Z moves along in world mode, see + <>. Outside the machine frame + the status bar shows the frame next to the position type. +* 'Joint mode', 'World mode' - Jog the joints one by one, or the axis letters. + Not shown with an identity kinematics. ==== Help Menu @@ -662,6 +668,7 @@ The most frequently used keyboard shortcuts are shown in the following table: |Shift-PgUp, PgDn |Rapid Z Axis |Manual |@ |Toggle Actual/Commanded |Any |# |Toggle Relative/Machine |Any +|% |Cycle jog frame Machine/Plane/Tool |Any |=== == Show LinuxCNC Status (`linuxcnctop`) @@ -1355,6 +1362,7 @@ print(vars.emcini.get()) interp_state = IntVar ja_rbutton = StringVar jog_aspeed = DoubleVar + jog_frame = IntVar jog_speed = DoubleVar kinematics_type = IntVar linuxcnctop_command = StringVar diff --git a/share/axis/tcl/axis.tcl b/share/axis/tcl/axis.tcl index 2b99ad86ae8..f1654e8f8f3 100644 --- a/share/axis/tcl/axis.tcl +++ b/share/axis/tcl/axis.tcl @@ -424,6 +424,31 @@ setup_menu_accel .menu.view end [_ "Show relative position"] .menu.view add separator +.menu.view add radiobutton \ + -value 0 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in machine frame"] + +.menu.view add radiobutton \ + -value 1 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in work plane frame"] + +.menu.view add radiobutton \ + -value 2 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in tool frame"] + +# kept last: AXIS takes the separator and the two entries out on an +# identity kinematics +.menu.view add separator + .menu.view add radiobutton \ -value 0 \ -variable teleop_mode \ @@ -1378,6 +1403,14 @@ label .info.program_time \ -textvariable ::program_remaining \ -width 22 +# packed after .info.position while jogs move along a frame other than +# the machine's +label .info.jog_frame \ + -anchor w \ + -borderwidth 2 \ + -relief sunken \ + -textvariable ::jog_frame_string + # Pack widget .info.task_state pack .info.task_state \ -side left @@ -1885,6 +1918,14 @@ proc update_state {args} { set ::position [concat [_ "Position:"] $coord_str $display_str] } + if {$::jog_frame == 0} { + pack forget .info.jog_frame + } else { + set ::jog_frame_string [concat [_ "Jog:"] \ + [lindex [list [_ Machine] [_ Plane] [_ Tool]] $::jog_frame]] + pack .info.jog_frame -side left -after .info.position + } + if {$::task_state == $::STATE_ON && $::interp_state == $::INTERP_IDLE} { if { ($::last_interp_state != $::INTERP_IDLE || $::last_task_state != $::task_state) \ && $::task_mode == $::TASK_MODE_AUTO} { @@ -2005,6 +2046,7 @@ set coord_type 1 set display_type 0 set spindledir {} set motion_mode 0 +set jog_frame 0 set kinematics_type -1 set metric 0 set max_speed 1 @@ -2026,6 +2068,7 @@ trace add variable spindledir write queue_update_state trace add variable coord_type write queue_update_state trace add variable display_type write queue_update_state trace add variable motion_mode write queue_update_state +trace add variable jog_frame write queue_update_state trace add variable kinematics_type write queue_update_state trace add variable on_any_limit write queue_update_state trace add variable motion_mode write joint_mode_switch diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index a76a371b2bf..fbeda6ef381 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -275,6 +275,7 @@ def General_Halt(): ("F4", _("Cycle among preview, DRO, and user tabs")), ("@", _("toggle Actual/Commanded")), ("#", _("toggle Relative/Machine")), + ("%", _("cycle jog frame Machine/Plane/Tool")), (_("Ctrl-Space"), _("Clear notifications")), (_("Alt-F, M, V"), _("Open a Menu")), ] @@ -950,6 +951,7 @@ def update(self): vupdate(vars.brake, self.stat.spindle[0]['brake']) vupdate(vars.spindledir, self.stat.spindle[0]['direction']) vupdate(vars.motion_mode, self.stat.motion_mode) + vupdate(vars.jog_frame, self.stat.jog_frame) vupdate(vars.optional_stop, self.stat.optional_stop) vupdate(vars.block_delete, self.stat.block_delete) if time.time() > spindlerate_blackout: @@ -2976,6 +2978,16 @@ def toggle_teleop_mode(*args): s.poll() set_motion_teleop(not (s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP)) + # the frame world jogs of X, Y and Z move along; motion refuses a + # change while a jog is under way, and the next poll shows the frame + # in force + def set_jog_frame(*args): + c.jog_frame(vars.jog_frame.get()) + + def cycle_jog_frame(*args): + s.poll() + c.jog_frame((s.jog_frame + 1) % 3) + def toggle_coord_type(*args): vars.coord_type.set(not vars.coord_type.get()) o.tkRedraw() @@ -3116,6 +3128,7 @@ def clear_recent_files(): ("maxvel_speed", DoubleVar), ("max_maxvel", DoubleVar), ("teleop_mode", IntVar), + ("jog_frame", IntVar), ("motion_mode", IntVar), ("kinematics_type", IntVar), ("optional_stop", BooleanVar), @@ -3239,6 +3252,7 @@ def kp_wrap(f,g): root_window.bind("@", commands.toggle_display_type) root_window.bind("#", commands.toggle_coord_type) root_window.bind("$", commands.toggle_teleop_mode) +root_window.bind("%", commands.cycle_jog_frame) root_window.bind("", commands.home_joint) root_window.bind("", kp_wrap(commands.home_joint, "KeyPress")) From 9a902279ee40cbca3fefd889b9f8253424a7f33c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 19 Oct 2026 17:40:22 +1100 Subject: [PATCH 26/44] interp: cut a tilted work plane on the part under the machine frame kinematics Under the machine frame kinematics, what G13.1 selects, a plane was in the machine frame and only head rotaries could orient the tool to it, so a table machine could not do 3+2 there: G53.1 refused a plane that needed the table. Sigma1912 asked for it on a mill-turn, the rotaries set once and the slides alone moving, without the TCP kinematics dragging the slides along on every rotary move. This is Fanuc's second feature coordinate system (0i-F 5.7.3.1). The interpreter takes a fixed map from the module's primary kinematics to the machine frame with the rotaries held, forward on the machine frame type of the primary type's inverse, read off at four points, and applies it after the whole offset chain. It carries the tool length along the tilted tool, which the primary type applies and the machine frame type does not. G68.2, G68.3 and G68.4 take it at the rotaries as they stand; G53.1, G53.3 and G53.6 orient the tool on the primary type's frames, so a table rotary helps, and take it where they end; G43 and G49 in the plane take it again with the new length, the slides staying, and the same offset given again keeps it; a kinematics switch in a plane takes it again or drops it. A rotary word keeps it, as Fanuc, Heidenhain and Siemens keep their plane: Fanuc's manual warns against turning a rotary in a tilted working plane and re-issues the plane after one. Canon gets the plane with the map folded in, so canon's maths and motion's are unchanged. Status keeps the two apart, as the standard controls do: g68_offset and g68_rotation stay the plane as defined under every kinematics type, as Fanuc's #_FCOORD and Heidenhain's FN 18 ID210 report it, and the map goes in the new g68_kins_offset and g68_kins_rotation, as Siemens keeps CYCLE800's workpiece reference frame apart from its table and tool reference frames, the identity where there is none. Task turns the plane jog frame by it, and the DROs of glcanon and halui and the preview's plane drawing compose it after the offset chain. SET_G68_FRAME carries both forms. Where a plane comes back from status, the preview restating the machine's plane and an abort with RETAIN_WORK_PLANE, the interpreter takes the map status has, so a rotary turned since counts; preview_helpers restates G43 ahead of the plane for it. tests/twp-static runs the same programs under the TCP kinematics and under G13.1 and checks the joints agree, then the rotary word, G68.4, G53, G69 and a switch in the plane. tests/twp-static-preview checks the status split across a switch, the plane jog frame, an abort and the preview, after a rotary turn too. Both run on a table machine, and their -head twins on a head machine, sharing the INI and HAL files and the helpers in tests/twp-static/static.py; each starts LinuxCNC once and takes under 10 s, on fast sims with task on a 1 ms cycle. --- docs/src/config/python-interface.adoc | 8 +- docs/src/gcode/g-code.adoc | 48 ++- docs/src/motion/switchkins.adoc | 5 +- lib/python/preview_helpers.py | 11 +- lib/python/rs274/glcanon.py | 13 + lib/python/rs274/glcanon_scene.py | 13 +- lib/python/rs274/interpret.py | 5 + src/emc/nml_intf/canon.hh | 25 +- src/emc/nml_intf/emc.cc | 4 + src/emc/nml_intf/emc_nml.hh | 7 +- src/emc/nml_intf/emcops.cc | 2 + src/emc/rs274ngc/gcodemodule.cc | 42 +- src/emc/rs274ngc/interp_convert.cc | 61 ++- src/emc/rs274ngc/interp_internal.hh | 9 + src/emc/rs274ngc/interp_setup.cc | 5 + src/emc/rs274ngc/interp_workplane.cc | 396 ++++++++++++++++-- src/emc/rs274ngc/rs274ngc_interp.hh | 9 + src/emc/sai/saicanon.cc | 11 +- src/emc/task/emccanon.cc | 29 +- src/emc/task/emctaskmain.cc | 22 +- src/emc/usr_intf/axis/extensions/emcmodule.cc | 16 + src/emc/usr_intf/halui.cc | 22 +- tests/twp-static-head/README | 2 + tests/twp-static-head/checkresult | 3 + tests/twp-static-head/head.hal | 1 + tests/twp-static-head/head.ini | 1 + tests/twp-static-head/test-ui.py | 1 + tests/twp-static-head/test.sh | 6 + tests/twp-static-head/tool.tbl | 1 + tests/twp-static-preview-head/README | 2 + tests/twp-static-preview-head/checkresult | 3 + tests/twp-static-preview-head/head.hal | 1 + tests/twp-static-preview-head/head.ini | 1 + tests/twp-static-preview-head/test-ui.py | 1 + tests/twp-static-preview-head/test.sh | 6 + tests/twp-static-preview-head/tool.tbl | 1 + tests/twp-static-preview/README | 17 + tests/twp-static-preview/checkresult | 3 + tests/twp-static-preview/table.hal | 1 + tests/twp-static-preview/table.ini | 1 + tests/twp-static-preview/test-ui.py | 187 +++++++++ tests/twp-static-preview/test.sh | 6 + tests/twp-static-preview/tool.tbl | 1 + tests/twp-static/README | 18 + tests/twp-static/checkresult | 3 + tests/twp-static/head.hal | 16 + tests/twp-static/head.ini | 131 ++++++ tests/twp-static/static.py | 123 ++++++ tests/twp-static/table.hal | 15 + tests/twp-static/table.ini | 123 ++++++ tests/twp-static/test-ui.py | 57 +++ tests/twp-static/test.sh | 6 + tests/twp-static/tool.tbl | 1 + 53 files changed, 1420 insertions(+), 82 deletions(-) create mode 100644 tests/twp-static-head/README create mode 100755 tests/twp-static-head/checkresult create mode 120000 tests/twp-static-head/head.hal create mode 120000 tests/twp-static-head/head.ini create mode 120000 tests/twp-static-head/test-ui.py create mode 100755 tests/twp-static-head/test.sh create mode 120000 tests/twp-static-head/tool.tbl create mode 100644 tests/twp-static-preview-head/README create mode 100755 tests/twp-static-preview-head/checkresult create mode 120000 tests/twp-static-preview-head/head.hal create mode 120000 tests/twp-static-preview-head/head.ini create mode 120000 tests/twp-static-preview-head/test-ui.py create mode 100755 tests/twp-static-preview-head/test.sh create mode 120000 tests/twp-static-preview-head/tool.tbl create mode 100644 tests/twp-static-preview/README create mode 100755 tests/twp-static-preview/checkresult create mode 120000 tests/twp-static-preview/table.hal create mode 120000 tests/twp-static-preview/table.ini create mode 100755 tests/twp-static-preview/test-ui.py create mode 100755 tests/twp-static-preview/test.sh create mode 120000 tests/twp-static-preview/tool.tbl create mode 100644 tests/twp-static/README create mode 100755 tests/twp-static/checkresult create mode 100644 tests/twp-static/head.hal create mode 100644 tests/twp-static/head.ini create mode 100644 tests/twp-static/static.py create mode 100644 tests/twp-static/table.hal create mode 100644 tests/twp-static/table.ini create mode 100755 tests/twp-static/test-ui.py create mode 100755 tests/twp-static/test.sh create mode 100644 tests/twp-static/tool.tbl diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index f3e985606f8..28616cc198f 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -182,8 +182,14 @@ see <> for an example. *g68_active*:: '(returns integer)' - a tilted work plane (G68.2) is in effect. +*g68_kins_offset*:: '(returns tuple of floats)' - + under the machine frame kinematics, the shift of the second feature coordinate system the tilted work plane is cut through (3+2), in machine units: the part as the primary kinematics places it, carried onto the machine frame with the rotaries held, after the whole offset chain. Zero where there is none. + +*g68_kins_rotation*:: '(returns tuple of floats)' - + the rotation of that second feature coordinate system, nine values row by row; the identity where there is none. + *g68_offset*:: '(returns tuple of floats)' - - origin of the tilted work plane, in the coordinate system it was defined in. + origin of the tilted work plane, in the coordinate system it was defined in, the same under every kinematics type. *g68_rotation*:: '(returns tuple of floats)' - rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 4bf5795a850..05f8433e116 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2125,8 +2125,7 @@ so 'Q' means the same thing on every module and needs no INI entry. The orientation is evaluated by the interpreter, ahead of the moves before it, on the kinematics type the program is in. It needs a kinematics type whose frames describe the machine: on a switchable module that is its TCP -kinematics, selected with <>, and not the identity -kinematics the module starts in. +kinematics, selected with <>. Under the module's machine frame kinematics, which <> selects, the interpreter orients the tool with the TCP kinematics' frames, for 3+2 machining with the slides alone moving; see <>. .G53.1 Example [source,ngc] @@ -2142,9 +2141,10 @@ G69 It is an error if: * No tilted work plane is active. -* The kinematics type in force is the identity kinematics, the module cannot - be evaluated by the interpreter, or it reports no frames, so it cannot say - where its joints point the tool. +* The kinematics type in force is an identity kinematics the module declares + no TCP kinematics beside, the module cannot be evaluated by the + interpreter, or it reports no frames, so it cannot say where its joints + point the tool. * The plane's normal cannot be reached by the rotary joints. * 'P' is anything but 0, 1 or 2, no reachable pose has the rotary that names it the way 'P' asks for, or the machine has no pair of poses a @@ -2550,7 +2550,7 @@ conventions, tool X turned about the tool axis by the smaller angle that makes it parallel to the machine XY plane, and machine X when the tool is vertical; 'R' turns the plane from there. It asks the kinematics module for the tool direction, so it needs a kinematics type whose frames describe the -machine, as <> does. +machine, as <> does, and under the machine frame kinematics it reads the tool on the part, as the TCP kinematics places it. 'G68.4' takes any 'G68.2' form and composes it onto the active plane: the words are in the plane, and the result is a new plane relative to the old @@ -2648,13 +2648,45 @@ It is an error if: * 'R' is given on more than one block of a 'P2' definition. * 'G68.4' is used with no plane active. * 'G68.2', 'G68.3' or 'G68.4' is used while a 'G52' given in the plane is in effect. -* 'G68.3' is used where the kinematics cannot be evaluated, is the identity - kinematics, or reports no frames. +* 'G68.3' is used where the kinematics cannot be evaluated, is an identity + kinematics the module declares no TCP kinematics beside, or reports no + frames. * 'G68.3' is given 'I', 'J', 'K', 'P' or 'Q', or 'G69' any of those or 'R': a word the code does not use is an error, not ignored. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. +[[sec:twp-machine-frame]] +=== 3+2 under the machine frame kinematics(((3+2 machining))) + +Under a module's machine frame kinematics, the type <> selects, a plane is cut where the module's TCP kinematics would cut it with the rotary joints held where they stand. The plane sits on the part, turned with the table, and the moves under it are made by the slides alone, so a rotary move never drags them along as it does under the TCP kinematics. This is 3+2 machining with the rotaries set once, what Fanuc calls the second feature coordinate system. + +When 'G68.2', 'G68.3' or 'G68.4' defines a plane, the interpreter takes the map from the part, as the TCP kinematics places it, to the machine frame, at the rotaries as they stand. With the rotaries held that map is a fixed rotation and shift, and it carries the tool length along the tilted tool, which the TCP kinematics applies and the machine frame kinematics does not. 'G53.1', 'G53.3' and 'G53.6' orient the tool to the plane on the part with the TCP kinematics' frames, so a table rotary helps where the plane needs it, and take the map again at the rotaries they end at. 'G53.6' keeps the tool tip where it is at the ends of its move; in between, the slides and the rotaries move in straight lines in the machine frame and the tip leaves the point while they turn. 'G53.2' moves nothing and leaves the map as it was. + +A rotary word under the plane moves the rotary alone and the map stays, so the moves after it are still cut where the plane sat on the part before the turn, which is now somewhere else. Fanuc and Heidenhain do the same, and Fanuc's manual tells the programmer not to turn a rotary in a tilted working plane. To work on the turned part, give the plane again after the turn: 'G68.4 X0 Y0 Z0 I0 J0 K0' is the same plane at the rotaries as they now stand, and 'G53.1' turns the rotaries to the plane and takes the map where they end. + +.3+2 Example +[source,ngc] +---- +G13.1 (the machine frame kinematics) +G43 H1 +G68.2 X0 Y0 Z0 I0 J30 K0 (a plane on the part, tilted 30 degrees about X) +G53.1 (the rotaries to it, the table helping) +G0 X0 Y0 Z5 (above the plane's origin, the slides alone) +G1 Z-2 F200 (down along the plane's normal) +G0 Z5 +C90 (the table alone; the plane stays where it was) +G68.4 X0 Y0 Z0 I0 J0 K0 (the plane again, on the part as it now stands) +G0 X0 Y0 Z5 +G69 +---- + +A change of tool length offset in the plane, 'G43' or 'G49', keeps the slides where they are and takes the map again with the new length. 'G53', 'G28' and 'G30' move in the machine frame, as Fanuc's do. A change of kinematics type in the plane keeps the plane on the part: under the TCP kinematics it follows the table as the rotaries turn, and back under the machine frame kinematics the map is taken at the rotaries as they stand. + +Status keeps the two apart, as the Siemens swivel cycle keeps its workpiece reference frame apart from its table and tool reference frames: 'g68_offset' and 'g68_rotation' are the plane as defined, the same under every kinematics type, and 'g68_kins_offset' and 'g68_kins_rotation' are the map, the identity where none is in effect. A display composes the two after the offset chain to show plane coordinates and draw the plane, and the plane jog frame moves along the plane turned by the map. The G-code preview restates the plane from status with the map status has, so a program run after a plane set in MDI is drawn where it is cut, a rotary turned since included, and so does an abort with `RETAIN_WORK_PLANE`. + +A module that declares no TCP kinematics, or whose machine frame kinematics is its TCP kinematics, a robot's, has no map to take: there a plane is in the machine frame, and only head rotaries can orient the tool to it. + [[sec:jog-frame]] === Jogging along the plane or the tool(((Jog frame))) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 661870fd27d..ef83962d082 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -651,7 +651,10 @@ are not in the conventional order still gets working spellings. With `MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the INI file, 'G53', 'G28' and 'G30' move in the machine frame kinstype's world too whenever another kinstype is in force; the five axis sims set -it. A +it. A tilted work plane under the machine frame kinstype is read on the +part through the lowest numbered primary kinstype with the rotaries held, +for 3+2 machining with the slides alone moving, so a table machine needs +a primary kinstype for it; see the G-code documentation for 'G68.2'. A machine with two tool heads, a saw blade and a waterjet say, declares one primary kinstype per head and selects the head with 'G12.1 P-': 'G43.4' stays on the kinstype in force when it is primary, and from any diff --git a/lib/python/preview_helpers.py b/lib/python/preview_helpers.py index cda36aab747..2a07bf42136 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -64,7 +64,10 @@ def workplane_code(s): words are in machine units, so the block goes where the machine's G20 or G21 is in force; a later unit change converts the plane. The block selects the machine's coordinate system first, since the plane sits on - it and a later block may reselect it but not change it.""" + it and a later block may reselect it but not change it, and the tool + length offset, since under the machine frame kinematics the plane is + cut through the machine's second feature coordinate system, which the + interpreter takes from status and which comes after the tool.""" if not getattr(s, "g68_active", 0): return None r = s.g68_rotation @@ -79,5 +82,7 @@ def workplane_code(s): i = 0.0 k = atan2(-r[1], r[4]) system = next((g for g in s.gcodes[1:] if 540 <= g <= 593), 540) - return "G%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( - system // 10 if system % 10 == 0 else system / 10, o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) + length = "G43 " if s.gcodes[9] == 430 else "" + return "%sG%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( + length, system // 10 if system % 10 == 0 else system / 10, + o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 264570e48c9..6873530cb4e 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -1265,6 +1265,19 @@ def posstrs(self): positions = s.actual_position if self.get_show_relative(): + k = getattr(s, 'g68_kins_rotation', None) if s.g68_active else None + if k: + # off the second feature coordinate system a plane is + # cut through under the machine frame kinematics, which + # comes after the whole offset chain + positions = list(positions) + o = s.g68_kins_offset + _x = positions[X] - o[X] + _y = positions[Y] - o[Y] + _z = positions[Z] - o[Z] + positions[X] = k[0]*_x + k[3]*_y + k[6]*_z + positions[Y] = k[1]*_x + k[4]*_y + k[7]*_z + positions[Z] = k[2]*_x + k[5]*_y + k[8]*_z positions = [(i-j) for i, j in zip(positions, s.tool_offset)] positions = [(i-j) for i, j in zip(positions, s.g5x_offset)] diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index fbec0cc6ad0..34f2a8fdca9 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -2758,7 +2758,9 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": Status carries it as the interpreter gave it, the origin in the coordinate system the plane was defined in and in machine units, so it goes through the g92 offset, the XY rotation and the g5x offset the way - the canon takes a definition, in the canon's units. Where it is one the + the canon takes a definition, in the canon's units, and under the + machine frame kinematics through the second feature coordinate system + status has apart, which comes after the tool offset. Where it is one the loaded program defined, that record is returned, extents and all; a plane from MDI, or from a program no longer loaded, comes back as a record of its own with nothing under it. @@ -2772,6 +2774,9 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": g92 = ctx.to_internal_units(s.g92_offset) g5x = ctx.to_internal_units(s.g5x_offset) t = math.radians(s.rotation_xy) + k = getattr(s, 'g68_kins_rotation', (1, 0, 0, 0, 1, 0, 0, 0, 1)) + ko = ctx.to_internal_units(getattr(s, 'g68_kins_offset', (0,) * 9)) + tlo = ctx.to_internal_units(getattr(s, 'tool_offset', (0,) * 9)) except (AttributeError, TypeError): return None c, sn = math.cos(t), math.sin(t) @@ -2780,7 +2785,11 @@ def through(x: float, y: float, z: float) -> tuple[float, float, float]: x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) - return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) + x, y, z = (x*c - y*sn + g5x[X] + tlo[X], x*sn + y*c + g5x[Y] + tlo[Y], + z + g5x[Z] + tlo[Z]) + return (k[0]*x + k[1]*y + k[2]*z + ko[X] - tlo[X], + k[3]*x + k[4]*y + k[5]*z + ko[Y] - tlo[Y], + k[6]*x + k[7]*y + k[8]*z + ko[Z] - tlo[Z]) origin = through(0, 0, 0) axes = [] diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index d7f3d17493a..9f86a99e1ac 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -196,6 +196,11 @@ def get_block_delete(self): def get_external_joint_positions(self): return tuple(self.s.joint_actual_position[:self.s.joints]) + def get_external_g68_kins_frame(self): + # the second feature coordinate system the machine's plane is cut + # through, which a plane restated from status is cut through too + return (tuple(self.s.g68_kins_offset[:3]), tuple(self.s.g68_kins_rotation)) + def get_kinematics_type(self): return self.s.kinematics_type diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 4e2635db30a..c823a6329f0 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -262,9 +262,18 @@ extern void HOME_CYCLE_JOINT(int joint); /* The tilted work plane. Origin in program units and a row major rotation matrix, both in the coordinate system active when the plane was defined; - active 0 cancels it. */ + active 0 cancels it. Under the machine frame kinematics a plane is cut on + the part through the second feature coordinate system (3+2), and x, y, z + and rotation are the plane with it folded in, the one canon applies. The + rest is what status reports: the plane as defined, and the second feature + coordinate system on its own, a row major rotation and a shift in the + machine's length units, the identity and zero where there is none. */ extern void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active); + const double rotation[9], int active, + const double defined_origin[3], + const double defined_rotation[9], + const double kins_shift[3], + const double kins_rotation[9]); /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real @@ -894,11 +903,15 @@ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); no motion controller is attached (sai, a preview without a machine) */ extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); -/* The tilted work plane the machine last executed, in the form - SET_G68_FRAME takes: 1 with origin and rotation filled in, 0 when there - is none or the coordinate system under it is no longer the one the - interpreter has, -1 when there is no machine to ask (sai, preview). */ +/* The tilted work plane the machine last executed, as defined: 1 with + origin and rotation filled in, 0 when there is none or the coordinate + system under it is no longer the one the interpreter has, -1 when there + is no machine to ask (sai, preview). */ extern int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]); +/* The second feature coordinate system the machine's plane is cut through, + as SET_G68_FRAME gave it: 1 with shift and rotation filled in, the + identity where there is none, -1 when nothing can say. */ +extern int GET_EXTERNAL_G68_KINS_FRAME(double shift[3], double rotation[9]); // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 5616d4f4aa7..3cdd0604329 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1378,6 +1378,8 @@ void EMC_TASK_STAT::update(CMS * cms) EmcPose_update(cms, &g68_offset); cms->update(g68_rotation, 9); cms->update(g68_active); + EmcPose_update(cms, &g68_kins_offset); + cms->update(g68_kins_rotation, 9); EmcPose_update(cms, &toolOffset); cms->update(activeGCodes, ACTIVE_G_CODES); cms->update(activeMCodes, ACTIVE_M_CODES); @@ -1670,6 +1672,8 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) EmcPose_update(cms, &origin); cms->update(rotation, 9); cms->update(active); + EmcPose_update(cms, &kins_offset); + cms->update(kins_rotation, 9); } // cppcheck-suppress duplInheritedMember diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index f8ad6691ca3..61c0ddcf4d2 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -873,7 +873,8 @@ class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_G68() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_G68_TYPE, sizeof(EMC_TRAJ_SET_G68)), - origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0) + origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0), + kins_offset{}, kins_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1} {}; // For internal NML/CMS use only. @@ -884,6 +885,8 @@ class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { EmcPose origin; double rotation[9]; // row major int active; + EmcPose kins_offset; // the second feature coordinate system (3+2) + double kins_rotation[9]; // row major }; // the frame world jogs of X, Y and Z move along, EMC_JOG_FRAME_* @@ -1575,6 +1578,8 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { EmcPose g68_offset; // tilted work plane origin, in user units double g68_rotation[9]; // tilted work plane rotation, row major int g68_active; // a tilted work plane is in effect + EmcPose g68_kins_offset; // under the machine frame kinematics, the second + double g68_kins_rotation[9]; // feature coordinate system the plane is cut through EmcPose toolOffset; // tool offset, in general pose form int activeGCodes[ACTIVE_G_CODES]; int activeMCodes[ACTIVE_M_CODES]; diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 3be7199a484..7f9fdbae371 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -159,6 +159,8 @@ EMC_TASK_STAT::EMC_TASK_STAT() g68_offset{}, g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, g68_active(0), + g68_kins_offset{}, + g68_kins_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, toolOffset{}, activeSettings{}, programUnits(CANON_UNITS_MM), diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index caf96640d77..73c9610c141 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -577,8 +577,14 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; +// the preview draws the plane canon applies; what status reports apart is +// the machine's business void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) { + const double rotation[9], int active, + const double * /*defined_origin*/, + const double * /*defined_rotation*/, + const double * /*kins_shift*/, + const double * /*kins_rotation*/) { WorkFrame frame; frame.active = active != 0; frame.origin = ensure_inch({x, y, z}); @@ -970,6 +976,28 @@ int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { return kins_type_flags[ktype]; } int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) { return -1; } +// the machine's second feature coordinate system, from a canon that watches +// the status buffer, for the plane the preview restates from it +int GET_EXTERNAL_G68_KINS_FRAME(double *shift, double *rotation) { + if(parse_state.interp_error) return -1; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_external_g68_kins_frame")) return -1; + try { + py::object got = canon.attr("get_external_g68_kins_frame")(); + if(got.is_none()) return -1; + py::sequence pair = got.cast(); + py::sequence o = py::reinterpret_borrow(pair[0]).cast(); + py::sequence r = py::reinterpret_borrow(pair[1]).cast(); + if(py::len(o) < 3 || py::len(r) != 9) return -1; + for(int i = 0; i < 3; i++) shift[i] = o[i].cast(); + for(int i = 0; i < 9; i++) rotation[i] = r[i].cast(); + } catch(py::error_already_set &) { + return -1; // the error goes with the exception + } catch(py::builtin_exception &) { + return -1; + } + return 1; +} // the kind of transform the machine runs, from a canon that watches the // status buffer; one that cannot answer has no machine, and the joints @@ -1111,6 +1139,8 @@ static py::object parse_file(const char *f, py::handle canon, if(own) { machine_moves = own->_setup.machine_moves_in_machine_frame; own->_setup.machine_moves_in_machine_frame = false; + // and a tilted work plane they restate is the one status has + own->_setup.g68_as_sent = true; } if(initcodes) { @@ -1137,7 +1167,10 @@ static py::object parse_file(const char *f, py::handle canon, if(!RESULT_OK) goto out_error; result = parse_state.pinterp->execute(); } - if(own) own->_setup.machine_moves_in_machine_frame = machine_moves; + if(own) { + own->_setup.machine_moves_in_machine_frame = machine_moves; + own->_setup.g68_as_sent = false; + } while(!parse_state.interp_error && RESULT_OK) { error_line_offset = 1; @@ -1168,7 +1201,10 @@ static py::object parse_file(const char *f, py::handle canon, if(parse_state.pinterp) { auto interp = dynamic_cast(parse_state.pinterp); - if(interp) interp->_setup.use_lazy_close = false; + if(interp) { + interp->_setup.use_lazy_close = false; + interp->_setup.g68_as_sent = false; + } parse_state.pinterp->close(); } if(parse_state.interp_error) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 81589efa21a..a64158b8a41 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -2464,6 +2464,9 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->w_axis_offset); SET_XY_ROTATION(settings->rotation_xy); + // canon's plane has the second feature coordinate system folded in, over + // the offsets that just changed + if (settings->g68_snap_active) { work_plane_send(settings); } return INTERP_OK; } @@ -5614,6 +5617,7 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 if (settings->g68_active) { world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, &settings->current_y, &settings->current_z); + if (settings->g68_snap_active) { work_plane_send(settings); } } if (block->m_modes[4] == 30) @@ -6732,6 +6736,7 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu have_standing = true; } switch_kins_type(primary, settings); + CHP(snap_after_switch(settings, standing, have_standing)); } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { @@ -6829,9 +6834,45 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // motion will, and send the point along for motion to check against. EmcPose point; bool point_known = false; - CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, - &point, &point_known)); - if (point_known) { + if (!settings->g68_snap_active) { + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + } + if (settings->g68_snap_active) { + // Under the second feature coordinate system the machine frame type + // reads no tool, so the slides stay where they are, and the frame is + // taken again at its rotaries with the new length, which it carries + // along the tilted tool. + double wx, wy, wz; + EmcPose before = settings->tool_offset; + int status; + + program_to_world_xyz(settings, settings->current_x, settings->current_y, + settings->current_z, &wx, &wy, &wz); + settings->tool_offset = tool_offset; + // the same offset again, as a GUI restates it, keeps the frame: one + // taken from status may stand on rotaries turned since + status = (tool_offset.tran.x == before.tran.x && tool_offset.tran.y == before.tran.y + && tool_offset.tran.z == before.tran.z && tool_offset.a == before.a + && tool_offset.b == before.b && tool_offset.c == before.c + && tool_offset.u == before.u && tool_offset.v == before.v + && tool_offset.w == before.w) + ? INTERP_OK : snap_take(settings, settings->g68_snap_joints); + if (status != INTERP_OK) { + settings->tool_offset = before; + return status; + } + world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, + &settings->current_y, &settings->current_z); + settings->AA_current += before.a - tool_offset.a; + settings->BB_current += before.b - tool_offset.b; + settings->CC_current += before.c - tool_offset.c; + settings->u_current += before.u - tool_offset.u; + settings->v_current += before.v - tool_offset.v; + settings->w_current += before.w - tool_offset.w; + USE_TOOL_LENGTH_OFFSET(tool_offset); + work_plane_send(settings); + } else if (point_known) { double prog[9]; EmcPose in_program; @@ -6908,7 +6949,13 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // the program selected itself is left alone, and a module that // declares no such type keeps the plain cancel. int identity = flagged_kins_type(KINSTYPE_MACHINE); - if (identity >= 0) { switch_kins_type(identity, settings); } + if (identity >= 0 && identity != settings->kins_type) { + double joints[EMCMOT_MAX_JOINTS]; + bool have; + CHP(snap_before_switch(settings, joints, &have)); + switch_kins_type(identity, settings); + CHP(snap_after_switch(settings, joints, have)); + } settings->kins_by_g43_4 = false; } @@ -6994,8 +7041,14 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 NCE_KINS_TYPE_NOT_PROVIDED); } + // a tilted work plane stays; the joints stay, so its second feature + // coordinate system is taken where they are, or dropped + double joints[EMCMOT_MAX_JOINTS]; + bool have = false, changed = (kins_type != settings->kins_type); + if (changed) { CHP(snap_before_switch(settings, joints, &have)); } SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; + if (changed) { CHP(snap_after_switch(settings, joints, have)); } // the program has taken the kinematics over from G43.4 settings->kins_by_g43_4 = false; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 18f00dc79a7..517d81ad9fe 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -808,6 +808,15 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r + // the second feature coordinate system: under the machine frame type a + // plane is cut on the part as the primary type places it with the + // rotaries held, a rigid map from the primary type's world to the + // machine frame, after the whole chain; machine units + bool g68_snap_active; + double g68_snap_rotation[3][3]; + double g68_snap_shift[3]; + double g68_snap_joints[EMCMOT_MAX_JOINTS]; // the joints it was taken at + bool g68_as_sent; // a plane given now is the machine's, as a GUI restates it from status // the pose G53.2 last solved, in program words, for #<_orient_x> and kin bool orient_valid; double orient_pose[6]; // x y z, the rotaries in orient order diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 80021e5a4b7..2367ea74442 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -114,6 +114,11 @@ setup::setup() : g68_seq_have(0), g68_seq_r_q(-1), g68_seq_word{}, + g68_snap_active(false), + g68_snap_rotation{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + g68_snap_shift{0.0, 0.0, 0.0}, + g68_snap_joints{}, + g68_as_sent(false), orient_valid(false), orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, kins_ctx(nullptr), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 4eac723ba1e..416ee0607ba 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -43,6 +43,7 @@ #include "rs274ngc_return.hh" #include "interp_internal.hh" #include "rs274ngc_interp.hh" +#include "units.h" //---------------------------------------------------------------------- // small matrix helpers, row major double[3][3] @@ -151,7 +152,8 @@ void Interp::g68_unrotate(setup_pointer s, double *x, double *y, double *z) } // The whole chain for X Y Z, program coordinates to the absolute (G53) -// frame: the plane, G92, the XY rotation, G5x and the tool offset. +// frame: the plane, G92, the XY rotation, G5x and the tool offset, then +// the second feature coordinate system where one is in effect. void Interp::program_to_world_xyz(setup_pointer s, double px, double py, double pz, double *wx, double *wy, double *wz) @@ -163,15 +165,31 @@ void Interp::program_to_world_xyz(setup_pointer s, y += s->axis_offset_y; z += s->axis_offset_z; rotate(&x, &y, s->rotation_xy); - *wx = x + s->origin_offset_x + s->tool_offset.tran.x; - *wy = y + s->origin_offset_y + s->tool_offset.tran.y; - *wz = z + s->origin_offset_z + s->tool_offset.tran.z; + x += s->origin_offset_x + s->tool_offset.tran.x; + y += s->origin_offset_y + s->tool_offset.tran.y; + z += s->origin_offset_z + s->tool_offset.tran.z; + if (s->g68_snap_active) { + mat_apply(s->g68_snap_rotation, &x, &y, &z); + x += USER_TO_PROGRAM_LEN(s->g68_snap_shift[0]); + y += USER_TO_PROGRAM_LEN(s->g68_snap_shift[1]); + z += USER_TO_PROGRAM_LEN(s->g68_snap_shift[2]); + } + *wx = x; + *wy = y; + *wz = z; } void Interp::world_to_program_xyz(setup_pointer s, double wx, double wy, double wz, double *px, double *py, double *pz) { + if (s->g68_snap_active) { + wx -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[0]); + wy -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[1]); + wz -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[2]); + mat_apply_transposed(s->g68_snap_rotation, &wx, &wy, &wz); + } + double x = wx - s->origin_offset_x - s->tool_offset.tran.x; double y = wy - s->origin_offset_y - s->tool_offset.tran.y; double z = wz - s->origin_offset_z - s->tool_offset.tran.z; @@ -190,29 +208,112 @@ void Interp::world_to_program_xyz(setup_pointer s, // setting and clearing the plane //---------------------------------------------------------------------- -// Tell canon the plane in effect. Canon knows no G52 in the plane: it -// gets the origin with L applied, which is the frame the moves are in. -static void work_plane_send(setup_pointer s) +// The plane as defined, the form status reports it in. Canon knows no G52 +// in the plane: it gets the origin with L applied, which is the frame the +// moves are in. +void Interp::work_plane_defined(setup_pointer s, double o[3], double flat[9]) { - double o[3] = { s->g68_local[0], s->g68_local[1], s->g68_local[2] }, flat[9]; - + for (int i = 0; i < 3; i++) { o[i] = s->g68_local[i]; } mat_apply(s->g68_rotation, &o[0], &o[1], &o[2]); for (int i = 0; i < 3; i++) { o[i] += s->g68_offset[i]; for (int j = 0; j < 3; j++) { flat[3*i + j] = s->g68_rotation[i][j]; } } - SET_G68_FRAME(o[0], o[1], o[2], flat, 1); +} + +// The plane canon applies. Canon knows no second feature coordinate system: +// the stages between the plane and it are rigid, so canon gets the plane +// that lands where the chain with it does, read off at four points. +void Interp::work_plane_sent(setup_pointer s, double o[3], double flat[9]) +{ + work_plane_defined(s, o, flat); + if (s->g68_snap_active) { + double v[4][3]; + + for (int k = 0; k < 4; k++) { + double x, y, z; + + program_to_world_xyz(s, k == 1, k == 2, k == 3, &x, &y, &z); + // back through the stages canon applies outside the plane + x -= s->origin_offset_x + s->tool_offset.tran.x; + y -= s->origin_offset_y + s->tool_offset.tran.y; + z -= s->origin_offset_z + s->tool_offset.tran.z; + rotate(&x, &y, -s->rotation_xy); + v[k][0] = x - s->axis_offset_x; + v[k][1] = y - s->axis_offset_y; + v[k][2] = z - s->axis_offset_z; + } + for (int i = 0; i < 3; i++) { + o[i] = v[0][i]; + for (int j = 0; j < 3; j++) { flat[3*i + j] = v[j + 1][i] - v[0][i]; } + } + } +} + +// tell canon the plane in effect, and status the plane as defined and the +// second feature coordinate system apart +void Interp::work_plane_send(setup_pointer s) +{ + double o[3], flat[9], def_o[3], def_flat[9], shift[3] = { 0, 0, 0 }; + double kins[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + work_plane_sent(s, o, flat); + work_plane_defined(s, def_o, def_flat); + if (s->g68_snap_active) { + for (int i = 0; i < 3; i++) { + shift[i] = s->g68_snap_shift[i]; + for (int j = 0; j < 3; j++) { kins[3*i + j] = s->g68_snap_rotation[i][j]; } + } + } + SET_G68_FRAME(o[0], o[1], o[2], flat, 1, def_o, def_flat, shift, kins); +} + +// tell canon there is no plane +static void work_plane_send_none() +{ + static const double zero[3] = { 0, 0, 0 }; + static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0, zero, identity, zero, identity); +} + +// The second feature coordinate system for a plane that comes from the +// machine, a GUI restating it or an abort keeping it: the one status has +// where the machine reports one, else one taken at the joints it stands in. +int Interp::snap_from_machine(setup_pointer s, const double *joints) +{ + double shift[3], flat[9]; + + if (GET_EXTERNAL_G68_KINS_FRAME(shift, flat) <= 0) { return snap_take(s, joints); } + for (int i = 0; i < 3; i++) { + s->g68_snap_shift[i] = shift[i]; + for (int j = 0; j < 3; j++) { s->g68_snap_rotation[i][j] = flat[3*i + j]; } + } + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->g68_snap_joints[i] = joints[i]; } + s->g68_snap_active = true; + return INTERP_OK; } // Install a plane. The tool does not move, so its program coordinates // change: take the current point through the old chain to the absolute -// frame and back through the new one. +// frame and back through the new one. Under the machine frame type the +// second feature coordinate system is taken at the rotaries as they stand, +// or for a plane a GUI restates, the machine's. int Interp::work_plane_set(setup_pointer s, int code, const double origin[3], const double rotation[3][3]) { - double wx, wy, wz; + double wx, wy, wz, joints[EMCMOT_MAX_JOINTS]; + bool have; program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + CHP(snap_standing(s, joints, &have, s->g68_as_sent)); + if (!have) { + s->g68_snap_active = false; + } else if (s->g68_as_sent) { + CHP(snap_from_machine(s, joints)); + } else { + CHP(snap_take(s, joints)); + } for (int i = 0; i < 3; i++) { s->g68_offset[i] = origin[i]; @@ -235,22 +336,22 @@ int Interp::work_plane_set(setup_pointer s, int code, int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) { double wx, wy, wz; - static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; s->g68_seq_code = 0; if (!s->g68_active) { - if (tell_canon_anyway) { SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); } + if (tell_canon_anyway) { work_plane_send_none(); } return INTERP_OK; } program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); s->g68_active = false; + s->g68_snap_active = false; s->g68_code = 0; for (int i = 0; i < 3; i++) { s->g68_offset[i] = s->g68_local[i] = 0.0; } mat_identity(s->g68_rotation); world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); - SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); + work_plane_send_none(); return INTERP_OK; } @@ -260,10 +361,12 @@ int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) // the shifted origin; a G52 in the plane is kept when the machine was // still in the plane the interpreter has, and becomes part of the origin // otherwise. A plane whose coordinate system the read ahead has changed -// is cancelled. +// is cancelled. Under the machine frame type the second feature coordinate +// system is the machine's too. int Interp::work_plane_restore(setup_pointer s) { - double origin[3], flat[9], here[3], wx, wy, wz; + double origin[3], flat[9], joints[EMCMOT_MAX_JOINTS], wx, wy, wz; + bool have; int got = GET_EXTERNAL_G68_FRAME(origin, flat); s->g68_seq_code = 0; @@ -275,14 +378,22 @@ int Interp::work_plane_restore(setup_pointer s) if (got == 0) { return work_plane_cancel(s, true); } if (s->g68_active) { + double def_o[3], def_flat[9], shift[3], kins[9]; bool same = true; - for (int i = 0; i < 3; i++) { here[i] = s->g68_local[i]; } - mat_apply(s->g68_rotation, &here[0], &here[1], &here[2]); + work_plane_defined(s, def_o, def_flat); for (int i = 0; i < 3; i++) { - if (fabs(here[i] + s->g68_offset[i] - origin[i]) > 1e-9) { same = false; } + if (fabs(def_o[i] - origin[i]) > 1e-9) { same = false; } for (int j = 0; j < 3; j++) { - if (fabs(s->g68_rotation[i][j] - flat[3*i + j]) > 1e-12) { same = false; } + if (fabs(def_flat[3*i + j] - flat[3*i + j]) > 1e-12) { same = false; } + } + } + if (s->g68_snap_active && GET_EXTERNAL_G68_KINS_FRAME(shift, kins) > 0) { + for (int i = 0; i < 3; i++) { + if (fabs(s->g68_snap_shift[i] - shift[i]) > 1e-9) { same = false; } + for (int j = 0; j < 3; j++) { + if (fabs(s->g68_snap_rotation[i][j] - kins[3*i + j]) > 1e-12) { same = false; } + } } } if (same) { @@ -292,6 +403,12 @@ int Interp::work_plane_restore(setup_pointer s) } program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + CHP(snap_standing(s, joints, &have, true)); + if (have) { + CHP(snap_from_machine(s, joints)); + } else { + s->g68_snap_active = false; + } for (int i = 0; i < 3; i++) { s->g68_offset[i] = origin[i]; s->g68_local[i] = 0.0; @@ -576,7 +693,6 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) #include #include -#include "units.h" #include #include @@ -826,6 +942,176 @@ int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const doub return INTERP_OK; } +//---------------------------------------------------------------------- +// The second feature coordinate system. Under the machine frame type a +// plane is cut where the primary type, the one that follows the part, +// would cut it with the rotaries held where they stood when the plane was +// defined or the tool oriented to it: Fanuc's second feature coordinate +// system, 3+2 with the slides alone moving. With the rotaries held the +// map from the primary type's world to the machine frame is rigid, +// Q = forward_machine(inverse_primary(W)), and it carries the tool length +// along the tilted tool, which the primary type applies and the machine +// frame type does not. A rotary word does not take it again: the moves +// after it keep the frame, as Fanuc's do. +//---------------------------------------------------------------------- + +// The primary type the plane is read through, where the type in force is +// the machine frame type and does not itself follow the part; -1 where the +// plane is in the frame of the type in force. +int Interp::snap_primary_type(setup_pointer s) +{ + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + + if (flags < 0 || !(flags & KINSTYPE_MACHINE) || (flags & KINSTYPE_PRIMARY)) { return -1; } + return flagged_kins_type(KINSTYPE_PRIMARY); +} + +// The joints a snapshot taken now is taken at: where the program is, or +// from_machine where the machine stands, for a plane read back from status, +// which the program's point may not have reached yet. have stays false +// where none is wanted or nothing can be evaluated here. +int Interp::snap_standing(setup_pointer s, double *joints, bool *have, bool from_machine) +{ + void *vctx; + + *have = false; + if (snap_primary_type(s) < 0) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + if (!vctx) { return INTERP_OK; } + if (from_machine) { + int n = GET_EXTERNAL_JOINT_POSITIONS(joints, EMCMOT_MAX_JOINTS); + if (n > 0) { + for (int i = n; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = 0.0; } + *have = true; + return INTERP_OK; + } + } + CHP(current_joints(s, vctx, joints)); + *have = true; + return INTERP_OK; +} + +// Take the second feature coordinate system at the joints given, with the +// tool offset the program is under. Nothing changes on an error. +int Interp::snap_take(setup_pointer s, const double *joints) +{ + void *vctx; + KinematicsUserContext *ctx; + const int primary = snap_primary_type(s); + double held[4][EMCMOT_MAX_JOINTS], qv[4][3], rotation[3][3], shift[3]; + EmcPose base, q[4]; + int k, i, j, njoints, pass, status = 0; + + if (primary < 0) { + s->g68_snap_active = false; + return INTERP_OK; + } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx) { + s->g68_snap_active = false; + return INTERP_OK; + } + njoints = kinematicsUserGetNumJoints(ctx); + CHKS((kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + // the part as the primary type has it, at the point the joints place + // and a unit along each axis from it, the rotaries held + current_machine_pose(s, &base); + if (kinematicsUserForward(ctx, joints, &base) != 0) { status = -1; } + for (k = 0; status == 0 && k < 4; k++) { + EmcPose pose = base; + + pose.tran.x += (k == 1); + pose.tran.y += (k == 2); + pose.tran.z += (k == 3); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { held[k][i] = joints[i]; } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = held[k][i]; } + if (kinematicsUserInverse(ctx, &pose, held[k]) != 0) { + status = -1; + break; + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(held[k][i] - prev[i])); } + if (worst < 1e-9) { break; } + } + } + // and the same joints in the machine frame + kinematicsUserSetType(ctx, s->kins_type); + for (k = 0; status == 0 && k < 4; k++) { + q[k] = base; + if (kinematicsUserForward(ctx, held[k], &q[k]) != 0) { status = -1; } + } + CHKS((status != 0), + _("the kinematics cannot carry the tilted work plane from type %d onto the machine frame"), primary); + for (k = 0; k < 4; k++) { + for (j = 0; j < njoints; j++) { + double turn; + if (!(s->kins_angular_joints & (1 << j))) { continue; } + turn = fmod(fabs(held[k][j] - joints[j]), 360.0); + CHKS((fmin(turn, 360.0 - turn) > 1e-6), + _("kinematics type %d turns joint %d to follow the part, so a tilted work plane" + " cannot be held under the machine frame type"), primary, j); + } + } + for (k = 0; k < 4; k++) { + qv[k][0] = q[k].tran.x; + qv[k][1] = q[k].tran.y; + qv[k][2] = q[k].tran.z; + } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { rotation[i][j] = qv[j + 1][i] - qv[0][i]; } + } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + double dot = rotation[0][i]*rotation[0][j] + rotation[1][i]*rotation[1][j] + + rotation[2][i]*rotation[2][j]; + CHKS((fabs(dot - (i == j)) > 1e-6), + _("kinematics type %d does not carry the part rigidly onto the machine frame" + " with the rotaries held, so a tilted work plane cannot be cut under the" + " machine frame type"), primary); + } + } + shift[0] = base.tran.x; + shift[1] = base.tran.y; + shift[2] = base.tran.z; + mat_apply(rotation, &shift[0], &shift[1], &shift[2]); + for (i = 0; i < 3; i++) { shift[i] = qv[0][i] - shift[i]; } + memcpy(s->g68_snap_rotation, rotation, sizeof(rotation)); + memcpy(s->g68_snap_shift, shift, sizeof(shift)); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->g68_snap_joints[i] = joints[i]; } + s->g68_snap_active = true; + return INTERP_OK; +} + +// the joints a kinematics switch keeps, read while the type they are known +// in is in force, for the plane to be carried over the switch +int Interp::snap_before_switch(setup_pointer s, double *joints, bool *have) +{ + void *vctx; + + *have = false; + if (!s->g68_active) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + if (!vctx) { return INTERP_OK; } + CHP(current_joints(s, vctx, joints)); + *have = true; + return INTERP_OK; +} + +// After the switch: the second feature coordinate system where the new type +// wants one, taken at the joints the switch kept, and canon told. +int Interp::snap_after_switch(setup_pointer s, const double *joints, bool have) +{ + if (!s->g68_active) { return INTERP_OK; } + s->g68_snap_active = false; + if (have) { CHP(snap_take(s, joints)); } + work_plane_send(s); + return INTERP_OK; +} + // a direction the program gives, in world coordinates: through the // tilted work plane's rotation where one is active, then the XY rotation // of the coordinate system it sits on @@ -875,6 +1161,7 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) PmRotationMatrix work, tool, in_work; PmCartesian zt, xt, yt, zm, xm, x, y; double origin[3], rotation[3][3], r, t, along; + int primary, identity, framed; CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), _("Cannot define a tilted work plane with cutter radius compensation on")); @@ -882,11 +1169,20 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) _("Cancel the G52 in the tilted work plane (G52 X0 Y0 Z0) before G68.3")); CHP(kins_context(s, &vctx)); ctx = (KinematicsUserContext *)vctx; - CHKS((kinematicsUserIsIdentity(ctx)), - _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); CHP(current_joints(s, ctx, joints)); - CHKS((kinematicsUserWorkFrame(ctx, joints, &work) != 0 - || kinematicsUserToolFrame(ctx, joints, &tool) != 0), + // under the machine frame type the tool is read on the part, as the + // primary type has it + primary = snap_primary_type(s); + CHKS((primary >= 0 && kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + identity = kinematicsUserIsIdentity(ctx); + framed = !identity + && kinematicsUserWorkFrame(ctx, joints, &work) == 0 + && kinematicsUserToolFrame(ctx, joints, &tool) == 0; + kinematicsUserSetType(ctx, s->kins_type); + CHKS((identity), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHKS((!framed), _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); toolFrameInWork(&work, &tool, &in_work); zt = in_work.z; @@ -965,7 +1261,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) PmCartesian axis, xdir; EmcPose end_pose; double end_prog[9]; - int p, q, i; + int p, q, i, primary, identity, solved; const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); @@ -980,24 +1276,50 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHP(kins_context(s, &vctx)); ctx = (KinematicsUserContext *)vctx; - CHKS((kinematicsUserIsIdentity(ctx)), - _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); CHP(current_joints(s, ctx, now)); - plane_axis_in_world(s, 2, s->rotation_xy, &axis); - plane_axis_in_world(s, 0, s->rotation_xy, &xdir); - CHP(orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name)); - - // where that puts the machine, and what the program calls it + // under the machine frame type the tool is oriented on the part, as the + // primary type has it, every rotary free to help + primary = snap_primary_type(s); + CHKS((primary >= 0 && kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + identity = kinematicsUserIsIdentity(ctx); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + solved = INTERP_OK; + if (!identity) { + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + solved = orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name); + } + kinematicsUserSetType(ctx, s->kins_type); + CHKS((identity), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHP(solved); + + // where that puts the machine, and what the program calls it: in the + // second feature coordinate system taken at the rotaries it ends at. + // STAY moves nothing, so its own is put back after. + bool kept_active = s->g68_snap_active; + double kept_rotation[3][3], kept_shift[3], kept_joints[EMCMOT_MAX_JOINTS]; + memcpy(kept_rotation, s->g68_snap_rotation, sizeof(kept_rotation)); + memcpy(kept_shift, s->g68_snap_shift, sizeof(kept_shift)); + memcpy(kept_joints, s->g68_snap_joints, sizeof(kept_joints)); end_pose = (EmcPose){}; current_machine_pose(s, &end_pose); CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), _("%s: the kinematics cannot place the orientation it found"), name); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + if (primary >= 0) { CHP(snap_take(s, sol)); } machine_pose_to_program(s, &end_pose, end_prog); - - int orienting[3]; - int count = orienting_letters(ctx, now, orienting); + if (code == G_53_2) { + s->g68_snap_active = kept_active; + memcpy(s->g68_snap_rotation, kept_rotation, sizeof(kept_rotation)); + memcpy(s->g68_snap_shift, kept_shift, sizeof(kept_shift)); + memcpy(s->g68_snap_joints, kept_joints, sizeof(kept_joints)); + } else if (primary >= 0) { + work_plane_send(s); + } if (code == G_53_2) { // STAY: solve only, nothing moves. X Y Z and the rotaries in the diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 1ee24a796f6..15a459bb41e 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -405,6 +405,15 @@ public: void current_machine_pose(setup_pointer settings, EmcPose *pose); void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); int current_joints(setup_pointer settings, void *ctx, double *joints); + int snap_primary_type(setup_pointer settings); + int snap_standing(setup_pointer settings, double *joints, bool *have, bool from_machine); + int snap_from_machine(setup_pointer settings, const double *joints); + int snap_take(setup_pointer settings, const double *joints); + int snap_before_switch(setup_pointer settings, double *joints, bool *have); + int snap_after_switch(setup_pointer settings, const double *joints, bool have); + void work_plane_defined(setup_pointer settings, double origin[3], double rotation[9]); + void work_plane_sent(setup_pointer settings, double origin[3], double rotation[9]); + void work_plane_send(setup_pointer settings); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index a7547f0ed69..fb0ddb0ae88 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -112,7 +112,11 @@ void HOME_CYCLE(void) { ECHO_WITH_ARGS(""); } void HOME_CYCLE_JOINT(int joint) { ECHO_WITH_ARGS("{}", joint); } void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) { + const double rotation[9], int active, + const double * /*defined_origin*/, + const double * /*defined_rotation*/, + const double * /*kins_shift*/, + const double * /*kins_rotation*/) { ECHO_WITH_ARGS("{:.4f}, {:.4f}, {:.4f}, " "[{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}], {}", x, y, z, @@ -860,6 +864,11 @@ extern int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) return -1; } +extern int GET_EXTERNAL_G68_KINS_FRAME(double * /*shift*/, double * /*rotation*/) +{ + return -1; +} + extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() { return true; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index c7745947b6f..2cfa7fee513 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -568,7 +568,11 @@ void HOME_CYCLE_JOINT(int joint) } void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) + const double rotation[9], int active, + const double defined_origin[3], + const double defined_rotation[9], + const double kins_shift[3], + const double kins_rotation[9]) { flush_segments(); @@ -578,12 +582,18 @@ void SET_G68_FRAME(double x, double y, double z, for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } canon.g68Active = active; + // status has the plane as defined and the second feature coordinate + // system apart, as the standard controls report them auto msg = std::make_unique(); - msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); - msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); - msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); - for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->origin.tran.x = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[0])); + msg->origin.tran.y = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[1])); + msg->origin.tran.z = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[2])); + for (int i = 0; i < 9; i++) { msg->rotation[i] = defined_rotation[i]; } msg->active = active; + msg->kins_offset.tran.x = kins_shift[0]; + msg->kins_offset.tran.y = kins_shift[1]; + msg->kins_offset.tran.z = kins_shift[2]; + for (int i = 0; i < 9; i++) { msg->kins_rotation[i] = kins_rotation[i]; } interp_list.append(std::move(msg)); } @@ -4357,6 +4367,15 @@ int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]) return 1; } +int GET_EXTERNAL_G68_KINS_FRAME(double shift[3], double rotation[9]) +{ + shift[0] = emcStatus->task.g68_kins_offset.tran.x; + shift[1] = emcStatus->task.g68_kins_offset.tran.y; + shift[2] = emcStatus->task.g68_kins_offset.tran.z; + for (int i = 0; i < 9; i++) { rotation[i] = emcStatus->task.g68_kins_rotation[i]; } + return 1; +} + bool GET_EXTERNAL_KINEMATICS_IDENTITY() { return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index e134d2c6a32..b7f88a6bfbc 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1715,20 +1715,28 @@ int emcTaskQueueCommand(std::unique_ptr &&cmd) // The tilted work plane's axes in world coordinates, for motion to jog // along: the plane's rotation turned by the XY rotation of the coordinate -// system it was defined in, as canon applies the two. Motion hears only -// of a change, so a program without a plane sends it nothing +// system it was defined in, as canon applies the two, and then by the +// second feature coordinate system where the plane is cut through one. +// Motion hears only of a change, so a program without a plane sends it +// nothing static int sendWorkPlane() { static double sent[9]; static int sent_active = 0; const double *g = emcStatus->task.g68_rotation; - double t = emcStatus->task.rotation_xy * M_PI / 180.0, c = cos(t), s = sin(t), r[9]; + const double *k = emcStatus->task.g68_kins_rotation; + double t = emcStatus->task.rotation_xy * M_PI / 180.0, c = cos(t), s = sin(t), q[9], r[9]; int active = emcStatus->task.g68_active, retval; for (int j = 0; j < 3; j++) { - r[j] = c * g[j] - s * g[3 + j]; - r[3 + j] = s * g[j] + c * g[3 + j]; - r[6 + j] = g[6 + j]; + q[j] = c * g[j] - s * g[3 + j]; + q[3 + j] = s * g[j] + c * g[3 + j]; + q[6 + j] = g[6 + j]; + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[3*i + j] = k[3*i] * q[j] + k[3*i + 1] * q[3 + j] + k[3*i + 2] * q[6 + j]; + } } if (active == sent_active && (!active || std::equal(r, r + 9, sent))) { return 0; @@ -2064,6 +2072,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcStatus->task.g68_offset = g68->origin; for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } emcStatus->task.g68_active = g68->active; + emcStatus->task.g68_kins_offset = g68->kins_offset; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_kins_rotation[i] = g68->kins_rotation[i]; } retval = sendWorkPlane(); break; } diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 6386a145596..469d4bc17ee 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1300,6 +1300,18 @@ static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { return res; } +static PyObject *Stat_g68_kins_offset(pyStatChannel *s, void *) { + return pose(s->status.task.g68_kins_offset); +} + +static PyObject *Stat_g68_kins_rotation(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.task.g68_kins_rotation[i])); + } + return res; +} + static PyObject *Stat_kins_type_flags(pyStatChannel *s, void *) { PyObject *res = PyTuple_New(SWITCHKINS_MAX_TYPES); for (int i = 0; i < SWITCHKINS_MAX_TYPES; i++) { @@ -1623,6 +1635,10 @@ static PyGetSetDef Stat_getsetlist[] = { (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, + {(char*)"g68_kins_offset", (getter)Stat_g68_kins_offset, NULL, + (char*)"Shift of the second feature coordinate system a tilted work plane is cut through under the machine frame kinematics (3+2), in machine units; zero where there is none.", NULL}, + {(char*)"g68_kins_rotation", (getter)Stat_g68_kins_rotation, NULL, + (char*)"Rotation of that second feature coordinate system, nine values row by row; the identity where there is none.", NULL}, {(char*)"kins_type_flags", (getter)Stat_kins_type_flags, NULL, (char*)"What the module declares each kinematics type to be, one entry per type: KINSTYPE_ flags, or -1 for a type it does not provide.", NULL}, {(char*)"tool_frame", (getter)Stat_tool_frame, NULL, diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index bd47864a39c..7f168417d1b 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -2043,10 +2043,24 @@ static void modify_hal_pins() } // the relative position: the offset chain taken off in reverse, the - // tool offset, G5x, the XY rotation, G92 and the tilted work plane - double rx = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double ry = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - double rz = emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.toolOffset.tran.z; + // second feature coordinate system of 3+2 under the machine frame + // kinematics, the tool offset, G5x, the XY rotation, G92 and the + // tilted work plane + double rx = emcStatus->motion.traj.actualPosition.tran.x; + double ry = emcStatus->motion.traj.actualPosition.tran.y; + double rz = emcStatus->motion.traj.actualPosition.tran.z; + if (emcStatus->task.g68_active) { + const double *k = emcStatus->task.g68_kins_rotation; + double x = rx - emcStatus->task.g68_kins_offset.tran.x; + double y = ry - emcStatus->task.g68_kins_offset.tran.y; + double z = rz - emcStatus->task.g68_kins_offset.tran.z; + rx = k[0]*x + k[3]*y + k[6]*z; + ry = k[1]*x + k[4]*y + k[7]*z; + rz = k[2]*x + k[5]*y + k[8]*z; + } + rx -= emcStatus->task.g5x_offset.tran.x + emcStatus->task.toolOffset.tran.x; + ry -= emcStatus->task.g5x_offset.tran.y + emcStatus->task.toolOffset.tran.y; + rz -= emcStatus->task.g5x_offset.tran.z + emcStatus->task.toolOffset.tran.z; { double t = -emcStatus->task.rotation_xy * TO_RAD; double x = rx * cos(t) - ry * sin(t); diff --git a/tests/twp-static-head/README b/tests/twp-static-head/README new file mode 100644 index 00000000000..07cd3aed7e8 --- /dev/null +++ b/tests/twp-static-head/README @@ -0,0 +1,2 @@ +twp-static on the head machine, 5axiskins with a 100 mm pivot (head.ini): see +twp-static/README. diff --git a/tests/twp-static-head/checkresult b/tests/twp-static-head/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static-head/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static-head/head.hal b/tests/twp-static-head/head.hal new file mode 120000 index 00000000000..ac7a6410758 --- /dev/null +++ b/tests/twp-static-head/head.hal @@ -0,0 +1 @@ +../twp-static/head.hal \ No newline at end of file diff --git a/tests/twp-static-head/head.ini b/tests/twp-static-head/head.ini new file mode 120000 index 00000000000..4359ba23a1f --- /dev/null +++ b/tests/twp-static-head/head.ini @@ -0,0 +1 @@ +../twp-static/head.ini \ No newline at end of file diff --git a/tests/twp-static-head/test-ui.py b/tests/twp-static-head/test-ui.py new file mode 120000 index 00000000000..c59b81469df --- /dev/null +++ b/tests/twp-static-head/test-ui.py @@ -0,0 +1 @@ +../twp-static/test-ui.py \ No newline at end of file diff --git a/tests/twp-static-head/test.sh b/tests/twp-static-head/test.sh new file mode 100755 index 00000000000..eaf0a5bac2d --- /dev/null +++ b/tests/twp-static-head/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r head.ini diff --git a/tests/twp-static-head/tool.tbl b/tests/twp-static-head/tool.tbl new file mode 120000 index 00000000000..787a724629c --- /dev/null +++ b/tests/twp-static-head/tool.tbl @@ -0,0 +1 @@ +../twp-static/tool.tbl \ No newline at end of file diff --git a/tests/twp-static-preview-head/README b/tests/twp-static-preview-head/README new file mode 100644 index 00000000000..69194ca62b1 --- /dev/null +++ b/tests/twp-static-preview-head/README @@ -0,0 +1,2 @@ +twp-static-preview on the head machine, 5axiskins with a 100 mm pivot (head.ini): see +twp-static-preview/README. diff --git a/tests/twp-static-preview-head/checkresult b/tests/twp-static-preview-head/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static-preview-head/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static-preview-head/head.hal b/tests/twp-static-preview-head/head.hal new file mode 120000 index 00000000000..ac7a6410758 --- /dev/null +++ b/tests/twp-static-preview-head/head.hal @@ -0,0 +1 @@ +../twp-static/head.hal \ No newline at end of file diff --git a/tests/twp-static-preview-head/head.ini b/tests/twp-static-preview-head/head.ini new file mode 120000 index 00000000000..4359ba23a1f --- /dev/null +++ b/tests/twp-static-preview-head/head.ini @@ -0,0 +1 @@ +../twp-static/head.ini \ No newline at end of file diff --git a/tests/twp-static-preview-head/test-ui.py b/tests/twp-static-preview-head/test-ui.py new file mode 120000 index 00000000000..29abc6e0ef5 --- /dev/null +++ b/tests/twp-static-preview-head/test-ui.py @@ -0,0 +1 @@ +../twp-static-preview/test-ui.py \ No newline at end of file diff --git a/tests/twp-static-preview-head/test.sh b/tests/twp-static-preview-head/test.sh new file mode 100755 index 00000000000..eaf0a5bac2d --- /dev/null +++ b/tests/twp-static-preview-head/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r head.ini diff --git a/tests/twp-static-preview-head/tool.tbl b/tests/twp-static-preview-head/tool.tbl new file mode 120000 index 00000000000..787a724629c --- /dev/null +++ b/tests/twp-static-preview-head/tool.tbl @@ -0,0 +1 @@ +../twp-static/tool.tbl \ No newline at end of file diff --git a/tests/twp-static-preview/README b/tests/twp-static-preview/README new file mode 100644 index 00000000000..907801e2e8d --- /dev/null +++ b/tests/twp-static-preview/README @@ -0,0 +1,17 @@ +Status, a plane jog, an abort and the preview under the machine frame +kinematics (G13.1), on the table machine of twp-static, whose INI and HAL +files and helpers it shares; twp-static-preview-head runs it on the head +machine. + +Status keeps the plane as defined, the same under the TCP kinematics and +under G13.1, and the second feature coordinate system apart, the identity +except under G13.1; a plane X jog under G13.1 goes where G1 X+5 in the plane +does. + +table.ini sets RETAIN_WORK_PLANE: a program that goes on to another plane is +aborted during its first move, and the plane the machine was in comes back +onto the part it was defined on. + +Last the preview: after a plane set in MDI, with the tool in the spindle, +after a plane set in MDI and a rotary turned since, and with a plane in the +program itself, the preview ends where the machine does. diff --git a/tests/twp-static-preview/checkresult b/tests/twp-static-preview/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static-preview/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static-preview/table.hal b/tests/twp-static-preview/table.hal new file mode 120000 index 00000000000..8d42f68e380 --- /dev/null +++ b/tests/twp-static-preview/table.hal @@ -0,0 +1 @@ +../twp-static/table.hal \ No newline at end of file diff --git a/tests/twp-static-preview/table.ini b/tests/twp-static-preview/table.ini new file mode 120000 index 00000000000..a612af8cca7 --- /dev/null +++ b/tests/twp-static-preview/table.ini @@ -0,0 +1 @@ +../twp-static/table.ini \ No newline at end of file diff --git a/tests/twp-static-preview/test-ui.py b/tests/twp-static-preview/test-ui.py new file mode 100755 index 00000000000..c7143902125 --- /dev/null +++ b/tests/twp-static-preview/test-ui.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 +# Status, a plane jog, an abort and the preview under the machine frame type: see README. + +import os +import sys +import gcode +import preview_helpers +from rs274.interpret import Translated +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "twp-static")) +from static import * + +# Status has the plane as defined under every type, and the second feature +# coordinate system apart, as Siemens keeps its workpiece reference frame +# apart from its table and tool ones: a switch changes only the latter. +IDENTITY = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) + +def plane_status(): + s.poll() + return (tuple(s.g68_offset[:3]), tuple(s.g68_rotation), + tuple(s.g68_kins_offset[:3]), tuple(s.g68_kins_rotation)) + +def near(a, b): + return max(abs(p - q) for p, q in zip(a, b)) < 1e-9 + +run(TCP, [plane, "G53.1"]) +on_tcp = plane_status() +mdi("G13.1") +on_fixed = plane_status() +mdi(TCP) +back = plane_status() +if not near(on_tcp[0], (10, -5, 0)): + error("status has the plane's origin at %s, not X10 Y-5 Z0" % show(on_tcp[0])) +for name, got in (("under G13.1", on_fixed), ("back on the part's type", back)): + if not near(got[0], on_tcp[0]) or not near(got[1], on_tcp[1]): + error("%s status has the plane at %s %s, not %s %s" + % (name, show(got[0]), show(got[1]), show(on_tcp[0]), show(on_tcp[1]))) +for name, got in (("on the part's type", on_tcp), ("back on the part's type", back)): + if not near(got[2], (0, 0, 0)) or not near(got[3], IDENTITY): + error("%s status has a second feature coordinate system %s %s" + % (name, show(got[2]), show(got[3]))) +if near(on_fixed[2], (0, 0, 0)) and near(on_fixed[3], IDENTITY): + error("under G13.1 status has no second feature coordinate system") + +# The plane jog frame under G13.1 is the plane turned by the second feature +# coordinate system: a jog along the plane's X goes where G1 X+5 does. +def jog_plane_x(): + c.mode(linuxcnc.MODE_MANUAL) + done() + c.teleop_enable(1) + done() + c.jog_frame(linuxcnc.JOG_FRAME_PLANE) + done() + c.jog(linuxcnc.JOG_INCREMENT, False, 0, 500, 5) + done() + # in teleop motion keeps no in position: the joints still is the end + after = still() + drain() + c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) + done() + c.mode(linuxcnc.MODE_MDI) + done() + return after + +cut_x = run("G13.1", [plane, "G53.1", "G0 X10 Y5 Z20", "G91 G1 X5 F60000", "G90"])[-1] +run("G13.1", [plane, "G53.1", "G0 X10 Y5 Z20"]) +jogged = jog_plane_x() +# a jog stops within its planner's dead band of the end, the acceleration +# times the servo period squared over a thousand, on each axis +band = 2 * float(ini.find("AXIS_X", "MAX_ACCELERATION")) * (float(ini.find("EMCMOT", "SERVO_PERIOD")) * 1e-9) ** 2 / 1000 +if max(abs(p - q) for p, q in zip(jogged, cut_x)) > band: + error("a plane X jog under G13.1 ended at %s, G1 X+5 at %s" % (show(jogged), show(cut_x))) + +# An abort with RETAIN_WORK_PLANE keeps the plane the machine was in, and +# its second feature coordinate system, while the read ahead has gone on +# to another: the machine's plane comes back onto the part it was +# defined on. +if ini.find("RS274NGC", "RETAIN_WORK_PLANE") == "1": + want = run("G13.1", [plane, "G53.1", "G1 X-10 Y5 Z20 F60000"])[-1] + run("G13.1", [plane, "G53.1", "G1 X10 Y5 Z20 F60000"]) + with open("abort.ngc", "w") as f: + f.write("G1 X-200 F60\nG69\nG68.2 X0 Y0 Z0 I0 J45 K0\nG1 X0\nM2\n") + c.mode(linuxcnc.MODE_AUTO) + done() + c.program_open("abort.ngc") + done() + c.auto(linuxcnc.AUTO_RUN, 0) + # the read ahead past the other plane, the machine on its first move + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + if s.read_line >= 4 and s.motion_line == 1 and s.current_vel > 0: + break + time.sleep(0.002) + else: + error("abort.ngc never read ahead past its plane: line %d, the machine on %d" + % (s.read_line, s.motion_line)) + c.abort() + done() + settled() + drain() + c.mode(linuxcnc.MODE_MDI) + done() + got = mdi("G1 X-10 Y5 Z20 F60000") + if not same(got, want): + error("after an abort the plane put X-10 Y5 Z20 at %s, not %s" % (show(got), show(want))) + os.unlink("abort.ngc") + +# The preview: a program after a plane set in MDI starts from the plane +# as status has it, folded into the second feature coordinate system, +# and one with its own plane takes its own; both end where the machine +# does, which the preview draws without the tool offset. +class Canon(Translated): + def __init__(self): + self.parameter_file = "preview.var" + self.end = None + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def straight_feed(self, *a): + self.end = self.rotate_and_translate(*a)[:3] + def get_tool(self, pocket): + return tuple(s.tool_table[pocket]) if 0 <= pocket < len(s.tool_table) else (-1,) + (0,) * 13 + def get_external_length_units(self): + return s.linear_units + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return s.axis_mask + def get_block_delete(self): + return 0 + def get_external_joint_positions(self): + return tuple(s.joint_actual_position[:s.joints]) + def get_external_g68_kins_frame(self): + return (tuple(s.g68_kins_offset[:3]), tuple(s.g68_kins_rotation)) + def get_kinematics_type(self): + return s.kinematics_type + def get_kins_type(self): + return s.kins_type + def get_kins_type_flags(self): + return s.kins_type_flags + +def preview_case(name, prog, text): + with open(prog, "w") as f: + f.write(text) + s.poll() + canon = Canon() + res, seq = gcode.parse(prog, canon, preview_helpers.preview_initcodes(s, ini)) + if res > gcode.MIN_ERROR: + error("%s: the preview stops at line %d: %s" % (name, seq, gcode.strerror(res).strip())) + return + c.mode(linuxcnc.MODE_AUTO) + done() + c.program_open(prog) + done() + c.auto(linuxcnc.AUTO_RUN, 0) + done() + settled() + drain() + c.mode(linuxcnc.MODE_MDI) + done() + s.poll() + machine = [s.position[i] - s.tool_offset[i] for i in range(3)] + # the preview works in inches + end = [25.4 * v for v in canon.end] + if max(abs(p - q) for p, q in zip(end, machine)) > 1e-6: + error("%s: the preview ends at %s, the machine at %s" % (name, end, machine)) + os.unlink(prog) + +# the tool in the spindle, so that the G43 the preview restates has its length +for cmd in (RESET, START, "T1 M6", "G43", plane, "G53.1"): + mdi(cmd) +preview_case("plane from MDI", "mdi-plane.ngc", + "G1 X-10 Y5 Z20 F60000\nG1 Z-5\nM2\n") +# a rotary turned since the plane was set keeps the second feature +# coordinate system it was cut through, which the preview takes from status +for cmd in (RESET, START, "T1 M6", "G43", plane, "G53.1", + "G91 G0 %s30" % SECOND, "G90"): + mdi(cmd) +preview_case("plane from MDI, a rotary turned since", "mdi-turned.ngc", + "G1 X-10 Y5 Z20 F60000\nG1 Z-5\nM2\n") +for cmd in (RESET, START): + mdi(cmd) +preview_case("plane in the program", "own-plane.ngc", + "G43 H1\n%s\nG53.1\nG1 X-10 Y5 Z20 F60000\nG1 Z-5\nM2\n" % plane) + +finish() diff --git a/tests/twp-static-preview/test.sh b/tests/twp-static-preview/test.sh new file mode 100755 index 00000000000..a3fcfe97627 --- /dev/null +++ b/tests/twp-static-preview/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r table.ini diff --git a/tests/twp-static-preview/tool.tbl b/tests/twp-static-preview/tool.tbl new file mode 120000 index 00000000000..787a724629c --- /dev/null +++ b/tests/twp-static-preview/tool.tbl @@ -0,0 +1 @@ +../twp-static/tool.tbl \ No newline at end of file diff --git a/tests/twp-static/README b/tests/twp-static/README new file mode 100644 index 00000000000..39f346b6bcc --- /dev/null +++ b/tests/twp-static/README @@ -0,0 +1,18 @@ +3+2 under the machine frame kinematics, Fanuc's second feature coordinate +system, on a table machine (xyzac-trt-kins, table.ini); twp-static-head +runs it on a head machine (5axiskins, head.ini). The INI and HAL files and +the helpers (static.py) are shared with twp-static-preview and their -head twins. + +The same program runs under the TCP kinematics and under the machine frame +kinematics (G13.1), from the same joints: a plane, G53.1, G53.6 or G68.3, +then moves and an arc in the plane, with no tool, a tool given before the +plane and a tool given in it. The machine goes to the same joints every +time: under G13.1 the plane is on the part as the TCP kinematics places it, +with the tool along the tilted tool. + +Then, under G13.1 only: a C word moves C alone and the frame stays where it +was taken (the slides do not move and a move to the same point stays put); +G68.4 with nothing in it after the turn lands where the TCP kinematics does +after the same turn; G53 goes to machine coordinates and G69 leaves plain +machine moves; a switch to the TCP kinematics and back in the plane keeps +the plane on the part. diff --git a/tests/twp-static/checkresult b/tests/twp-static/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static/head.hal b/tests/twp-static/head.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-static/head.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-static/head.ini b/tests/twp-static/head.ini new file mode 100644 index 00000000000..ad721ee5150 --- /dev/null +++ b/tests/twp-static/head.ini @@ -0,0 +1,131 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, 1 is identity, 2 is the userk template +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = head.hal +HALCMD = setp 5axiskins.pivot-length 100 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 10000 +MAX_LINEAR_VELOCITY = 10000 +MAX_LINEAR_ACCELERATION = 1000000 +DEFAULT_LINEAR_ACCELERATION = 1000000 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/twp-static/static.py b/tests/twp-static/static.py new file mode 100644 index 00000000000..4efe0862386 --- /dev/null +++ b/tests/twp-static/static.py @@ -0,0 +1,123 @@ +# The machine, the helpers and the finish that the twp-static tests share: +# see twp-static/README. +# +# The sims move fast, and task runs a 1 ms cycle: most checks look only at +# where a move ends. The tests wait on what c.wait_complete() waits for, +# polled every 2 ms, and on the joints at rest over two status copies. + +import linuxcnc +import sys +import os +import time + +ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) +HEAD = ini.find("KINS", "KINEMATICS").startswith("5axiskins") +JOINTS = int(ini.find("KINS", "JOINTS")) +# the type that follows the part, and the rotaries' letters +TCP = "G12.1 P0" if HEAD else "G12.1 P1" +FIRST, SECOND = ("B", "C") if HEAD else ("A", "C") +START = "G53 G0 X20 Y-10 Z120 B0 C0" if HEAD else "G53 G0 X20 Y-10 Z120 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(expect_error=False): + got = False + while True: + m = e.poll() + if not m: + return got + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + got = True + if not expect_error: + error("reported: %s" % m[1]) + +# what c.wait_complete() waits for, task done with the last command sent, +# looked at every 2 ms rather than every 10 +def done(timeout=60): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if s.echo_serial_number >= c.serial and s.state != linuxcnc.RCS_EXEC: + return s.state + time.sleep(0.002) + error("timed out waiting for task") + return s.state + +# the joints still over two status copies a servo cycle or more apart, on a +# copy written two task cycles after the command: two polls can read the +# same copy +def still(ready=lambda: True): + deadline = time.time() + 60 + s.poll() + beat = s.taskbeat + 2 + last = None + last_hb = None + while time.time() < deadline: + s.poll() + if s.taskbeat >= beat: + now = [s.joint_position[i] for i in range(JOINTS)] + if ready() and now == last and s.heartbeat != last_hb: + return now + last = now + last_hb = s.heartbeat + time.sleep(0.002) + error("timed out waiting for the move") + return last + +# the move has ended when task has nothing left to run and motion is at +# rest; in position with an empty queue alone is also true before task has +# sent the move on +def settled(): + return still(lambda: s.interp_state == linuxcnc.INTERP_IDLE and s.inpos and not s.queue) + +def mdi(cmd): + c.mdi(cmd) + done() + j = settled() + drain() + return j + +# no tool, no plane, the machine frame type, in one block +RESET = "G49 G69 G13.1" + +def run(select, steps): + # from one machine point, on the type asked for, the joints after each step + for cmd in (RESET, START, select): + mdi(cmd) + return [mdi(cmd) for cmd in steps] + +def same(a, b, n=None): + n = JOINTS if n is None else n + return max(abs(p - q) for p, q in zip(a[:n], b[:n])) < 1e-6 + +def show(j): + return " ".join("%.6f" % v for v in j) + +plane = "G68.2 X10 Y-5 Z0 I60 J30 K0" + +def finish(): + mdi("G69") + mdi("G49") + for f in ("sim.var", "sim.var.bak", "preview.var"): + try: + os.unlink(f) + except OSError: + pass + print("Exiting with %d errors" % errors) + sys.exit(1 if errors else 0) diff --git a/tests/twp-static/table.hal b/tests/twp-static/table.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/twp-static/table.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-static/table.ini b/tests/twp-static/table.ini new file mode 100644 index 00000000000..4dcf1479242 --- /dev/null +++ b/tests/twp-static/table.ini @@ -0,0 +1,123 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var +RETAIN_WORK_PLANE = 1 + +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = table.hal +HALCMD = setp xyzac-trt-kins.x-rot-point 100 +HALCMD = setp xyzac-trt-kins.y-rot-point -50 +HALCMD = setp xyzac-trt-kins.z-rot-point -80 + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 10000 +MAX_LINEAR_VELOCITY = 10000 +MAX_LINEAR_ACCELERATION = 1000000 +DEFAULT_LINEAR_ACCELERATION = 1000000 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 + +[AXIS_A] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 + +[AXIS_C] +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +MAX_JERK = 100000000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +MAX_JERK = 100000000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 10000 +MAX_ACCELERATION = 1000000 +MAX_JERK = 100000000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 +MAX_JERK = 360000000 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 36000 +MAX_ACCELERATION = 3600000 +MAX_JERK = 360000000 +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +HOME_SEQUENCE = 0 diff --git a/tests/twp-static/test-ui.py b/tests/twp-static/test-ui.py new file mode 100755 index 00000000000..353b5f38784 --- /dev/null +++ b/tests/twp-static/test-ui.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 +# 3+2 under the machine frame type, the same programs under both types: see README. + +import os +import sys +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "twp-static")) +from static import * + +# the same program under the type that follows the part and under the +# machine frame type: the machine goes to the same joints +cut = ["G0 X10 Y5 Z20", "G1 X-10 F60000", "G2 X10 R10 F60000", "G1 Z-5"] +programs = { + "no tool": [plane, "G53.1"] + cut, + "tool first": ["G43 H1", plane, "G53.1"] + cut, + "tool in the plane": [plane, "G53.1", "G43 H1"] + cut, + "G53.6": [plane, "G0 X0 Y0 Z30", "G53.6"] + cut, + "G68.3": [plane, "G53.1", "G69", "G68.3 X10 Y-5 Z0 R20"] + cut, +} +for name, steps in programs.items(): + tcp = run(TCP, steps) + fixed = run("G13.1", steps) + for cmd, a, b in zip(steps, tcp, fixed): + if not same(a, b): + error("%s: after %s the machine frame type is at %s, the part's type at %s" + % (name, cmd, show(b), show(a))) + print("%s: %s" % (name, show(fixed[-1]))) + +# a rotary word moves the rotary alone and the frame stays where the +# plane was taken (Fanuc); a new plane at the turned table follows it +steps = [plane, "G53.1", "G0 X10 Y5 Z20", "G91", "G0 %s30" % SECOND, "G90", + "G0 X10 Y5 Z20"] +fixed = run("G13.1", steps) +if not same(fixed[2], fixed[4], 3): + error("a %s word moved the slides: %s to %s" % (SECOND, show(fixed[2]), show(fixed[4]))) +if not same(fixed[2], fixed[6], 3): + error("after a %s word the frame moved: %s to %s" % (SECOND, show(fixed[2]), show(fixed[6]))) +tcp = run(TCP, [plane, "G53.1", "G91", "G0 %s30" % SECOND, "G90", "G1 X-10 Y5 Z20 F60000"]) +fixed = run("G13.1", [plane, "G53.1", "G91", "G0 %s30" % SECOND, "G90", + "G68.4 X0 Y0 Z0 I0 J0 K0", "G1 X-10 Y5 Z20 F60000"]) +if not same(tcp[-1], fixed[-1]): + error("G68.4 after a %s word: the machine frame type is at %s, the part's type at %s" + % (SECOND, show(fixed[-1]), show(tcp[-1]))) + +# G53 stays in the machine frame, and G69 leaves plain machine moves +fixed = run("G13.1", [plane, "G53.1", "G53 G0 X0 Y0 Z100", "G69", "G0 X10 Y0 Z50"]) +if not same(fixed[2], [0, 0, 100], 3): + error("G53 in the plane went to %s, not X0 Y0 Z100" % show(fixed[2])) +if not same(fixed[4], [10, 0, 50], 3): + error("G0 after G69 went to %s, not X10 Y0 Z50" % show(fixed[4])) + +# the type can change in the plane: on to the part's type and back +tcp = run(TCP, [plane, "G53.1", "G1 X-10 Y5 Z20 F60000"]) +fixed = run("G13.1", [plane, "G53.1", TCP, "G13.1", "G1 X-10 Y5 Z20 F60000"]) +if not same(tcp[-1], fixed[-1]): + error("a switch in the plane: %s, the part's type at %s" % (show(fixed[-1]), show(tcp[-1]))) + +finish() diff --git a/tests/twp-static/test.sh b/tests/twp-static/test.sh new file mode 100755 index 00000000000..a3fcfe97627 --- /dev/null +++ b/tests/twp-static/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r table.ini diff --git a/tests/twp-static/tool.tbl b/tests/twp-static/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-static/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 From bb11797e8d5143fa888f92912e46689a2d357d73 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 27 Oct 2026 01:19:45 +1100 Subject: [PATCH 27/44] preview: draw the tool on the part under the machine frame kinematics Under the machine frame kinematics, which G13.1 selects, the preview drew the slides' coordinates. With the table or the head tilted, the program, the backplot and a tilted work plane were drawn away from where the tool cuts the part, while the tool marker leaned as GEOMETRY has it against the part. Sigma1912 reported it on the xyzab-tdr sim with 3+2: the plane lay flat after G53.1 had turned the table. Under the TCP kinematics the same program was drawn on the part, so a switch moved the whole picture. The standard controls keep the part fixed and move the tool: the Siemens simulation holds the workpiece fixed in space whatever the machine type (milling operating manual 10/2020, 7.1), and the Heidenhain graphic depicts the workpiece as it is machined, with the machine model a view of its own. The primary kinematics type knows where the tool stands against the part for any joints. switchkins exports kinematicsForwardOf(), the forward of a type other than the one in force, and motion reports the primary type's forward of the commanded joints as part_position in status: the commanded position itself under that type, so a switch moves nothing. The backplot draws it minus the tool offset, and the tool marker stands at its end. The preview's interpreter maps each drawn point the same way, outside realtime: the joints the type in force puts the machine in, through the primary type's forward. A rotary move is drawn as the curve the tool sweeps across the part, a move after a rotary word in a plane where it cuts the turned part, and the plane as defined. The extents the axis limits are checked against stay on the slides. The loader read every pin of the kinematics by name for each point, so kinematicsUserHoldPins() reads them once for a preview: 60000 moves preview in 0.10 s under G13.1, against 0.06 s under the TCP kinematics and 7.5 s without the hold. tests/twp-static-preview draws the same 3+2 program under G13.1 and under the TCP kinematics and checks one plane in the same place and the same end of the cut. With no plane it turns a rotary under G13.1 at a tilt and checks the preview through the point the machine passes half way and where it ends, and that the live plot and part_position agree with the TCP kinematics at the same joints. --- docs/src/config/python-interface.adoc | 5 + docs/src/gcode/g-code.adoc | 20 +- include/kinematics.h | 12 ++ include/kinematics_user.h | 12 ++ lib/python/rs274/glcanon_bake.py | 5 +- lib/python/rs274/glcanon_scene.py | 23 +- src/emc/kinematics/switchkins.c | 11 + .../kinematics_userspace/kinematics_user.c | 50 +++-- src/emc/motion/control.c | 31 ++- src/emc/motion/motion.h | 3 + src/emc/nml_intf/emc.cc | 1 + src/emc/nml_intf/emc_nml.hh | 2 + src/emc/nml_intf/emcops.cc | 1 + src/emc/rs274ngc/gcode_renderer.cc | 143 +++++++----- src/emc/rs274ngc/gcode_renderer.hh | 46 ++-- src/emc/rs274ngc/gcodemodule.cc | 62 +++++- src/emc/rs274ngc/gcodemodule.hh | 14 ++ src/emc/rs274ngc/interp_workplane.cc | 87 ++++++++ src/emc/rs274ngc/rs274ngc_interp.hh | 5 + src/emc/task/taskintf.cc | 2 + src/emc/usr_intf/axis/extensions/emcmodule.cc | 29 ++- tests/glcanon/test_workplane.py | 13 ++ tests/twp-static-preview/README | 13 +- tests/twp-static-preview/test-ui.py | 203 ++++++++++++++++-- 24 files changed, 658 insertions(+), 135 deletions(-) diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 28616cc198f..ffd0e25e08d 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -303,6 +303,11 @@ see <> for an example. *optional_stop*:: '(returns integer)' - option stop flag. +*part_position*:: '(returns tuple of floats)' - + the commanded joints as the module's primary kinematics type, the one that follows the part, reads them: where the tool stands against the part, in machine units. + The same as *position*, except under another type of a switchable module, the machine frame type <> selects, where *position* is in machine coordinates. + The backplot of the G-code preview is drawn from it, minus the tool offset. + *paused*:: '(returns boolean)' - `motion paused` flag. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 05f8433e116..36eaf72093f 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -983,6 +983,20 @@ Selecting a kinematics does not move the machine. It changes how joint positions and coordinate positions map onto each other, so the position readout can change even though nothing has moved. +The G-code preview, the backplot and the tool shown in a display stay on +the part whichever kinematics is selected: the module's primary +kinematics, the one that follows the part, places the tool against the +part from the joints, so a switch moves nothing on the screen. Under the +machine frame kinematics with a head or a table tilted, the slides alone +do not say where the tool cuts the part; the preview draws each move +where it does, as the primary kinematics would, and a rotary move as the +curve the tool sweeps across the part. This is what the standard controls +show: the Siemens simulation keeps the workpiece fixed and moves only the +tool, whatever the machine type, and the Heidenhain graphic shows the +workpiece as it is machined. Status has the point as `part_position`. The +preview evaluates the kinematics outside realtime for this; with a module +that runs only in realtime it draws the coordinates of the type in force. + The active kinematics is available to the program as the read-only parameter '#<_kins_type>', which lets a subroutine put back whatever was selected before it ran: @@ -2663,7 +2677,7 @@ Under a module's machine frame kinematics, the type <> When 'G68.2', 'G68.3' or 'G68.4' defines a plane, the interpreter takes the map from the part, as the TCP kinematics places it, to the machine frame, at the rotaries as they stand. With the rotaries held that map is a fixed rotation and shift, and it carries the tool length along the tilted tool, which the TCP kinematics applies and the machine frame kinematics does not. 'G53.1', 'G53.3' and 'G53.6' orient the tool to the plane on the part with the TCP kinematics' frames, so a table rotary helps where the plane needs it, and take the map again at the rotaries they end at. 'G53.6' keeps the tool tip where it is at the ends of its move; in between, the slides and the rotaries move in straight lines in the machine frame and the tip leaves the point while they turn. 'G53.2' moves nothing and leaves the map as it was. -A rotary word under the plane moves the rotary alone and the map stays, so the moves after it are still cut where the plane sat on the part before the turn, which is now somewhere else. Fanuc and Heidenhain do the same, and Fanuc's manual tells the programmer not to turn a rotary in a tilted working plane. To work on the turned part, give the plane again after the turn: 'G68.4 X0 Y0 Z0 I0 J0 K0' is the same plane at the rotaries as they now stand, and 'G53.1' turns the rotaries to the plane and takes the map where they end. +A rotary word under the plane moves the rotary alone and the map stays, so the moves after it are still cut where the plane sat on the part before the turn, which is now somewhere else. Fanuc and Heidenhain do the same, and Fanuc's manual tells the programmer not to turn a rotary in a tilted working plane. The preview draws those moves where they land on the turned part. To work on the turned part, give the plane again after the turn: 'G68.4 X0 Y0 Z0 I0 J0 K0' is the same plane at the rotaries as they now stand, and 'G53.1' turns the rotaries to the plane and takes the map where they end. .3+2 Example [source,ngc] @@ -2683,7 +2697,9 @@ G69 A change of tool length offset in the plane, 'G43' or 'G49', keeps the slides where they are and takes the map again with the new length. 'G53', 'G28' and 'G30' move in the machine frame, as Fanuc's do. A change of kinematics type in the plane keeps the plane on the part: under the TCP kinematics it follows the table as the rotaries turn, and back under the machine frame kinematics the map is taken at the rotaries as they stand. -Status keeps the two apart, as the Siemens swivel cycle keeps its workpiece reference frame apart from its table and tool reference frames: 'g68_offset' and 'g68_rotation' are the plane as defined, the same under every kinematics type, and 'g68_kins_offset' and 'g68_kins_rotation' are the map, the identity where none is in effect. A display composes the two after the offset chain to show plane coordinates and draw the plane, and the plane jog frame moves along the plane turned by the map. The G-code preview restates the plane from status with the map status has, so a program run after a plane set in MDI is drawn where it is cut, a rotary turned since included, and so does an abort with `RETAIN_WORK_PLANE`. +Status keeps the two apart, as the Siemens swivel cycle keeps its workpiece reference frame apart from its table and tool reference frames: 'g68_offset' and 'g68_rotation' are the plane as defined, the same under every kinematics type, and 'g68_kins_offset' and 'g68_kins_rotation' are the map, the identity where none is in effect. A display composes the two after the offset chain to show plane coordinates, and the plane jog frame moves along the plane turned by the map. The G-code preview restates the plane from status with the map status has, so a program run after a plane set in MDI moves where the machine does, a rotary turned since included, and so does an abort with `RETAIN_WORK_PLANE`. + +The G-code preview draws the plane as defined, on the part, and the moves in it where they cut the part, the same under both types, as <> describes. A module that declares no TCP kinematics, or whose machine frame kinematics is its TCP kinematics, a robot's, has no map to take: there a plane is in the machine frame, and only head rotaries can orient the tool to it. diff --git a/include/kinematics.h b/include/kinematics.h index b90ed4fe7a9..c65324e54e9 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -153,6 +153,18 @@ extern int kinematicsTypeFlags(int ktype); ** fails. */ extern int kinematicsMachineFrame(const double *joint, struct EmcPose *pos); +/* The forward of any type the module provides, ktype being the number +** G12.1 takes, whatever type is in force: under the machine frame type +** the primary type's forward of the same joints is where the tool stands +** against the part, which a display draws. pos carries the caller's +** estimate in for an iterative forward. Exported by modules built on +** switchkins.c alone; returns -1 for a type the module does not provide. */ +extern int kinematicsForwardOf(int ktype, + const double *joint, + struct EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 59ff8dc046a..445defce1e3 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -99,6 +99,18 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); */ int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); +/** + * Read the pins once and evaluate with what they read until released, for + * a caller that evaluates thousands of points at one moment, a preview + * drawing a program: each read finds a pin by name, which costs more than + * the maths. 1 holds the pins as the next evaluation reads them, 0 goes + * back to reading them every time; kinematicsUserRefreshParams() reads + * them again either way. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserHoldPins(KinematicsUserContext* ctx, int hold); + /** * Perform inverse kinematics (world coords -> joint positions) * diff --git a/lib/python/rs274/glcanon_bake.py b/lib/python/rs274/glcanon_bake.py index d42ff29ea97..b866239d65b 100644 --- a/lib/python/rs274/glcanon_bake.py +++ b/lib/python/rs274/glcanon_bake.py @@ -381,8 +381,9 @@ def axis_positions(self) -> npt.NDArray[np.float32]: and ``kindtool[i]`` - event vertices included, since axes are dropped as columns and never as rows. The coordinates are machine-frame, after g92 -> XY rotation -> g5x, which is the point the drawn planes are - built from; they are not the numbers as written in the program, and an - arc has already been segmented by the time it reaches here. + built from, read back onto the part first under the machine frame + kinematics; they are not the numbers as written in the program, and + an arc has already been segmented by the time it reaches here. ``(N, 0)`` unless the parse was configured with ``want_axis_positions``. diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 34f2a8fdca9..517bd17548b 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -2758,12 +2758,14 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": Status carries it as the interpreter gave it, the origin in the coordinate system the plane was defined in and in machine units, so it goes through the g92 offset, the XY rotation and the g5x offset the way - the canon takes a definition, in the canon's units, and under the - machine frame kinematics through the second feature coordinate system - status has apart, which comes after the tool offset. Where it is one the - loaded program defined, that record is returned, extents and all; a - plane from MDI, or from a program no longer loaded, comes back as a - record of its own with nothing under it. + the canon takes a definition, in the canon's units. It is drawn on the + part, as defined: under the machine frame kinematics the second feature + coordinate system status has apart carries it onto the slides, but the + renderer draws the program under it on the part too, and the backplot + and the tool follow. Where it is one the loaded program defined, that + record is returned, extents and all; a plane from MDI, or from a program + no longer loaded, comes back as a record of its own with nothing under + it. """ s = ctx.stat if s is None or not getattr(s, 'g68_active', 0): @@ -2774,9 +2776,6 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": g92 = ctx.to_internal_units(s.g92_offset) g5x = ctx.to_internal_units(s.g5x_offset) t = math.radians(s.rotation_xy) - k = getattr(s, 'g68_kins_rotation', (1, 0, 0, 0, 1, 0, 0, 0, 1)) - ko = ctx.to_internal_units(getattr(s, 'g68_kins_offset', (0,) * 9)) - tlo = ctx.to_internal_units(getattr(s, 'tool_offset', (0,) * 9)) except (AttributeError, TypeError): return None c, sn = math.cos(t), math.sin(t) @@ -2785,11 +2784,7 @@ def through(x: float, y: float, z: float) -> tuple[float, float, float]: x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) - x, y, z = (x*c - y*sn + g5x[X] + tlo[X], x*sn + y*c + g5x[Y] + tlo[Y], - z + g5x[Z] + tlo[Z]) - return (k[0]*x + k[1]*y + k[2]*z + ko[X] - tlo[X], - k[3]*x + k[4]*y + k[5]*z + ko[Y] - tlo[Y], - k[6]*x + k[7]*y + k[8]*z + ko[Z] - tlo[Z]) + return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) origin = through(0, 0, 0) axes = [] diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index fe641c3c890..a3f07c25886 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -503,6 +503,16 @@ int kinematicsMachineFrame(const double *joint, EmcPose *pos) return -1; } // kinematicsMachineFrame() +int kinematicsForwardOf(int ktype, + const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (kinematicsTypeFlags(ktype) < 0) { return -1; } + return call_forward(ktype, joint, pos, fflags, iflags); +} // kinematicsForwardOf() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -614,6 +624,7 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsMachineFrame); +EXPORT_SYMBOL(kinematicsForwardOf); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index d0fb6dda9f4..248a15152c1 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -62,6 +62,10 @@ struct KinematicsUserContext { int warned_tool; EmcPose caller_tool; /* from kinematicsUserSetTool() */ int have_caller_tool; + double motion_tool[AXIS_COUNT]; /* motion.tooloffset.*, as last read */ + int have_motion_tool; + int hold; /* kinematicsUserHoldPins() */ + int held; /* and the pins read since */ double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; @@ -164,15 +168,15 @@ static int note_all(KinematicsUserContext *ctx) } /* The block sees the pins as they are now, and the tool of whoever - knows it best: the caller, then motion, then the module's own pin. */ + knows it best: the caller, then motion, then the module's own pin. + Held, it sees the pins as they read when the hold began. */ static void refresh(KinematicsUserContext *ctx) { int i; - double tool[AXIS_COUNT]; - int have_motion_tool = 0; + int fresh = !ctx->hold || !ctx->held; /* a pin that stops answering, its module unloaded, keeps its last value */ - for (i = 0; i < ctx->info.nparams; i++) { + for (i = 0; fresh && i < ctx->info.nparams; i++) { int c = ctx->pin_of_param[i]; double value; if (c < 0) continue; @@ -189,17 +193,22 @@ static void refresh(KinematicsUserContext *ctx) if (ctx->tool_param >= 0) { ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; } + ctx->held = ctx->hold; return; } - for (i = 0; i < AXIS_COUNT; i++) { - int c = ctx->pin_of_tool[i]; - tool[i] = 0.0; - if (c < 0) continue; - if (pin_value(ctx->pin_name[c], HAL_REAL, &tool[i]) != 0) continue; - have_motion_tool = 1; + if (fresh) { + ctx->have_motion_tool = 0; + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->pin_of_tool[i]; + ctx->motion_tool[i] = 0.0; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], HAL_REAL, &ctx->motion_tool[i]) != 0) continue; + ctx->have_motion_tool = 1; + } } - if (!have_motion_tool) return; + ctx->held = ctx->hold; + if (!ctx->have_motion_tool) return; /* the module's pin and motion disagree: the config lost the tool somewhere between them. Say so once; motion's value is the one @@ -207,17 +216,17 @@ static void refresh(KinematicsUserContext *ctx) disagreement. */ if (ctx->tool_param >= 0 && !ctx->warned_tool && ctx->params.geometry[ctx->tool_param] != 0.0 - && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + && fabs(ctx->motion_tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { fprintf(stderr, "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" " using motion's value\n", ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, - ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->params.geometry[ctx->tool_param], ctx->motion_tool[AXIS_Z]); ctx->warned_tool = 1; } - for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, ctx->motion_tool[i]); if (ctx->tool_param >= 0) { - ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; + ctx->params.geometry[ctx->tool_param] = ctx->motion_tool[AXIS_Z]; } } @@ -470,10 +479,20 @@ int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) ctx->have_caller_tool = 1; } else { ctx->have_caller_tool = 0; + /* motion's tool, which a hold begun with the caller's never read */ + ctx->held = 0; } return 0; } +int kinematicsUserHoldPins(KinematicsUserContext* ctx, int hold) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + ctx->hold = hold != 0; + ctx->held = 0; + return 0; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) @@ -600,6 +619,7 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + ctx->held = 0; refresh(ctx); return 0; } diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 4157e5f6bfa..dcb7ff844bb 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -53,9 +53,11 @@ KINEMATICS_INVERSE_FLAGS iflags = 0; // idea of the tool orientation #pragma weak kinematicsToolFrame #pragma weak kinematicsWorkFrame -// and only modules built on switchkins.c those of another type +// and only modules built on switchkins.c those of another type, and its +// forward #pragma weak kinematicsToolFrameOf #pragma weak kinematicsWorkFrameOf +#pragma weak kinematicsForwardOf /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ @@ -2523,6 +2525,32 @@ static int primary_kins_type(void) return -1; } +/* The commanded joints as the primary type reads them, where the tool + stands against the part: the commanded position itself on that type, or + where no type is declared primary, and the primary type's forward under + any other. A display draws the tool there, so under the machine frame + type G13.1 selects it is on the part as under the tool centre point + type, and a switch between them moves nothing. Where the forward fails + the last answer stands. */ +static void update_part_pos(void) +{ + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inv = 0; + /* the last answer is the estimate an iterative forward starts from */ + EmcPose pos = emcmotStatus->part_pos_cmd; + int j, primary = primary_kins_type(); + + if (primary < 0 || primary == switchkins_type || !kinematicsForwardOf) { + emcmotStatus->part_pos_cmd = emcmotStatus->carte_pos_cmd; + return; + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsForwardOf(primary, joint_pos, &pos, &ff, &inv) == 0) { + emcmotStatus->part_pos_cmd = pos; + } +} + /* The tool frame in the work frame, transpose(work) * tool as toolFrameInWork() has it, at the commanded joints, for the status; and the machine axes in the work frame, transpose(work), for the machine @@ -2708,6 +2736,7 @@ static void update_status(void) emcmotStatus->eoffset_pose.w = axis_get_ext_offset_curr_pos(8); emcmotStatus->external_offsets_applied = hal_get_bool(emcmot_hal_data->eoffset_active); + update_part_pos(); update_tool_frame(); emcmotUpdateJogFrame(); diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index ed9cc8c1495..e3579aaa8ca 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -627,6 +627,9 @@ Suggestion: Split this in to an Error and a Status flag register.. int carte_pos_cmd_ok; /* non-zero if command is valid */ EmcPose carte_pos_fb; /* actual Cartesian position */ int carte_pos_fb_ok; /* non-zero if feedback is valid */ + EmcPose part_pos_cmd; /* the commanded joints as the primary type + reads them: where the tool stands against + the part, carte_pos_cmd on that type */ double tool_frame[9]; /* the tool frame in the work frame at the commanded joints, row major */ int tool_frame_ok; /* non-zero if the module reports its frames */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 3cdd0604329..ce03e3451cd 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1733,6 +1733,7 @@ void EMC_TRAJ_STAT::update(CMS * cms) cms->update(rapid_scale); EmcPose_update(cms, &position); EmcPose_update(cms, &actualPosition); + EmcPose_update(cms, &part_position); cms->update(tool_frame, 9); cms->update(tool_frame_ok); cms->update(jog_frame); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 61c0ddcf4d2..8a851102e80 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -1058,6 +1058,8 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { EmcPose position; // current commanded position EmcPose actualPosition; // current actual position, from forward kins + EmcPose part_position; // commanded joints as the primary type reads + // them, the tool against the part double tool_frame[9]; // tool frame in the work frame, row major bool tool_frame_ok; // the kinematics reports its frames int jog_frame; // the frame world jogs move along, EMC_JOG_FRAME_* diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 7f9fdbae371..d9a0f3e931d 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -84,6 +84,7 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() position{}, actualPosition{}, + part_position{}, tool_frame{1, 0, 0, 0, 1, 0, 0, 0, 1}, tool_frame_ok(false), jog_frame(0), diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 816ad639011..0631e65b14d 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -58,24 +58,26 @@ void GCodeRenderer::set_spindle_speed(double rpm) { / (2 * M_PI) * rpm; } -// The plane's origin and axes on the machine, through the transform the way -// a move endpoint on this line goes, so the drawn plane and the moves in it -// agree. Recorded here rather than at hand over because the offsets in -// force at the definition are what place it, and they move on. -void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { +// The plane's origin and axes as the program defined it, through the +// offsets the way a move endpoint on this line goes, so the drawn plane and +// the moves in it agree. Recorded here rather than at hand over because the +// offsets in force at the definition are what place it, and they move on. +// Under the machine frame kinematics the moves go through the second +// feature coordinate system onto the slides and are drawn back on the part, +// where this is. +void GCodeRenderer::set_g68_drawn(const WorkFrame &drawn) { if(parse_state.interp_error) return; - frame_ = frame; workplane_ = -1; lo_unreached_ = false; - if(!frame.active || !data_) return; + if(!drawn.active || !data_) return; Point9 at; - transform({}, at); + chain(drawn, {}, at); Point3 origin = {at[P9_X], at[P9_Y], at[P9_Z]}; std::array axes; for(int i = 0; i < P3_COUNT; i++) { Point9 unit = {}; unit[i] = 1.0; - transform(unit, at); + chain(drawn, unit, at); for(int j = 0; j < P3_COUNT; j++) axes[i][j] = at[j] - origin[j]; } std::vector &planes = data_->workplanes; @@ -336,8 +338,9 @@ void preview_geometry_register(py::module_ &m) { triple(r.extents[BOX_MAX]))); return out; }, "(lineno, origin, (x, y, z) axes, min, max) per tilted work " - "plane; origin and axes in the machine frame, the extents " - "of the moves made under it in the plane's own coordinates") + "plane; origin and axes where it is drawn, on the part, the " + "extents of the moves made under it in the plane's own " + "coordinates") .def("tool_offsets", [](const PreviewData &d) { py::list out; for(const ToolOffsetRecord &r : d.tool_offsets) @@ -882,9 +885,10 @@ void GCodeRenderer::publish_line() { canon_guard([&]{ canon_.attr("lineno") = last_line_; }); } -void GCodeRenderer::transform(const Point9 &in, Point9 &out) const { +void GCodeRenderer::chain(const WorkFrame &frame, const Point9 &in, + Point9 &out) const { out = in; - frame_.apply(out); + frame.apply(out); out += g92_; if(rotation_xy_ != 0.0) { double rotx = out[P9_X] * rotation_cos_ - out[P9_Y] * rotation_sin_; @@ -906,30 +910,32 @@ void GCodeRenderer::untransform(const Point9 &in, Point9 &out) const { out -= g92_; } -// Points through the live transform, by the two steps a move endpoint takes: -// the g92 -> XY rotation -> g5x chain, and then the GEOMETRY string of the -// plane drawn first. Nothing is sampled, linearised or rebuilt, so a point -// carrying an A/B/C value is turned by its own angle exactly as its move -// would be, and a GEOMETRY naming U/V/W reads them off the point rather than -// losing them. +// Points through the live transform, by the steps a move endpoint takes: +// the g92 -> XY rotation -> g5x chain, onto the part under the machine +// frame kinematics, and then the GEOMETRY string of the plane drawn first. +// Nothing is sampled, linearised or rebuilt, so a point carrying an A/B/C +// value is turned by its own angle exactly as its move would be, and a +// GEOMETRY naming U/V/W reads them off the point rather than losing them. bool GCodeRenderer::transform_points(const Point9 *in, size_t n, double *machine, double *display) const { for(size_t i = 0; i < n; i++) { - Point9 at; + Point9 at, part; transform(in[i], at); machine[i * P3_COUNT + P3_X] = at[P9_X]; machine[i * P3_COUNT + P3_Y] = at[P9_Y]; machine[i * P3_COUNT + P3_Z] = at[P9_Z]; + on_part(at, part); Point3 drawn; - plane_point(data_->ops[0], at, drawn); + plane_point(data_->ops[0], part, drawn); for(int c = 0; c < P3_COUNT; c++) display[i * P3_COUNT + c] = drawn[c]; } return true; } -void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, - unsigned char kind, PlanePoints *points) { +void GCodeRenderer::write_vertex(const Point9 &axes, const Point9 &drawn, + int line_number, unsigned char kind, + PlanePoints *points) { if(!data_->reserve(1)) { if(!parse_state.interp_error) PyErr_NoMemory(); parse_state.interp_error ++; @@ -938,7 +944,7 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, size_t at = data_->n; for(int i = 0; i < data_->nplanes; i++) { Point3 p; - plane_point(data_->ops[i], pts9, p); + plane_point(data_->ops[i], drawn, p); for(int j = 0; j < P3_COUNT; j++) { if(p[j] < data_->drawn[BOX_MIN][j]) data_->drawn[BOX_MIN][j] = p[j]; if(p[j] > data_->drawn[BOX_MAX][j]) data_->drawn[BOX_MAX][j] = p[j]; @@ -952,7 +958,7 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, if(data_->axis_pos) for(int k = 0; k < data_->n_axes; k++) data_->axis_pos[at * (size_t)data_->n_axes + k] = - (float)pts9[data_->axis_cols[k]]; + (float)axes[data_->axis_cols[k]]; data_->attrs[at * ATTRS_PER_VERTEX] = (uint32_t)line_number; data_->attrs[at * ATTRS_PER_VERTEX + 1] = (uint32_t)kind | (data_->tool << 8); @@ -961,36 +967,48 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, void GCodeRenderer::mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points) { - write_vertex(at, line_number, kind, points); + Point9 drawn; + on_part(at, drawn); + write_vertex(at, drawn, line_number, kind, points); } -// The four machine-frame extent pairs, from one move's raw endpoints. -void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2) { - Box3 box; - for(int j = 0; j < P3_COUNT; j++) { - box[BOX_MIN][j] = std::min(p1[j], p2[j]); - box[BOX_MAX][j] = std::max(p1[j], p2[j]); - } +// The four extent pairs, from one move's endpoints: as drawn, and with the +// tool offset added back to the endpoints as the slides' world has them, +// which is what the axis limits bound. Only the machine frame kinematics +// draws a point anywhere but where it is. +void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2, + const Point9 &d1, const Point9 &d2) { + // With no rotation to remove, the zero_rxy kinds are the plain ones. + bool rotated = rotation_xy_ != 0.0; + // A pair's box, and the same with the rotation taken out. + struct Boxes { Box3 box, rot; }; + auto boxes = [&](const Point9 &a, const Point9 &b) { + Boxes out; + for(int j = 0; j < P3_COUNT; j++) { + out.box[BOX_MIN][j] = std::min(a[j], b[j]); + out.box[BOX_MAX][j] = std::max(a[j], b[j]); + } + if(rotated) { + Point3 u1, u2; + unrotate_xy(a, u1); + unrotate_xy(b, u2); + for(int j = 0; j < P3_COUNT; j++) { + out.rot[BOX_MIN][j] = std::min(u1[j], u2[j]); + out.rot[BOX_MAX][j] = std::max(u1[j], u2[j]); + } + } + return out; + }; + Boxes slides = boxes(p1, p2), drawn = boxes(d1, d2); // The tool-corrected box is the raw box shifted: adding a constant is // monotonic, so this is the same box, not an approximation of it. Point3 shift = {tool_[P9_X], tool_[P9_Y], tool_[P9_Z]}; - Box3 rot; - if(rotation_xy_ != 0.0) { - Point3 u1, u2; - unrotate_xy(p1, u1); - unrotate_xy(p2, u2); - for(int j = 0; j < P3_COUNT; j++) { - rot[BOX_MIN][j] = std::min(u1[j], u2[j]); - rot[BOX_MAX][j] = std::max(u1[j], u2[j]); - } - } - // With no rotation to remove, the zero_rxy kinds are the plain ones. - bool rotated = rotation_xy_ != 0.0; for(int i = 0; i < EXTENT_KINDS; i++) { - bool unrotated = (i >= EXT_ZERO_RXY) && rotated; - const Point3 &lo = unrotated ? rot[BOX_MIN] : box[BOX_MIN]; - const Point3 &hi = unrotated ? rot[BOX_MAX] : box[BOX_MAX]; bool notool = (i == EXT_NOTOOL || i == EXT_NOTOOL_ZERO_RXY); + const Boxes &from = notool ? slides : drawn; + bool unrotated = (i >= EXT_ZERO_RXY) && rotated; + const Point3 &lo = unrotated ? from.rot[BOX_MIN] : from.box[BOX_MIN]; + const Point3 &hi = unrotated ? from.rot[BOX_MAX] : from.box[BOX_MAX]; for(int j = 0; j < P3_COUNT; j++) { double a = notool ? lo[j] + shift[j] : lo[j]; double b = notool ? hi[j] + shift[j] : hi[j]; @@ -1015,15 +1033,20 @@ void GCodeRenderer::unrotate_xy(const Point9 &p, Point3 &out) const { void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, unsigned char cat) { data_->moves ++; - accumulate_extents(p1, p2); + // Drawn where the tool stands against the part, which under the machine + // frame kinematics the slides' own world does not say. + Point9 d1, d2; + on_part(p1, d1); + on_part(p2, d2); + accumulate_extents(p1, p2, d1, d2); // A cut drawn from where traverses the drawing left out went under the // plane starts in it. - if(lo_unreached_ && cat != CAT_TRAVERSE) reach(p1); + if(lo_unreached_ && cat != CAT_TRAVERSE) reach(d1); lo_unreached_ = false; - reach(p2); + reach(d2); - double dx = p2[P9_X] - p1[P9_X], dy = p2[P9_Y] - p1[P9_Y], - dz = p2[P9_Z] - p1[P9_Z]; + double dx = d2[P9_X] - d1[P9_X], dy = d2[P9_Y] - d1[P9_Y], + dz = d2[P9_Z] - d1[P9_Z]; double len = sqrt(dx * dx + dy * dy + dz * dz); if(cat == CAT_TRAVERSE) data_->rapid_length += len; else data_->cut_length += len; @@ -1032,7 +1055,8 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, // at its start; the shaders discard the segment into it. bool jump = !data_->has_cur || p1 != data_->cur9; // Rotary subdivision: a move that turns A, B or C is drawn as a polyline, - // since the tool's path through the machine's frame is not a straight line. + // since the tool's path through the machine's frame is not a straight + // line, nor on the part where the slides carry it there. long steps = 1; bool turning = false; double dc = 0.0; @@ -1048,15 +1072,18 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, long count = steps + (jump ? 1 : 0); for(long i = 0; i < count; i++) { long sub = i - (jump ? 1 : 0) + 1; - Point9 pt; - if(steps == 1 && !jump) { - pt = p2; + if(sub == 0) { + write_vertex(p1, d1, line_number, KIND_NOOP, nullptr); + } else if(sub == steps) { + write_vertex(p2, d2, line_number, cat, nullptr); } else { double t = (double)sub / (double)steps; + Point9 pt, drawn; for(int k = 0; k < P9_COUNT; k++) pt[k] = t * p2[k] + (1.0 - t) * p1[k]; + on_part(pt, drawn); + write_vertex(pt, drawn, line_number, cat, nullptr); } - write_vertex(pt, line_number, sub == 0 ? KIND_NOOP : cat, nullptr); } data_->cur9 = p2; data_->has_cur = true; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 01aa92d7887..7b33ef78061 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -135,13 +135,14 @@ struct ToolOffsetRecord { }; // A tilted work plane the program defined, G68.2, G68.3 or G68.4: where it -// sits on the machine, and what the program did in it. The origin and the -// axes are in the machine frame, through the offsets in force at the -// definition the way a move endpoint on the same line gets them; the -// extents are in the plane's own coordinates, over every move made while -// it was in effect, empty (min > max) while nothing moved. A definition -// that names the plane in effect again is the same record, so a program -// that restates its plane in a loop has one plane, not one per pass. +// is drawn, and what the program did in it. The origin and the axes are +// through the offsets in force at the definition the way a move endpoint +// on the same line gets them, and on the part, where the moves are drawn, +// under the machine frame kinematics too; the extents are in the plane's +// own coordinates, over every move made while it was in effect, empty +// (min > max) while nothing moved. A definition that names the plane in +// effect again is the same record, so a program that restates its plane in +// a loop has one plane, not one per pass. struct WorkPlaneRecord { int lineno; Point3 origin; @@ -156,7 +157,7 @@ struct WorkPlaneRecord { } return true; } - // Take in a machine point the program reached under the plane. + // Take in a point the program reached under the plane, as drawn. void extend(const Point9 &p) { Point3 d = {p[P9_X] - origin[P3_X], p[P9_Y] - origin[P3_Y], p[P9_Z] - origin[P3_Z]}; @@ -409,7 +410,13 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; - void set_g68_frame(const WorkFrame &frame) override; + // The moves go through the frame canon applies; the plane is drawn as + // the program defined it, which set_g68_drawn() brings. + void set_g68_frame(const WorkFrame &frame) override { + if(parse_state.interp_error) return; + frame_ = frame; + } + void set_g68_drawn(const WorkFrame &drawn) override; // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -512,9 +519,18 @@ private: // One record vertex at `at`, writing its per-plane position to `points`. void mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points); - void write_vertex(const Point9 &pts9, int line_number, unsigned char kind, - PlanePoints *points); - void accumulate_extents(const Point9 &p1, const Point9 &p2); + // A vertex at `axes`, drawn at `drawn`: the same point but under the + // machine frame kinematics, where on_part() has moved it. + void write_vertex(const Point9 &axes, const Point9 &drawn, int line_number, + unsigned char kind, PlanePoints *points); + void accumulate_extents(const Point9 &p1, const Point9 &p2, + const Point9 &d1, const Point9 &d2); + // Where a point of the chain is drawn: where the tool stands against the + // part, which under the machine frame kinematics the slides' own world + // does not say, and the point itself everywhere else. + void on_part(const Point9 &at, Point9 &out) const { + if(!point_on_part(at, tool_, out)) out = at; + } bool read_planes(); // program_geometry.machine_limits -> the estimator, or none. bool read_limits(); @@ -532,7 +548,11 @@ private: // where this came from, though that method no longer runs on a rendered // parse. Not bit-identical to it by construction: the compiler is free to // contract the rotation's multiply-add, so the tests allow a few ULPs. - void transform(const Point9 &in, Point9 &out) const; + void transform(const Point9 &in, Point9 &out) const { + chain(frame_, in, out); + } + // The same through a plane other than the one moves go through. + void chain(const WorkFrame &frame, const Point9 &in, Point9 &out) const; // The inverse, to recover an arc segment's program X. void untransform(const Point9 &in, Point9 &out) const; void event(Kind kind, int line_number, const Point9 &axes); diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 73c9610c141..b1684ecac33 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -577,19 +577,25 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; -// the preview draws the plane canon applies; what status reports apart is -// the machine's business +// The plane canon applies, which carries the moves onto the slides, and the +// plane as defined, which the renderer draws on the part: under the machine +// frame kinematics the one has the second feature coordinate system folded +// in, and the renderer reads the moves back onto the part, where the plane +// is. A callback canon has only the first. void SET_G68_FRAME(double x, double y, double z, const double rotation[9], int active, - const double * /*defined_origin*/, - const double * /*defined_rotation*/, + const double *defined_origin, + const double *defined_rotation, const double * /*kins_shift*/, const double * /*kins_rotation*/) { - WorkFrame frame; - frame.active = active != 0; + WorkFrame frame, drawn; + frame.active = drawn.active = active != 0; frame.origin = ensure_inch({x, y, z}); std::copy(rotation, rotation + 9, frame.rotation.begin()); + drawn.origin = ensure_inch({defined_origin[0], defined_origin[1], defined_origin[2]}); + std::copy(defined_rotation, defined_rotation + 9, drawn.rotation.begin()); parse_state.canon->set_g68_frame(frame); + parse_state.canon->set_g68_drawn(drawn); }; void USE_LENGTH_UNITS(CANON_UNITS u) { parse_state.metric = u == CANON_UNITS_MM; } @@ -1016,6 +1022,49 @@ bool GET_EXTERNAL_KINEMATICS_IDENTITY() { } void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } +// The renderer's points on the part, through the interpreter of the parse: +// whether the type in force reads back onto the part is asked once per +// type, then each point is mapped. The answer belongs to one parse. +struct OnPart { + int kins_type = -1; // the type the answer is for + bool ready = false; +}; +static OnPart on_part; + +// A renderer's point, in inches with the tool offset out, through one of +// the interpreter's kinematics maps, which work in the machine's units with +// the tool offset in. +template +static bool through_kins(const Point9 &at, const Point9 &tool, Point9 &out, Map map) { + // inches to the machine's units; the angles are degrees either way + double scale = 25.4 * GET_EXTERNAL_LENGTH_UNITS(); + double world[P9_COUNT], got[P9_COUNT]; + for(int i = 0; i < P9_COUNT; i++) { + double v = at[i] + tool[i]; + world[i] = (parse_state.angular >> i) & 1 ? v : v * scale; + } + if(!map(world, got)) return false; + for(int i = 0; i < P9_COUNT; i++) { + double v = (parse_state.angular >> i) & 1 ? got[i] : got[i] / scale; + out[i] = v - tool[i]; + } + return true; +} + +bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return false; + setup_pointer s = &interp->_setup; + if(s->kins_type != on_part.kins_type) { + on_part.kins_type = s->kins_type; + on_part.ready = interp->kins_part_ready(s); + } + if(!on_part.ready) return false; + return through_kins(at, tool, out, [&](const double *world, double *part) { + return interp->kins_on_part(s, world, part); + }); +} + #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) // The parse. `initcodes` is the list of the new signature or null for the @@ -1124,6 +1173,7 @@ static py::object parse_file(const char *f, py::handle canon, // the last parse ended on is not the machine's: the GUI restates it parse_state.tool_offset = {}; read_kins_types(); + on_part = OnPart(); parse_state.pinterp->init(); parse_state.pinterp->open(f); diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index 3dd53e3c39a..a91319f1f1b 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -179,6 +179,13 @@ public: // The tilted work plane, or its cancellation; the origin already in // inches. virtual void set_g68_frame(const WorkFrame &frame) = 0; + // The same plane as the program defined it, on the part. Under the + // machine frame kinematics the frame canon applies has the second + // feature coordinate system folded in, and carries the moves onto the + // slides; a preview draws the plane on the part instead, where it reads + // the moves back to. Elsewhere the two are the same. Only the renderer + // listens. + virtual void set_g68_drawn(const WorkFrame & /*drawn*/) {} virtual void set_plane(int plane) = 0; virtual void set_feed_rate(double rate) = 0; virtual void set_traverse_rate(double rate) = 0; @@ -318,5 +325,12 @@ static inline void forward(const char *method, A &&...args) { // external_*_units. Defined in gcodemodule.cc. double read_external_length_units(); double read_external_angle_units(); +// Where the tool tip stands against the part for a point of the +// kinematics type the program is in, in inches with the tool offset out as +// the renderer has its points: the primary type's reading of the joints +// the slides stand in there. False, and out left alone, where the two are +// the same: under the primary type, on a machine without one, and where +// the kinematics cannot be evaluated here. +bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out); #endif // GCODEMODULE_HH diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 416ee0607ba..7ba31d4f6bb 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -909,6 +909,93 @@ int Interp::kins_here(setup_pointer s, void **out) return kins_context(s, out); } +// The type the preview reads the joints back through to draw the tool on +// the part: the primary type, the one that follows the part, where another +// is in force, as motion reports the part position. -1 under the primary +// type itself and where the module declares none. +int Interp::part_kins_type(setup_pointer s) +{ + int primary = flagged_kins_type(KINSTYPE_PRIMARY); + + return primary == s->kins_type ? -1 : primary; +} + +// The kinematics for a preview, loaded, its pins read once: the points +// come in their thousands and the pins do not move under a preview. NULL +// where nothing can be evaluated here. Nothing here is an error: a +// preview that cannot map a point draws it as it is. +void *Interp::kins_preview(setup_pointer s) +{ + void *vctx; + + if (kins_here(s, &vctx) != INTERP_OK || !vctx) { return NULL; } + kinematicsUserHoldPins((KinematicsUserContext *)vctx, 1); + return vctx; +} + +// Whether the preview can draw the tool on the part under the type in +// force, with part_kins_type() and a module evaluated here; asked again +// only when the type changes. +bool Interp::kins_part_ready(setup_pointer s) +{ + return part_kins_type(s) >= 0 && kins_preview(s) != NULL; +} + +// The joints type from puts the machine in at a point of its world, read +// back by type to. world and out are absolute and in the machine's units +// with the tool offset in, as each type's world has them. False where +// either type cannot answer, or nothing is loaded. +bool Interp::kins_carry(setup_pointer s, int from, int to, const double world[9], double out[9]) +{ + KinematicsUserContext *ctx = KINS_CTX(s); + double joints[EMCMOT_MAX_JOINTS]; + EmcPose pose; + bool ok; + int i; + + if (!ctx || kinematicsUserIsRtOnly(ctx)) { return false; } + pose.tran.x = world[0]; + pose.tran.y = world[1]; + pose.tran.z = world[2]; + pose.a = world[3]; + pose.b = world[4]; + pose.c = world[5]; + pose.u = world[6]; + pose.v = world[7]; + pose.w = world[8]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + kins_set_tool(ctx, &s->tool_offset); + ok = kinematicsUserSetType(ctx, from) == 0 + && kinematicsUserInverse(ctx, &pose, joints) == 0 + && kinematicsUserSetType(ctx, to) == 0 + && kinematicsUserForward(ctx, joints, &pose) == 0; + kinematicsUserSetType(ctx, s->kins_type); + if (!ok) { return false; } + out[0] = pose.tran.x; + out[1] = pose.tran.y; + out[2] = pose.tran.z; + out[3] = pose.a; + out[4] = pose.b; + out[5] = pose.c; + out[6] = pose.u; + out[7] = pose.v; + out[8] = pose.w; + return true; +} + +// Where the tool stands against the part at a point of the type in force: +// the joints that type puts the machine in there, read back by the type +// that follows the part. Under the machine frame type the slides put the +// tool on the part somewhere their own world does not say, and a preview +// draws it there, as under the TCP kinematics. Once kins_part_ready() +// has said yes. +bool Interp::kins_on_part(setup_pointer s, const double world[9], double part[9]) +{ + const int primary = part_kins_type(s); + + return primary >= 0 && kins_carry(s, s->kins_type, primary, world, part); +} + // Where the point goes when the tool offset changes: the joints stay, so it is // read back from them under the new offset, as motion reads it. known stays // false under the identity and where nothing can be evaluated; the caller diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 15a459bb41e..ad123e6f914 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -397,6 +397,11 @@ public: int kins_load(setup_pointer settings); int kins_context(setup_pointer settings, void **ctx); int kins_here(setup_pointer settings, void **ctx); + int part_kins_type(setup_pointer settings); + void *kins_preview(setup_pointer settings); + bool kins_part_ready(setup_pointer settings); + bool kins_carry(setup_pointer settings, int from, int to, const double world[9], double out[9]); + bool kins_on_part(setup_pointer settings, const double world[9], double part[9]); bool kins_machine_frame(setup_pointer settings); int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, EmcPose *point, bool *known); diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index fa7b60677f3..adde3409e4a 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1768,6 +1768,8 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) stat->actualPosition = emcmotStatus.carte_pos_fb; + stat->part_position = emcmotStatus.part_pos_cmd; + for (int i = 0; i < 9; i++) { stat->tool_frame[i] = emcmotStatus.tool_frame[i]; } stat->tool_frame_ok = emcmotStatus.tool_frame_ok; stat->jog_frame = emcmotStatus.jog_frame; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 469d4bc17ee..c0b5a968ce9 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1333,6 +1333,10 @@ static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } +static PyObject *Stat_part_position(pyStatChannel *s, void *) { + return pose(s->status.motion.traj.part_position); +} + static PyObject *Stat_dtg(pyStatChannel *s, void *) { return pose(s->status.motion.traj.dtg); } @@ -1644,6 +1648,8 @@ static PyGetSetDef Stat_getsetlist[] = { {(char*)"tool_frame", (getter)Stat_tool_frame, NULL, (char*)"Tool frame in the work frame at the commanded joints, nine values row by row, the columns being tool X, Y and Z; None where the kinematics reports no frames.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, + {(char*)"part_position", (getter)Stat_part_position, NULL, + (char*)"The commanded joints as the primary kinematics type reads them, where the tool stands against the part: position itself, except under another type, the machine frame type G13.1 selects, where position is in machine coordinates.", NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, {(char*)"joint_actual_position", (getter)Stat_joint_actual, NULL, NULL, NULL}, @@ -3103,16 +3109,21 @@ static PyObject *Logger_start(pyPositionLogger *s, PyObject *o) { op->rx, op->ry, op->rz, oop->rx, oop->ry, oop->rz); } else { + // where the tool stands against the part, as the preview + // draws the program: the commanded position, except under + // the machine frame kinematics, where the slides put the + // tool on the part where the primary type reads them + const EmcPose &at = status->motion.traj.part_position; double pt[9] = { - status->motion.traj.position.tran.x - status->task.toolOffset.tran.x, - status->motion.traj.position.tran.y - status->task.toolOffset.tran.y, - status->motion.traj.position.tran.z - status->task.toolOffset.tran.z, - status->motion.traj.position.a - status->task.toolOffset.a, - status->motion.traj.position.b - status->task.toolOffset.b, - status->motion.traj.position.c - status->task.toolOffset.c, - status->motion.traj.position.u - status->task.toolOffset.u, - status->motion.traj.position.v - status->task.toolOffset.v, - status->motion.traj.position.w - status->task.toolOffset.w}; + at.tran.x - status->task.toolOffset.tran.x, + at.tran.y - status->task.toolOffset.tran.y, + at.tran.z - status->task.toolOffset.tran.z, + at.a - status->task.toolOffset.a, + at.b - status->task.toolOffset.b, + at.c - status->task.toolOffset.c, + at.u - status->task.toolOffset.u, + at.v - status->task.toolOffset.v, + at.w - status->task.toolOffset.w}; double p[3]; vertex9(pt, p, s->geometry); diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py index 674b49e01d9..8ac61e98734 100755 --- a/tests/glcanon/test_workplane.py +++ b/tests/glcanon/test_workplane.py @@ -207,6 +207,19 @@ def test_no_plane_in_effect(self): self.assertIsNone(glcanon_scene.active_workplane(ctx)) self.assertEqual(len(ctx.prim.calls), 6) + def test_under_the_machine_frame_kinematics_the_plane_is_on_the_part(self): + # the map the plane is cut through onto the slides, a turn about Z + # and a shift, and a tool offset it carries: the plane is drawn as + # defined all the same, where the renderer draws the program + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + stat.g68_kins_rotation = (0, -1, 0, 1, 0, 0, 0, 0, 1) + stat.g68_kins_offset = (5 * 25.4, 6 * 25.4, 7 * 25.4) + (0,) * 6 + stat.tool_offset = (0, 0, 2 * 25.4) + (0,) * 6 + canon = run(program(TILTED, "G1 X-30 Y-20 Z5")) + canon.calc_extents() + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + if __name__ == '__main__': unittest.main() diff --git a/tests/twp-static-preview/README b/tests/twp-static-preview/README index 907801e2e8d..f5345846659 100644 --- a/tests/twp-static-preview/README +++ b/tests/twp-static-preview/README @@ -14,4 +14,15 @@ onto the part it was defined on. Last the preview: after a plane set in MDI, with the tool in the spindle, after a plane set in MDI and a rotary turned since, and with a plane in the -program itself, the preview ends where the machine does. +program itself, the preview's interpreter ends where the machine does. What +the preview draws is on the part, as under the TCP kinematics: the same +program draws one plane in the same place, and ends its cut at the same +point, under G13.1 as under the TCP kinematics. The live plot is on the +part too: under G13.1 in the plane it is where the TCP kinematics plots the +same point, with the tool frame in status the TCP kinematics', and switches +to the TCP kinematics and back neither move it nor draw a line. + +With no plane, a rotary turned under G13.1 at a tilted head or table: the +preview draws the turn through the point the machine reaches half way and +ends where it ends, and the live plot and status part_position agree, the +TCP kinematics reading the same point from the same joints. diff --git a/tests/twp-static-preview/test-ui.py b/tests/twp-static-preview/test-ui.py index c7143902125..a164d9c34e5 100755 --- a/tests/twp-static-preview/test-ui.py +++ b/tests/twp-static-preview/test-ui.py @@ -3,8 +3,12 @@ import os import sys +import threading +import collections import gcode import preview_helpers +import numpy as np +from rs274 import glcanon_bake from rs274.interpret import Translated sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "twp-static")) from static import * @@ -105,20 +109,13 @@ def jog_plane_x(): error("after an abort the plane put X-10 Y5 Z20 at %s, not %s" % (show(got), show(want))) os.unlink("abort.ngc") -# The preview: a program after a plane set in MDI starts from the plane -# as status has it, folded into the second feature coordinate system, -# and one with its own plane takes its own; both end where the machine -# does, which the preview draws without the tool offset. -class Canon(Translated): - def __init__(self): - self.parameter_file = "preview.var" - self.end = None - def __getattr__(self, name): - if name.startswith("_"): - raise AttributeError(name) - return lambda *a, **k: None - def straight_feed(self, *a): - self.end = self.rotate_and_translate(*a)[:3] +# The preview's interpreter: a program after a plane set in MDI starts +# from the plane as status has it, folded into the second feature +# coordinate system, and one with its own plane takes its own; through the +# plane canon applies, which a callback canon gets, both end where the +# machine does, without the tool offset. +class Machine: + # what a preview reads off the machine def get_tool(self, pocket): return tuple(s.tool_table[pocket]) if 0 <= pocket < len(s.tool_table) else (-1,) + (0,) * 13 def get_external_length_units(self): @@ -140,6 +137,17 @@ def get_kins_type(self): def get_kins_type_flags(self): return s.kins_type_flags +class Canon(Machine, Translated): + def __init__(self): + self.parameter_file = "preview.var" + self.end = None + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def straight_feed(self, *a): + self.end = self.rotate_and_translate(*a)[:3] + def preview_case(name, prog, text): with open(prog, "w") as f: f.write(text) @@ -184,4 +192,171 @@ def preview_case(name, prog, text): preview_case("plane in the program", "own-plane.ngc", "G43 H1\n%s\nG53.1\nG1 X-10 Y5 Z20 F60000\nG1 Z-5\nM2\n" % plane) +# What the preview draws is on the part, as under the part's type, where +# the program cuts: the plane as defined and the moves in it, not the +# slides that cut them. The same program, under G13.1 and under the part's +# type, draws one plane in the same place and ends at the same point. +# The program as the renderer hands it to a GUI, with no display: a +# headless build has no OpenGL. +Plane = collections.namedtuple("Plane", "origin axes") + +class Rendered(Machine, gcode.RendererCanon): + COLORS = {"dwell": (1, 0, 0, 1), "m1xx": (0, 0, 1, 1)} + def __init__(self): + self.program_geometry = glcanon_bake.ProgramGeometry(geometry="XYZ") + self.parameter_file = "preview.var" + self.workplanes = [] + def adopt_geometry(self, program): + self.program_geometry.adopt(program, self.COLORS) + self.workplanes = [Plane(origin, axes) + for _line, origin, axes, _lo, _hi in program.workplanes()] + def comment(self, text): pass + def message(self, text): pass + def check_abort(self): return False + def next_line(self, state): pass + def change_tool(self, pocket): pass + +def rendered(name, text, axes=False): + prog = "rendered.ngc" + with open(prog, "w") as f: + f.write(text) + s.poll() + canon = Rendered() + canon.program_geometry.configure(want_axis_positions=axes) + res, seq = gcode.parse(prog, canon, preview_helpers.preview_initcodes(s, ini)) + os.unlink(prog) + if res > gcode.MIN_ERROR: + error("%s: the preview stops at line %d: %s" % (name, seq, gcode.strerror(res).strip())) + return None + return canon + +def close(a, b): + # the drawn points are single precision + return max(abs(p - q) for p, q in zip(a, b)) < 1e-5 + +for cmd in (RESET, START): + mdi(cmd) +body = "G43 H1\n%s\nG53.1\nG1 X-10 Y5 Z20 F60000\nG1 Z-5\nM2\n" % plane +on_part = rendered("drawn under the part's type", TCP + "\n" + body) +on_g131 = rendered("drawn under G13.1", "G13.1\n" + body) +if on_part and on_g131: + a, b = on_part.workplanes, on_g131.workplanes + if len(a) != 1 or len(b) != 1: + error("the preview draws %d plane(s) under the part's type and %d under G13.1, not one" + % (len(a), len(b))) + elif not close(a[0].origin, b[0].origin) or not all(close(u, v) for u, v in zip(a[0].axes, b[0].axes)): + error("the preview draws the plane under G13.1 at %s %s, under the part's type at %s %s" + % (show(b[0].origin), b[0].axes, show(a[0].origin), a[0].axes)) + end_part = on_part.program_geometry.positions()[-1] + end_g131 = on_g131.program_geometry.positions()[-1] + if not close(end_part, end_g131): + error("the preview ends the cut under G13.1 at %s, under the part's type at %s" + % (show(end_g131), show(end_part))) + +# The live plot is on the part too, so the tool marker that stands at its +# end is: under G13.1 in the plane the point the machine plots is the one +# the part's type plots at the same joints, and a switch to that type and +# back moves it nowhere and draws no line, at the moment the type has +# changed and the plane's map not yet too. +lp = linuxcnc.positionlogger(linuxcnc.stat(), (255, 255, 0, 255), (255, 0, 0, 255), + (0, 255, 0, 255), (0, 0, 255, 255), (255, 255, 255, 255), + (0, 255, 255, 255), "XYZ", 0) +sampler = threading.Thread(target=lp.start, args=(0.002,), daemon=True) +sampler.start() + +def plotted(): + # the plot once it has caught up with the tool on the part in status, + # or where it stands after a second + deadline = time.time() + 1 + while True: + s.poll() + status = [s.part_position[i] - s.tool_offset[i] for i in range(3)] + plot = lp.last(0)[:3] + if max(abs(p - q) for p, q in zip(status, plot)) < 1e-4 or time.time() > deadline: + return plot + time.sleep(0.002) + +def logged(): + raw, n, xyuv = lp.points() + return np.frombuffer(raw, dtype=glcanon_bake.LOGGER_DTYPE, count=n) + +run(TCP, [plane, "G53.1", "G1 X-10 Y5 Z20 F60000"]) +on_part = plotted() +s.poll() +tool_on_part = s.tool_frame +run("G13.1", [plane, "G53.1", "G1 X-10 Y5 Z20 F60000"]) +on_g131 = plotted() +if max(abs(p - q) for p, q in zip(on_part, on_g131)) > 1e-4: + error("under G13.1 the live plot is at %s, under the part's type at %s" + % (show(on_g131), show(on_part))) +# and the tool marker stands where the part's type has the tool against +# the part +s.poll() +if s.tool_frame is None or tool_on_part is None \ + or max(abs(p - q) for p, q in zip(s.tool_frame, tool_on_part)) > 1e-9: + error("under G13.1 status has the tool frame %s, under the part's type %s" + % (s.tool_frame, tool_on_part)) +start = len(logged()) +for i in range(3): + mdi(TCP) + mdi("G13.1") +back = plotted() +if max(abs(p - q) for p, q in zip(on_g131, back)) > 1e-4: + error("a switch to the part's type and back moved the live plot from %s to %s" + % (show(on_g131), show(back))) +for q in logged()[max(start - 1, 0):]: + if np.abs(q['pos'] - np.asarray(back)).max() > 1e-3: + error("a switch in the plane drew a line in the live plot to %s" % show(q['pos'])) + break + +# With no plane, under G13.1 alone, the preview and the live plot are on +# the part as well: the slides put the tool on the part where the part's +# type reads the joints. A rotary turned under G13.1 sweeps the tool round +# the part, which the preview draws through the points the machine passes. +def on_part_now(): + # status, then the live plot, where the tool stands against the part + s.poll() + status = [s.part_position[i] - s.tool_offset[i] for i in range(3)] + plot = plotted() + if max(abs(p - q) for p, q in zip(status, plot)) > 1e-3: + error("the live plot is at %s, status has the tool on the part at %s" + % (show(plot), show(status))) + return status + +def drawn_at(canon, where, letter, value): + # the vertex drawn where the axis stands at a value, in machine units + pg = canon.program_geometry + turned = np.abs(pg.axis_position(letter) - value) < 1e-4 + turned &= pg.kinds != glcanon_bake.KIND_NOOP + if not turned.any(): + error("%s: the preview draws no point at %s%g" % (where, letter, value)) + return None + return [25.4 * v for v in pg.positions()[np.nonzero(turned)[0][-1]]] + +turn = ["G43 H1", "G0 %s30 %s20" % (FIRST, SECOND), "G1 X10 Y5 Z20 F60000", + "G1 %s60 F60000" % SECOND] +for cmd in (RESET, START): + mdi(cmd) +canon = rendered("a rotary turned under G13.1", "\n".join(turn) + "\nM2\n", axes=True) +run("G13.1", turn[:-1] + ["G1 %s40 F60000" % SECOND]) +middle = on_part_now() +mdi(TCP) +s.poll() +tcp = [s.position[i] - s.tool_offset[i] for i in range(3)] +if max(abs(p - q) for p, q in zip(middle, tcp)) > 1e-6: + error("status has the tool on the part at %s under G13.1, the part's type at %s" + % (show(middle), show(tcp))) +mdi("G13.1") +mdi("G1 %s60 F60000" % SECOND) +end = on_part_now() +if canon: + for where, value, want in (("half way round", 40, middle), ("at the end", 60, end)): + got = drawn_at(canon, "a rotary turned under G13.1, " + where, SECOND, value) + if got and max(abs(p - q) for p, q in zip(got, want)) > 1e-3: + error("a rotary turned under G13.1, %s: the preview draws %s, the machine is at %s" + % (where, show(got), show(want))) + +lp.stop() +sampler.join(5) + finish() From 0bd6cac3eaaa5a3866176d596eb092eb74a7bfe9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 27 Oct 2026 13:31:45 +1100 Subject: [PATCH 28/44] preview: read the point back from the joints at a kinematics switch A switch moves no joint, but the point changes: on the machine the queue drains and task reads the new point from motion. A preview has no motion to ask, so its interpreter went on from the old type's coordinates under the new type. At a tilt the moves after a switch were drawn tens of millimetres from where they run: on the xyzac-trt sim at A30 C20, G91 G1 X5 after G12.1 P1 was drawn ending at X15 Y5 Z20 instead of X53.8 Y72.5 Z-20.9. In a tilted work plane taken again at the same joints the program's coordinates do not change, which is why a switch in the plane already previewed right. The preview's interpreter now takes the joints before the switch and reads the point from them under the new type, for G12.1, G13.1 and the switch G49 makes after G43.4. The renderer takes that point from the interpreter at the next thing it draws, so the drawing goes on from where the tool is, in the same numbers the next move starts from. Task does not read the point back: it gets it from motion, as before. tests/twp-static-preview switches both ways at a tilt with no plane and checks that the move after the switch ends where the machine ends it. --- src/emc/rs274ngc/gcode_renderer.cc | 28 ++++++++++++++++++++ src/emc/rs274ngc/gcode_renderer.hh | 3 +++ src/emc/rs274ngc/gcodemodule.cc | 20 +++++++++++++- src/emc/rs274ngc/gcodemodule.hh | 8 ++++++ src/emc/rs274ngc/interp_convert.cc | 19 +++++++++----- src/emc/rs274ngc/interp_internal.hh | 2 ++ src/emc/rs274ngc/interp_setup.cc | 2 ++ src/emc/rs274ngc/interp_workplane.cc | 39 ++++++++++++++++++++++++++++ src/emc/rs274ngc/rs274ngc_interp.hh | 2 ++ tests/twp-static-preview/README | 5 +++- tests/twp-static-preview/test-ui.py | 20 ++++++++++++++ 11 files changed, 140 insertions(+), 8 deletions(-) diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 0631e65b14d..04c7f40e674 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -97,6 +97,30 @@ void GCodeRenderer::set_g68_drawn(const WorkFrame &drawn) { workplane_ = (long)planes.size() - 1; } +// A kinematics switch moves no joint, so the point the next move starts +// from, in the world of the type in force, is where the new type reads the +// joints; drawn on the part it stays where it was. The interpreter reads it +// back once the switch is through, after the plane it may take again, so +// the chain point follows at the next thing drawn. Where nothing can be +// evaluated here it keeps its numbers, as it always has. +void GCodeRenderer::select_kins_type(int /*type*/) { + if(parse_state.interp_error) return; + kins_switched_ = true; +} + +// The point the interpreter read back, through the frame in force then, +// the one its program point is in. The same numbers as the next move's +// start, so a move that turns no rotary is not cut up for one. +void GCodeRenderer::after_kins_switch() { + if(!kins_switched_) return; + kins_switched_ = false; + Point9 prog, p; + if(!point_after_switch(prog)) return; + transform(prog, p); + lo_ = p; + if(timer_) { time_lo_ = p; time_lo_x_ = prog[P9_X]; } +} + void GCodeRenderer::arc_feed(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c, @@ -1142,6 +1166,9 @@ void GCodeRenderer::move(Kind kind, int line_number, const Point9 &in, double rate) { last_line_ = line_number; consumed_ = true; + // an offset change moves the chain point by itself; the point read back + // comes in at whatever follows, in the new offset as the program has it + if(kind != ToolOffset) after_kins_switch(); if(kind >= Dwell) { event(kind, line_number, in); return; } // A hidden move touches nothing at all, not even the chain point - and it // is not timed either: `(AXIS,hide)` is what a program wraps the parts a @@ -1333,6 +1360,7 @@ void GCodeRenderer::render_arc(int line_number, double first_end, double second_ double u, double v, double w, double rate) { last_line_ = line_number; consumed_ = true; + after_kins_switch(); if(suppress_ > 0) return; double radius = 0.0; arc_segments(lo_, plane_, rotation_cos_, rotation_sin_, diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 7b33ef78061..08aa55e2c75 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -417,6 +417,7 @@ public: frame_ = frame; } void set_g68_drawn(const WorkFrame &drawn) override; + void select_kins_type(int type) override; // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -556,6 +557,7 @@ private: // The inverse, to recover an arc segment's program X. void untransform(const Point9 &in, Point9 &out) const; void event(Kind kind, int line_number, const Point9 &axes); + void after_kins_switch(); pybind11::handle canon_; // borrowed, as parse_state.callback is pybind11::object progress_; // canon.renderer_progress, or empty @@ -573,6 +575,7 @@ private: WorkFrame frame_; // the tilted work plane, inside g92 long workplane_ = -1; // its record, or none in effect bool lo_unreached_ = false; // lo_ reached under it by an undrawn traverse + bool kins_switched_ = false; // the chain point waits for the point read back Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index b1684ecac33..c0526c09efa 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -707,7 +707,9 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {parse_state.selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void SELECT_KINS_TYPE(int /*switchkins_type*/) {} +void SELECT_KINS_TYPE(int switchkins_type) { + parse_state.canon->select_kins_type(switchkins_type); +} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} @@ -1065,6 +1067,18 @@ bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out) { }); } +bool point_after_switch(Point9 &out) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return false; + setup_pointer s = &interp->_setup; + if(!s->kins_switch_read) return false; + s->kins_switch_read = false; + out = ensure_inch({s->current_x, s->current_y, s->current_z, + s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current}); + return true; +} + #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) // The parse. `initcodes` is the list of the new signature or null for the @@ -1187,6 +1201,10 @@ static py::object parse_file(const char *f, py::handle canon, // program's G53, so it waits for the program. own = dynamic_cast(parse_state.pinterp); if(own) { + // no motion reports the point after a kinematics switch here: the + // interpreter reads it back from the joints itself + own->_setup.kins_switch_reads_back = true; + own->_setup.kins_switch_read = false; machine_moves = own->_setup.machine_moves_in_machine_frame; own->_setup.machine_moves_in_machine_frame = false; // and a tilted work plane they restate is the one status has diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index a91319f1f1b..78fd2e76d08 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -186,6 +186,10 @@ public: // the moves back to. Elsewhere the two are the same. Only the renderer // listens. virtual void set_g68_drawn(const WorkFrame & /*drawn*/) {} + // A kinematics switch, before the interpreter takes it: the machine + // does not move, and the point a renderer chains from is read back + // from the joints under the new type. Only the renderer listens. + virtual void select_kins_type(int /*type*/) {} virtual void set_plane(int plane) = 0; virtual void set_feed_rate(double rate) = 0; virtual void set_traverse_rate(double rate) = 0; @@ -332,5 +336,9 @@ double read_external_angle_units(); // the same: under the primary type, on a machine without one, and where // the kinematics cannot be evaluated here. bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out); +// The program's point where the interpreter read it back from the joints +// at the last kinematics switch, in inches, once: the drawing goes on from +// the point the program does. False where it read nothing back. +bool point_after_switch(Point9 &out); #endif // GCODEMODULE_HH diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index a64158b8a41..ccc78ad976d 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6950,11 +6950,13 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // declares no such type keeps the plain cancel. int identity = flagged_kins_type(KINSTYPE_MACHINE); if (identity >= 0 && identity != settings->kins_type) { - double joints[EMCMOT_MAX_JOINTS]; - bool have; + double joints[EMCMOT_MAX_JOINTS], kept[EMCMOT_MAX_JOINTS]; + bool have, back; CHP(snap_before_switch(settings, joints, &have)); + switch_joints(settings, kept, &back); switch_kins_type(identity, settings); CHP(snap_after_switch(settings, joints, have)); + if (back) { switch_point(settings, kept); } } settings->kins_by_g43_4 = false; } @@ -7042,13 +7044,18 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 } // a tilted work plane stays; the joints stay, so its second feature - // coordinate system is taken where they are, or dropped - double joints[EMCMOT_MAX_JOINTS]; - bool have = false, changed = (kins_type != settings->kins_type); - if (changed) { CHP(snap_before_switch(settings, joints, &have)); } + // coordinate system is taken where they are, or dropped, and a preview + // reads the point back from them + double joints[EMCMOT_MAX_JOINTS], kept[EMCMOT_MAX_JOINTS]; + bool have = false, back = false, changed = (kins_type != settings->kins_type); + if (changed) { + CHP(snap_before_switch(settings, joints, &have)); + switch_joints(settings, kept, &back); + } SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; if (changed) { CHP(snap_after_switch(settings, joints, have)); } + if (back) { switch_point(settings, kept); } // the program has taken the kinematics over from G43.4 settings->kins_by_g43_4 = false; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 517d81ad9fe..b9ebed90451 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -842,6 +842,8 @@ struct setup bool probe_flag; // flag indicating probing done bool input_flag; // flag indicating waiting for input done bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done + bool kins_switch_reads_back; // a preview: no motion reports the point after a switch, so it is read from the joints here + bool kins_switch_read; // and it was, for the preview's drawing to go on from that same point int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 2367ea74442..0fa8e36257b 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -142,6 +142,8 @@ setup::setup() : probe_flag(0), input_flag(0), kinsSwitch_flag(0), + kins_switch_reads_back(false), + kins_switch_read(false), kins_type(0), kins_by_g43_4(false), tool_vector(false), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 7ba31d4f6bb..f995e8c0095 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -996,6 +996,45 @@ bool Interp::kins_on_part(setup_pointer s, const double world[9], double part[9] return primary >= 0 && kins_carry(s, s->kins_type, primary, world, part); } +// The joints a kinematics switch keeps, for a preview to read the point +// back from under the new type: on the machine the queue drains at the +// switch and task reads the point from motion, but a preview has no +// motion to ask. have stays false where nothing can be evaluated. +void Interp::switch_joints(setup_pointer s, double *joints, bool *have) +{ + void *vctx = s->kins_switch_reads_back ? kins_preview(s) : NULL; + + *have = vctx && current_joints(s, vctx, joints) == INTERP_OK; +} + +// After the switch: the point where the new type reads those joints, as +// motion reads it. In a tilted work plane taken again at the same joints +// it comes out where it was. +void Interp::switch_point(setup_pointer s, const double *joints) +{ + void *vctx = kins_preview(s); + EmcPose pose; + double prog[9]; + int i; + + if (!vctx) { return; } + // the point as it stood, the estimate an iterative forward starts from + current_machine_pose(s, &pose); + if (kinematicsUserForward((KinematicsUserContext *)vctx, joints, &pose) != 0) { return; } + machine_pose_to_program(s, &pose, prog); + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + s->kins_switch_read = true; +} + // Where the point goes when the tool offset changes: the joints stay, so it is // read back from them under the new offset, as motion reads it. known stays // false under the identity and where nothing can be evaluated; the caller diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index ad123e6f914..6abfd18c8f4 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -402,6 +402,8 @@ public: bool kins_part_ready(setup_pointer settings); bool kins_carry(setup_pointer settings, int from, int to, const double world[9], double out[9]); bool kins_on_part(setup_pointer settings, const double world[9], double part[9]); + void switch_joints(setup_pointer settings, double *joints, bool *have); + void switch_point(setup_pointer settings, const double *joints); bool kins_machine_frame(setup_pointer settings); int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, EmcPose *point, bool *known); diff --git a/tests/twp-static-preview/README b/tests/twp-static-preview/README index f5345846659..d28b63b5b5a 100644 --- a/tests/twp-static-preview/README +++ b/tests/twp-static-preview/README @@ -25,4 +25,7 @@ to the TCP kinematics and back neither move it nor draw a line. With no plane, a rotary turned under G13.1 at a tilted head or table: the preview draws the turn through the point the machine reaches half way and ends where it ends, and the live plot and status part_position agree, the -TCP kinematics reading the same point from the same joints. +TCP kinematics reading the same point from the same joints. A switch to +the TCP kinematics and one to G13.1, at a tilt and with no plane: the +preview reads the point back from the joints as the machine does, and the +move after the switch ends where the machine ends it. diff --git a/tests/twp-static-preview/test-ui.py b/tests/twp-static-preview/test-ui.py index a164d9c34e5..213b647378d 100755 --- a/tests/twp-static-preview/test-ui.py +++ b/tests/twp-static-preview/test-ui.py @@ -356,6 +356,26 @@ def drawn_at(canon, where, letter, value): error("a rotary turned under G13.1, %s: the preview draws %s, the machine is at %s" % (where, show(got), show(want))) +# A switch moves no joint: the preview's interpreter reads the point back +# from the joints under the new type, as the machine does, and goes on +# from there, with a rotary turned and no plane, both ways. +tilt = "G0 %s30 %s20" % (FIRST, SECOND) +switches = { + "a switch to the part's type": ["G43 H1", tilt, "G1 X10 Y5 Z20 F60000", TCP, + "G91 G1 X5", "G90"], + "a switch to G13.1": [TCP, "G43 H1", tilt, "G1 X10 Y5 Z20 F60000", "G13.1", "G1 X5"], +} +for name, lines in switches.items(): + for cmd in (RESET, START): + mdi(cmd) + canon = rendered(name, "\n".join(lines) + "\nM2\n") + run("G13.1", lines) + end = on_part_now() + if canon: + got = [25.4 * v for v in canon.program_geometry.positions()[-1]] + if max(abs(p - q) for p, q in zip(got, end)) > 1e-3: + error("after %s the preview ends at %s, the machine at %s" + % (name, show(got), show(end))) lp.stop() sampler.join(5) From b417322696d77f1f59ec02b80198f5fe7584f562 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 28 Oct 2026 20:10:40 +1100 Subject: [PATCH 29/44] kinematics: frames from the pose, for a type whose forward iterates The frame entry points take the joints alone. A parallel machine has its closed form in the inverse: the orientation of its platform is the A B C of the pose, and the joints give it only through an iterative forward that needs a seed, which the frame functions have no argument for. So genhexkins and pentakins could report no frames, and got none of what reads them: the status tool frame, the tool jog frame, G68.3, the G53.n solve. A type may now state its frames from the pose instead, as pose_work and pose_tool in its ops table, and declare the letters that orient the tool in orient. kinsOpsWorkFrame() and kinsOpsToolFrame() take the caller's scratch, read only. On such a type they use the pose of the last forward or inverse while the joints are still those, which is motion's case every cycle: the pose it just inverted, or the forward of the commanded joints in free mode. Otherwise they run a forward on a copy of the scratch, started from that pose, so the caller's seed does not move. The At forms take the pose a caller already holds. No forward runs for the status frame. This matches the controllers that drive these machines: PI, Siemens (the setpoint orientation, $AC_TOOLO_ACT), Turbo PMAC and the NIST hexapod all work from the commanded pose, with the orientation as virtual axes. switchkins records the pair for a forward of a type not in force too, so the primary type's frame under the machine frame type reads the pose motion found for it. kinematicsUserOrientAxes() answers the declared letters on such a type, all of them a head's. --- include/kinematics_user.h | 3 +- include/kins_module.h | 49 +++++++- src/emc/kinematics/kins_single.c | 4 +- src/emc/kinematics/kins_util.c | 117 ++++++++++++++++-- src/emc/kinematics/switchkins.c | 9 +- .../kinematics_userspace/kinematics_user.c | 23 +++- 6 files changed, 182 insertions(+), 23 deletions(-) diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 445defce1e3..8338cbc2c9b 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -291,7 +291,8 @@ int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, * declared orient, else those the joint map gives the joints. Three * rotaries come tables first, each group in joint order. A type whose * tool is turned by more than three joints and no table, a robot arm, - * orients with the A B C of its pose, all three head. Returns how many + * orients with the A B C of its pose, all three head; so does a type with + * pose frames, a parallel machine, with the letters it declares. Returns how many * axes, 2 or 3, the unused entries -1; or -1 with every entry -1 where the * type turns other rotaries between tool and work, or a letter cannot be * found. diff --git a/include/kins_module.h b/include/kins_module.h index d29bc143332..68d326519e0 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -313,6 +313,12 @@ typedef struct kins_scratch { int failed; double aux[8]; /* whatever else a module carries between calls */ double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ + /* on a type with pose frames, the joints and the pose of the last + forward or inverse that succeeded, which the frames read the pose + from while the joints are still these */ + double pair_joint[EMCMOT_MAX_JOINTS]; + EmcPose pair_pose; + int have_pair; } kins_scratch; typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, @@ -329,6 +335,12 @@ typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* A frame as a function of the pose, for a type whose closed form is the + inverse, a parallel machine, where the pose is what states the + orientation and the joints give it only through an iterative forward. */ +typedef int (*kins_pose_frame_fn)(const kins_params *p, const EmcPose *pos, + PmRotationMatrix *rot); + typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], @@ -356,7 +368,10 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, slides says the joints are the machine's slides and rotaries, each in the letter the joint map gives it, so that the carriage is the joints read through the map: the machine frame of a module with one type, - which has no machine frame type to name. */ + which has no machine frame type to name. + pose_work and pose_tool are the frames of a type that states them from + the pose instead, a parallel machine; such a type sets them and not work + and tool, and declares orient, the letters of the pose that orient. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -370,6 +385,8 @@ typedef struct kins_ops { int machine; /* the machine frame */ const char *orient; /* orienting letters, NULL: from the joint map */ int slides; /* joints are the slides, letters by the map */ + kins_pose_frame_fn pose_work; + kins_pose_frame_fn pose_tool; } kins_ops; /* A module described for a caller outside RT: its table, its joint @@ -439,13 +456,23 @@ extern int kinsIdentityJacobian(const kins_params *p, const double *joint, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityPoseFrame(const kins_params *p, const EmcPose *pos, + PmRotationMatrix *rot); extern const kins_ops KINS_IDENTITY_OPS; /* The five questions asked of an ops table, with the defaults applied: identity for a missing frame, the native rotation applied to the tool frame, and the Jacobian differenced from the inverse when there is no closed form. These are what the RT wrappers and a caller outside RT - both go through, so both get the same answers. */ + both go through, so both get the same answers. + + The frames take the caller's scratch, read only. A type with pose + frames reads the pose from it: the pose of the last forward or inverse + while the joints are still those, else a forward of the joints started + from that pose, on a copy, so the caller's seed stays where it was. + Without a scratch, or before any call has filled it, such a type has no + pose to start from and answers -1. The At forms take the pose the + caller holds for the joints, and use it on a type with pose frames. */ extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, kins_scratch *s, const double *joint, EmcPose *pos, const KINEMATICS_FORWARD_FLAGS *fflags, @@ -455,11 +482,29 @@ extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags); extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrameAt(const kins_ops *ops, const kins_params *p, + const double *joint, const EmcPose *pos, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrameAt(const kins_ops *ops, const kins_params *p, + const double *joint, const EmcPose *pos, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +/* Record the joints and the pose of a forward or inverse that ran on the + ops table directly rather than through kinsOpsForward() or + kinsOpsInverse(), so the pose frames read that pose; r is what the call + returned, and a failure forgets the pair. Nothing on a type without pose + frames. */ +extern void kinsOpsKeepPair(const kins_ops *ops, const kins_params *p, + kins_scratch *s, int r, const double *joint, + const EmcPose *pos); extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, kins_scratch *s, const double *joint, const EmcPose *pos, diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index fb08407b951..e63956ead4f 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -101,7 +101,7 @@ int kinematicsWorkFrame(const double *joint, { if (!inited) { return -1; } read_pins(); - return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); + return kinsOpsWorkFrame(ops(), &rt_params, &rt_scratch, joint, rot, fflags); } int kinematicsToolFrame(const double *joint, @@ -110,7 +110,7 @@ int kinematicsToolFrame(const double *joint, { if (!inited) { return -1; } read_pins(); - return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); + return kinsOpsToolFrame(ops(), &rt_params, &rt_scratch, joint, rot, fflags); } int kinematicsJacobian(const double *joint, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index ac75f0e41de..752d4ca3ead 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1667,6 +1667,14 @@ int kinsIdentityJacobian(const kins_params *p, const double *joint, return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); } +int kinsIdentityPoseFrame(const kins_params *p, const EmcPose *pos, + PmRotationMatrix *rot) +{ + (void)p; (void)pos; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + const kins_ops KINS_IDENTITY_OPS = { .forward = kinsIdentityForward, .inverse = kinsIdentityInverse, @@ -1704,9 +1712,9 @@ static int tool_across(const kins_ops *ops, const kins_params *p, PmRotationMatrix work, r; const double x = p->tool.tran.x, y = p->tool.tran.y; - if (kinsOpsToolFrame(ops, p, joint, &r, &fflags)) { return -1; } + if (kinsOpsToolFrame(ops, p, NULL, joint, &r, &fflags)) { return -1; } // where nothing turns the work, the work frame is the machine's - if (kinsOpsWorkFrame(ops, p, joint, &work, &fflags) == 0 + if (kinsOpsWorkFrame(ops, p, NULL, joint, &work, &fflags) == 0 && toolFrameInWork(&work, &r, &r)) { return -1; } d->x = x - (r.x.x * x + r.y.x * y); d->y = y - (r.x.y * x + r.y.y * y); @@ -1766,6 +1774,49 @@ static int tool_across_jacobian(const kins_ops *ops, const kins_params *p, // asking an ops table, defaults applied //---------------------------------------------------------------------- +// The pose at these joints for a type with pose frames: the pair the +// scratch last answered for while the joints are still those, else a +// forward started from that pair's pose, or from the last forward's, on a +// copy of the scratch. -1 with no pose to start from. +static int pose_of_joints(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, + EmcPose *pos) +{ + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + KINEMATICS_FORWARD_FLAGS ffl = 0; + KINEMATICS_INVERSE_FLAGS ifl = 0; + kins_scratch c; + int same = 1; + + if (!s || !joint) { return -1; } + if (s->have_pair) { + *pos = s->pair_pose; + for (int i = 0; i < n && same; i++) { + same = fabs(joint[i] - s->pair_joint[i]) <= 1e-12 * (1.0 + fabs(joint[i])); + } + if (same) { return 0; } + } else if (s->pose_seed_ok) { + *pos = s->pose_seed; + } else { + return -1; + } + c = *s; + return ops->forward(p, &c, joint, pos, &ffl, &ifl); +} + +void kinsOpsKeepPair(const kins_ops *ops, const kins_params *p, + kins_scratch *s, int r, const double *joint, + const EmcPose *pos) +{ + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + + if (!ops || !s || (!ops->pose_tool && !ops->pose_work)) { return; } + s->have_pair = (r == 0); + if (r != 0) { return; } + memcpy(s->pair_joint, joint, n * sizeof(joint[0])); + s->pair_pose = *pos; +} + int kinsOpsForward(const kins_ops *ops, const kins_params *p, kins_scratch *s, const double *joint, EmcPose *pos, const KINEMATICS_FORWARD_FLAGS *fflags, @@ -1789,6 +1840,7 @@ int kinsOpsForward(const kins_ops *ops, const kins_params *p, s->pose_seed = *pos; s->pose_seed_ok = 1; } + kinsOpsKeepPair(ops, p, s, r, joint, pos); if (r == 0 && across) { pose_shift(pos, &d, 1); } return r; } @@ -1805,7 +1857,9 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, if (!ops || !ops->inverse || !p || !s) { return -1; } if (!tool_across_applies(ops, p)) { - return ops->inverse(p, s, pos, joint, iflags, fflags); + r = ops->inverse(p, s, pos, joint, iflags, fflags); + kinsOpsKeepPair(ops, p, s, r, joint, pos); + return r; } // a module may read where the machine is from the joints it is // handed, so every pass starts from the caller's @@ -1824,29 +1878,66 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, memcpy(joint, seed, n * sizeof(seed[0])); r = ops->inverse(p, s, &q, joint, iflags, fflags); } + kinsOpsKeepPair(ops, p, s, r, joint, pos); return r; } +int kinsOpsWorkFrameAt(const kins_ops *ops, const kins_params *p, + const double *joint, const EmcPose *pos, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (ops->work) { return ops->work(p, joint, rot, fflags); } + if (!ops->pose_work || !pos) { return -1; } // not supplied; not an error + return ops->pose_work(p, pos, rot); +} + +int kinsOpsToolFrameAt(const kins_ops *ops, const kins_params *p, + const double *joint, const EmcPose *pos, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (ops->tool) { + r = ops->tool(p, joint, rot, fflags); + } else if (ops->pose_tool && pos) { + r = ops->pose_tool(p, pos, rot); + } else { + return -1; // not supplied; not an error + } + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + EmcPose pos; if (!ops || !p || !rot) { return -1; } - if (!ops->work) { return -1; } // not supplied; not an error - return ops->work(p, joint, rot, fflags); + if (ops->work || !ops->pose_work) { + return kinsOpsWorkFrameAt(ops, p, joint, NULL, rot, fflags); + } + if (pose_of_joints(ops, p, s, joint, &pos)) { return -1; } + return kinsOpsWorkFrameAt(ops, p, joint, &pos, rot, fflags); } int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - int r; + EmcPose pos; if (!ops || !p || !rot) { return -1; } - if (!ops->tool) { return -1; } // not supplied; not an error - r = ops->tool(p, joint, rot, fflags); - if (r) { return r; } - return toolFrameApplyNative(rot, ops->native ? ops->native - : &TOOL_FRAME_SPINDLE); + if (ops->tool || !ops->pose_tool) { + return kinsOpsToolFrameAt(ops, p, joint, NULL, rot, fflags); + } + if (pose_of_joints(ops, p, s, joint, &pos)) { return -1; } + return kinsOpsToolFrameAt(ops, p, joint, &pos, rot, fflags); } int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, @@ -2074,6 +2165,10 @@ EXPORT_SYMBOL(kinsOpsForward); EXPORT_SYMBOL(kinsOpsInverse); EXPORT_SYMBOL(kinsOpsWorkFrame); EXPORT_SYMBOL(kinsOpsToolFrame); +EXPORT_SYMBOL(kinsOpsWorkFrameAt); +EXPORT_SYMBOL(kinsOpsToolFrameAt); +EXPORT_SYMBOL(kinsOpsKeepPair); +EXPORT_SYMBOL(kinsIdentityPoseFrame); EXPORT_SYMBOL(kinsOpsJacobian); EXPORT_SYMBOL(kinsParamsPinsCreate); EXPORT_SYMBOL(kinsParamsPinsRead); diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index a3f07c25886..b3cb0eb634a 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -149,6 +149,9 @@ static int call_forward(int ktype, const double *joint, EmcPose *pos, read_block(ktype); r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], joint, pos, fflags, iflags); + // the frames of a type that is not in force, motion's primary under + // the machine frame type, read the pose from here + kinsOpsKeepPair(kops[ktype], &rt_params, &rt_scratch[ktype], r, joint, pos); write_block(ktype); return r; } @@ -334,7 +337,7 @@ int kinematicsToolFrameOf(int ktype, if (ktype < 0 || ktype >= kins_count) { return -1; } if (kops[ktype]) { read_block(ktype); - return kinsOpsToolFrame(kops[ktype], &rt_params, joint, rot, fflags); + return kinsOpsToolFrame(kops[ktype], &rt_params, &rt_scratch[ktype], joint, rot, fflags); } if (!ktools[ktype]) { return -1; // this type does not supply one; not an error @@ -355,7 +358,7 @@ int kinematicsWorkFrameOf(int ktype, if (ktype < 0 || ktype >= kins_count) { return -1; } if (kops[ktype]) { read_block(ktype); - return kinsOpsWorkFrame(kops[ktype], &rt_params, joint, rot, fflags); + return kinsOpsWorkFrame(kops[ktype], &rt_params, &rt_scratch[ktype], joint, rot, fflags); } if (!kworks[ktype]) { return -1; // this type does not supply one; not an error @@ -537,7 +540,7 @@ int switchkinsRegisterOps(int ktype, const kins_ops *ops) register_error = 1; return -1; } - if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + if ((ops->tool || ops->pose_tool) && ops->native && !toolFrameIsProper(ops->native)) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegisterOps: switchkins-type %d" " declared a rotation that is not orthonormal with" diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 248a15152c1..b10c176400c 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -654,14 +654,14 @@ static int frame_work(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { KinematicsUserContext *ctx = frame_ctx; - return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, joint, rot, fflags); } static int frame_tool(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { KinematicsUserContext *ctx = frame_ctx; - return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, joint, rot, fflags); } static void pad_joints(KinematicsUserContext *ctx, const double *in, double *out) @@ -681,7 +681,7 @@ int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; refresh(ctx); pad_joints(ctx, joints, j); - return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, rot, &fflags); } int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, @@ -693,7 +693,7 @@ int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; refresh(ctx); pad_joints(ctx, joints, j); - return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, rot, &fflags); } int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, @@ -816,6 +816,21 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; ops = ctx->info.ops[ctx->ktype]; + // a parallel machine orients with the letters of its pose, a head's + if (!ops->tool && ops->pose_tool) { + int n = ops->orient ? (int)strlen(ops->orient) : 0; + if (n < 2 || n > 3) return -1; + for (i = 0; i < n; i++) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[i])); + if (!at) { + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + return -1; + } + axes[i] = (int)(at - letters); + head[i] = 1; + } + return n; + } if (!ops->tool || !ops->work) return -1; refresh(ctx); pad_joints(ctx, seed, j); From 3cb5b4063e1c3adfa26b2ff13d841188ec1b5d3b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 28 Oct 2026 20:10:55 +1100 Subject: [PATCH 30/44] genhexkins, pentakins: state the frames from the pose genhexkins' tool frame is its platform frame, Rz(C) Ry(B) Rx(A) as its inverse builds it, the frame PI's hexapods call the tool frame and Fanuc's robots write as W P R. The tool length goes onto the platform joints' z, so platform z runs from the tip towards the holder, the convention's tool axis. The orienting letters are A B C. pentakins' is its effector frame, Ry(B) Rx(A). The ring joints leave the turn about the spindle free in the machine, so x is the chain's, machine x turned by Ry(B): the unit vector square to the spindle in the machine's Z/X plane, the choice Siemens' TOFRAME makes (MD21110). To every consumer the pentapod is a head with B carrying A, and orients with A B. Neither turns the work. On the hexapod sim the status tool frame now follows the platform: at A10 B-5 C15 it is the rotation those angles describe to 1e-16, and under M429, the identity type, it is still the genhexkins frame. tests/kins-frames checks both modules from poses over the sim configs' geometry and travel: the frame at the joints of a pose is the rotation its angles describe, the tool length goes along the reported axis in the module's own maths, and joints a cycle away from the last inverse get the frame of their own pose, with the last answer left alone. Composing the frame in the other order fails all three on both modules; using the last pose for any joints fails the third. tests/kins-params checks the letters, A B C and A B. --- docs/src/motion/kinematics-conventions.adoc | 19 ++- src/emc/kinematics/genhexkins.c | 21 +++ src/emc/kinematics/pentakins.c | 23 +++ tests/genhexkins-forward/hexfwdcheck.c | 13 ++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 150 +++++++++++++++++++- tests/kins-frames/test.sh | 8 ++ tests/kins-params/check.py | 26 +++- tests/kins-params/test.sh | 19 ++- 9 files changed, 264 insertions(+), 17 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 3ce73233498..526b4155744 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -304,11 +304,20 @@ the stylus faces. The frames are optional. A module that leaves them out answers neither, and a caller that needs them, a tilted work plane first of all, refuses under it. Of the in-tree modules whose joints tilt the tool or the work, all report both -frames except scorbot-kins, genhexkins and pentakins. scorbot-kins controls the -wrist centre and passes the wrist joints through as A and B, so it has no tool -frame to report. genhexkins and pentakins solve the forward iteratively, so the -platform's orientation needs the seed the forward keeps, which the frame calls -do not take. +frames except scorbot-kins. scorbot-kins controls the wrist centre and passes +the wrist joints through as A and B, so it has no tool frame to report. + +genhexkins and pentakins state their frames from the pose. Their joints are +struts, the orientation is the A B C or the A B of the pose, and the joints give +it only through a forward that iterates from a seed. So the frame is a function +of the pose: genhexkins' tool frame is its platform's, Rz(C) Ry(B) Rx(A), and +pentakins' its effector's, Ry(B) Rx(A), with x in the machine's Z/X plane, +since the ring joints leave the turn about the spindle free. Asked at joints, +the frame calls read the pose of the last forward or inverse for those joints, +which in motion is the pose just commanded, so no forward runs for it. Neither +turns the work. The machines that drive such kinematics work the same way: +Siemens, PI and the NIST hexapod take the orientation from the commanded pose, +as virtual axes, not from the actuators. The joint values that reach a requested orientation, through `kinematicsToolFrameInverse()`, which is the inverse of the pair above and the diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 3f6298f3e2e..630c864f53a 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -699,14 +699,35 @@ static int genhex_jacobian(const kins_params *p, const double * joints, return 0; } // genhex_jacobian() +/* The platform frame is the tool frame, R = Rz(c) Ry(b) Rx(a) as the + inverse builds it; the tool length goes onto the platform joints' z, so + platform z runs from the tip towards the holder, as the convention has + it. A function of the pose: the forward that would give it from the + struts iterates. */ +static int genhex_pose_tool(const kins_params *p, const EmcPose *pos, + PmRotationMatrix *rot) +{ + PmRpy rpy; + + (void)p; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + return pmRpyMatConvert(&rpy, rot) ? -1 : 0; +} + // the forward iterates from the pose it is handed, so it is seeded with // the last answer after a switch static const kins_ops genhex_ops = { .forward = genhex_forward, .inverse = genhex_inverse, + .pose_work = kinsIdentityPoseFrame, + .pose_tool = genhex_pose_tool, + .native = &TOOL_FRAME_SPINDLE, .jacobian = genhex_jacobian, .fwd_iterates = 1, .primary = 1, + .orient = "ABC", }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 31acd5cc68a..9c9ebb303ee 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -502,12 +502,35 @@ static int penta_jacobian(const kins_params *p, const double * joints, return 0; } +/* The effector frame is the tool frame, R = Ry(b) Rx(a) as the inverse + builds it, the spindle along its z. The ring joints leave the turn + about the spindle free in the machine, so x is the chain's, machine x + turned by Ry(b): the unit vector square to the spindle in the machine's + Z/X plane, and to every consumer the pentapod is a head with B carrying + A. A function of the pose: the forward that would give it from the + struts iterates. */ +static int penta_pose_tool(const kins_params *p, const EmcPose *pos, + PmRotationMatrix *rot) +{ + PmRpy rpy; + + (void)p; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = 0.0; + return pmRpyMatConvert(&rpy, rot) ? -1 : 0; +} + // the forward iterates from the pose it is handed static const kins_ops penta_ops = { .forward = penta_forward, .inverse = penta_inverse, + .pose_work = kinsIdentityPoseFrame, + .pose_tool = penta_pose_tool, + .native = &TOOL_FRAME_SPINDLE, .jacobian = penta_jacobian, .fwd_iterates = 1, + .orient = "AB", }; const kins_module_info kins_module = { diff --git a/tests/genhexkins-forward/hexfwdcheck.c b/tests/genhexkins-forward/hexfwdcheck.c index 62d42a614f4..7fdb58b7770 100644 --- a/tests/genhexkins-forward/hexfwdcheck.c +++ b/tests/genhexkins-forward/hexfwdcheck.c @@ -71,6 +71,19 @@ static void check(const EmcPose *pose) } seed.tran.x += off[0]; seed.tran.y += off[1]; seed.tran.z += off[2]; seed.a += off[3]; seed.b += off[4]; seed.c += off[5]; + /* the module keeps the last inverse's joints and pose, and answers a + forward of those joints from that pose; motion's feedback is never + exactly the last command, so solve the seed first and leave the + forward to Newton */ + { + double other[EMCMOT_MAX_JOINTS]; + if (kinematicsInverse(&seed, other, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "hexfwdcheck: FAIL inverse of the seed at %g %g %g %g %g %g\n", + seed.tran.x, seed.tran.y, seed.tran.z, seed.a, seed.b, seed.c); + failures++; + return; + } + } if (kinematicsForward(joints, &seed, &ff, &inf)) { rtapi_print_msg(RTAPI_MSG_ERR, "hexfwdcheck: FAIL forward did not converge at %g %g %g %g %g %g\n", diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index e92522018b1..19702e5749f 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 10 ] \ +[ "$(grep -c 'frames agree' "$1")" = 12 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index 8ca5ce37ca0..1efe4be0c66 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -17,6 +17,13 @@ * frame with it and does nothing else. That last one catches a frame * built for the wrong joint or composed in the wrong order. * + * A parallel machine states its frames from the pose, and its joints are + * struts, so it is checked from poses instead (pose_letters): the tool + * frame at the joints the inverse gives for a pose is the rotation the + * pose's angles describe, its axis is the one the module's maths puts the + * tool length along, and joints the inverse did not answer for last get + * the frame of their own pose, not of the last one. + * * Author: LinuxCNC * License: GPL Version 2 * System: Linux @@ -58,6 +65,15 @@ RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none static int abc_flange = 0; RTAPI_MP_INT(abc_flange, "1 when the forward reports the flange orientation as roll, pitch and yaw in A B C"); +static int pose_letters = 0; +RTAPI_MP_INT(pose_letters, "3 for a pose with A B C, 2 for A B, 0 for a module with joint frames"); + +static int pose_z = 0, pose_lin = 0, pose_ang = 0, tool_len = 0; +RTAPI_MP_INT(pose_z, "Z of the poses checked, a parallel machine's workspace is off the base"); +RTAPI_MP_INT(pose_lin, "X and Y of the poses checked run from -pose_lin to pose_lin"); +RTAPI_MP_INT(pose_ang, "angles of the poses checked run from -pose_ang to pose_ang"); +RTAPI_MP_INT(tool_len, "tool length the axis check puts on the tool"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -260,6 +276,132 @@ static void check(const double *j, int own_kinematics) } } +/* Rz(c) Ry(b) Rx(a), the frame a pose's roll, pitch and yaw describe, + worked out here rather than taken from posemath */ +static void rpy_frame(double a, double b, double c, PmRotationMatrix *m) +{ + const double ca = cos(a * TO_RAD), sa = sin(a * TO_RAD); + const double cb = cos(b * TO_RAD), sb = sin(b * TO_RAD); + const double cc = cos(c * TO_RAD), sc = sin(c * TO_RAD); + + m->x.x = cc * cb; m->x.y = sc * cb; m->x.z = -sb; + m->y.x = cc * sb * sa - sc * ca; m->y.y = sc * sb * sa + cc * ca; m->y.z = cb * sa; + m->z.x = cc * sb * ca + sc * sa; m->z.y = sc * sb * ca - cc * sa; m->z.z = cb * ca; +} + +static int same_frame(const PmRotationMatrix *a, const PmRotationMatrix *b) +{ + return close3(&a->x, b->x.x, b->x.y, b->x.z) + && close3(&a->y, b->y.x, b->y.y, b->y.z) + && close3(&a->z, b->z.x, b->z.y, b->z.z); +} + +static int inverse_at(const EmcPose *pos, double len, double *j) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + EmcPose tool; + + memset(&tool, 0, sizeof(tool)); + tool.tran.z = len; + kinematicsSetTool(&tool); + memset(j, 0, EMCMOT_MAX_JOINTS * sizeof(j[0])); + return kinematicsInverse(pos, j, &inf, &ff); +} + +static void check_pose(const EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, want, near_want; + EmcPose shifted, near; + double j[EMCMOT_MAX_JOINTS], jl[EMCMOT_MAX_JOINTS], jn[EMCMOT_MAX_JOINTS]; + int i, ok; + + rpy_frame(pos->a, pos->b, pos->c, &want); + + /* the joints of a pose: the frame is the pose's */ + if (inverse_at(pos, 0.0, j)) { + expect(0, "the inverse answers", j); + return; + } + ok = kinematicsToolFrame(j, &tool, &ff) == 0; + expect(ok, "the tool frame answers at the joints of a pose", j); + if (!ok) { return; } + expect(is_rotation(&tool), "tool frame is a rotation", j); + expect(same_frame(&tool, &want), "tool frame is the pose's Rz(C) Ry(B) Rx(A)", j); + + /* The tool length goes along the tool axis and the pose carries it on + the work axes, so a tool of length L at the pose leaves the struts + where no tool leaves them at the pose moved by L along the reported + axis and back by L along Z. The module's maths and the reported + frame have to agree for that to hold. */ + shifted = *pos; + shifted.tran.x += tool_len * tool.z.x; + shifted.tran.y += tool_len * tool.z.y; + shifted.tran.z += tool_len * (tool.z.z - 1.0); + if (inverse_at(pos, tool_len, jl) || inverse_at(&shifted, 0.0, j)) { + expect(0, "the inverse answers with a tool", j); + return; + } + ok = 1; + for (i = 0; i < joints; i++) { + if (fabs(jl[i] - j[i]) > TOL) { ok = 0; } + } + expect(ok, "the tool length goes along the reported tool axis", jl); + + /* Joints the inverse did not answer for last: a pose a cycle away, + asked after the inverse moved on, still gets its own frame, and the + frame asked there leaves the answer at the last joints alone. */ + near = *pos; + near.tran.x += 0.01; + near.tran.z -= 0.01; + near.a += 0.01; + near.b -= 0.01; + if (pose_letters > 2) { near.c += 0.01; } + rpy_frame(near.a, near.b, near.c, &near_want); + if (inverse_at(&near, 0.0, jn) || inverse_at(pos, 0.0, j)) { + expect(0, "the inverse answers near the pose", j); + return; + } + ok = kinematicsToolFrame(jn, &tool, &ff) == 0; + expect(ok && same_frame(&tool, &near_want), + "joints a cycle away get the frame of their own pose", jn); + ok = kinematicsToolFrame(j, &tool, &ff) == 0; + expect(ok && same_frame(&tool, &want), + "the frame of the last joints is unchanged after", j); +} + +/* poses over the workspace, X and Y at both ends and the middle, each + angle at both ends and zero */ +static int check_poses(void) +{ + const double lin[] = { -pose_lin, 0, pose_lin }; + const double ang[] = { -pose_ang, 0, pose_ang }; + EmcPose pos; + int x, y, a, b, c; + + memset(&pos, 0, sizeof(pos)); + pos.tran.z = pose_z; + for (x = 0; x < 3; x++) { + for (y = 0; y < 3; y++) { + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + for (c = 0; c < 3; c++) { + pos.tran.x = lin[x]; + pos.tran.y = lin[y]; + pos.a = ang[a]; + pos.b = ang[b]; + pos.c = pose_letters > 2 ? ang[c] : 0.0; + check_pose(&pos); + if (pose_letters < 3) { break; } + } + } + } + } + } + return 1; +} + int rtapi_app_main(void) { /* rotary values away from the identity, including the quarter and @@ -288,10 +430,16 @@ int rtapi_app_main(void) if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } own = 0; + /* a parallel machine is checked from poses, in the type it starts in */ + if (pose_letters) { + checked = check_poses(); + own = 1; + } + /* every kinematics the module offers, not just the one it starts in: the frames a switchable module reports are per type, and the type that turns the work is rarely the default */ - for (t = 0; t < MAX_TYPES; t++) { + for (t = 0; t < MAX_TYPES && !pose_letters; t++) { if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } if (!supplies_frames(j)) { continue; } checked++; diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 3a18f726919..0cc32da0138 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -75,3 +75,11 @@ run "pumakins" "setp pumakins.A2 300" \ # the same arm described by its DH chain run "genserkins" "" \ "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0 abc_flange=1" + +# the parallel machines state their frames from the pose, so they are +# checked from poses over the sim configs' geometry and travel +configs="$EMC2_HOME/configs/sim/axis" +run "genhexkins" "$(grep '^setp' "$configs/vismach/hexapod-sim/kinematics.hal")" \ + "joints=6 pose_letters=3 pose_z=20 pose_lin=5 pose_ang=20 tool_len=3" +run "pentakins" "$(grep '^setp' "$configs/pentapod/kinematics.hal")" \ + "joints=5 pose_letters=2 pose_z=0 pose_lin=100 pose_ang=15 tool_len=50" diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index 5a514e9d8a4..a4804e0adf3 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -49,6 +49,8 @@ class EmcPose(ctypes.Structure): orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" posejoint_in = sys.argv[10] if len(sys.argv) > 10 else "no-frame" tool_in = [float(v) for v in sys.argv[11].split(",")] if len(sys.argv) > 11 and sys.argv[11] != "-" else None +# the axis letters that orient the tool, "AC" for instance, where given +axes_in = sys.argv[12] if len(sys.argv) > 12 and sys.argv[12] != "-" else None pose_in += [0.0] * (AXES - len(pose_in)) jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) @@ -173,6 +175,10 @@ def compare(what, ours, theirs): for j in range(joints): compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) +# The frames are asked at the joints the module answered for: a parallel +# machine states them from the pose, which it has for those joints. +jframe = list(qi) if frompose else jnt_in + # the two rotaries that orient the tool, told apart by which carries the # other. The sign of the secondary is what G53.1 P names, so a module that # gets this wrong sends the machine to the other pose without saying so. @@ -180,13 +186,13 @@ def compare(what, ours, theirs): ctypes.POINTER(ctypes.c_int), ctypes.POINTER(ctypes.c_int)] primary, secondary = ctypes.c_int(-1), ctypes.c_int(-1) -r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jnt_in), +r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jframe), ctypes.byref(primary), ctypes.byref(secondary)) class Rot(ctypes.Structure): _fields_ = [(n, ctypes.c_double * 3) for n in "xyz"] kins.kinematicsUserToolFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] frame = Rot() -has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jnt_in), ctypes.byref(frame)) == 0 +has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jframe), ctypes.byref(frame)) == 0 if r_orient == 0: got = "%d,%d" % (primary.value, secondary.value) else: @@ -201,7 +207,7 @@ class Rot(ctypes.Structure): # the head of one of each kins.kinematicsUserPoseJoint.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(ctypes.c_int)] pose_joint = ctypes.c_int(-1) -if kins.kinematicsUserPoseJoint(ctx, Joints(*jnt_in), ctypes.byref(pose_joint)) == 0: +if kins.kinematicsUserPoseJoint(ctx, Joints(*jframe), ctypes.byref(pose_joint)) == 0: got = "%d" % pose_joint.value else: got = "-" if has_frame else "no-frame" @@ -210,6 +216,20 @@ class Rot(ctypes.Structure): else: print("kins-params: %s type %d pose joint %s" % (module, ktype, got)) +# the letters G68.2 and G53.n orient with, which a parallel machine +# declares since no joint of its turns the tool +if axes_in: + kins.kinematicsUserOrientAxes.argtypes = [ctypes.c_void_p, Joints, + ctypes.c_int * 3, ctypes.c_int * 3] + axes, head = (ctypes.c_int * 3)(), (ctypes.c_int * 3)() + n = kins.kinematicsUserOrientAxes(ctx, Joints(*jframe), axes, head) + got = "".join("XYZABCUVW"[axes[i]] for i in range(n)) if n > 0 else "-" + heads = "".join("ht"[1 - head[i]] for i in range(n)) if n > 0 else "" + if got != axes_in: + fail("orienting letters are %s, expected %s" % (got, axes_in)) + else: + print("kins-params: %s type %d orients with %s (%s)" % (module, ktype, got, heads)) + # The offset across the tool. The offset is the tool's own, given against # the machine with the rotaries at zero, and X and Y of it turn with the # tool as the length does along the tool axis: the tip is the point the diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index a81a5f3aabf..dcc6c64b38a 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -20,7 +20,7 @@ rm -f oot_switchkins.comp oot_userkins.comp # ONLY= in the environment runs the entries for that module alone run() { local loadrt="$1" setp="$2" parms="$3" ktype="$4" - local module coords sparm joints frompose pose jnt orient posejoint tool rtparms hal tok + local module coords sparm joints frompose pose jnt orient posejoint tool axes rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -40,6 +40,8 @@ run() { posejoint="" # a tool offset x,y,z both sides evaluate under, as motion hands it over tool="-" + # the letters that orient the tool, checked where given + axes="-" for tok in $parms; do case "$tok" in joints=*) joints=${tok#joints=} ;; @@ -49,19 +51,20 @@ run() { orient=*) orient=${tok#orient=} ;; posejoint=*) posejoint=${tok#posejoint=} ;; tool=*) tool=${tok#tool=} ;; + axes=*) axes=${tok#axes=} ;; esac done [ -n "$posejoint" ] || posejoint=$orient # what the module under test is loaded with: everything but our own words - rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g; s/ posejoint=[^ ]*//g') + rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g; s/ posejoint=[^ ]*//g; s/ axes=[^ ]*//g') hal=$(mktemp --suffix=.hal) { if [ -n "${CORE:-}" ]; then printf 'loadrt %s\n' "$CORE"; fi printf 'loadrt %s\n' "$loadrt" printf '%s\n' "$setp" printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" # halcmd keeps quotes, so an absent value travels as a dash - printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s %s\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" "$tool" + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" "$tool" "$axes" } > "$hal" if [ "$tool" = "-" ]; then echo "=== $loadrt type ${ktype:-0}" @@ -248,6 +251,8 @@ setp tripodkins.Cy 2" \ "joints=3 frompose=1 pose=1,1,2" run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" -run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" -run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" -run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" +# the parallel machines state their frames from the pose, and orient with +# its letters; no joint of theirs is a rotary +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7 orient=- axes=AB" From c216e78258885d4de3c5911b7fcfcdfb90fc1ffc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 28 Oct 2026 22:11:11 +1100 Subject: [PATCH 31/44] kinematics: turn the offset across the tool with the pose The X and Y of a tool offset turn with the tool, (I - R)(x, y, 0) added to the module's pose with R the tool frame in work coordinates. The shared code read R from the joints, and so passed over a type with pose frames. On such a type R is a function of the pose, which every caller holds, so the turn needs no joints. The inverse takes it off the pose it is asked for and calls the module once, where a type with joint frames has to iterate, the turn following the joints it is solving for. The forward adds it to the pose the module found. The Jacobian is the module's at the pose less the turn, times (I - D), D the change of the turn per pose coordinate, read from the pose frame by differences. A forward of the joints the last inverse or forward answered for now starts from that answer's pose on such a type, so it only confirms it, and the interpreter's forward of a solution it just inverted cannot settle on another assembly of the struts. tests/kins-params runs genhexkins, with the Jacobian closed and differenced, and pentakins with an offset across the tool: the tip, the joints back from the pose and the Jacobian against the inverse's differences. Taking the turn off the inverse's pose, adding it to the forward's, or applying (I - D) each fail it on both modules when left out. --- src/emc/kinematics/kins_util.c | 118 +++++++++++++++++++++++++++------ tests/kins-params/check.py | 14 +++- tests/kins-params/test.sh | 4 ++ 3 files changed, 113 insertions(+), 23 deletions(-) diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 752d4ca3ead..2e6d4e64adc 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1698,23 +1698,35 @@ const kins_ops KINS_IDENTITY_OPS = { // the tool frame in work coordinates. Nothing to turn without an X or Y, // on a type that is the identity or the machine frame, neither of which // applies the tool, or for a module that reports no tool frame. +// +// On a type with pose frames R is a function of the pose, which the +// caller has, so the turn needs no joints: the inverse takes it off the +// pose it is asked for in one pass, and the forward adds it to the pose +// the module found. static int tool_across_applies(const kins_ops *ops, const kins_params *p) { return (p->tool.tran.x != 0.0 || p->tool.tran.y != 0.0) - && !ops->identity && !ops->machine && ops->tool; + && !ops->identity && !ops->machine && (ops->tool || ops->pose_tool); +} + +// the frames are the pose's rather than the joints' +static int pose_frames(const kins_ops *ops) +{ + return !ops->tool && ops->pose_tool; } -// (I - R)(x, y, 0) at these joints, or -1 where the frames cannot say +// (I - R)(x, y, 0) at these joints, or at this pose on a type with pose +// frames, or -1 where the frames cannot say static int tool_across(const kins_ops *ops, const kins_params *p, - const double *joint, PmCartesian *d) + const double *joint, const EmcPose *pos, PmCartesian *d) { KINEMATICS_FORWARD_FLAGS fflags = 0; PmRotationMatrix work, r; const double x = p->tool.tran.x, y = p->tool.tran.y; - if (kinsOpsToolFrame(ops, p, NULL, joint, &r, &fflags)) { return -1; } + if (kinsOpsToolFrameAt(ops, p, joint, pos, &r, &fflags)) { return -1; } // where nothing turns the work, the work frame is the machine's - if (kinsOpsWorkFrame(ops, p, NULL, joint, &work, &fflags) == 0 + if (kinsOpsWorkFrameAt(ops, p, joint, pos, &work, &fflags) == 0 && toolFrameInWork(&work, &r, &r)) { return -1; } d->x = x - (r.x.x * x + r.y.x * y); d->y = y - (r.x.y * x + r.y.y * y); @@ -1747,9 +1759,9 @@ static int tool_across_jacobian(const kins_ops *ops, const kins_params *p, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { q[i] = joint[i]; } for (i = 0; i < n; i++) { q[i] = joint[i] + KINS_JACOBIAN_STEP; - if (tool_across(ops, p, q, &dp)) { return -1; } + if (tool_across(ops, p, q, NULL, &dp)) { return -1; } q[i] = joint[i] - KINS_JACOBIAN_STEP; - if (tool_across(ops, p, q, &dm)) { return -1; } + if (tool_across(ops, p, q, NULL, &dm)) { return -1; } q[i] = joint[i]; D[0][i] = (dp.x - dm.x) / (2 * KINS_JACOBIAN_STEP); D[1][i] = (dp.y - dm.y) / (2 * KINS_JACOBIAN_STEP); @@ -1770,10 +1782,54 @@ static int tool_across_jacobian(const kins_ops *ops, const kins_params *p, return 0; } +// The same on a type with pose frames. The module's maths has the joints +// at the pose less d, and d is a function of the pose alone, so dq = +// J (I - D) dP with D the change of d per pose coordinate. +static int tool_across_jacobian_pose(const kins_ops *ops, const kins_params *p, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double D[3][EMCMOT_MAX_AXIS]; + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + + for (int b = 0; b < EMCMOT_MAX_AXIS; b++) { + EmcPose q = *pos; + PmCartesian dp, dm; + + *kj_coord(&q, b) += KINS_JACOBIAN_STEP; + if (tool_across(ops, p, NULL, &q, &dp)) { return -1; } + *kj_coord(&q, b) -= 2 * KINS_JACOBIAN_STEP; + if (tool_across(ops, p, NULL, &q, &dm)) { return -1; } + D[0][b] = (dp.x - dm.x) / (2 * KINS_JACOBIAN_STEP); + D[1][b] = (dp.y - dm.y) / (2 * KINS_JACOBIAN_STEP); + D[2][b] = (dp.z - dm.z) / (2 * KINS_JACOBIAN_STEP); + } + for (int i = 0; i < n; i++) { + const double row[3] = { jac[i][0], jac[i][1], jac[i][2] }; + for (int b = 0; b < EMCMOT_MAX_AXIS; b++) { + jac[i][b] -= row[0] * D[0][b] + row[1] * D[1][b] + row[2] * D[2][b]; + } + } + return 0; +} + //---------------------------------------------------------------------- // asking an ops table, defaults applied //---------------------------------------------------------------------- +// the joints are those the scratch last answered for +static int pair_matches(const kins_params *p, const kins_scratch *s, + const double *joint) +{ + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + + if (!s->have_pair) { return 0; } + for (int i = 0; i < n; i++) { + if (fabs(joint[i] - s->pair_joint[i]) > 1e-12 * (1.0 + fabs(joint[i]))) { return 0; } + } + return 1; +} + // The pose at these joints for a type with pose frames: the pair the // scratch last answered for while the joints are still those, else a // forward started from that pair's pose, or from the last forward's, on a @@ -1782,19 +1838,14 @@ static int pose_of_joints(const kins_ops *ops, const kins_params *p, const kins_scratch *s, const double *joint, EmcPose *pos) { - const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; KINEMATICS_FORWARD_FLAGS ffl = 0; KINEMATICS_INVERSE_FLAGS ifl = 0; kins_scratch c; - int same = 1; if (!s || !joint) { return -1; } if (s->have_pair) { *pos = s->pair_pose; - for (int i = 0; i < n && same; i++) { - same = fabs(joint[i] - s->pair_joint[i]) <= 1e-12 * (1.0 + fabs(joint[i])); - } - if (same) { return 0; } + if (pair_matches(p, s, joint)) { return 0; } } else if (s->pose_seed_ok) { *pos = s->pose_seed; } else { @@ -1825,15 +1876,25 @@ int kinsOpsForward(const kins_ops *ops, const kins_params *p, PmCartesian d; int r, across, seeded = 0; if (!ops || !ops->forward || !p || !s) { return -1; } - across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; - if (ops->fwd_iterates && s->have_pose_seed) { + across = tool_across_applies(ops, p); + if (across && !pose_frames(ops)) { across = tool_across(ops, p, joint, NULL, &d) == 0; } + if (pose_frames(ops) && pair_matches(p, s, joint)) { + /* the joints of the last answer: start from its pose, which the + iteration then only confirms */ + *pos = s->pair_pose; + seeded = 1; + s->have_pose_seed = 0; + } else if (ops->fwd_iterates && s->have_pose_seed) { /* no pose of our own yet: start from the caller's estimate, which stays in *pos, rather than from a never-solved seed */ if (s->pose_seed_ok) { *pos = s->pose_seed; seeded = 1; } s->have_pose_seed = 0; } // the caller's estimate, as the module's maths sees it - if (ops->fwd_iterates && across && !seeded) { pose_shift(pos, &d, -1); } + if (ops->fwd_iterates && across && !seeded + && (!pose_frames(ops) || tool_across(ops, p, joint, pos, &d) == 0)) { + pose_shift(pos, &d, -1); + } r = ops->forward(p, s, joint, pos, fflags, iflags); if (ops->fwd_iterates && r == 0) { /* keep the result only when the solve succeeds */ @@ -1841,7 +1902,10 @@ int kinsOpsForward(const kins_ops *ops, const kins_params *p, s->pose_seed_ok = 1; } kinsOpsKeepPair(ops, p, s, r, joint, pos); - if (r == 0 && across) { pose_shift(pos, &d, 1); } + if (r == 0 && across) { + if (pose_frames(ops) && tool_across(ops, p, joint, pos, &d)) { return -1; } + pose_shift(pos, &d, 1); + } return r; } @@ -1861,6 +1925,15 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, kinsOpsKeepPair(ops, p, s, r, joint, pos); return r; } + if (pose_frames(ops)) { + // the turn is the pose's, so it comes off before the one call + if (tool_across(ops, p, joint, pos, &d)) { return -1; } + q = *pos; + pose_shift(&q, &d, -1); + r = ops->inverse(p, s, &q, joint, iflags, fflags); + kinsOpsKeepPair(ops, p, s, r, joint, &q); + return r; + } // a module may read where the machine is from the joints it is // handed, so every pass starts from the caller's n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; @@ -1869,7 +1942,7 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, // the turn follows the joints and the joints the turn: one more pass // settles it, the tool turning with the rotaries the pose names for (pass = 0; r == 0 && pass < 4; pass++) { - if (tool_across(ops, p, joint, &d)) { break; } + if (tool_across(ops, p, joint, NULL, &d)) { break; } if (fabs(d.x - used.x) < 1e-12 && fabs(d.y - used.y) < 1e-12 && fabs(d.z - used.z) < 1e-12) { break; } used = d; @@ -1957,12 +2030,13 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, // the module's maths has these joints at the pose less the turn of // the offset across the tool at = *pos; - across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; + across = tool_across_applies(ops, p) && tool_across(ops, p, joint, pos, &d) == 0; if (across) { pose_shift(&at, &d, -1); } if (ops->jacobian) { r = ops->jacobian(p, joint, &at, jac, iflags); if (r || !across) { return r; } - return tool_across_jacobian(ops, p, joint, jac); + return pose_frames(ops) ? tool_across_jacobian_pose(ops, p, pos, jac) + : tool_across_jacobian(ops, p, joint, jac); } if (!ops->inverse) { return -1; } @@ -1982,7 +2056,9 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); } } - return across ? tool_across_jacobian(ops, p, joint, jac) : 0; + if (!across) { return 0; } + return pose_frames(ops) ? tool_across_jacobian_pose(ops, p, pos, jac) + : tool_across_jacobian(ops, p, joint, jac); } // kinsOpsJacobian() //---------------------------------------------------------------------- diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index a4804e0adf3..224e447e817 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -276,8 +276,18 @@ def settled_inverse(P, seed): T = tool_in xyz = lambda p: [p.x, p.y, p.z] - # the tool's own axes are the machine's with the rotaries at zero - zero = frame_rows(kins.kinematicsUserToolFrame, [0.0] * MAX_JOINTS) + # the tool's own axes are the machine's with the rotaries at zero; a + # parallel machine has no rotaries, and its angles at zero are a pose + if frompose: + Z = pose_of(pose_in[:3]) + jz = Joints(*j0) + placed = kins.kinematicsUserInverse(ctx, ctypes.byref(Z), jz) == 0 + zero = frame_rows(kins.kinematicsUserToolFrame, list(jz)) if placed else None + else: + zero = frame_rows(kins.kinematicsUserToolFrame, [0.0] * MAX_JOINTS) + if zero is None: + fail("no tool frame with the rotaries at zero") + zero = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] if max(abs(zero[a][b] - (a == b)) for a in range(3) for b in range(3)) > 1e-12: fail("the tool frame at zero is not the machine's: %s" % zero) diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index dcc6c64b38a..c15221a78bc 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -256,3 +256,7 @@ run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC" run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC" run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7 orient=- axes=AB" +# the offset across the tool turns with the platform, from the pose +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC tool=1,1,3" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7 orient=- axes=ABC tool=1,1,3" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7 orient=- axes=AB tool=4,3,50" From d9da122bd85f928dd7571a22f227f85bc121c577 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 28 Oct 2026 22:37:54 +1100 Subject: [PATCH 32/44] motion: read the [AXIS_L] box on the platform of a parallel machine The box is the machine frame envelope: the carriage, or a robot's flange, the frame that does not move when a head tilts or a tool is loaded. A module that names no machine frame type has its identity type stand in, which is the truth on a machine whose slides line up with its frame. A parallel machine has struts, not slides, and its identity type is the strut lengths read as X Y Z A B C. So on genhexkins the box was read on the struts, and every move from home was refused as past X's and Y's limits as soon as the axis travel was narrower than the struts' length; the hexapod sim only passed because its limits are 1000. The machine frame of a parallel machine is now its platform: the pose of the joints with the tool left out, as a robot's is its flange. kinsOpsMachinePose() runs the type's forward on a copy of the block with no tool, started from the pose the frames read, and switchkins and kins_single.c answer kinematicsMachineFrame() with it for a module with a type whose frames come from the pose. The box then limits where the platform goes whatever tool is loaded, as it limits the carriage of a head, and PI's soft limits on its hexapods are limits on the platform's pose. The struts keep their [JOINT_n] limits, the machine axes Siemens' software limit switches act on. The identity type keeps its flag, so G13.1 still selects it, which tests/kins-switch-unsolved does. tests/twp-parallel, in the next commit, runs the hexapod and the pentapod with their axis travel round the sims' workspace and a long tool on a tilted platform: a move whose tool pose is inside the box with the platform past it is refused, and one whose tool pose is past it with the platform inside is accepted. With the box on the tool's pose both fail on both machines. --- include/kins_module.h | 10 ++++++++++ src/emc/kinematics/kins_single.c | 7 ++++++- src/emc/kinematics/kins_util.c | 19 +++++++++++++++++++ src/emc/kinematics/switchkins.c | 10 ++++++++++ 4 files changed, 45 insertions(+), 1 deletion(-) diff --git a/include/kins_module.h b/include/kins_module.h index 68d326519e0..d4f6e34dc3f 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -489,6 +489,16 @@ extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* The machine frame of a parallel machine: the pose of these joints with + the tool left out, the platform as its strut lengths place it, the way + a robot's flange is its machine frame. The forward runs on copies of + the block and the scratch, started from the pose the frames would read, + so neither the caller's tool nor its seed changes. Returns 0, -1 where + the type has no pose frames or no pose to start from, -2 where the + forward fails. */ +extern int kinsOpsMachinePose(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, + EmcPose *pos); extern int kinsOpsWorkFrameAt(const kins_ops *ops, const kins_params *p, const double *joint, const EmcPose *pos, PmRotationMatrix *rot, diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index e63956ead4f..177e3f8021c 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -151,13 +151,18 @@ int kinematicsTypeFlags(int ktype) } // and so no machine frame type either: the carriage is the joints, where -// they are the slides, and otherwise there is no machine frame to read +// they are the slides, the platform without the tool on a parallel +// machine, and otherwise there is no machine frame to read int kinematicsMachineFrame(const double *joint, EmcPose *pos) { if (!inited || !joint || !pos) { return -1; } if (ops()->slides) { return kinsMappedJointsToPose(&rt_params, joint, pos) == 0 ? 0 : -2; } + if (!ops()->tool && ops()->pose_tool) { + read_pins(); + return kinsOpsMachinePose(ops(), &rt_params, &rt_scratch, joint, pos); + } return -1; } diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 2e6d4e64adc..e7f7d152880 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1855,6 +1855,24 @@ static int pose_of_joints(const kins_ops *ops, const kins_params *p, return ops->forward(p, &c, joint, pos, &ffl, &ifl); } +int kinsOpsMachinePose(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, + EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS ffl = 0; + KINEMATICS_INVERSE_FLAGS ifl = 0; + kins_params bare; + kins_scratch c; + + if (!ops || !ops->forward || !p || !s || !joint || !pos) { return -1; } + if (!pose_frames(ops)) { return -1; } + if (pose_of_joints(ops, p, s, joint, pos)) { return -1; } + bare = *p; + memset(&bare.tool, 0, sizeof(bare.tool)); + c = *s; + return ops->forward(&bare, &c, joint, pos, &ffl, &ifl) == 0 ? 0 : -2; +} + void kinsOpsKeepPair(const kins_ops *ops, const kins_params *p, kins_scratch *s, int r, const double *joint, const EmcPose *pos) @@ -2244,6 +2262,7 @@ EXPORT_SYMBOL(kinsOpsToolFrame); EXPORT_SYMBOL(kinsOpsWorkFrameAt); EXPORT_SYMBOL(kinsOpsToolFrameAt); EXPORT_SYMBOL(kinsOpsKeepPair); +EXPORT_SYMBOL(kinsOpsMachinePose); EXPORT_SYMBOL(kinsIdentityPoseFrame); EXPORT_SYMBOL(kinsOpsJacobian); EXPORT_SYMBOL(kinsParamsPinsCreate); diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index b3cb0eb634a..55cd038f36b 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -498,6 +498,16 @@ int kinematicsMachineFrame(const double *joint, EmcPose *pos) KINEMATICS_INVERSE_FLAGS iflags = 0; int k; + // A parallel machine, a type with pose frames, has struts, not slides: + // the identity type standing in for its machine frame is the strut + // lengths, not a frame. Its machine frame is the platform, the pose + // with the tool left out, as a robot's is its flange. + for (k = 0; k < kins_count; k++) { + if (kops[k] && !kops[k]->tool && kops[k]->pose_tool) { + read_block(k); + return kinsOpsMachinePose(kops[k], &rt_params, &rt_scratch[k], joint, pos); + } + } for (k = 0; k < kins_count; k++) { int f = kinematicsTypeFlags(k); if (f < 0 || !(f & KINSTYPE_MACHINE)) { continue; } From c83bd89bc9025297f5bf6104b42fd5232a3a3ddf Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 28 Oct 2026 22:37:58 +1100 Subject: [PATCH 33/44] interp: orient the tool on a parallel machine G53.6, G53.3, G53.2 and G43.5 ask the tool frame inverse for joints that point the tool along a direction, and the shared search moves the joints that turn the frame. A parallel machine has none: its struts set the pose, and the pose's own angles orient the tool, A B C on genhexkins, A B on pentakins. On a type with pose frames the loader now runs the same search over the letters that orient, X Y Z held where the machine stands, and each answer goes to joints through the inverse. Angles that put the machine in the same place, the two sets of roll, pitch and yaw for one platform orientation or the pentapod's (180 - A, B + 180), are one answer, under the angles nearest where it stands. Tool x is left to the reported turn, as on a head, so the plane's X is the coordinate system's. On the hexapod the tool axis alone leaves the turn about it free, and the search's point on that curve is no policy (it moved C by up to 8.5 degrees from the start). The platform takes the shortest rotation from where it stands, with no turn about the tool. That is the hexapod treated as a five-axis machine, as the NIST Ingersoll hexapod was "programmed as if it were a 5-axis machine", and as Siemens makes the turn about the tool a word of its own, THETA. G53.1 is refused on such a machine. It turns the rotary joints and holds the linear ones, and with every joint a strut that is no move at all. Fanuc defines G53.1 for machines with rotary axes only (0i-F, 5.7.1), and Heidenhain's TURN positions the rotary axes alone while a machine without them has STAY only (TNC 640, 11.2). The message points at G53.6, G53.3 and G53.2. tests/twp-parallel runs the hexapod and the pentapod sims' geometry: G53.6 brings the tool axis to the plane's normal with the tool point still, the hexapod by the shortest rotation and the pentapod's tool x in the machine's Z/X plane; G53.1 is refused and moves nothing; G53.3 ends at the point in the plane on the normal; G68.3 reads the normal back off the tool. Solving for the whole frame of the turned platform left out, so the search picks its own point of the curve, fails the shortest rotation check; without the refusal G53.1 fails. --- docs/src/gcode/g-code.adoc | 4 + docs/src/motion/kinematics-conventions.adoc | 14 ++ include/kinematics_user.h | 15 ++ include/kins_module.h | 6 + src/emc/kinematics/kins_util.c | 9 + .../kinematics_userspace/kinematics_user.c | 236 ++++++++++++++++++ src/emc/rs274ngc/interp_workplane.cc | 11 +- tests/twp-parallel/README | 19 ++ tests/twp-parallel/checkresult | 3 + tests/twp-parallel/hexapod-kins.hal | 37 +++ tests/twp-parallel/hexapod.hal | 16 ++ tests/twp-parallel/hexapod.ini | 141 +++++++++++ tests/twp-parallel/pentapod-kins.hal | 27 ++ tests/twp-parallel/pentapod.hal | 15 ++ tests/twp-parallel/pentapod.ini | 125 ++++++++++ tests/twp-parallel/test-ui.py | 183 ++++++++++++++ tests/twp-parallel/test.sh | 7 + tests/twp-parallel/tool.tbl | 2 + 18 files changed, 869 insertions(+), 1 deletion(-) create mode 100644 tests/twp-parallel/README create mode 100755 tests/twp-parallel/checkresult create mode 100644 tests/twp-parallel/hexapod-kins.hal create mode 100644 tests/twp-parallel/hexapod.hal create mode 100644 tests/twp-parallel/hexapod.ini create mode 100644 tests/twp-parallel/pentapod-kins.hal create mode 100644 tests/twp-parallel/pentapod.hal create mode 100644 tests/twp-parallel/pentapod.ini create mode 100755 tests/twp-parallel/test-ui.py create mode 100755 tests/twp-parallel/test.sh create mode 100644 tests/twp-parallel/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 36eaf72093f..ce3124b09cf 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2119,6 +2119,8 @@ Every form stays inside the travel `[JOINT_n] MIN_LIMIT` and `MAX_LIMIT` give th On a machine that is not of this shape, a robot among them, only the nearest form is available. +A parallel machine, a hexapod on genhexkins or a pentapod on pentakins, has no rotary joints: its struts set the pose, and it orients the tool with the pose's own angles, A B C on the hexapod and A B on the pentapod. The rotary words are what move there, so 'G53.6', 'G53.3' and 'G53.2' work as on any five-axis machine, and 'G53.1', which turns rotary joints alone, is refused. The tool axis is all the machine is asked for, as on a head, and the plane's X is left to the coordinate system. Where the axis leaves a turn about it free, the hexapod's three angles, the platform takes the shortest rotation from where it stands and does not turn about the tool. + 'Q' says whether the joints that carry the work, the table, take part. * 'Q0', the default, holds them and lets the head do the work. On a machine @@ -2160,6 +2162,8 @@ It is an error if: interpreter, or it reports no frames, so it cannot say where its joints point the tool. * The plane's normal cannot be reached by the rotary joints. +* 'G53.1' is used on a machine that orients the tool with no rotary joint, + a hexapod or a pentapod. * 'P' is anything but 0, 1 or 2, no reachable pose has the rotary that names it the way 'P' asks for, or the machine has no pair of poses a tilting joint tells apart. diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 526b4155744..ec3ab6c7511 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -404,6 +404,20 @@ solutions and the turn about the tool that finishes the frame, which is what the TWP remap computes; holding nothing lets the table place tool X and the turn comes back zero. +A parallel machine, a type with frames from the pose, has no joint that turns +the tool, so the search runs over the letters of the pose that orient it, A B C +on genhexkins and A B on pentakins, with X Y Z held where the machine stands, +and each answer goes to joints through the inverse. Angles that put the +machine in the same place, the two sets of roll, pitch and yaw for one +platform orientation or the pentapod's (180 - A, B + 180), are one answer, the +angles nearest where the machine stands. Tool x is left to the reported turn, +as on a head. On the hexapod a tool axis alone leaves the turn about it free, +and the platform takes the shortest rotation from where it stands, with no turn +about the tool, which is what treating it as a five-axis machine means: the +NIST hexapod was "programmed as if it were a 5-axis machine", and Siemens makes +the turn about the tool an explicit word of its own, `THETA`. `held` is not +read, since nothing carries the work. + The request is taken as a program carries it. A direction written to a few digits is a unit vector only to within its rounding, and a tool X written the same way is at right angles to the axis only to within it, so both are diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 8338cbc2c9b..bb89bbca163 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -183,6 +183,13 @@ const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); */ int kinematicsUserIsIdentity(KinematicsUserContext* ctx); +/** + * Whether the selected type states its frames from the pose, a parallel + * machine whose joints are struts: it orients the tool with the letters + * of its pose and with no rotary joint. + */ +int kinematicsUserPoseFrames(KinematicsUserContext* ctx); + /** * Get number of joints * @@ -243,6 +250,14 @@ int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, * selected type: the joint sets that point the tool axis, and where given * the tool x, along the directions asked for, in work coordinates. Same * arguments and answers as kinematicsToolFrameInverse(). + * + * On a type with pose frames the solve runs over the letters that orient, + * X Y Z where the seed has them, and each answer goes to joints through + * the inverse. The tool x is never the joints': as on a head, it is left + * to tool_spin. Where the tool axis leaves a turn about it free, the + * hexapod's three letters, the platform takes the shortest rotation from + * the orientation at the seed, with no turn about the tool. held is not + * read, since no joint carries the work. */ int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, const PmCartesian* axis_in_work, diff --git a/include/kins_module.h b/include/kins_module.h index d4f6e34dc3f..4a58fcedc63 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -489,6 +489,12 @@ extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, const kins_scratch *s, const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* The pose at these joints on a type with pose frames, found the way the + frames find it: the pair of the last answer, else a forward from its + pose on a copy of the scratch. -1 with no pose to start from. */ +extern int kinsOpsPoseOfJoints(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, + EmcPose *pos); /* The machine frame of a parallel machine: the pose of these joints with the tool left out, the platform as its strut lengths place it, the way a robot's flange is its machine frame. The forward runs on copies of diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index e7f7d152880..1f640354042 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1855,6 +1855,14 @@ static int pose_of_joints(const kins_ops *ops, const kins_params *p, return ops->forward(p, &c, joint, pos, &ffl, &ifl); } +int kinsOpsPoseOfJoints(const kins_ops *ops, const kins_params *p, + const kins_scratch *s, const double *joint, + EmcPose *pos) +{ + if (!ops || !ops->forward || !p || !pos) { return -1; } + return pose_of_joints(ops, p, s, joint, pos); +} + int kinsOpsMachinePose(const kins_ops *ops, const kins_params *p, const kins_scratch *s, const double *joint, EmcPose *pos) @@ -2262,6 +2270,7 @@ EXPORT_SYMBOL(kinsOpsToolFrame); EXPORT_SYMBOL(kinsOpsWorkFrameAt); EXPORT_SYMBOL(kinsOpsToolFrameAt); EXPORT_SYMBOL(kinsOpsKeepPair); +EXPORT_SYMBOL(kinsOpsPoseOfJoints); EXPORT_SYMBOL(kinsOpsMachinePose); EXPORT_SYMBOL(kinsIdentityPoseFrame); EXPORT_SYMBOL(kinsOpsJacobian); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b10c176400c..813ce844864 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -598,6 +598,15 @@ int kinematicsUserIsIdentity(KinematicsUserContext* ctx) return ctx->info.ops[ctx->ktype]->identity; } +int kinematicsUserPoseFrames(KinematicsUserContext* ctx) +{ + const kins_ops *ops; + + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + ops = ctx->info.ops[ctx->ktype]; + return !ops->tool && ops->pose_tool; +} + int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) { if (!ctx || !ctx->initialized) return 0; @@ -696,6 +705,227 @@ int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, rot, &fflags); } +/* ------------------------------------------------------------------------ + * The orientation on a type with pose frames + * + * A parallel machine orients the tool with the angles of its pose, and no + * joint of it turns anything. So the solve runs over the letters that + * orient, toolFrameSolve() unchanged with a letter where it expects a + * joint, the rest of the pose held where the machine stands. + * ------------------------------------------------------------------------ */ + +static struct { + KinematicsUserContext *ctx; + const kins_ops *ops; + EmcPose at; // the pose the letters are set into + int letter[3]; // axis numbers, 3 A to 8 W + int n; +} by_letter; + +static double *pose_axis(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +static void letters_pose(const double *v, EmcPose *pos) +{ + *pos = by_letter.at; + for (int i = 0; i < by_letter.n; i++) { *pose_axis(pos, by_letter.letter[i]) = v[i]; } +} + +static int letter_work(const double *v, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose pos; + letters_pose(v, &pos); + return kinsOpsWorkFrameAt(by_letter.ops, &by_letter.ctx->params, NULL, &pos, rot, fflags); +} + +static int letter_tool(const double *v, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose pos; + letters_pose(v, &pos); + return kinsOpsToolFrameAt(by_letter.ops, &by_letter.ctx->params, NULL, &pos, rot, fflags); +} + +// the tool frame in work coordinates at a pose +static int pose_in_work(const double *v, PmRotationMatrix *m) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix w, t; + + if (letter_work(v, &w, &fflags) || letter_tool(v, &t, &fflags)) { return -1; } + return toolFrameInWork(&w, &t, m); +} + +// v turned about the unit axis k by the angle whose sine and cosine are given +static PmCartesian turn_about(const PmCartesian *k, double sn, double cs, const PmCartesian *v) +{ + const double along = k->x * v->x + k->y * v->y + k->z * v->z; + PmCartesian r; + + r.x = v->x * cs + (k->y * v->z - k->z * v->y) * sn + k->x * along * (1.0 - cs); + r.y = v->y * cs + (k->z * v->x - k->x * v->z) * sn + k->y * along * (1.0 - cs); + r.z = v->z * cs + (k->x * v->y - k->y * v->x) * sn + k->z * along * (1.0 - cs); + return r; +} + +static int pose_frame_inverse(KinematicsUserContext *ctx, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin) +{ + static const char letters[] = "XYZABCUVW"; + const kins_ops *ops = ctx->info.ops[ctx->ktype]; + double now[3], roots[TOOL_FRAME_MAX_SOLUTIONS * 3], dist[TOOL_FRAME_MAX_SOLUTIONS]; + double joints[TOOL_FRAME_MAX_SOLUTIONS][EMCMOT_MAX_JOINTS]; + double spins[TOOL_FRAME_MAX_SOLUTIONS]; + PmRotationMatrix m; + PmCartesian z, x, k, *want_x = NULL; + double len, sn, cs; + int n, found, kept = 0; + + if (!axis_in_work || !solutions || max_solutions <= 0) return -1; + n = ops->orient ? (int)strlen(ops->orient) : 0; + if (n < 2 || n > 3) return -1; + by_letter.ctx = ctx; + by_letter.ops = ops; + by_letter.n = n; + for (int i = 0; i < n; i++) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[i])); + if (!at || at - letters < 3) return -1; + by_letter.letter[i] = (int)(at - letters); + } + if (kinsOpsPoseOfJoints(ops, &ctx->params, &ctx->scratch, seed, &by_letter.at)) return -1; + for (int i = 0; i < n; i++) { now[i] = *pose_axis(&by_letter.at, by_letter.letter[i]); } + + z = *axis_in_work; + len = sqrt(z.x * z.x + z.y * z.y + z.z * z.z); + if (len < 1e-12) return -1; + z.x /= len; z.y /= len; z.z /= len; + + // three letters reach a whole frame, and the axis alone leaves the turn + // about it to them: the shortest rotation from where the platform + // stands, which turns nothing about the tool, names that turn + if (n == 3) { + if (pose_in_work(now, &m)) return -1; + k.x = m.z.y * z.z - m.z.z * z.y; + k.y = m.z.z * z.x - m.z.x * z.z; + k.z = m.z.x * z.y - m.z.y * z.x; + sn = sqrt(k.x * k.x + k.y * k.y + k.z * k.z); + cs = m.z.x * z.x + m.z.y * z.y + m.z.z * z.z; + if (sn > 1e-12) { + k.x /= sn; k.y /= sn; k.z /= sn; + x = turn_about(&k, sn, cs, &m.x); + } else { + // there already, or straight over, a half turn about x + x = m.x; + } + want_x = &x; + } + + found = toolFrameSolve(letter_work, letter_tool, n, &z, want_x, now, 0, + roots, TOOL_FRAME_MAX_SOLUTIONS, NULL, NULL); + if (found <= 0) return found; + + // each root on the turn nearest where the machine stands, to joints; + // roots that give the same joints are one place under other angles, + // and the nearer angles name it + for (int r = 0; r < found; r++) { + KINEMATICS_INVERSE_FLAGS ifl = 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + double *v = &roots[r * 3], q[EMCMOT_MAX_JOINTS], d = 0.0; + EmcPose pos; + int same = -1; + + for (int i = 0; i < n; i++) { + v[i] += 360.0 * floor((now[i] - v[i]) / 360.0 + 0.5); + d += fabs(v[i] - now[i]); + } + letters_pose(v, &pos); + pad_joints(ctx, seed, q); + if (kinsOpsInverse(ops, &ctx->params, &ctx->scratch, &pos, q, &ifl, &ffl) != 0) continue; + for (int o = 0; o < kept && same < 0; o++) { + int all = 1; + for (int j = 0; j < ctx->num_joints; j++) { + if (fabs(joints[o][j] - q[j]) > 1e-9 * (1.0 + fabs(q[j]))) all = 0; + } + if (all) same = o; + } + if (same >= 0 && dist[same] <= d) continue; + if (same < 0) { + if (kept >= max_solutions) continue; + same = kept++; + } + dist[same] = d; + memcpy(joints[same], q, sizeof(q)); + spins[same] = 0.0; + if (x_in_work && pose_in_work(v, &m) == 0) { + spins[same] = atan2(m.y.x * x_in_work->x + m.y.y * x_in_work->y + m.y.z * x_in_work->z, + m.x.x * x_in_work->x + m.x.y * x_in_work->y + m.x.z * x_in_work->z); + } + // the letters of the pose these joints are, for the forward after + roots[same * 3 + 0] = v[0]; + roots[same * 3 + 1] = v[1]; + roots[same * 3 + 2] = n > 2 ? v[2] : 0.0; + } + + // nearest first, and its inverse the last answer, so a forward of its + // joints starts from its pose + for (int i = 1; i < kept; i++) { + for (int j = i; j > 0 && dist[j - 1] > dist[j]; j--) { + double t = dist[j]; dist[j] = dist[j - 1]; dist[j - 1] = t; + t = spins[j]; spins[j] = spins[j - 1]; spins[j - 1] = t; + for (int l = 0; l < 3; l++) { + t = roots[j * 3 + l]; roots[j * 3 + l] = roots[(j - 1) * 3 + l]; roots[(j - 1) * 3 + l] = t; + } + for (int l = 0; l < EMCMOT_MAX_JOINTS; l++) { + t = joints[j][l]; joints[j][l] = joints[j - 1][l]; joints[j - 1][l] = t; + } + } + } + if (kept > 0) { + KINEMATICS_INVERSE_FLAGS ifl = 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + double q[EMCMOT_MAX_JOINTS]; + EmcPose pos; + + letters_pose(&roots[0], &pos); + memcpy(q, joints[0], sizeof(q)); + kinsOpsInverse(ops, &ctx->params, &ctx->scratch, &pos, q, &ifl, &ffl); + } + + // the joints place the axis; the turn about it that brings tool x to + // the one asked for is the spin, as on a head, and with nowhere to put + // it a request for x that needs one is not reached + found = 0; + for (int i = 0; i < kept; i++) { + if (x_in_work && !tool_spin && fabs(spins[i]) > 1e-9) continue; + for (int j = 0; j < ctx->num_joints; j++) { + solutions[found * ctx->num_joints + j] = joints[i][j]; + } + if (free_directions) free_directions[found] = 0; + if (tool_spin) tool_spin[found] = spins[i]; + found++; + } + return found; +} + int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, const PmCartesian* axis_in_work, const PmCartesian* x_in_work, @@ -710,6 +940,12 @@ int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, int found; if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + if (kinematicsUserPoseFrames(ctx)) { + refresh(ctx); + pad_joints(ctx, seed, j); + return pose_frame_inverse(ctx, axis_in_work, x_in_work, j, solutions, + max_solutions, free_directions, tool_spin); + } if (!ctx->info.ops[ctx->ktype]->work || !ctx->info.ops[ctx->ktype]->tool) return -1; refresh(ctx); pad_joints(ctx, seed, j); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index f995e8c0095..17380a5d9ae 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1410,10 +1410,15 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((primary >= 0 && kinematicsUserSetType(ctx, primary) != 0), _("kinematics type %d is not available outside realtime"), primary); identity = kinematicsUserIsIdentity(ctx); + // G53.1 turns the rotary joints and holds the linear ones, and a + // machine that orients the tool with its pose, a hexapod, has no rotary + // joint to turn: Heidenhain's TURN is for the rotary axes alone, and a + // machine without them has only STAY + const bool by_pose = !identity && kinematicsUserPoseFrames(ctx); int orienting[3]; int count = orienting_letters(ctx, now, orienting); solved = INTERP_OK; - if (!identity) { + if (!identity && !(code == G_53_1 && by_pose)) { plane_axis_in_world(s, 2, s->rotation_xy, &axis); plane_axis_in_world(s, 0, s->rotation_xy, &xdir); solved = orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name); @@ -1421,6 +1426,10 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) kinematicsUserSetType(ctx, s->kins_type); CHKS((identity), _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHKS((code == G_53_1 && by_pose), + _("G53.1 turns the rotary joints alone, and this machine orients the tool with no rotary" + " joint; use G53.6 to turn about the tool tip, G53.3 to go to a point in the plane," + " or G53.2 to solve only")); CHP(solved); // where that puts the machine, and what the program calls it: in the diff --git a/tests/twp-parallel/README b/tests/twp-parallel/README new file mode 100644 index 00000000000..f6d0304b81e --- /dev/null +++ b/tests/twp-parallel/README @@ -0,0 +1,19 @@ +The tilted work plane on the two parallel machines, the hexapod sim on +genhexkins and the pentapod sim on pentakins, with their configs' +geometry. Neither has a rotary joint: the struts set the pose, and the +tool is oriented with the pose's own angles, A B C on the hexapod and +A B on the pentapod. + +From a tilted start, G68.2 defines a plane and G53.6 turns the tool to +its normal about the tool tip: the reported tool axis is the normal and +X Y Z have not moved. On the hexapod the axis leaves the turn about it +free, and the platform takes the shortest rotation from where it stood, +no turn about the tool. G53.1 is refused, since it turns rotary joints +alone. G53.3 ends at the point asked for in the plane with the tool on +the normal, and G68.3 reads the plane's normal back off the tool. + +The [AXIS_L] box is the platform's, its pose with the tool left out, as +the carriage is on a head: with a long tool and the platform tilted about +Y, a move whose tool pose is inside the box with the platform past it is +refused, and one whose tool pose is past it with the platform inside is +accepted. diff --git a/tests/twp-parallel/checkresult b/tests/twp-parallel/checkresult new file mode 100755 index 00000000000..8f4f3f58ba7 --- /dev/null +++ b/tests/twp-parallel/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures, once per machine +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/twp-parallel/hexapod-kins.hal b/tests/twp-parallel/hexapod-kins.hal new file mode 100644 index 00000000000..c0c2b6c33c6 --- /dev/null +++ b/tests/twp-parallel/hexapod-kins.hal @@ -0,0 +1,37 @@ +# the hexapod sim's geometry, configs/sim/axis/vismach/hexapod-sim/kinematics.hal +setp genhexkins.base.0.x -22.950 +setp genhexkins.base.0.y 13.250 +setp genhexkins.base.0.z 0.000 +setp genhexkins.base.1.x 22.950 +setp genhexkins.base.1.y 13.250 +setp genhexkins.base.1.z 0.000 +setp genhexkins.base.2.x 22.950 +setp genhexkins.base.2.y 13.250 +setp genhexkins.base.2.z 0.000 +setp genhexkins.base.3.x 0.000 +setp genhexkins.base.3.y -26.500 +setp genhexkins.base.3.z 0.000 +setp genhexkins.base.4.x 0.000 +setp genhexkins.base.4.y -26.500 +setp genhexkins.base.4.z 0.000 +setp genhexkins.base.5.x -22.950 +setp genhexkins.base.5.y 13.250 +setp genhexkins.base.5.z 0.000 +setp genhexkins.platform.0.x -1.000 +setp genhexkins.platform.0.y 11.500 +setp genhexkins.platform.0.z 0.000 +setp genhexkins.platform.1.x 1.000 +setp genhexkins.platform.1.y 11.500 +setp genhexkins.platform.1.z 0.000 +setp genhexkins.platform.2.x 10.459 +setp genhexkins.platform.2.y -4.884 +setp genhexkins.platform.2.z 0.000 +setp genhexkins.platform.3.x 9.459 +setp genhexkins.platform.3.y -6.616 +setp genhexkins.platform.3.z 0.000 +setp genhexkins.platform.4.x -9.459 +setp genhexkins.platform.4.y -6.616 +setp genhexkins.platform.4.z 0.000 +setp genhexkins.platform.5.x -10.459 +setp genhexkins.platform.5.y -4.884 +setp genhexkins.platform.5.z 0.000 diff --git a/tests/twp-parallel/hexapod.hal b/tests/twp-parallel/hexapod.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-parallel/hexapod.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-parallel/hexapod.ini b/tests/twp-parallel/hexapod.ini new file mode 100644 index 00000000000..7679079381a --- /dev/null +++ b/tests/twp-parallel/hexapod.ini @@ -0,0 +1,141 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = genhexkins +JOINTS = 6 + +[HAL] +HALFILE = hexapod.hal +HALFILE = hexapod-kins.hal + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZABC +HOME = 0 0 20 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 500 +MAX_LINEAR_ACCELERATION = 50000 +DEFAULT_LINEAR_ACCELERATION = 20000 +DEFAULT_ANGULAR_VELOCITY = 200 +MAX_ANGULAR_VELOCITY = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 20000 + +[AXIS_Y] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 20000 + +[AXIS_Z] +MIN_LIMIT = 10 +MAX_LIMIT = 30 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 20000 + +[AXIS_A] +MIN_LIMIT = -30 +MAX_LIMIT = 30 +MAX_VELOCITY = 300 +MAX_ACCELERATION = 30000 + +[AXIS_B] +MIN_LIMIT = -30 +MAX_LIMIT = 30 +MAX_VELOCITY = 300 +MAX_ACCELERATION = 30000 + +[AXIS_C] +MIN_LIMIT = -40 +MAX_LIMIT = 40 +MAX_VELOCITY = 300 +MAX_ACCELERATION = 30000 + +[JOINT_0] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +HOME = 29.7466 +HOME_OFFSET = 29.7466 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 10 +MAX_LIMIT = 60 +HOME_SEQUENCE = 0 diff --git a/tests/twp-parallel/pentapod-kins.hal b/tests/twp-parallel/pentapod-kins.hal new file mode 100644 index 00000000000..c4577db17f8 --- /dev/null +++ b/tests/twp-parallel/pentapod-kins.hal @@ -0,0 +1,27 @@ +# the pentapod sim's geometry, configs/sim/axis/pentapod/kinematics.hal +setp pentakins.base.0.x -418.00 +setp pentakins.base.0.y 500.00 +setp pentakins.base.0.z -140.00 +setp pentakins.base.1.x 418.00 +setp pentakins.base.1.y 500.00 +setp pentakins.base.1.z -140.00 +setp pentakins.base.2.x 0.00 +setp pentakins.base.2.y 500.00 +setp pentakins.base.2.z -310.00 +setp pentakins.base.3.x 418.00 +setp pentakins.base.3.y 500.00 +setp pentakins.base.3.z 510.00 +setp pentakins.base.4.x -418.00 +setp pentakins.base.4.y 500.00 +setp pentakins.base.4.z 510.00 +setp pentakins.effector.0.r 80.30 +setp pentakins.effector.0.z 87.00 +setp pentakins.effector.1.r 80.30 +setp pentakins.effector.1.z 114.50 +setp pentakins.effector.2.r 80.30 +setp pentakins.effector.2.z 140.50 +setp pentakins.effector.3.r 80.30 +setp pentakins.effector.3.z 232.50 +setp pentakins.effector.4.r 80.30 +setp pentakins.effector.4.z 258.50 +setp pentakins.max-error 1000.0 diff --git a/tests/twp-parallel/pentapod.hal b/tests/twp-parallel/pentapod.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/twp-parallel/pentapod.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-parallel/pentapod.ini b/tests/twp-parallel/pentapod.ini new file mode 100644 index 00000000000..3a923ab5d7d --- /dev/null +++ b/tests/twp-parallel/pentapod.ini @@ -0,0 +1,125 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = pentakins +JOINTS = 5 + +[HAL] +HALFILE = pentapod.hal +HALFILE = pentapod-kins.hal + +[TRAJ] +# the rates are high so the moves are short: the test checks only where +# they end +COORDINATES = XYZAB +HOME = 0 0 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 500 +MAX_LINEAR_ACCELERATION = 50000 +DEFAULT_LINEAR_ACCELERATION = 20000 +DEFAULT_ANGULAR_VELOCITY = 200 +MAX_ANGULAR_VELOCITY = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -250 +MAX_LIMIT = 250 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 50000 + +[AXIS_Y] +MIN_LIMIT = -250 +MAX_LIMIT = 250 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 50000 + +[AXIS_Z] +MIN_LIMIT = -250 +MAX_LIMIT = 250 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 50000 + +[AXIS_A] +MIN_LIMIT = -50 +MAX_LIMIT = 50 +MAX_VELOCITY = 300 +MAX_ACCELERATION = 30000 + +[AXIS_B] +MIN_LIMIT = -15 +MAX_LIMIT = 15 +MAX_VELOCITY = 300 +MAX_ACCELERATION = 30000 + +[JOINT_0] +TYPE = LINEAR +HOME = 598.5792 +HOME_OFFSET = 598.5792 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 400 +MAX_LIMIT = 1500 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 697.1982 +HOME_OFFSET = 697.1982 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 400 +MAX_LIMIT = 1500 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 682.785 +HOME_OFFSET = 682.785 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 400 +MAX_LIMIT = 1500 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = LINEAR +HOME = 787.9175 +HOME_OFFSET = 787.9175 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 400 +MAX_LIMIT = 1500 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = LINEAR +HOME = 806.0397 +HOME_OFFSET = 806.0397 +MAX_VELOCITY = 2000 +MAX_ACCELERATION = 200000 +MIN_LIMIT = 400 +MAX_LIMIT = 1500 +HOME_SEQUENCE = 0 diff --git a/tests/twp-parallel/test-ui.py b/tests/twp-parallel/test-ui.py new file mode 100755 index 00000000000..fef609c0287 --- /dev/null +++ b/tests/twp-parallel/test-ui.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +# The tilted work plane on a parallel machine: see README. + +import linuxcnc +import sys +import os +import time +import math +import numpy as np + +ini = linuxcnc.ini(sys.argv[2]) +HEXA = ini.find("KINS", "KINEMATICS") == "genhexkins" +JOINTS = int(ini.find("KINS", "JOINTS")) + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + got = [] + while True: + m = e.poll() + if not m: + return got + got.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + j = settled() + for m in drain(): + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + return j + +def refused(cmd, words): + c.mdi(cmd) + c.wait_complete(60) + settled() + msgs = [m[1] for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if not any(words in m for m in msgs): + error("%s was not refused with '%s': %s" % (cmd, words, msgs)) + +# columns are the tool's axes, in the work frame +def tool(): + s.poll() + if s.tool_frame is None: + error("no tool frame reported") + return np.eye(3) + return np.array(s.tool_frame).reshape(3, 3) + +# columns are the plane's axes; and its origin +def plane(): + s.poll() + return np.array(s.g68_rotation).reshape(3, 3), np.array(s.g68_offset[:3]) + +def point(): + s.poll() + return np.array(s.position[:3]) + +def close(a, b, tol=1e-6): + return np.max(np.abs(np.array(a) - np.array(b))) < tol + +# the rotation of least angle taking unit vector a to unit vector b +def shortest(a, b): + k = np.cross(a, b) + sn, cs = np.linalg.norm(k), np.dot(a, b) + if sn < 1e-12: + return np.eye(3) + k = k / sn + K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]]) + return np.eye(3) + sn * K + (1 - cs) * K.dot(K) + +if HEXA: + start = "G0 X0 Y0 Z20 A5 B-3 C10" + define = "G68.2 X1 Y2 Z3 I30 J15 K0" + target = [0, 0, 17] +else: + start = "G0 X10 Y-20 Z0 A5 B-3" + define = "G68.2 X1 Y2 Z3 I30 J12 K0" + target = [5, 5, 5] + +# --- G53.6 turns the tool to the plane about the tool tip ---------------- +mdi(start) +T0, P0 = tool(), point() +mdi(define) +R, O = plane() +n = R[:, 2] +mdi("G53.6") +T1 = tool() +if not close(T1[:, 2], n): + error("the tool axis after G53.6 is %s, not the plane normal %s" % (T1[:, 2], n)) +if not close(point(), P0): + error("G53.6 moved the tool point from %s to %s" % (P0, point())) +if HEXA: + # the axis leaves the turn about it free: the platform takes the + # shortest rotation from where it stood, no turn about the tool + if not close(T1, shortest(T0[:, 2], T1[:, 2]).dot(T0)): + error("G53.6 turned the platform about the tool:\n%s\nnot\n%s" + % (T1, shortest(T0[:, 2], T1[:, 2]).dot(T0))) +else: + # the spindle's turn is free in the machine, and tool x is the chain's, + # in the machine's Z/X plane + if abs(T1[1, 0]) > 1e-9: + error("the pentapod's tool x %s is out of the machine's Z/X plane" % T1[:, 0]) + +# --- G53.1 turns rotary joints alone, and there are none ----------------- +before = point() +refused("G53.1", "no rotary joint") +if not close(point(), before): + error("the refused G53.1 moved the machine") + +# --- G53.3 goes to a point in the plane with the tool on its normal ------- +mdi("G69", start, define, "G53.3 X%g Y%g Z%g" % tuple(target)) +R, O = plane() +prog = R.T.dot(point() - O) +if not close(prog, target): + error("G53.3 ended at %s in the plane, not %s" % (prog, target)) +T3 = tool() +if not close(T3[:, 2], R[:, 2]): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool ----------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +s.poll() +R3, O3 = plane() +if not s.g68_active or not close(O3, [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(R3[:, 2], T3[:, 2]): + error("G68.3's normal %s is not the tool axis %s" % (R3[:, 2], T3[:, 2])) +mdi("G69") + +# --- the [AXIS_L] box is the platform's, the tool left out --------------- +# A long tool and a tilt about Y put the platform off the tool's pose along +# X, by L sin B: a move whose pose is inside the box with the platform past +# it is refused, and one whose pose is past it with the platform inside is +# accepted, as on a head, where the box is the carriage's. +length, h, tilt, edge = (10, 1, 20, 10) if HEXA else (100, 2, 15, 250) +shift = length * math.sin(math.radians(tilt)) +z = (20 if HEXA else 0) - length +mdi("G43 H%d" % h, "G0 X0 Y0 Z%g A0 B%g%s" % (z, tilt, " C0" if HEXA else "")) +refused("G0 X%g" % (edge - shift / 2), "limit") +mdi("G0 X%g" % (-edge - shift / 2)) +if not close(point()[0], -edge - shift / 2, 1e-3): + error("the move with the platform inside the box did not get there: X %.4f" % point()[0]) +mdi("G0 X0", "G0 B0", "G49") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-parallel/test.sh b/tests/twp-parallel/test.sh new file mode 100755 index 00000000000..04305e22a7d --- /dev/null +++ b/tests/twp-parallel/test.sh @@ -0,0 +1,7 @@ +#!/bin/bash -e +for machine in hexapod pentapod; do + # a failed run leaves the var file behind, and it carries offsets + rm -f sim.var sim.var.bak + echo "=== $machine" + linuxcnc -r $machine.ini +done diff --git a/tests/twp-parallel/tool.tbl b/tests/twp-parallel/tool.tbl new file mode 100644 index 00000000000..dc2717fc464 --- /dev/null +++ b/tests/twp-parallel/tool.tbl @@ -0,0 +1,2 @@ +T1 P1 Z10 D0 ; +T2 P2 Z100 D0 ; From b64d6da78204fcd518819208aa8d03e4c445df49 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 30 Oct 2026 13:13:03 +1100 Subject: [PATCH 34/44] kinematics: the forward and the inverse of any type, as the type in force has them kinematicsForwardOf() ran the bare forward of a type written on the parameter block, without what kinsOpsForward() adds around it: the offset across the tool turned with the tool, the seed of an iterating forward, the pose a type with pose frames reads its frames from. Under the machine frame type the part position in status is the primary type's forward of the commanded joints, so a tool with an X or Y offset put the point it reports off the tip the primary type places. It now runs kinsOpsForward(), the same call kinematicsForward() makes for the type in force. kinematicsInverseOf() is its inverse, through kinsOpsInverse(), seeded from the joints handed in, for a move that holds the tool tip on the part while the machine frame type stays in force: it follows the primary type's inverse. kinematicsInverse() goes through the same helper for the type in force. Exported by modules built on switchkins.c alone, as kinematicsForwardOf() is. --- include/kinematics.h | 11 ++++++++ src/emc/kinematics/switchkins.c | 50 +++++++++++++++++++++------------ 2 files changed, 43 insertions(+), 18 deletions(-) diff --git a/include/kinematics.h b/include/kinematics.h index c65324e54e9..5de73b6aa29 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -165,6 +165,17 @@ extern int kinematicsForwardOf(int ktype, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags); +/* The inverse of any type the module provides, whatever type is in force, +** seeded from the joints handed in: a move that keeps the tool tip on the +** part under the machine frame type follows the primary type's inverse. +** Exported by modules built on switchkins.c alone; returns -1 for a type +** the module does not provide. */ +extern int kinematicsInverseOf(int ktype, + const struct EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 55cd038f36b..1b7c5f4c0cc 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -139,7 +139,7 @@ static void write_block(int ktype) } // the forward of one type, whichever way it was provided, from the pose -// it is handed: no seeding, which is the caller's business +// it is handed, with the tool as the type in force applies it static int call_forward(int ktype, const double *joint, EmcPose *pos, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) @@ -147,11 +147,8 @@ static int call_forward(int ktype, const double *joint, EmcPose *pos, int r; if (kops[ktype]) { read_block(ktype); - r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], - joint, pos, fflags, iflags); - // the frames of a type that is not in force, motion's primary under - // the machine frame type, read the pose from here - kinsOpsKeepPair(kops[ktype], &rt_params, &rt_scratch[ktype], r, joint, pos); + r = kinsOpsForward(kops[ktype], &rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); write_block(ktype); return r; } @@ -159,6 +156,23 @@ static int call_forward(int ktype, const double *joint, EmcPose *pos, return kfwds[ktype](joint, pos, fflags, iflags); } +// and its inverse, seeded from the joints it is handed +static int call_inverse(int ktype, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kinsOpsInverse(kops[ktype], &rt_params, &rt_scratch[ktype], + pos, joint, iflags, fflags); + write_block(ktype); + return r; + } + if (!kinvs[ktype]) { return -1; } + return kinvs[ktype](pos, joint, iflags, fflags); +} + static void set_gui_pins(const EmcPose *p) { hal_set_real(swdata->gui_x, p->tran.x); @@ -305,8 +319,6 @@ int kinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { - int r; - if ( switchkins_type < 0 || switchkins_type >= kins_count || (!kinvs[switchkins_type] && !kops[switchkins_type])) { @@ -315,16 +327,7 @@ int kinematicsInverse(const EmcPose * pos, switchkins_type); return -1; } - if (kops[switchkins_type]) { - read_block(switchkins_type); - r = kinsOpsInverse(kops[switchkins_type], &rt_params, - &rt_scratch[switchkins_type], - pos, joint, iflags, fflags); - write_block(switchkins_type); - return r; - } - r = kinvs[switchkins_type](pos, joint, iflags, fflags); - return r; + return call_inverse(switchkins_type, pos, joint, iflags, fflags); } // kinematicsInverse() int kinematicsToolFrameOf(int ktype, @@ -526,6 +529,16 @@ int kinematicsForwardOf(int ktype, return call_forward(ktype, joint, pos, fflags, iflags); } // kinematicsForwardOf() +int kinematicsInverseOf(int ktype, + const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (kinematicsTypeFlags(ktype) < 0) { return -1; } + return call_inverse(ktype, pos, joint, iflags, fflags); +} // kinematicsInverseOf() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -638,6 +651,7 @@ EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(kinematicsForwardOf); +EXPORT_SYMBOL(kinematicsInverseOf); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); From 13b8e80a3ebbbc972077d992d76daff312883d26 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 30 Oct 2026 13:13:03 +1100 Subject: [PATCH 35/44] tp, motion: a joint segment that follows a line on another kinematics type A joint interpolated segment can now be a line on a kinematics type other than the one in force: the pose runs straight from one end to the other as that type has them, and every servo cycle the joints are that type's inverse of the pose at the planner's progress, seeded from the last cycle's. The segment keeps the joints the servo thread solved, so the next cycle starts from them and an abort holds them where the machine stopped, through the path the joint segment already has for that; the ends are the joints given, exact. The position in status is the forward of the type in force, as for any joint segment, and nothing blends into or out of it. EMCMOT_SET_JOINT_LINE carries the type, the two poses, the length canon measured the line with and canon's velocity and acceleration for it, and EMC_TRAJ_JOINT_MOVE carries them from canon. Motion checks that the type's forward of the joints the queue ends on and of the joints given are the two poses, angles on the turns the interpreter gave, so that a rotary goes the way the program asked, and refuses the move otherwise: the queue does not end where the interpreter solved it, or the tool is not the one it solved with. Each joint's limits scaled onto the line's length still cap it, since a joint has at least its own distance to go over that length, below canon's caps. An inverse that fails part way stops the machine as one under the type in force does. --- src/emc/motion/command.c | 82 +++++++++++++++++++++++++++++++++++-- src/emc/motion/control.c | 22 ++++++++-- src/emc/motion/motion.h | 7 ++++ src/emc/nml_intf/emc.cc | 6 +++ src/emc/nml_intf/emc.hh | 4 +- src/emc/nml_intf/emc_nml.hh | 13 +++++- src/emc/task/emctaskmain.cc | 4 +- src/emc/task/taskintf.cc | 10 ++++- src/emc/tp/tc.c | 41 ++++++++++++++++++- src/emc/tp/tc.h | 1 + src/emc/tp/tc_types.h | 13 +++++- src/emc/tp/tp.c | 55 +++++++++++++++++++++++-- src/emc/tp/tp.h | 6 ++- 13 files changed, 246 insertions(+), 18 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 35ba4578d5b..b14817adc10 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -76,6 +76,8 @@ #pragma weak kinematicsSetTool // old modules export no kinematicsMachineFrame; keep it optional #pragma weak kinematicsMachineFrame +// and only modules built on switchkins.c the forward of another type +#pragma weak kinematicsForwardOf #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -142,6 +144,38 @@ static int inverse_settled(EmcPose *pos, double *joints, return 0; } +/* An end of a line on another kinematics type: that type's forward of the + joints, the angles put on the turn the interpreter gave, so that a rotary + goes the way the program asked. -1 where the forward fails, or where it + puts the joints off the pose the interpreter has there: the queue does + not end where the interpreter thinks, or the tool is not the one it + solved with. */ +static int path_end_pose(int type, const double *joint, const EmcPose *asked, EmcPose *pose) +{ + const double tol = 1e-4; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inv = 0; + double *angle[3] = { &pose->a, &pose->b, &pose->c }; + const double want[3] = { asked->a, asked->b, asked->c }; + + *pose = *asked; + if (!kinematicsForwardOf || kinematicsForwardOf(type, joint, pose, &ff, &inv) != 0) { + return -1; + } + for (int k = 0; k < 3; k++) { + *angle[k] += 360.0 * floor((want[k] - *angle[k]) / 360.0 + 0.5); + } + if (fabs(pose->tran.x - asked->tran.x) > tol || fabs(pose->tran.y - asked->tran.y) > tol + || fabs(pose->tran.z - asked->tran.z) > tol + || fabs(pose->a - asked->a) > tol || fabs(pose->b - asked->b) > tol + || fabs(pose->c - asked->c) > tol + || fabs(pose->u - asked->u) > tol || fabs(pose->v - asked->v) > tol + || fabs(pose->w - asked->w) > tol) { + return -1; + } + return 0; +} + /* Where the queue leaves the joints, which is the seed an iterative inverse wants: the reading runs ahead of the machine, so the joints to hand it are not the ones the machine is standing in. A joint @@ -1202,10 +1236,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) /* a move interpolated in joint space to a Cartesian endpoint: the inverse runs once here, at the endpoint, and the planner takes the joints from there; or the endpoint is given as joints and - the forward says where that is */ + the forward says where that is. Or a line on another + kinematics type to the joints given, which the servo thread + follows with that type's inverse */ double start[EMCMOT_MAX_JOINTS], target[EMCMOT_MAX_JOINTS]; EmcPose end = emcmotCommand->pos; + EmcPose path_start, path_end; double length = 0.0, vmax = 0.0, amax = 0.0, jmax = 0.0; + const int path = emcmotCommand->path_type; int moving = 0, jerk_limited = 0, bad = 0, axis_num; rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_LINE"); @@ -1252,6 +1290,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { target[joint_num] = start[joint_num]; } + if (path >= 0 && !emcmotCommand->have_joint_target) { + reportError(_("the move on line %d along kinematics type %d names no joints to end on"), + emcmotCommand->id, path); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } if (emcmotCommand->have_joint_target) { for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { target[joint_num] = emcmotCommand->joint_target[joint_num]; @@ -1290,10 +1336,28 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } } + /* a line on another type runs between that type's poses of the + two joint sets: the ones the interpreter solved, if the joints + still give them */ + if (path >= 0 + && (path_end_pose(path, start, &emcmotCommand->path_start, &path_start) != 0 + || path_end_pose(path, target, &emcmotCommand->path_end, &path_end) != 0 + || !(emcmotCommand->path_length > 0.0))) { + reportError(_("the move on line %d along kinematics type %d does not start or end" + " where the interpreter solved it"), + emcmotCommand->id, path); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + /* the endpoint must be inside the joint limits, and every joint that moves needs limits to move within; the segment length is the joint space distance and each joint's limits are scaled - onto it so that the slowest joint sets the pace */ + onto it so that the slowest joint sets the pace. A line on + another type is as long as canon measured it, and a joint has + at least its own distance to go over that length */ for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { double d = target[joint_num] - start[joint_num]; joint = &joints[joint_num]; @@ -1309,7 +1373,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } length += d * d; } - length = sqrt(length); + length = path >= 0 ? emcmotCommand->path_length : sqrt(length); for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS && !bad; joint_num++) { double d = fabs(target[joint_num] - start[joint_num]); joint = &joints[joint_num]; @@ -1335,6 +1399,17 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; } + /* and canon's caps for the line, which sampled what the joints + do along it on that type */ + if (path >= 0) { + if (emcmotCommand->vel > 0.0 && (!moving || emcmotCommand->vel < vmax)) { + vmax = emcmotCommand->vel; + } + if (emcmotCommand->acc > 0.0 && (!moving || emcmotCommand->acc < amax)) { + amax = emcmotCommand->acc; + } + } + /* a feed asks for a time; the joint limits still cap it */ double vreq = vmax; if (emcmotCommand->joint_seconds > 0.0 && length / emcmotCommand->joint_seconds < vmax) { @@ -1343,6 +1418,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id); int res_addjoint = tpAddJointLine(&emcmotInternal->coord_tp, start, target, NO_OF_KINS_JOINTS, end, + path, &path_start, &path_end, length, emcmotCommand->motion_type, vreq, vmax, amax, jmax, emcmotStatus->enables_new, diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index dcb7ff844bb..cb8d51caf10 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -58,6 +58,7 @@ KINEMATICS_INVERSE_FLAGS iflags = 0; #pragma weak kinematicsToolFrameOf #pragma weak kinematicsWorkFrameOf #pragma weak kinematicsForwardOf +#pragma weak kinematicsInverseOf /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ @@ -1677,15 +1678,30 @@ static void get_pos_cmds(long period) the planner's own position is the chord between the ends. The joints are commanded either way; a forward that fails, as an iterating one can at a singularity, leaves the last - solved position reported rather than an unsolved one */ + solved position reported rather than an unsolved one. + On a line on another kinematics type the joints between + the ends are that type's inverse of the line's pose, seeded + from the last cycle's and kept on the segment, as the type + in force would solve its own line */ EmcPose pose = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { + int path = tpGetJointPath(&emcmotInternal->coord_tp, &pose); + result = 0; + if (path >= 0) { + result = kinematicsInverseOf + ? kinematicsInverseOf(path, &pose, positions, &iflags, &fflags) : -1; + if (result == 0) { + tpSetJointPos(&emcmotInternal->coord_tp, positions); + } + pose = emcmotStatus->carte_pos_cmd; + } + if (result != 0) { + /* reported below */ + } else if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { emcmotStatus->carte_pos_cmd = pose; emcmotStatus->carte_pos_cmd_ok = 1; } else { emcmotStatus->carte_pos_cmd_ok = 0; } - result = 0; } else { /* a joint interpolated segment that ended this cycle is gone from the queue: its end joints seed the inverse, since the diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index e3579aaa8ca..e51363e6a00 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -288,6 +288,13 @@ extern "C" { double joint_target[EMCMOT_MAX_JOINTS]; int have_joint_target; double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + /* or a line on another kinematics type to those joints: path_type the + type, -1 for none, the line from path_start to path_end as that type + has them, path_length long as canon measured it, vel and acc canon's */ + int path_type; + EmcPose path_start; + EmcPose path_end; + double path_length; /* SET_OFFSET: pos is where the interpreter expects the point to be once the offset is on, for motion to check its own answer against */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index ce03e3451cd..7d4b1ea5f5f 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1691,6 +1691,12 @@ void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) cms->update(joints, EMCMOT_MAX_JOINTS); cms->update(have_joints); cms->update(seconds); + cms->update(path_type); + EmcPose_update(cms, &path_start); + EmcPose_update(cms, &path_end); + cms->update(path_length); + cms->update(vel); + cms->update(acc); } /* diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 3ce852cf480..000f9aade81 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -379,7 +379,9 @@ extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale, int indexer_jnum); -extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds); +extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds, + int path_type, const EmcPose& path_start, const EmcPose& path_end, + double path_length, double vel, double acc); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 8a851102e80..febe03d99ad 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,12 +960,16 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { // a move interpolated in joint space: to the world endpoint, whose joints // motion finds with the inverse; or to the joints given, whose world -// position motion finds with the forward +// position motion finds with the forward. Or, with path_type a kinematics +// type, a line on that type to the joints given: from path_start to +// path_end as that type has them, path_length long as canon measured it, +// at no more than vel and acc class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_JOINT_MOVE() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), - end{}, joints{}, have_joints(0), seconds(0.0) + end{}, joints{}, have_joints(0), seconds(0.0), + path_type(-1), path_start{}, path_end{}, path_length(0.0), vel(0.0), acc(0.0) {}; // For internal NML/CMS use only. @@ -977,6 +981,11 @@ class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { double joints[EMCMOT_MAX_JOINTS]; int have_joints; double seconds; /* 0 for a rapid, else the time it is to take */ + int path_type; + EmcPose path_start; + EmcPose path_end; + double path_length; + double vel, acc; }; class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index b7f88a6bfbc..f95b813442c 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1999,7 +1999,9 @@ static int emcTaskIssueCommand(NMLmsg * cmd) case EMC_TRAJ_JOINT_MOVE_TYPE: { EMC_TRAJ_JOINT_MOVE *jm = reinterpret_cast(cmd); emcTrajUpdateTag(jm->tag); - retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds); + retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds, + jm->path_type, jm->path_start, jm->path_end, + jm->path_length, jm->vel, jm->acc); break; } diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index adde3409e4a..448d3ae7370 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1506,7 +1506,9 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } -int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds) +int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds, + int path_type, const EmcPose& path_start, const EmcPose& path_end, + double path_length, double vel, double acc) { int i; @@ -1520,6 +1522,12 @@ int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { emcmotCommand.joint_target[i] = (have_joints && joints) ? joints[i] : 0.0; } + emcmotCommand.path_type = path_type; + emcmotCommand.path_start = path_start; + emcmotCommand.path_end = path_end; + emcmotCommand.path_length = path_length; + emcmotCommand.vel = vel; + emcmotCommand.acc = acc; return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 72976edb34b..4ec1dd05c6e 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -586,7 +586,9 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) /** - * The joints of a joint interpolated segment at its progress. + * The joints of a joint interpolated segment at its progress; on a line on + * another kinematics type the ones the servo thread last solved, which + * seed its next inverse, and the ends exact. * Returns the joint count, or 0 for any other segment. */ int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) @@ -600,11 +602,46 @@ int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; if (f > 1.0) { f = 1.0; } for (i = 0; i < jl->num_joints; i++) { - joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + if (jl->path_type < 0) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } else if (f >= 1.0) { + joints[i] = jl->end[i]; + } else { + joints[i] = jl->now_valid ? jl->now[i] : jl->start[i]; + } } return jl->num_joints; } +/** + * The pose on a line on another kinematics type at its progress, strictly + * between the ends, whose joints are the segment's own. + * Returns the type, or -1 for any other segment or at an end. + */ +int tcGetJointPath(TC_STRUCT const * const tc, EmcPose * const pose) +{ + const PmJointLine *jl; + EmcPose d; + double f; + + if (!tc || tc->motion_type != TC_JOINT) { return -1; } + jl = &tc->coords.joint; + if (jl->path_type < 0 || tc->target <= 0.0) { return -1; } + f = tc->progress / tc->target; + if (f <= 0.0 || f >= 1.0) { return -1; } + emcPoseSub(&jl->path_end, &jl->path_start, &d); + pose->tran.x = jl->path_start.tran.x + f * d.tran.x; + pose->tran.y = jl->path_start.tran.y + f * d.tran.y; + pose->tran.z = jl->path_start.tran.z + f * d.tran.z; + pose->a = jl->path_start.a + f * d.a; + pose->b = jl->path_start.b + f * d.b; + pose->c = jl->path_start.c + f * d.c; + pose->u = jl->path_start.u + f * d.u; + pose->v = jl->path_start.v + f * d.v; + pose->w = jl->path_start.w + f * d.w; + return jl->path_type; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index d6072f824c2..f66b7679eb4 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -38,6 +38,7 @@ int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); int tcGetJointPos(TC_STRUCT const * const tc, double * const joints); +int tcGetJointPath(TC_STRUCT const * const tc, EmcPose * const pose); int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 53e11266d31..5d46e00ff48 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -122,13 +122,24 @@ typedef struct { * end together, the longest one setting the pace. The world poses at the * two ends are what the segments around it see; the position along the way * is not a line in world space and the servo thread reports it from the - * forward kinematics. */ + * forward kinematics. + * + * Or, where path_type is a kinematics type, a line on that type while + * another is in force: the pose runs straight from path_start to path_end + * as that type has them, the progress measured as canon measured it, and + * the servo thread solves the joints with that type's inverse every cycle + * and writes them back to now. start and end are still the ends, exact. */ typedef struct { double start[EMCMOT_MAX_JOINTS]; double end[EMCMOT_MAX_JOINTS]; int num_joints; EmcPose world_start; EmcPose world_end; + int path_type; + EmcPose path_start; + EmcPose path_end; + double now[EMCMOT_MAX_JOINTS]; + int now_valid; } PmJointLine; typedef struct { diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 8c700ce339e..7346d735f2b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2140,9 +2140,16 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { * Nothing blends into or out of it: the segment before it is made to stop * and so is this one, since the path between the two world poses is not a * line and the next segment has to start from rest at world_end. + * + * With path_type a kinematics type it is a line on that type from + * path_start to path_end instead, path_length long as canon measured it, + * vel and acc canon's for it: the servo thread solves its joints with that + * type's inverse, and start and end are where it begins and ends. */ int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, - int num_joints, EmcPose world_end, int canon_motion_type, + int num_joints, EmcPose world_end, int path_type, + const EmcPose *path_start, const EmcPose *path_end, double path_length, + int canon_motion_type, double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, struct state_tag_t tag) { @@ -2152,7 +2159,8 @@ int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, double length = 0.0; int i; - if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS + || (path_type >= 0 && (!path_start || !path_end))) { return TP_ERR_MISSING_INPUT; } if (tp->aborting) { @@ -2177,8 +2185,13 @@ int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, } jl->world_start = tp->goalPos; jl->world_end = world_end; + jl->path_type = path_type < 0 ? -1 : path_type; + if (path_type >= 0) { + jl->path_start = *path_start; + jl->path_end = *path_end; + } - tc.target = pmSqrt(length); + tc.target = path_type >= 0 ? path_length : pmSqrt(length); if (tc.target < TP_POS_EPSILON) { return TP_ERR_ZERO_LENGTH; } @@ -4645,6 +4658,40 @@ int tpGetJointPos(TP_STRUCT const * const tp, double * const joints) return tcGetJointPos(tc, joints); } +/** + * The pose the active segment is at, when it is a line on another + * kinematics type and between its ends: the servo thread solves the + * joints there with that type's inverse, seeded from tpGetJointPos(). + * Returns the type, or -1. + */ +int tpGetJointPath(TP_STRUCT const * const tp, EmcPose * const pose) +{ + TC_STRUCT const *tc; + + if (!tp || !pose) { return -1; } + tc = tcqItem((TC_QUEUE_STRUCT *)&tp->queue, 0); + if (!tc || !tc->active) { return -1; } + return tcGetJointPath(tc, pose); +} + +/** + * The joints the servo thread solved for the active segment's pose, kept + * on it: the next cycle's seed, and where an abort holds the joints. + */ +void tpSetJointPos(TP_STRUCT * const tp, const double * const joints) +{ + TC_STRUCT *tc; + PmJointLine *jl; + int i; + + if (!tp || !joints) { return; } + tc = tcqItem(&tp->queue, 0); + if (!tc || !tc->active || tc->motion_type != TC_JOINT) { return; } + jl = &tc->coords.joint; + for (i = 0; i < jl->num_joints; i++) { jl->now[i] = joints[i]; } + jl->now_valid = 1; +} + /** * The end joints of a joint interpolated segment that completed this * cycle, once: the seed for the servo thread's inverse of the position the @@ -4781,6 +4828,8 @@ EXPORT_SYMBOL(tpAddLine); EXPORT_SYMBOL(tpAddJointLine); EXPORT_SYMBOL(tpGetGoalPos); EXPORT_SYMBOL(tpGetJointPos); +EXPORT_SYMBOL(tpGetJointPath); +EXPORT_SYMBOL(tpSetJointPos); EXPORT_SYMBOL(tpTakeJointEnd); EXPORT_SYMBOL(tpJointSegmentsQueued); EXPORT_SYMBOL(tpGetQueueEndJoints); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index f11955e0442..8b7f1a26d8b 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -67,10 +67,14 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos); int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, - int num_joints, EmcPose world_end, int canon_motion_type, + int num_joints, EmcPose world_end, int path_type, + const EmcPose *path_start, const EmcPose *path_end, double path_length, + int canon_motion_type, double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, struct state_tag_t tag); int tpGetJointPos(TP_STRUCT const * const tp, double * const joints); +int tpGetJointPath(TP_STRUCT const * const tp, EmcPose * const pose); +void tpSetJointPos(TP_STRUCT * const tp, const double * const joints); int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints); int tpJointSegmentsQueued(TP_STRUCT const * const tp); int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints); From 5fa1e082f259143274b605fcfd71a65dfe8bab65 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 30 Oct 2026 13:13:03 +1100 Subject: [PATCH 36/44] interp: G53.6 keeps the tool tip on the part all along under the machine frame kinematics Under the machine frame type G13.1 selects, G53.6 kept the tool tip at the ends of its move only: it went as a straight traverse in the machine frame, the slides in straight lines while the rotaries turned, and the tip left its point in between, by L(1 - cos(t/2)) for a tool L long turned through t, 3.4 mm for a 100 mm tool tilted 30 degrees. Sigma1912 reported it. Switching to the TCP type for the move and back after would hold the tip, but an abort or a feed hold part way leaves the machine on a type the program did not select, with nothing to put it back. Fanuc gives G53.6 in G49 mode and keeps the tool centre point throughout; Heidenhain's PLANE MOVE keeps the tool and the workpiece where they are relative to each other with a compensating move, without M128. Neither switches anything. So G53.6 under G13.1 is now the primary type's line from where the tip stands to the same tip at the orientation found, which motion follows with that type's inverse while G13.1 stays in force: the same move G53.6 makes under the TCP type, joint for joint. The solver leaves the linear joints where they were, so the joints at the end are the primary type's inverse of the tip at the new orientation. Under the TCP type G53.6 is the straight traverse it was. KINS_TYPE_TRAVERSE carries it to canon, which plans it as the type would plan the line in force: canon's measure, the axis limits, and the joint velocity, acceleration and travel along it sampled through that type's kinematics, from where the line starts rather than from canon's end point, which is on the type in force. A line of no length, a G53.6 that has nothing left to turn, is no move, as a straight traverse of no length is none; motion refuses such a line along another type. The preview draws it as that line on the part: the renderer under G13.1 maps the points of a move onto the part through the primary type, and the slides' straight line between the ends would come out as the loop the tip no longer makes, so a part_traverse is drawn straight between its ends on the part, as under the TCP type. tests/twp-static-tip, on the table machine of tests/twp-static, and tests/twp-static-tip-head, on its head machine: status part_position stays within 1e-3 of its point all along G53.6 under G13.1 (4e-4 measured with the rotaries at 180 deg/s, which the test sets through the INI's HAL pins for these moves: the joints between servo cycles come from motion's interpolation, whose error grows with the square of the speed), and the move ends on the joints the TCP type ends on, G13.1 still in force; a second G53.6 there is no move and no error. An abort and a feed hold part way leave the tip on its point, and the machine goes on from there to the same end. A slide that peaks half way round, the tool turning from one side of the vertical to the other, refuses the move under G13.1 as under the TCP type when its limit is set between its ends and its peak. In tests/twp-static-preview the preview of a G53.6 under G13.1 draws the same vertices as under the TCP type, to 1e-3 (identical as measured); drawn as a plain traverse it is 11.4 mm and 7.3 mm off. With the joints interpolated straight between the ends instead, the tip leaves its point by 11.4 mm on the table and 7.3 mm on the head and the test fails. --- docs/src/gcode/g-code.adoc | 2 +- src/emc/nml_intf/canon.hh | 11 ++ src/emc/rs274ngc/gcode_renderer.cc | 9 +- src/emc/rs274ngc/gcode_renderer.hh | 6 + src/emc/rs274ngc/gcodemodule.cc | 17 +++ src/emc/rs274ngc/gcodemodule.hh | 6 + src/emc/rs274ngc/interp_workplane.cc | 32 ++++ src/emc/sai/saicanon.cc | 19 +++ src/emc/task/emccanon.cc | 59 ++++++++ tests/twp-static-preview/README | 4 +- tests/twp-static-preview/test-ui.py | 30 ++++ tests/twp-static-tip-head/README | 2 + tests/twp-static-tip-head/checkresult | 3 + tests/twp-static-tip-head/head.hal | 1 + tests/twp-static-tip-head/head.ini | 1 + tests/twp-static-tip-head/test-ui.py | 1 + tests/twp-static-tip-head/test.sh | 6 + tests/twp-static-tip-head/tool.tbl | 1 + tests/twp-static-tip/README | 17 +++ tests/twp-static-tip/checkresult | 3 + tests/twp-static-tip/table.hal | 1 + tests/twp-static-tip/table.ini | 1 + tests/twp-static-tip/test-ui.py | 202 ++++++++++++++++++++++++++ tests/twp-static-tip/test.sh | 6 + tests/twp-static-tip/tool.tbl | 1 + tests/twp-static/README | 2 +- 26 files changed, 439 insertions(+), 4 deletions(-) create mode 100644 tests/twp-static-tip-head/README create mode 100755 tests/twp-static-tip-head/checkresult create mode 120000 tests/twp-static-tip-head/head.hal create mode 120000 tests/twp-static-tip-head/head.ini create mode 120000 tests/twp-static-tip-head/test-ui.py create mode 100755 tests/twp-static-tip-head/test.sh create mode 120000 tests/twp-static-tip-head/tool.tbl create mode 100644 tests/twp-static-tip/README create mode 100755 tests/twp-static-tip/checkresult create mode 120000 tests/twp-static-tip/table.hal create mode 120000 tests/twp-static-tip/table.ini create mode 100755 tests/twp-static-tip/test-ui.py create mode 100755 tests/twp-static-tip/test.sh create mode 120000 tests/twp-static-tip/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index ce3124b09cf..af11e14ac9f 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2679,7 +2679,7 @@ It is an error if: Under a module's machine frame kinematics, the type <> selects, a plane is cut where the module's TCP kinematics would cut it with the rotary joints held where they stand. The plane sits on the part, turned with the table, and the moves under it are made by the slides alone, so a rotary move never drags them along as it does under the TCP kinematics. This is 3+2 machining with the rotaries set once, what Fanuc calls the second feature coordinate system. -When 'G68.2', 'G68.3' or 'G68.4' defines a plane, the interpreter takes the map from the part, as the TCP kinematics places it, to the machine frame, at the rotaries as they stand. With the rotaries held that map is a fixed rotation and shift, and it carries the tool length along the tilted tool, which the TCP kinematics applies and the machine frame kinematics does not. 'G53.1', 'G53.3' and 'G53.6' orient the tool to the plane on the part with the TCP kinematics' frames, so a table rotary helps where the plane needs it, and take the map again at the rotaries they end at. 'G53.6' keeps the tool tip where it is at the ends of its move; in between, the slides and the rotaries move in straight lines in the machine frame and the tip leaves the point while they turn. 'G53.2' moves nothing and leaves the map as it was. +When 'G68.2', 'G68.3' or 'G68.4' defines a plane, the interpreter takes the map from the part, as the TCP kinematics places it, to the machine frame, at the rotaries as they stand. With the rotaries held that map is a fixed rotation and shift, and it carries the tool length along the tilted tool, which the TCP kinematics applies and the machine frame kinematics does not. 'G53.1', 'G53.3' and 'G53.6' orient the tool to the plane on the part with the TCP kinematics' frames, so a table rotary helps where the plane needs it, and take the map again at the rotaries they end at. 'G53.6' keeps the tool tip where it is all along its move, as under the TCP kinematics: motion follows the TCP kinematics' joints for the turn while the machine frame kinematics stays in force, so an abort or a feed hold part way leaves the tip on its point and nothing to switch back. 'G53.2' moves nothing and leaves the map as it was. A rotary word under the plane moves the rotary alone and the map stays, so the moves after it are still cut where the plane sat on the part before the turn, which is now somewhere else. Fanuc and Heidenhain do the same, and Fanuc's manual tells the programmer not to turn a rotary in a tilted working plane. The preview draws those moves where they land on the turned part. To work on the turned part, give the plane again after the turn: 'G68.4 X0 Y0 Z0 I0 J0 K0' is the same plane at the rotaries as they now stand, and 'G53.1' turns the rotaries to the plane and takes the map where they end. diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index c823a6329f0..7428f4ff0df 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -333,6 +333,17 @@ extern void JOINT_FEED(int lineno, const double *joints, int have_joints, double a, double b, double c, double u, double v, double w, double seconds); +/* A traverse that kinematics type ktype interpolates while another is in + force: the pose runs straight from start to end as that type has them, + in machine units, and motion follows it with that type's inverse to the + joints given. It is planned as that type would plan the line, and x..w + say where it ends in program coordinates on the type in force. */ +extern void KINS_TYPE_TRAVERSE(int lineno, int ktype, + const EmcPose &start, const EmcPose &end, + const double *joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); /* Move at traverse rate so that at any time during the move, all axes diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 04c7f40e674..95edab13fd0 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -1105,7 +1105,14 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, Point9 pt, drawn; for(int k = 0; k < P9_COUNT; k++) pt[k] = t * p2[k] + (1.0 - t) * p1[k]; - on_part(pt, drawn); + // a line on the part is one between its ends there, whatever + // the slides do to keep to it + if(part_line_) { + for(int k = 0; k < P9_COUNT; k++) + drawn[k] = t * d2[k] + (1.0 - t) * d1[k]; + } else { + on_part(pt, drawn); + } write_vertex(pt, drawn, line_number, cat, nullptr); } } diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 08aa55e2c75..6f53886f0fc 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -371,6 +371,11 @@ public: void straight_traverse(int line_number, const Point9 &p) override { append(Traverse, line_number, p); } + void part_traverse(int line_number, const Point9 &p) override { + part_line_ = true; + append(Traverse, line_number, p); + part_line_ = false; + } void straight_probe(int line_number, const Point9 &p) override { append(Probe, line_number, p); } @@ -575,6 +580,7 @@ private: WorkFrame frame_; // the tilted work plane, inside g92 long workplane_ = -1; // its record, or none in effect bool lo_unreached_ = false; // lo_ reached under it by an undrawn traverse + bool part_line_ = false; // the move is a line on the part bool kins_switched_ = false; // the chain point waits for the point read back Point9 lo_ = {}; // chain point diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index c0526c09efa..eb89d39ee7a 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -549,6 +549,23 @@ void JOINT_FEED(int line_number, const double *joints, int have_joints, STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); } +// a move the primary type interpolates under the machine frame type is a +// line on the part, which the preview draws through that type +void KINS_TYPE_TRAVERSE(int line_number, int ktype, + const EmcPose &start, const EmcPose &end, + const double *joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)ktype; + (void)start; + (void)end; + (void)joints; + parse_state.pos = {x, y, z, a, b, c, u, v, w}; + parse_state.canon->part_traverse(line_number, + ensure_inch({x, y, z, a, b, c, u, v, w})); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index 78fd2e76d08..da381c5b39d 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -157,6 +157,12 @@ public: // not axes, so there is no point to pack. virtual void straight_feed(int line_number, const Point9 &p) = 0; virtual void straight_traverse(int line_number, const Point9 &p) = 0; + // A traverse the type the part is drawn on interpolates, under another: + // a line on the part, where the type in force would bend it. A canon + // that draws no part has it as a traverse. + virtual void part_traverse(int line_number, const Point9 &p) { + straight_traverse(line_number, p); + } virtual void straight_probe(int line_number, const Point9 &p) = 0; // `retract_scale`: G33.1's I word, the retract's spindle-speed multiple. // Not in the Python callback, which stays three-argument. diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 17380a5d9ae..7840e343194 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1493,6 +1493,38 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) s->current_x = end_prog[0]; s->current_y = end_prog[1]; s->current_z = end_prog[2]; + } else if (code == G_53_6 && primary >= 0) { + // under the machine frame type the tool centre point stays on the + // part all the way, as under the primary type: the move is that + // type's line from where the tip stands to the same tip at the + // orientation found, which motion follows with its inverse while + // the type in force stays. The solver left the linear joints + // where they were, so the end's joints are that type's inverse. + EmcPose from = {}, to = {}; + double end[EMCMOT_MAX_JOINTS]; + bool placed; + CHKS((kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + current_machine_pose(s, &from); + placed = kinematicsUserForward(ctx, now, &from) == 0; + to = from; + placed = placed && kinematicsUserForward(ctx, sol, &to) == 0; + to.tran = from.tran; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { end[i] = sol[i]; } + for (int pass = 0; placed && pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = end[i]; } + placed = kinematicsUserInverse(ctx, &to, end) == 0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(end[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + kinematicsUserSetType(ctx, s->kins_type); + CHKS((!placed), _("%s: kinematics type %d cannot hold the tool tip at the orientation found"), + name, primary); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = end[i]; } + KINS_TYPE_TRAVERSE(block->line_number, primary, from, to, end, + s->current_x, s->current_y, s->current_z, + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); } else if (code == G_53_6) { // the tool centre point stays: a Cartesian move of the rotaries STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index fb0ddb0ae88..d8d4709f767 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -243,6 +243,25 @@ void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, _sai._program_position_c = c; } +void KINS_TYPE_TRAVERSE(int /*line_number*/, int ktype, + const EmcPose & /*start*/, const EmcPose & /*end*/, + const double *joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + ECHO_WITH_ARGS("{}, [{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}], " + "{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}", + ktype, joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 2cfa7fee513..78f21d16b87 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1642,6 +1642,65 @@ void JOINT_FEED(int line_number, const double *joints, int have_joints, joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, seconds); } +void KINS_TYPE_TRAVERSE(int line_number, int ktype, + const EmcPose &start, const EmcPose &end, + const double *joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + auto msg = std::make_unique(); + const double from[9] = {start.tran.x, start.tran.y, start.tran.z, + start.a, start.b, start.c, start.u, start.v, start.w}; + const double to[9] = {end.tran.x, end.tran.y, end.tran.z, + end.a, end.b, end.c, end.u, end.v, end.w}; + double f[9], e[9]; + + flush_segments(); + if (kins_dropped(line_number)) { return; } + for (int n = 0; n < 9; n++) { + f[n] = FROM_EXT_AX(n, from[n]); + e[n] = FROM_EXT_AX(n, to[n]); + } + + // the line as type ktype would plan it in force: measured, limited by + // the axes and capped through its kinematics from where it starts, + // which is not canon.endPoint, a point on the type in force + CANON_POSITION here = canon.endPoint; + int type = kins.type; + canon.endPoint = CANON_POSITION(f[0], f[1], f[2], f[3], f[4], f[5], f[6], f[7], f[8]); + kins.type = ktype; + kins.memo.valid = false; + StraightSpan span = getStraightSpan(e[0], e[1], e[2], e[3], e[4], e[5], e[6], e[7], e[8]); + VelData veldata = getStraightVelocity(e[0], e[1], e[2], e[3], e[4], e[5], e[6], e[7], e[8]); + AccelData accdata = getStraightAcceleration(e[0], e[1], e[2], e[3], e[4], e[5], e[6], e[7], e[8]); + msg->path_length = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + msg->vel = toExtVel(veldata.vel); + msg->acc = toExtAcc(accdata.acc); + canon.endPoint = here; + kins.type = type; + kins.memo.valid = false; + if (kins_dropped(line_number)) { return; } + + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = 1; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { msg->joints[i] = joints[i]; } + msg->path_type = ktype; + msg->path_start = start; + msg->path_end = end; + // a line of no length, nothing to turn, is no move, as a straight + // traverse of no length is none + if (msg->path_length > 0.0) { + interp_list.set_line_number(line_number); + tag_and_send(std::move(msg), _tag); + } + + canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); +} + void generate_move(double vel,double x, double y, double z, double a, double b, double c, double u, double v, double w) diff --git a/tests/twp-static-preview/README b/tests/twp-static-preview/README index d28b63b5b5a..79b380106fa 100644 --- a/tests/twp-static-preview/README +++ b/tests/twp-static-preview/README @@ -17,7 +17,9 @@ after a plane set in MDI and a rotary turned since, and with a plane in the program itself, the preview's interpreter ends where the machine does. What the preview draws is on the part, as under the TCP kinematics: the same program draws one plane in the same place, and ends its cut at the same -point, under G13.1 as under the TCP kinematics. The live plot is on the +point, under G13.1 as under the TCP kinematics; G53.6 is drawn with the same +vertices under both, the tip held, not the loop the slides would make +running straight between the ends. The live plot is on the part too: under G13.1 in the plane it is where the TCP kinematics plots the same point, with the tool frame in status the TCP kinematics', and switches to the TCP kinematics and back neither move it nor draw a line. diff --git a/tests/twp-static-preview/test-ui.py b/tests/twp-static-preview/test-ui.py index 213b647378d..33f831ce52c 100755 --- a/tests/twp-static-preview/test-ui.py +++ b/tests/twp-static-preview/test-ui.py @@ -309,6 +309,36 @@ def logged(): error("a switch in the plane drew a line in the live plot to %s" % show(q['pos'])) break +# G53.6 under G13.1 is drawn as the machine runs it, the tip held on the +# part all along, as under the part's type: the same vertices, and no loop +# where the slides swing round the tip. +for cmd in (RESET, START): + mdi(cmd) +body = "G43 H1\n%s\nG1 X0 Y0 Z30 F60000\nG53.6\nM2\n" % plane +on_part = rendered("G53.6 drawn under the part's type", TCP + "\n" + body) +on_g131 = rendered("G53.6 drawn under G13.1", "G13.1\n" + body) +def g536_drawn(canon): + # the vertices of the G53.6 line, the fifth, that are drawn + pg = canon.program_geometry + keep = (np.asarray(pg.lines) == 5) & (np.asarray(pg.kinds) != glcanon_bake.KIND_NOOP) + return 25.4 * np.asarray(pg.positions())[keep] +if on_part and on_g131: + a = 25.4 * np.asarray(on_part.program_geometry.positions()) + b = 25.4 * np.asarray(on_g131.program_geometry.positions()) + tip = g536_drawn(on_g131) + if len(tip) == 0 or a.shape != b.shape: + error("the preview draws the program with %d vertices under G13.1, %d under the part's type" + % (len(b), len(a))) + else: + worst = np.max(np.abs(a - b)) + spread = np.max(np.ptp(tip, axis=0)) + print("G53.6 drawn under G13.1: %d vertices within %.6f of the part's type, spread %.6f" + % (len(b), worst, spread)) + if worst > 1e-3: + error("the preview draws G53.6 under G13.1 up to %.4f away from the part's type" % worst) + if spread > 1e-3: + error("the preview draws the tip of G53.6 under G13.1 moving %.4f" % spread) + # With no plane, under G13.1 alone, the preview and the live plot are on # the part as well: the slides put the tool on the part where the part's # type reads the joints. A rotary turned under G13.1 sweeps the tool round diff --git a/tests/twp-static-tip-head/README b/tests/twp-static-tip-head/README new file mode 100644 index 00000000000..c4869de2c76 --- /dev/null +++ b/tests/twp-static-tip-head/README @@ -0,0 +1,2 @@ +twp-static-tip on the head machine, 5axiskins with a 100 mm pivot (head.ini): see +twp-static-tip/README. diff --git a/tests/twp-static-tip-head/checkresult b/tests/twp-static-tip-head/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static-tip-head/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static-tip-head/head.hal b/tests/twp-static-tip-head/head.hal new file mode 120000 index 00000000000..ac7a6410758 --- /dev/null +++ b/tests/twp-static-tip-head/head.hal @@ -0,0 +1 @@ +../twp-static/head.hal \ No newline at end of file diff --git a/tests/twp-static-tip-head/head.ini b/tests/twp-static-tip-head/head.ini new file mode 120000 index 00000000000..4359ba23a1f --- /dev/null +++ b/tests/twp-static-tip-head/head.ini @@ -0,0 +1 @@ +../twp-static/head.ini \ No newline at end of file diff --git a/tests/twp-static-tip-head/test-ui.py b/tests/twp-static-tip-head/test-ui.py new file mode 120000 index 00000000000..0eb3fce7fe0 --- /dev/null +++ b/tests/twp-static-tip-head/test-ui.py @@ -0,0 +1 @@ +../twp-static-tip/test-ui.py \ No newline at end of file diff --git a/tests/twp-static-tip-head/test.sh b/tests/twp-static-tip-head/test.sh new file mode 100755 index 00000000000..eaf0a5bac2d --- /dev/null +++ b/tests/twp-static-tip-head/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r head.ini diff --git a/tests/twp-static-tip-head/tool.tbl b/tests/twp-static-tip-head/tool.tbl new file mode 120000 index 00000000000..787a724629c --- /dev/null +++ b/tests/twp-static-tip-head/tool.tbl @@ -0,0 +1 @@ +../twp-static/tool.tbl \ No newline at end of file diff --git a/tests/twp-static-tip/README b/tests/twp-static-tip/README new file mode 100644 index 00000000000..7733df91696 --- /dev/null +++ b/tests/twp-static-tip/README @@ -0,0 +1,17 @@ +G53.6 under the machine frame kinematics (G13.1) on the table machine of +twp-static, whose INI and HAL files and helpers it shares; +twp-static-tip-head runs it on the head machine. + +G53.6 under G13.1 keeps the tip on the part the whole way, not only at the +ends: status part_position stays on its point while the rotaries turn, and +the move ends where the TCP kinematics ends it, G13.1 still in force; a +second G53.6 there has nothing to turn and is no move. An +abort and a feed hold part way leave the tip on its point, and the machine +goes on from there to the same end. A slide that swings past its limit +half way round, the tool turning from one side of the vertical to the +other, refuses G53.6 under G13.1 as under the TCP kinematics. + +These moves run the rotaries at 180 deg/s, set through the INI's HAL pins: +the joints between servo cycles come from motion's interpolation, which +puts the tip off the path by an amount that grows with the square of the +speed. diff --git a/tests/twp-static-tip/checkresult b/tests/twp-static-tip/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-static-tip/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-static-tip/table.hal b/tests/twp-static-tip/table.hal new file mode 120000 index 00000000000..8d42f68e380 --- /dev/null +++ b/tests/twp-static-tip/table.hal @@ -0,0 +1 @@ +../twp-static/table.hal \ No newline at end of file diff --git a/tests/twp-static-tip/table.ini b/tests/twp-static-tip/table.ini new file mode 120000 index 00000000000..a612af8cca7 --- /dev/null +++ b/tests/twp-static-tip/table.ini @@ -0,0 +1 @@ +../twp-static/table.ini \ No newline at end of file diff --git a/tests/twp-static-tip/test-ui.py b/tests/twp-static-tip/test-ui.py new file mode 100755 index 00000000000..4173ba18e87 --- /dev/null +++ b/tests/twp-static-tip/test-ui.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# G53.6 under the machine frame type, the tip held all along: see README. + +import os +import sys +import hal +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "twp-static")) +from static import * + +# G53.6 under G13.1 keeps the tool tip on the part the whole way, not only +# at its ends, as under the part's type: the slides swing round the tip +# while the rotaries turn, on the joints the part's type solves, and G13.1 +# stays in force. Status part_position, the part's type read from the +# commanded joints, is where the tip is. Stopped part way, by an abort or +# a feed hold, the tip is still on its point: nothing was switched, so +# nothing has to be switched back. +def tip(): + s.poll() + return [s.part_position[i] for i in range(3)] + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +# The joints between servo cycles come from motion's interpolation, which +# puts the tip off the path by an amount that grows with the square of the +# speed: about 0.2 um at 180 deg/s, 6 um at the sim's full speed. These +# moves run the rotaries at 180 deg/s, through the INI's HAL pins. +ROTARY = [k for k in range(JOINTS) if ini.find("JOINT_%d" % k, "TYPE") == "ANGULAR"] +def rotaries(slow): + for k, letter in zip(ROTARY, (FIRST, SECOND)): + for pin, section in (("ini.%d." % k, "JOINT_%d" % k), ("ini.%s." % letter.lower(), "AXIS_" + letter)): + for name, low in (("max_velocity", 180), ("max_acceleration", 1800)): + hal.set_p(pin + name, str(low if slow else ini.find(section, name.upper()))) + # task takes the pins up once a cycle + s.poll() + beat = s.taskbeat + 2 + while s.taskbeat < beat: + time.sleep(0.002) + s.poll() + +def turn_from(j0, j1): + # how far along from j0 to j1 the joints are, by the one moving most + k = max(range(JOINTS), key=lambda i: abs(j1[i] - j0[i])) + return lambda j: (j[k] - j0[k]) / (j1[k] - j0[k]) + +def watch(until, t0, act=None, at=0.3, rest=lambda: s.inpos and not s.queue): + # the tip's worst distance from t0 until the move is at rest, and what + # act() did once the move is at least 'at' of the way along + worst, middle, acted = 0.0, 0, None + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + p = [s.part_position[i] for i in range(3)] + worst = max(worst, max(abs(a - b) for a, b in zip(p, t0))) + f = until([s.joint_position[i] for i in range(JOINTS)]) + if 0.1 < f < 0.9: + middle += 1 + if act and acted is None and f >= at: + acted = act() + if rest() and (acted is not None or act is None) and f > 0.0: + break + time.sleep(0.002) + return worst, middle, acted + +tip_setup = ["G43 H1", plane, "G0 X0 Y0 Z30"] +want = run(TCP, tip_setup + ["G53.6"])[-1] +start = run("G13.1", tip_setup)[-1] +held_type = s.kinematics_type +t0 = tip() +along = turn_from(start, want) +rotaries(True) +c.mdi("G53.6") +worst, middle, _ = watch(along, t0) +got = settled() +rotaries(False) +drain() +print("G53.6 under G13.1: the tip within %.6f of its point over %d samples part way" % (worst, middle)) +if middle < 5: + error("G53.6 under G13.1 was not seen part way: %d samples" % middle) +if worst > 1e-3: + error("G53.6 under G13.1 took the tip %.4f off its point" % worst) +if not same(got, want): + error("G53.6 under G13.1 ended at %s, the part's type at %s" % (show(got), show(want))) +if s.kinematics_type != held_type: + error("G53.6 under G13.1 left kinematics type %d in force, not %d" % (s.kinematics_type, held_type)) +# again where it stands: nothing to turn, no move, and no error +c.mdi("G53.6") +ended = done() +again = settled() +drain() +if ended != linuxcnc.RCS_DONE or not same(again, got): + error("G53.6 under G13.1 again where it stood: %s, from %s to %s" % (ended, show(got), show(again))) + +# an abort part way: the tip on its point, the rotaries part way round, +# G13.1 in force, and G53.6 again from there ends where the whole move does +def abort(): + c.abort() + return True +start = run("G13.1", tip_setup)[-1] +rotaries(True) +c.mdi("G53.6") +worst, middle, _ = watch(along, t0, abort) +stopped = settled() +rotaries(False) +drain(expect_error=True) +f = along(stopped) +print("G53.6 under G13.1 aborted %.0f%% of the way: the tip within %.6f" % (100 * f, worst)) +if not 0.05 < f < 0.95: + error("the abort did not stop G53.6 part way: %.2f of the way" % f) +if worst > 1e-3 or max(abs(a - b) for a, b in zip(tip(), t0)) > 1e-3: + error("G53.6 aborted part way took the tip %.4f off its point" % worst) +if s.kinematics_type != held_type: + error("the abort left kinematics type %d in force, not %d" % (s.kinematics_type, held_type)) +mdi("G69") +mdi(plane) +got = mdi("G53.6") +if max(abs(a - b) for a, b in zip(got, want)) > 1e-5: + error("G53.6 after the abort ended at %s, the whole move at %s" % (show(got), show(want))) + +# a feed hold part way: the joints stop with the tip on its point, and the +# resumed move ends where the whole move does +def hold(): + c.auto(linuxcnc.AUTO_PAUSE) + return True +start = run("G13.1", tip_setup)[-1] +rotaries(True) +c.mdi("G53.6") +worst, middle, _ = watch(along, t0, hold, rest=lambda: s.paused) +held = still() +f = along(held) +paused = s.paused +c.auto(linuxcnc.AUTO_RESUME) +w2, _, _ = watch(along, t0) +got = settled() +rotaries(False) +drain() +print("G53.6 under G13.1 held %.0f%% of the way: the tip within %.6f" % (100 * f, max(worst, w2))) +if not paused or not 0.05 < f < 0.95: + error("the feed hold did not stop G53.6 part way: paused %s, %.2f of the way, at %s" + % (paused, f, show(held))) +if max(worst, w2) > 1e-3: + error("G53.6 held part way took the tip %.4f off its point" % max(worst, w2)) +if not same(got, want): + error("G53.6 resumed after a hold ended at %s, the whole move at %s" % (show(got), show(want))) +mdi("G49") + +# The joints' travel is followed along the move on the joints the part's +# type solves: a slide that swings past its limit part way round, with both +# ends inside, refuses G53.6 under G13.1 as under the part's type, and the +# machine does not move. The tool turns from one side of the vertical to +# the other, so the slides peak half way. +swing_setup = ["G43 H1", "G0 X0 Y0 Z30", "G68.2 X0 Y0 Z0 I0 J-30 K0", "G53.6", "G69", + "G68.2 X0 Y0 Z0 I0 J30 K0"] +start = run("G13.1", swing_setup)[-1] +rotaries(True) +c.mdi("G53.6") +path = [] +deadline = time.time() + 30 +while time.time() < deadline: + j = joints() + path.append(j) + if s.inpos and not s.queue and not same(j, start): + break + time.sleep(0.002) +end = settled() +rotaries(False) +drain() +linear = [k for k in range(JOINTS) if ini.find("JOINT_%d" % k, "TYPE") == "LINEAR"] +bulge, k, side = max((max(p[k] for p in path) - max(start[k], end[k]), k, 1) for k in linear) +low = max((min(start[k], end[k]) - min(p[k] for p in path), k, -1) for k in linear) +if low[0] > bulge: + bulge, k, side = low +print("G53.6 under G13.1 from %s to %s swings joint %d %.3f past its ends" + % (show(start), show(end), k, bulge)) +if bulge < 0.25: + error("no slide swings past the ends of G53.6 under G13.1, the travel is not checked along it") +else: + def set_limit(value): + c.mode(linuxcnc.MODE_MANUAL) + done() + (c.set_max_limit if side > 0 else c.set_min_limit)(k, value) + done() + c.mode(linuxcnc.MODE_MDI) + done() + s.poll() + was = s.joint[k]["max_position_limit" if side > 0 else "min_position_limit"] + for select in (TCP, "G13.1"): + before = run(select, swing_setup)[-1] + set_limit(max(start[k], end[k]) + bulge / 2 if side > 0 else min(start[k], end[k]) - bulge / 2) + c.mdi("G53.6") + done() + after = settled() + if not drain(expect_error=True): + error("G53.6 under %s that swings joint %d past its limit part way was not refused" + % (select, k)) + if not same(before, after): + error("the refused G53.6 under %s moved the machine from %s to %s" + % (select, show(before), show(after))) + set_limit(was) + +finish() diff --git a/tests/twp-static-tip/test.sh b/tests/twp-static-tip/test.sh new file mode 100755 index 00000000000..a3fcfe97627 --- /dev/null +++ b/tests/twp-static-tip/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r table.ini diff --git a/tests/twp-static-tip/tool.tbl b/tests/twp-static-tip/tool.tbl new file mode 120000 index 00000000000..787a724629c --- /dev/null +++ b/tests/twp-static-tip/tool.tbl @@ -0,0 +1 @@ +../twp-static/tool.tbl \ No newline at end of file diff --git a/tests/twp-static/README b/tests/twp-static/README index 39f346b6bcc..ce652fd3809 100644 --- a/tests/twp-static/README +++ b/tests/twp-static/README @@ -1,7 +1,7 @@ 3+2 under the machine frame kinematics, Fanuc's second feature coordinate system, on a table machine (xyzac-trt-kins, table.ini); twp-static-head runs it on a head machine (5axiskins, head.ini). The INI and HAL files and -the helpers (static.py) are shared with twp-static-preview and their -head twins. +the helpers (static.py) are shared with twp-static-tip and twp-static-preview and their -head twins. The same program runs under the TCP kinematics and under the machine frame kinematics (G13.1), from the same joints: a plane, G53.1, G53.6 or G68.3, From aad3b6623d79173a321ba9c2ecafabb0b762e759 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 1 Nov 2026 20:19:28 +1100 Subject: [PATCH 37/44] interp: G92, G10 L20, G43 and M6 wait for the G49 that cancels G43.4 Under the kinematics G43.4 or G43.5 switches to, the current point is the tool tip, with the tool length along the tilted tool. G92 made that point the coordinates given and kept the offset after the G49, where the same numbers name another point: the spindle nose, or wherever the tilt put the length. G10 L20 did the same into a coordinate system. M6 changed the tool under the tip the program follows, so the point moved with the new length. A plain G43 changed the length and ended G43.4's hold on the kinematics, so G92 then passed with the machine still tilted on them and G49 no longer switched back. All of them ran without a word. Fanuc refuses G92 and G43 in tool centre point control (PS5421, not in its list of G codes allowed under G43.4/G43.5), Siemens refuses PRESETON on axes under a transformation (17600), and Heidenhain resets M128/TCPM before a TOOL CALL. G92, G92.1, G92.2, G92.3, G10 L20 and M6 are now refused while the switch G43.4 or G43.5 made is in effect, with a message to cancel it with G49 first; G43 is refused with a message to change the length with G43.4 H (G43.5 H under G43.5). G43.1 and G43.2 change the values of the offset in effect, the only way to give an offset across the tool, and keep the switch. G10 L2, G52 and G54 to G59.3 stay: they read no position, and Siemens and Heidenhain allow their frames and datum shifts under TCP (Fanuc refuses G52 and G54 to G59 too, tied to its own TCP coordinate handling). The rule follows the switch G43.4 made, not the kinematics type: once G12.1 or G13.1 has selected a kinematics the program has taken it over, and a hexapod or robot running G43 on its primary type keeps its codes; on a machine without switchable kinematics G43.4 is a plain G43 and nothing changes. Under a tilted work plane G92 and G10 were already refused and G52 shifts the plane. convert_g92_is_applied's result was stored and never checked, so every refusal of G92.1, G92.2 and G92.3 was dropped and the code did nothing without saying so: under cutter radius compensation and under a tilted work plane as well as here. It is checked now. tests/kins-switch: under G43.4, G92 X1, G92.1, G92.2, G92.3, T1 M6, G43 H1 and G10 L20 are each refused with the message; G43.4 H1, G10 L2, G52 and G55 pass, and after G43.1 or G43.2 G92 is still refused; under G43.5 G92, M6 and G43 are refused; after G49, and under G43 on a type G12.1 selected, G92, G92.1, M6 and G10 L20 pass. Each refusal aborts and the test's abort routine selects G13.1, so G43.4 is programmed again before each one. Without the change 12 checks fail; 12 of 12 runs pass and 12 of 12 fail as expected without it, at 6 copies in parallel with 4 busy loops, and 3 of 3 pass and fail likewise with 3 copies and a busy loop all on one CPU. --- docs/src/gcode/g-code.adoc | 28 ++++++++-- docs/src/gcode/m-code.adoc | 6 +++ src/emc/rs274ngc/interp_convert.cc | 65 ++++++++++++++++------ tests/kins-switch/README | 7 ++- tests/kins-switch/test-ui.py | 87 +++++++++++++++++++++++++++++- 5 files changed, 170 insertions(+), 23 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index af11e14ac9f..30009184f05 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -941,6 +941,8 @@ It is an error if: * The P number does not evaluate to an integer in the range 0 to 9. * An axis is programmed that is not defined in the configuration. +* '<>' or 'G43.5' is in effect; cancel it with 'G49' + first. [[gcode:g12.1-g13.1]] == G12.1, G13.1 Select Kinematics(((G12.1, G13.1 Select Kinematics))) @@ -1865,8 +1867,7 @@ compensation in the module's working kinematics. The switch happens first and the offset applies after it, as if the two had been written on consecutive lines. 'G49' is the matching cancel: it clears the offset and switches back to the machine frame kinematics, what 'G13.1' -selects, as long as the offset in effect is still 'G43.4''s and no 'G12.1' -or 'G13.1' has selected a kinematics since. +selects, as long as no 'G12.1' or 'G13.1' has selected a kinematics since. The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module @@ -1895,6 +1896,19 @@ frame sets `MACHINE_MOVES_IN_MACHINE_FRAME` in the `[RS274NGC]` section of the INI file, as the five axis sims shipped with LinuxCNC do; see <>. +While the switch 'G43.4' made is in effect, '<>', 'G92.1', +'G92.2', 'G92.3', '<>' and '<>' are +refused until 'G49' cancels it. The current point is then the tool tip, +with the tool length along the tilted tool: offsets a 'G92' or 'G10 L20' +takes from it stay after 'G49', where the same numbers name another point, +and a tool change moves the point with the tool length. A new tool length +from the tool table is 'G43.4 H-': a plain 'G43' is refused, since it would +end the switch with the machine left on the working kinematics, while +'G43.1' and 'G43.2' change the values of the offset in effect and keep the +switch. Once +'G12.1' or 'G13.1' has selected a kinematics, and on a machine without +switchable kinematics, these codes work as usual. + It is an error if: * the kinematics module is switchable but declares no primary @@ -3708,7 +3722,11 @@ on the action of `G92`. If this is the case, the offset is replaced with a new offset that makes the current point become the specified value. -It is an error if all axis words are omitted. +It is an error if: + +* all axis words are omitted, or +* '<>' or 'G43.5' is in effect; cancel it with 'G49' + first. LinuxCNC stores the G92 offsets and reuses them on the next run of a program. To prevent this, one can program a `G92.1` (to erase them), or @@ -3728,6 +3746,8 @@ See the <> section for more information. * 'G92.1' - turn off G92 offsets and reset <> 5211 - 5219 to zero. * 'G92.2' - turn off G92 offsets but keep <> 5211 - 5219 available. +Like 'G92', both are an error while '<>' or 'G43.5' is in effect. + [NOTE] G92.1 only clears G92 offsets, to change G53-G59.3 coordinate system offsets in G-code use either <> or <>. @@ -3744,6 +3764,8 @@ The parameter values will be saved when the first program exits and restored whe Use `G92.3` near the beginning of the second program. That will restore the offsets saved in the first program. +Like 'G92', 'G92.3' is an error while '<>' or 'G43.5' is in effect. + [[gcode:g93-g94-g95]] == G93, G94, G95 Feed Rate Mode(((G93, G94, G95 Feed Rate Mode))) diff --git a/docs/src/gcode/m-code.adoc b/docs/src/gcode/m-code.adoc index d09391c9146..0bf5f4ab328 100644 --- a/docs/src/gcode/m-code.adoc +++ b/docs/src/gcode/m-code.adoc @@ -190,6 +190,12 @@ the carousel (not its index). The tool length offset is not changed by 'M6', use '<>' after the 'M6' to change the tool length offset. +'M6' is an error while '<>' or 'G43.5' is in effect: the +point the program follows is the tool tip, which the new tool length would +move. Cancel it with 'G49' on a line before the 'M6' (on the same line the +tool change runs first, see <>), +change the tool, then program 'G43.4' again. + The tool change may include axis motion. It is OK (but not useful) to program a change to the tool already in the spindle. It is OK if there is no tool in the selected slot; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index ccc78ad976d..2a7eb03025e 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1572,6 +1572,36 @@ int Interp::convert_arc_comp2(int move, //!< either G_2 (cw arc) or G_3 (ccw ar /****************************************************************************/ +// the kinematics module declares what each type is (KINSTYPE_* flags); +// where the flags say nothing at all there is no kinematics attached +// (sai, a preview with no machine to ask) and the codes fall back to +// type 0, as before +static int kins_type_info_available() +{ + int k; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; + } + return 0; +} + +// G43.4 or G43.5 has put the machine on its working kinematics with the +// tool length along the tool: the current point is the tool tip, which a +// G92 would bake into offsets that outlive the G49, and a tool change +// would move it. Without switchable kinematics G43.4 is a plain G43. +static bool tcp_armed(setup_pointer settings) +{ + return settings->kins_by_g43_4 && kins_type_info_available(); +} + +static const char *tcp_code(setup_pointer settings) +{ + return settings->tool_vector ? "G43.5" : "G43.4"; +} + +/****************************************************************************/ + /*! convert_axis_offsets Returned Value: int @@ -1647,6 +1677,10 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus _("Cannot change G92 offsets while a tilted work plane (G68.2) is active")); return work_plane_local(block, settings); } + // G52 shifts the program's coordinates and reads no position + CHKS((g_code != G_52 && tcp_armed(settings)), + _("Cannot change G92 offsets while %s is active, cancel it with G49 first"), + tcp_code(settings)); CHKS((block->a_flag && settings->axis_wrapped[AXIS_A] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), @@ -4153,6 +4187,9 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 switch (block->m_modes[6]) { case 6: + CHKS(tcp_armed(settings), + _("Cannot change tools while %s is active, cancel it with G49 first"), + tcp_code(settings)); if (is_user_defined_m_code(block, settings, 6) && remapped_in_block) { return convert_remapped_code(block,settings, STEP_M_6, @@ -4999,6 +5036,10 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 CHKS((settings->g68_active), _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); + // L20 reads the current point, the tool tip under G43.4, like G92 + CHKS((block->l_number == 20 && tcp_armed(settings)), + _("Cannot use G10 L20 while %s is active, cancel it with G49 first"), + tcp_code(settings)); double *parameters; int p_int; @@ -6632,20 +6673,6 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine /****************************************************************************/ -// the kinematics module declares what each type is (KINSTYPE_* flags); -// where the flags say nothing at all there is no kinematics attached -// (sai, a preview with no machine to ask) and the codes fall back to -// type 0, as before -static int kins_type_info_available() -{ - int k; - - for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { - if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; - } - return 0; -} - // the lowest type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test @@ -6714,6 +6741,11 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); + // under G43.4 a new length from the table is G43.4 H-: a plain G43 + // would end G43.4's hold on the kinematics with the machine left on them + CHKS((g_code == G_43 && tcp_armed(settings)), + _("Cannot use G43 while %s is active, change the tool length with %s H-"), + tcp_code(settings), tcp_code(settings)); if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); int now = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); @@ -6739,8 +6771,9 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHP(snap_after_switch(settings, standing, have_standing)); } settings->kins_by_g43_4 = true; - } else if (g_code != G_49) { - // the offset in effect is no longer G43.4's, so G49 has no switch to undo + } else if (g_code == G_43) { + // the offset in effect is no longer G43.4's, so G49 has no switch to + // undo; G43.1 and G43.2 change the values of the offset in effect settings->kins_by_g43_4 = false; } settings->tool_vector = (g_code == G_43_5); diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e409e2b3cf9..e36103efd2b 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -14,5 +14,8 @@ agreeing on where the point went, that under G43.5 a tool vector I J K lands on the joints and the point the rotary words reach, is not incremental, keeps the free joint on the pole and is refused with a rotary word, as a zero vector, on the identity kinematics and outside -G43.5, and that G13.1 in an ON_ABORT_COMMAND routine does not swallow -the rest of the routine. +G43.5, that G92, G92.1, G92.2, G92.3, M6, plain G43 and G10 L20 are +refused while G43.4 or G43.5 is in effect and work again after G49 or on +a type G12.1 selected, while G43.4 H, G43.1, G43.2, G10 L2, G52 and G55 +pass under it and G43.1 and G43.2 keep it in effect, and that G13.1 in an ON_ABORT_COMMAND +routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 05f8abf3c04..65f2010eec3 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -304,12 +304,23 @@ def tool_length_check(what, before_joints, before_pose, after_joints, after_pose # (-sin B cos C, -sin B sin C, cos B), so the vector for the tilted pose # above has to land on the joints and the point the rotary words reach. +def said_error(timeout=5): + # the error can reach the channel after the command reports done + deadline = time.time() + timeout + while time.time() < deadline: + m = e.poll() + if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + return m + if not m: + time.sleep(0.01) + return None + def refused(cmd, needle): drain() c.mdi(cmd) done(30) - m = e.poll() - if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + m = said_error() + if not m: error("%s was accepted" % cmd) elif needle not in m[1]: error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) @@ -443,6 +454,78 @@ def param(name): print("refused as expected: %s" % m[1].strip()) drain() +# ---- G92, M6, G43 and G10 L20 while G43.4 or G43.5 is on ------------------ +# +# Under TCP the current point is the tool tip with the length along the +# tilted tool: a G92 or G10 L20 would bake it into offsets that outlive the +# G49, and a tool change would move it, so they wait for the G49. A new +# length from the table is G43.4 H, a plain G43 would end G43.4's hold on +# the kinematics. G10 L2, G52 and G54 to G59 read no position, so they stay. +# The same tool length on a type the program selected itself is its own +# business. + +def accepted(cmd): + drain() + mdi(cmd) + said = [m for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + +def refused_under(tcp, cmd, needle): + # the refusal aborts, and this test's abort routine selects G13.1 + mdi(tcp + " H1") + refused(cmd, needle) + +errors_before = errors +mdi("G13.1") +mdi("G0 X0 Y0 Z0 B-20 C30 W0") +for cmd in ("G92 X1", "G92.1", "G92.2", "G92.3"): + refused_under("G43.4", cmd, "while G43.4 is active, cancel it with G49 first") +refused_under("G43.4", "T1 M6", "Cannot change tools while G43.4") +refused_under("G43.4", "G43 H1", "change the tool length with G43.4 H-") +refused_under("G43.4", "G10 L20 P2 X1", "Cannot use G10 L20 while G43.4 is active") +mdi("G43.4 H1") +accepted("G43.4 H1") +# G43.1 and G43.2 change the values of the offset in effect: G43.4 still +# holds the kinematics, and G92 still waits for the G49 +accepted("G43.1 Z1") +refused("G92 X1", "while G43.4 is active") +mdi("G43.4 H1") +accepted("G43.2 H1") +refused("G92 X1", "while G43.4 is active") +mdi("G43.4 H1") +accepted("G10 L2 P2 X1") +accepted("G10 L2 P2 X0") +accepted("G52 X1") +accepted("G52 X0") +accepted("G55") +s.poll() +if s.g5x_index != 2: + error("G55 under G43.4 left G%d" % (53 + s.g5x_index)) +accepted("G54") +refused_under("G43.5", "G92 X1", "while G43.5 is active") +refused_under("G43.5", "T1 M6", "while G43.5 is active") +refused_under("G43.5", "G43 H1", "change the tool length with G43.5 H-") +mdi("G43.5 H1") +mdi("G49") +if kins_type() != 1: + error("G49 left kinematics %d, not the identity" % kins_type()) +accepted("G92 X1") +accepted("G92.1") +accepted("T1 M6") +accepted("G10 L20 P2 X1") +accepted("G10 L2 P2 X0") +mdi("G12.1 P0") +mdi("G43 H1") +accepted("G92 X1") +accepted("G92.1") +accepted("T1 M6") +mdi("G49") +mdi("G13.1") +mdi("G0 X0 Y0 Z0 B0 C0 W0") +if errors == errors_before: + print("G43.4 and G43.5 hold G92, M6, G43 and G10 L20 back; G43.4 H, G43.1, G43.2, G10 L2, G52 and G54 to G59 pass") + # ---- G13.1 in an abort routine does not swallow the rest of it ----------- # # The routine ends with G54, so leave the machine in G55 and see which one From 0e8326ca442996a4bda37dd3cc9bd3f05f80d1d4 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 9 Nov 2026 13:18:19 +1100 Subject: [PATCH 38/44] preview: restate G43.4 and G43.5 with the machine's offset The preamble every GUI hands the preview restated the tool offset as G43.1 values after the position and never the switch G43.4 or G43.5 made, so a program loaded after G43.4 in MDI previewed off the switch: an M6 or G92 the machine refuses was drawn, a G49 did not switch back, and under G43.5 a line with I J K alone was refused as an arc word. Restating the bare code alone takes the loaded tool's length, which after G43.4 H1 with no M6 is tool 0 and no length at all. Under G43.4 or G43.5 the preamble now restates the code, then the machine's values with G43.1, ahead of the position: the switch holds the joints, so a point restated before it would move by the tool. G43.1 and G43.2 keep the switch, and now also keep G43.5's tool axis given as a vector, which they reset before, in a program as much as in the preamble. tests/ptp-machine-frame checks the restated point under G43.4 and G43.5 against the machine's position less its offset, an M6 refused under G43.4 for the machine's reason, and I J K alone read as the tool axis under G43.5. All three fail with the previous preamble, the last also with the interpreter change alone undone. --- docs/src/gcode/g-code.adoc | 4 +++- lib/python/preview_helpers.py | 14 +++++++++++--- src/emc/rs274ngc/interp_convert.cc | 6 +++++- tests/ptp-machine-frame/m6.ngc | 2 ++ tests/ptp-machine-frame/test-ui.py | 29 +++++++++++++++++++++++++---- tests/ptp-machine-frame/vector.ngc | 2 ++ 6 files changed, 48 insertions(+), 9 deletions(-) create mode 100644 tests/ptp-machine-frame/m6.ngc create mode 100644 tests/ptp-machine-frame/vector.ngc diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 30009184f05..ee50c3e338b 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1905,10 +1905,12 @@ and a tool change moves the point with the tool length. A new tool length from the tool table is 'G43.4 H-': a plain 'G43' is refused, since it would end the switch with the machine left on the working kinematics, while 'G43.1' and 'G43.2' change the values of the offset in effect and keep the -switch. Once +switch, and under 'G43.5' the tool axis given as a vector. Once 'G12.1' or 'G13.1' has selected a kinematics, and on a machine without switchable kinematics, these codes work as usual. +The G-code preview restates 'G43.4' or 'G43.5' from status with the offset the machine has, so a program loaded after one set in MDI previews as it runs: from the tool tip, with the tool length in effect, and with the same codes refused. + It is an error if: * the kinematics module is switchable but declares no primary diff --git a/lib/python/preview_helpers.py b/lib/python/preview_helpers.py index 2a07bf42136..0c44d06eb0d 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -32,6 +32,15 @@ def preview_initcodes(s, inifile): codes.append("G90") codes.append("T%d M6" % s.tool_in_spindle) axes = [(i, "XYZABCUVW"[i]) for i in range(9) if s.axis_mask & (1 << i)] + offset = "G43.1" + "".join(" %s%.8f" % (axis, s.tool_offset[i]) for i, axis in axes) + # Under G43.4 or G43.5 the switch and the offset come ahead of the + # position: the switch holds the joints and would move a point restated + # before it. The bare code takes the loaded tool's length, and the G43.1 + # after it sets the machine's values and keeps the switch + tcp = s.gcodes[9] in (434, 435) + if tcp: + codes.append("G%.1f" % (s.gcodes[9] * .1)) + codes.append(offset) position = "G53 G0" for i, axis in axes: pos = s.position[i] % 360.0 if wrapped[i] else s.position[i] @@ -49,9 +58,8 @@ def preview_initcodes(s, inifile): # table rather than the machine's if s.gcodes[9] == 490: codes.append("G49") - else: - codes.append("G43.1" + "".join(" %s%.8f" % (axis, s.tool_offset[i]) - for i, axis in axes)) + elif not tcp: + codes.append(offset) for i in MODAL_MCODES: if s.mcodes[i] != -1: codes.append("M%d" % s.mcodes[i]) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 2a7eb03025e..431134e1d64 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6776,7 +6776,11 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // undo; G43.1 and G43.2 change the values of the offset in effect settings->kins_by_g43_4 = false; } - settings->tool_vector = (g_code == G_43_5); + // G43.1 and G43.2 change the values of the offset in effect, and keep + // G43.5's tool axis given as a vector as they keep the switch + if (g_code != G_43_1 && g_code != G_43_2) { + settings->tool_vector = (g_code == G_43_5); + } if (g_code == G_49) { idx = 0; } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { diff --git a/tests/ptp-machine-frame/m6.ngc b/tests/ptp-machine-frame/m6.ngc new file mode 100644 index 00000000000..24f74d598f2 --- /dev/null +++ b/tests/ptp-machine-frame/m6.ngc @@ -0,0 +1,2 @@ +T1 M6 +M2 diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index d9160a02baa..83ebfe2cc11 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -280,13 +280,24 @@ def expect_preview(what, program, same): if why or far(end, want): error("%s, %s: the preview ends at %s, %s at %s" % (what, form, end, same, want)) +# a program the machine refuses in the state it is in, refused by the +# preview the GUIs build, with the machine's own reason +def expect_refused(what, program, says): + line, why, end = preview(program, False) + print("%-34s %s" % (what, "line %d: %s" % (line, why) if why else "accepted")) + if not why or says not in why: + error("%s: the preview does not refuse %s as the machine does" % (what, program)) + # The restating G53 is the type in force's, never read in the machine frame: -# a move that goes nowhere ends on the position the machine reports. Both -# forms restate the tool offset after the position, which moves it, so -# they are checked only where no offset is in force. +# a move that goes nowhere ends on the position the machine reports, less +# the tool offset, which the program's coordinates leave out as the +# machine's do. The list as AXIS used to build it restates the offset +# after the position without the switch, so under G43.4 the switch moves +# the point it restated. def expect_restated(what, forms): s.poll() - want = [v if i in (3, 4, 5) else v / 25.4 for i, v in enumerate(s.position)] + want = [v - s.tool_offset[i] if i in (3, 4, 5) else (v - s.tool_offset[i]) / 25.4 + for i, v in enumerate(s.position)] for listed in forms: line, why, end = preview("here.ngc", listed) if why or far(end, want): @@ -323,6 +334,16 @@ def expect_run(what, program, *lines): expect_type("G43.4 for the preview", 1) expect_preview("G53 under the tip, tilted", "g53.ngc", "g535.ngc") expect_preview("G13.1 then G53 under the tip", "g13-g53.ngc", "g13-g535.ngc") +expect_restated("under the tip, tilted", (False,)) +# the preview is on G43.4 as the machine is, with its offset: M6 is refused +# there as running it would be +expect_refused("M6 under the tip", "m6.ngc", "G49") +# and on G43.5, which reads I J K alone as the tool axis: this module has +# no tool frame to orient by, so the line is refused for that, not taken +# for an arc word +mdi("G43.5 H1") +expect_refused("I J K alone under G43.5", "vector.ngc", "G43.5") +expect_restated("under G43.5", (False,)) mdi("G49", "G0 B0", "G12.1 P2") expect_type("G12.1 P2 for the preview", 2) expect_preview("G53 on the plain identity", "g53.ngc", "g535.ngc") diff --git a/tests/ptp-machine-frame/vector.ngc b/tests/ptp-machine-frame/vector.ngc new file mode 100644 index 00000000000..d2bebd3a12e --- /dev/null +++ b/tests/ptp-machine-frame/vector.ngc @@ -0,0 +1,2 @@ +G0 I0 J0 K1 +M2 From c3a3fd79e358d14d0b6b78a7fe764bd20ef4dbdc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 3 Nov 2026 13:08:27 +1100 Subject: [PATCH 39/44] motion_planning: the poses along a line and an arc, and how often to sample them, for any caller Canon sampled each segment through kins_line_pose, kins_arc_pose and kins_samples, statics in emccanon.cc. They move to libkinslimits beside segmentCap() as segmentLinePose, segmentArcPose and segmentSamples, so that the preview can follow a move on the same samples task does. The arc carries the scale from its circle's units to the poses' units, which canon had in TO_EXT_LEN, and the sample count takes the angular axes as a mask. No change in what canon samples. --- src/emc/motion_planning/segment_cap.cc | 44 ++++++++++++++++ src/emc/motion_planning/segment_cap.hh | 27 ++++++++++ src/emc/task/emccanon.cc | 71 ++++---------------------- 3 files changed, 82 insertions(+), 60 deletions(-) diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc index 761669e68c3..cdb2df065b9 100644 --- a/src/emc/motion_planning/segment_cap.cc +++ b/src/emc/motion_planning/segment_cap.cc @@ -416,4 +416,48 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, return 0; } // segmentCap() +void segmentLinePose(double f, EmcPose *pose, void *arg) +{ + const SegmentLine *line = (const SegmentLine *)arg; + + pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); + pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); + pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); + pose->a = line->start.a + f * (line->end.a - line->start.a); + pose->b = line->start.b + f * (line->end.b - line->start.b); + pose->c = line->start.c + f * (line->end.c - line->start.c); + pose->u = line->start.u + f * (line->end.u - line->start.u); + pose->v = line->start.v + f * (line->end.v - line->start.v); + pose->w = line->start.w + f * (line->end.w - line->start.w); +} + +void segmentArcPose(double f, EmcPose *pose, void *arg) +{ + const SegmentArc *arc = (const SegmentArc *)arg; + PmCartesian p; + + segmentLinePose(f, pose, (void *)&arc->ends); + if (arc->line) { return; } + pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); + pose->tran.x = arc->scale * p.x; + pose->tran.y = arc->scale * p.y; + pose->tran.z = arc->scale * p.z; +} + +int segmentSamples(const EmcPose *start, const EmcPose *end, double sweep_deg, + unsigned angular) +{ + const double turn[6] = {end->a - start->a, end->b - start->b, end->c - start->c, + end->u - start->u, end->v - start->v, end->w - start->w}; + double rot = 0.0; + for (int n = 0; n < 6; n++) { + if (angular & (1u << (n + 3))) { rot = fmax(rot, fabs(turn[n])); } + } + double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + + pow(end->tran.z - start->tran.z, 2)); + int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); + + return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; +} + } // namespace motion_planning diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh index 76407abd991..1b94fef754a 100644 --- a/src/emc/motion_planning/segment_cap.hh +++ b/src/emc/motion_planning/segment_cap.hh @@ -14,6 +14,7 @@ #define SEGMENT_CAP_HH #include +#include #include namespace motion_planning { @@ -91,6 +92,32 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, double length, int samples, SegmentPoseFn pose_at, void *arg, double *joints, SegmentCap *out); +/* A line between two poses, every axis interpolated, for segmentLinePose. */ +struct SegmentLine { + EmcPose start, end; +}; + +void segmentLinePose(double fraction, EmcPose *pose, void *arg); + +/* An arc: the circle in its own units, scale times them the units of the + poses, which give the axes off the circle; a line, no circle at all, is + the chord. For segmentArcPose. */ +struct SegmentArc { + SegmentLine ends; + PmCircle circle; + double scale; + bool line; +}; + +void segmentArcPose(double fraction, EmcPose *pose, void *arg); + +/* How many points to sample a segment at: enough that a turn of the + rotaries, a stretch of travel on a machine whose Jacobian changes with + position, or a sweep of arc is seen every few degrees or centimetres. + angular has bit n set for an axis n (0 X to 8 W) that turns. */ +int segmentSamples(const EmcPose *start, const EmcPose *end, double sweep_deg, + unsigned angular); + } // namespace motion_planning #endif // SEGMENT_CAP_HH diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 78f21d16b87..492a2292bd4 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1032,22 +1032,10 @@ static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) } } -// how many points to sample a segment at: enough that a turn of the -// rotaries, a stretch of travel on a machine whose Jacobian changes with -// position, or a sweep of arc is seen every few degrees or centimetres +// how many points to sample a segment at static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) { - const double turn[6] = {end->a - start->a, end->b - start->b, end->c - start->c, - end->u - start->u, end->v - start->v, end->w - start->w}; - double rot = 0.0; - for (int n = 0; n < 6; n++) { - if (AXIS_ANG(n + 3)) { rot = fmax(rot, fabs(turn[n])); } - } - double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) - + pow(end->tran.z - start->tran.z, 2)); - int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); - - return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; + return motion_planning::segmentSamples(start, end, sweep_deg, kinds.angular); } // the travel motion holds the joints to now, MIN_LIMIT and MAX_LIMIT or @@ -1121,25 +1109,6 @@ static void kins_refuse(const KinsTravel *travel) refusal.travel = *travel; } -struct KinsLine { - EmcPose start, end; -}; - -static void kins_line_pose(double f, EmcPose *pose, void *arg) -{ - const KinsLine *line = (const KinsLine *)arg; - - pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); - pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); - pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); - pose->a = line->start.a + f * (line->end.a - line->start.a); - pose->b = line->start.b + f * (line->end.b - line->start.b); - pose->c = line->start.c + f * (line->end.c - line->start.c); - pose->u = line->start.u + f * (line->end.u - line->start.u); - pose->v = line->start.v + f * (line->end.v - line->start.v); - pose->w = line->start.w + f * (line->end.w - line->start.w); -} - // the cap on a straight segment from canon.endPoint, in the machine's // units per second of the parameter getStraightVelocity() plans it with, // the length along the axes getStraightSpan() measures @@ -1149,7 +1118,7 @@ static bool kins_straight_cap(double x, double y, double z, double *vel, double *acc) { KinematicsUserContext *ctx; - KinsLine line; + motion_planning::SegmentLine line; double length; if (kins.in_arc) { return false; } @@ -1175,7 +1144,7 @@ static bool kins_straight_cap(double x, double y, double z, kins.memo.end = line.end; kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, kins_samples(&line.start, &line.end, 0.0), - kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc, + motion_planning::segmentLinePose, &line, &kins.memo.vel, &kins.memo.acc, &kins.memo.travel); kins_refuse(&kins.memo.travel); *vel = kins.memo.vel; @@ -1183,25 +1152,6 @@ static bool kins_straight_cap(double x, double y, double z, return kins.memo.capped; } -struct KinsArc { - PmCircle circle; // in canon's units, as ARC_FEED builds it - bool line; // no turn at all: the chord - EmcPose start, end; -}; - -static void kins_arc_pose(double f, EmcPose *pose, void *arg) -{ - const KinsArc *arc = (const KinsArc *)arg; - PmCartesian p; - - kins_line_pose(f, pose, (void *)&arc->start); - if (arc->line) { return; } - pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); - pose->tran.x = TO_EXT_LEN(p.x); - pose->tran.y = TO_EXT_LEN(p.y); - pose->tran.z = TO_EXT_LEN(p.z); -} - // the cap on an arc from canon.endPoint, in the machine's units per // second of the XYZ length ARC_FEED plans it with static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, @@ -1210,13 +1160,14 @@ static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, double *vel, double *acc) { KinematicsUserContext *ctx = kins_here(); - KinsArc arc; + motion_planning::SegmentArc arc; KinsTravel travel; bool capped; if (!ctx || length <= 0.0) { return false; } - arc.start = to_ext_pose(canon.endPoint); - arc.end = to_ext_pose(endpt); + arc.ends.start = to_ext_pose(canon.endPoint); + arc.ends.end = to_ext_pose(endpt); + arc.scale = TO_EXT_LEN(1.0); arc.line = rotation == 0; if (!arc.line) { PmCartesian s = { canon.endPoint.x, canon.endPoint.y, canon.endPoint.z }; @@ -1227,9 +1178,9 @@ static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, return false; } } - capped = kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), - kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), - kins_arc_pose, &arc, vel, acc, &travel); + capped = kins_cap(ctx, &arc.ends.start, &arc.ends.end, TO_EXT_LEN(length), + kins_samples(&arc.ends.start, &arc.ends.end, fabs(full_angle) * 180.0 / M_PI), + motion_planning::segmentArcPose, &arc, vel, acc, &travel); kins_refuse(&travel); return capped; } From d603294d979e24331f08e7437fe1b4b688dd9e2d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 3 Nov 2026 13:08:27 +1100 Subject: [PATCH 40/44] preview: follow the joints along each move and warn before the run Task refuses a move whose joints leave their travel along it, but only when it reaches the move, partway through a program. The preview now follows the joints along every move the same way when a program is loaded, and AXIS lists the moves it finds in the warning it shows when Run is pressed, next to the axis extents, by line and joint; Run Anyway stays, and the refusal at run time stays behind it. None of Fanuc, Siemens and Heidenhain refuses a program at load: Fanuc and Siemens check each block before it moves, which canon does, and Heidenhain's simulation checks the software limits on request, which the preview and this warning are in LinuxCNC. gcodemodule follows each straight move, arc, joint move and G53.6 line under G13.1 through the interpreter's kinematics context, on the kinstype the move runs under and the tool in force, with segmentCap() on the samples canon takes, against [JOINT_n] MIN_LIMIT and MAX_LIMIT from the INI file. The joints a move ends on start the next, as a kinematics switch and a tool length change keep them, so a move starts where the machine stands; a joint move is checked at its end, where the joints go straight; the first move of a parse, a GUI's restating move to where the machine is, only places the joints from the machine's own and is checked at its end. A move a joint leaves its travel along calls the canon's joint_travel(line, joint, side, where, limit); GLCanon keeps them in joint_travel_left. On a machine whose kinematics is the identity nothing is loaded: each joint is the axis the coordinates= of [KINS] KINEMATICS maps it to, trivkins' XYZABCUVW where none is given, checked at the end of every move and, on an arc, at the angles where X, Y or Z turn back on the circle, which catches a gantry's second joint with less travel than its axis. A module that cannot be evaluated outside realtime is reported once with line -1, which AXIS leaves out of its dialog. The kinematics context, the length units, the identity question and the tool offset are read once a parse, or when the offset changes: asked on every move, the calls into the GUI's python cost more than the following. On 50000 five axis moves the parse takes 0.34 s instead of 0.17 s, the rest being the inverse and the Jacobian at each sample; on an identity machine no difference shows. tests/kins-limits/preview previews programs under halrun with 5axiskins and a 400 mm pivot: the swing of tests/kins-limits/travel past joint 1's travel both ways round, with its peak 0.05 past between two samples, and not 0.05 inside; a first move not followed from zero and one whose end is past; an arc over the travel one way round and not the other, in XY and in ZX; a joint move ending past; a move on the identity type; on a trivkins gantry whose second Y joint has less travel than the axis, a line ending past it and an arc whose top is, not the arc the other way round; and a line ending past a three joint trivkins machine with no coordinates= given. Without the report 9 of its cases fail. --- docs/src/config/ini-config.adoc | 2 +- docs/src/gui/axis.adoc | 9 + docs/src/motion/switchkins.adoc | 12 + lib/python/rs274/glcanon.py | 7 + src/emc/rs274ngc/Submakefile | 2 +- src/emc/rs274ngc/gcodemodule.cc | 395 ++++++++++++++++++++++++-- src/emc/usr_intf/axis/scripts/axis.py | 16 ++ tests/kins-limits/preview/README | 15 + tests/kins-limits/preview/check.py | 113 ++++++++ tests/kins-limits/preview/checkresult | 3 + tests/kins-limits/preview/gantry.ini | 44 +++ tests/kins-limits/preview/plain.ini | 38 +++ tests/kins-limits/preview/preview.hal | 6 + tests/kins-limits/preview/test.ini | 67 +++++ tests/kins-limits/preview/test.sh | 6 + 15 files changed, 717 insertions(+), 18 deletions(-) create mode 100644 tests/kins-limits/preview/README create mode 100644 tests/kins-limits/preview/check.py create mode 100755 tests/kins-limits/preview/checkresult create mode 100644 tests/kins-limits/preview/gantry.ini create mode 100644 tests/kins-limits/preview/plain.ini create mode 100644 tests/kins-limits/preview/preview.hal create mode 100644 tests/kins-limits/preview/test.ini create mode 100755 tests/kins-limits/preview/test.sh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 541e5910a0f..9ebdbca1a34 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1207,7 +1207,7 @@ The `[JOINT_N]MIN_LIMIT`/`MAX_LIMIT` settings are enforced while jogging in join After homing, `[AXIS_L]MIN_LIMIT`/`MAX_LIMIT` coordinate limits are used as constraints for axis (coordinate letter) jogging and by the trajectory planning used for G-code moves (programs and MDI commands). The trajectory planner works in Cartesian space (XYZABCUVW). -On a kinematics that is not the identity, every move of a program or an MDI command is also followed through the kinematics module ahead of motion, and a move that would take a joint past `MIN_LIMIT` or `MAX_LIMIT` anywhere along it is refused before it runs, its line failing with the joint and the limit; see the <> chapter, INI file limit settings. +On a kinematics that is not the identity, every move of a program or an MDI command is also followed through the kinematics module ahead of motion, and a move that would take a joint past `MIN_LIMIT` or `MAX_LIMIT` anywhere along it is refused before it runs, its line failing with the joint and the limit; the preview follows the joints the same way when a program is loaded, and AXIS warns of such moves when Run is pressed; see the <> chapter, INI file limit settings. A kinematics module that can only be evaluated in realtime is checked at the ends of each move. The motion module detects a joint position limit violation during the execution of G-code commands and faults, for what cannot be seen ahead of it. See also related https://github.com/LinuxCNC/linuxcnc/issues/97[GitHub issue #97]. diff --git a/docs/src/gui/axis.adoc b/docs/src/gui/axis.adoc index 64bb0bd517d..05778195906 100644 --- a/docs/src/gui/axis.adoc +++ b/docs/src/gui/axis.adoc @@ -375,6 +375,15 @@ jog to the left and Touch Off X again. .Soft Limits(((Soft Limits))) image::images/axis-outofrange.png["Soft Limits",align="center"] +When Run is pressed with a program that exceeds the soft limits, a +warning lists the axes, and the program runs only with 'Run Anyway'. +The warning also lists, by line and joint, the moves along which a +joint would pass its `MIN_LIMIT` or `MAX_LIMIT`, which the preview +finds by following the joints through the kinematics; on a machine +whose kinematics is not the identity such a move is refused when it is +reached; see the <> +chapter. + .Tool Cone When no tool is loaded, the location of the tip of the tool is indicated by the 'tool cone'. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index ef83962d082..3918bebf4cd 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -331,6 +331,18 @@ puts past the limit refuses a move. A module that can only be evaluated in realtime is checked at the ends of each move; motion's own check of the joints while they move stays behind both. +The preview follows the joints the same way when a program is loaded, +through the module evaluated outside realtime, on the kinstype each +move runs under and against the `MIN_LIMIT` and `MAX_LIMIT` of the INI +file, homed or not, starting from where the machine stands. AXIS lists +the moves it finds, by line and joint, in the warning it shows when Run +is pressed, next to the axis extents, and the program can still be run; +the refusal at run time stays. On a machine whose kinematics is the +identity each joint is read off the axis the `coordinates=` of the +module maps it to, a gantry's second joint included, at the end of +every move and where an arc turns back. A module that can only be +evaluated in realtime cannot be followed outside it. + The velocity and acceleration limits, `MAX_VELOCITY` and `MAX_ACCELERATION`, bound the coordinate letter in the world of the kinstype in force. What the joints can follow is bound separately: diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 6873530cb4e..11daff797e7 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -171,6 +171,10 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0): self.colors = colors # Set if the parse was aborted, so the extents above are only partial. self.preview_incomplete = False + # Moves a joint would leave its travel along, as (line, joint, side, + # where, limit), side 1 past MAX_LIMIT and -1 past MIN_LIMIT; a line + # of -1 says the joints could not be followed at all. + self.joint_travel_left = [] self.dwell_time = 0 self.is_foam = is_foam self.foam_z = foam_z @@ -227,6 +231,9 @@ def comment(self, arg): def message(self, message): pass + def joint_travel(self, line, joint, side, where, limit): + self.joint_travel_left.append((line, joint, side, where, limit)) + def check_abort(self): pass def next_line(self, st): diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 801542ad8f5..f18a83da97e 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -53,7 +53,7 @@ GCODEMODULESRCS := emc/rs274ngc/gcodemodule.cc emc/rs274ngc/gcode_renderer.cc \ PYSRCS += $(GCODEMODULESRCS) GCODEMODULE := ../lib/python/gcode.so -$(GCODEMODULE): $(call TOOBJS, $(GCODEMODULESRCS)) ../lib/librs274.so.0 +$(GCODEMODULE): $(call TOOBJS, $(GCODEMODULESRCS)) ../lib/librs274.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking python module $(notdir $@) $(CXX) $(LDFLAGS) -shared -o $@ $^ -lstdc++ diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index eb89d39ee7a..ce96f8a2dd1 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -50,6 +50,8 @@ #include #include +#include +#include #include #include @@ -59,6 +61,9 @@ #include "nml_intf/canon.hh" #include "nml_intf/nurbs_math.hh" #include +#include +#include +#include "segment_cap.hh" #include // KINEMATICS_IDENTITY int _task = 0; // control preview behaviour when remapping @@ -410,6 +415,354 @@ static Point9 ensure_inch(const Point9 &p) { return q; } +// The joints along the preview's moves, followed through the interpreter's +// kinematics against [JOINT_n] MIN_LIMIT and MAX_LIMIT, so that a GUI can +// say before a run that task would refuse a move a joint leaves its travel +// along. The joints a move ends on start the next: a kinematics switch and +// a tool length change keep them, and they say where the machine stands. +struct Travel { + bool have = false; // joints holds where the last move ended + double joints[EMCMOT_MAX_JOINTS] = {}; + EmcPose at = {}; // where the last move ended, for when it does not + bool placed = false; // at is known: a parse starts from nowhere + CANON_PLANE plane = CANON_PLANE::XY; + bool unchecked = false; // said once that nothing can be checked + // asked once a parse: each answer is a call into the GUI's python + int identity = -1; // the machine's kinematics is the identity + double scale = 0.0; // inches to the machine's units + bool tried = false; // the interpreter's kinematics asked for + KinematicsUserContext *ctx = nullptr; + bool tool_set = false; // tool, the offset the kinematics has + EmcPose tool = {}; + // an identity machine: the axis each joint is, from coordinates= + int id_joints = -1; // -1 not read yet, 0 no map + int id_axis[EMCMOT_MAX_JOINTS] = {}; +}; +static Travel travel; + +// a canon point, in the canon's units with the tool offset out, in the +// machine's units with the tool offset in, as the kinematics take it +static EmcPose machine_pose(const Point9 &p) { + if(travel.scale == 0.0) travel.scale = 25.4 * GET_EXTERNAL_LENGTH_UNITS(); + double scale = travel.scale; + const EmcPose &o = parse_state.tool_offset; + const Point9 tool = {o.tran.x, o.tran.y, o.tran.z, o.a, o.b, o.c, o.u, o.v, o.w}; + Point9 in = ensure_inch(p), off = ensure_inch(tool); + double m[P9_COUNT]; + for(int i = 0; i < P9_COUNT; i++) { + double v = in[i] + off[i]; + m[i] = (parse_state.angular >> i) & 1 ? v : v * scale; + } + EmcPose pose; + pose.tran.x = m[0]; pose.tran.y = m[1]; pose.tran.z = m[2]; + pose.a = m[3]; pose.b = m[4]; pose.c = m[5]; + pose.u = m[6]; pose.v = m[7]; pose.w = m[8]; + return pose; +} + +static void report_travel(int line_number, int joint, int side, double pos, double limit) { + canon_guard([&]{ + py::handle canon(parse_state.callback); + if(py::hasattr(canon, "joint_travel")) + canon.attr("joint_travel")(line_number, joint, side, pos, limit); + }); +} + +// the kinematics a preview can follow the joints with, on type ktype (-1 +// the one in force) with the tool in force, or null where there are no +// joints to follow: an identity machine, whose joints are its axes and the +// extents say it all, or a module that cannot be evaluated here, said once +static KinematicsUserContext *travel_kins(Interp **pinterp, int ktype) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return nullptr; + if(travel.identity < 0) travel.identity = GET_EXTERNAL_KINEMATICS_IDENTITY(); + if(travel.identity) return nullptr; + setup_pointer s = &interp->_setup; + if(ktype < 0) ktype = s->kins_type; + // the context stays the interpreter's for the parse, its pins held + if(!travel.tried) { + travel.tried = true; + travel.ctx = static_cast(interp->kins_preview(s)); + } + KinematicsUserContext *ctx = travel.ctx; + if(ctx && ctx != s->kins_ctx) ctx = travel.ctx = nullptr; + if(!ctx || kinematicsUserSetType(ctx, ktype) != 0) { + if(ctx) kinematicsUserSetType(ctx, s->kins_type); + if(!travel.unchecked) { + travel.unchecked = true; + report_travel(-1, -1, 0, 0.0, 0.0); + } + return nullptr; + } + // the interpreter gives the context the same offset whenever it uses it + if(!travel.tool_set || memcmp(&travel.tool, &s->tool_offset, sizeof(EmcPose)) != 0) { + interp->kins_set_tool(ctx, &s->tool_offset); + travel.tool = s->tool_offset; + travel.tool_set = true; + } + *pinterp = interp; + return ctx; +} + +// the joints at a point: the inverse run to a fixed point from the joints +// the machine stands in, which the GUI's restating move goes to, else from +// the interpreter's seed +static bool travel_place(KinematicsUserContext *ctx, setup_pointer s, const EmcPose &at) { + double standing[EMCMOT_MAX_JOINTS] = {}; + int n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) travel.joints[i] = n > 0 ? standing[i] : s->kins_seed[i]; + for(int pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) prev[i] = travel.joints[i]; + if(kinematicsUserInverse(ctx, &at, travel.joints) != 0) return false; + for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) worst = fmax(worst, fabs(travel.joints[i] - prev[i])); + if(worst < 1e-9) break; + } + return true; +} + +// where the move starts: where the joints stand, else where the last move +// ended. False before the first move of a parse, which starts from +// nowhere the preview knows: that one only places the joints at its end, +// and only its end is checked. +static bool travel_start(KinematicsUserContext *ctx, setup_pointer s, EmcPose *start) { + if(travel.have && kinematicsUserForward(ctx, travel.joints, start) == 0) return true; + if(!travel.placed) return false; + *start = travel.at; + return travel_place(ctx, s, *start); +} + +static void travel_limits(setup_pointer s, motion_planning::SegmentCapLimits *lim) { + lim->joints = std::min(s->kins_joints, KINEMATICS_USER_MAX_JOINTS); + for(int j = 0; j < lim->joints; j++) { + lim->vel[j] = SEGMENT_CAP_NONE; + lim->acc[j] = SEGMENT_CAP_NONE; + lim->min[j] = s->kins_joint_min[j]; + lim->max[j] = s->kins_joint_max[j]; + } +} + +// a joint past its travel where the joints are now, reported for line_number +static void travel_check_end(int line_number, setup_pointer s) { + motion_planning::SegmentCapLimits lim; + travel_limits(s, &lim); + for(int j = 0; j < lim.joints; j++) { + if(travel.joints[j] > lim.max[j] || travel.joints[j] < lim.min[j]) { + int side = travel.joints[j] > lim.max[j] ? 1 : -1; + report_travel(line_number, j, side, travel.joints[j], side > 0 ? lim.max[j] : lim.min[j]); + return; + } + } +} + +// An identity machine, a trivkins one: each joint is the axis the +// coordinates= of [KINS] KINEMATICS maps it to, trivkins' XYZABCUVW where +// none is given, mapped over EMCMOT_MAX_JOINTS as trivkins maps it, so a +// machine with fewer joints than letters checks the first of them. The +// parameter block's map holds no state, where trivkins' own keeps the +// first coordinates it saw for the process. Nothing needs loading: a +// joint is past a limit along a line only at its end, and along an arc +// also where the circle turns back. +static bool identity_map(setup_pointer s) { + if(travel.id_joints >= 0) return travel.id_joints > 0; + travel.id_joints = 0; + std::string coords = "XYZABCUVW"; + if(const char *k = strstr(s->kins_module, "coordinates=")) { + k += strlen("coordinates="); + coords.assign(k, strcspn(k, " \t")); + } + int n = std::min(s->kins_joints, EMCMOT_MAX_JOINTS); + kins_params map; + if(n < 1 || kinsParamsMapCoordinates(&map, coords.c_str(), EMCMOT_MAX_JOINTS, 1, nullptr) != 0 + || map.max_joints < n) { + if(!travel.unchecked) { + travel.unchecked = true; + report_travel(-1, -1, 0, 0.0, 0.0); + } + return false; + } + for(int a = 0; a < EMCMOT_MAX_AXIS; a++) { + for(int j = 0; j < n; j++) { + if(map.joints_of_axis[a] & (1 << j)) travel.id_axis[j] = a; + } + } + travel.id_joints = n; + return true; +} + +// the joints of an identity machine at a point, against their travel +static bool identity_check(int line_number, setup_pointer s, const EmcPose &p) { + const double v[9] = {p.tran.x, p.tran.y, p.tran.z, p.a, p.b, p.c, p.u, p.v, p.w}; + for(int j = 0; j < travel.id_joints; j++) { + double q = v[travel.id_axis[j]]; + if(q > s->kins_joint_max[j] || q < s->kins_joint_min[j]) { + int side = q > s->kins_joint_max[j] ? 1 : -1; + report_travel(line_number, j, side, q, side > 0 ? s->kins_joint_max[j] : s->kins_joint_min[j]); + return true; + } + } + return false; +} + +// on an identity machine, whether the machine's kinematics is the identity +// and, when it is, the move checked: its end, and for an arc the angles +// where X, Y or Z turn back on the circle +static bool identity_move(int line_number, const EmcPose &end, const PmCircle *circle) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return false; + if(travel.identity < 0) travel.identity = GET_EXTERNAL_KINEMATICS_IDENTITY(); + if(!travel.identity) return false; + setup_pointer s = &interp->_setup; + EmcPose start = travel.at; + travel.at = end; + travel.placed = true; + if(!identity_map(s)) return true; + if(circle) { + const double tan_[3] = {circle->rTan.x, circle->rTan.y, circle->rTan.z}; + const double perp[3] = {circle->rPerp.x, circle->rPerp.y, circle->rPerp.z}; + for(int i = 0; i < 3; i++) { + if(fabs(tan_[i]) < 1e-12 && fabs(perp[i]) < 1e-12) continue; + double first = atan2(perp[i], tan_[i]); + for(double a = first - 2.0 * M_PI; a <= circle->angle; a += M_PI) { + if(a < 0.0) continue; + PmCartesian at; + pmCirclePoint(circle, a, &at); + motion_planning::SegmentLine ends = {start, end}; + EmcPose pose; + motion_planning::segmentLinePose(a / circle->angle, &pose, &ends); + pose.tran.x = at.x; pose.tran.y = at.y; pose.tran.z = at.z; + if(identity_check(line_number, s, pose)) return true; + } + } + } + identity_check(line_number, s, end); + return true; +} + +// follow the joints along a move on type ktype (-1 the one in force) from +// given, or from where they stand, to end: a line, or the arc given +static void follow_move(int line_number, int ktype, const EmcPose *given, const EmcPose &end, + const motion_planning::SegmentArc *arc, double sweep_deg) { + if(!arc && identity_move(line_number, end, nullptr)) return; + Interp *interp = nullptr; + KinematicsUserContext *ctx = travel_kins(&interp, ktype); + if(!ctx) { travel.at = end; travel.placed = true; travel.have = false; return; } + setup_pointer s = &interp->_setup; + motion_planning::SegmentArc seg = arc ? *arc : motion_planning::SegmentArc{}; + if(given) seg.ends.start = *given; + else if(!travel_start(ctx, s, &seg.ends.start)) { + travel.have = !travel.placed && travel_place(ctx, s, end); + travel.at = end; + travel.placed = true; + kinematicsUserSetType(ctx, s->kins_type); + if(travel.have) travel_check_end(line_number, s); + return; + } + travel.at = end; + travel.placed = true; + seg.ends.end = end; + if(!arc) seg.line = true; + motion_planning::SegmentCapLimits lim; + motion_planning::SegmentCap cap; + travel_limits(s, &lim); + int samples = motion_planning::segmentSamples(&seg.ends.start, &seg.ends.end, sweep_deg, + parse_state.angular); + int got = motion_planning::segmentCap(ctx, &lim, 1.0, samples, motion_planning::segmentArcPose, + &seg, travel.joints, &cap); + kinematicsUserSetType(ctx, s->kins_type); + travel.have = got == 0; + if(got == 0 && cap.travel_joint >= 0) { + report_travel(line_number, cap.travel_joint, cap.travel_side, cap.travel_pos, + cap.travel_side > 0 ? lim.max[cap.travel_joint] : lim.min[cap.travel_joint]); + } +} + +// a joint interpolated move: the joints go straight from where they stand +// to its end, so only the end can be past a limit +static void follow_joints(int line_number, const double *joints, int have_joints, const EmcPose &end) { + if(identity_move(line_number, end, nullptr)) return; + Interp *interp = nullptr; + KinematicsUserContext *ctx = travel_kins(&interp, -1); + travel.at = end; + travel.placed = true; + if(!ctx) { travel.have = false; return; } + setup_pointer s = &interp->_setup; + bool ok = true; + if(have_joints) { + for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) travel.joints[i] = joints[i]; + } else { + // motion runs the inverse once at the end, from where the joints stand + if(!travel.have) for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) travel.joints[i] = s->kins_seed[i]; + ok = kinematicsUserInverse(ctx, &end, travel.joints) == 0; + } + kinematicsUserSetType(ctx, s->kins_type); + travel.have = ok; + if(ok) travel_check_end(line_number, s); +} + +// an arc as canon builds it: the end and the centre given in the plane, +// the centre at the end's height across it, the other axes along; the +// circle in the machine's units, from where the joints stand +static void follow_arc(int line_number, double first_end, double second_end, + double first_axis, double second_axis, int rotation, double axis_end_point, + double a, double b, double c, double u, double v, double w) { + double e[3] = {first_end, second_end, axis_end_point}; + double m[3] = {first_axis, second_axis, axis_end_point}; + double n[3] = {0.0, 0.0, 1.0}; + int shift = travel.plane == CANON_PLANE::XZ ? 1 : travel.plane == CANON_PLANE::YZ ? 2 : 0; + Point9 end, centre; + for(int i = 0; i < 3; i++) { + end[i] = e[(i + shift) % 3]; + centre[i] = m[(i + shift) % 3]; + } + double normal[3] = {n[shift % 3], n[(1 + shift) % 3], n[(2 + shift) % 3]}; + end[3] = a; end[4] = b; end[5] = c; end[6] = u; end[7] = v; end[8] = w; + for(int i = 3; i < 9; i++) centre[i] = 0.0; + EmcPose to = machine_pose(end); + if(travel.identity < 0) travel.identity = GET_EXTERNAL_KINEMATICS_IDENTITY(); + if(travel.identity) { + // the circle from where the last move ended, as canon has it + EmcPose mc = machine_pose(centre); + PmCartesian ps = {travel.at.tran.x, travel.at.tran.y, travel.at.tran.z}; + PmCartesian pe = {to.tran.x, to.tran.y, to.tran.z}; + PmCartesian pc = {mc.tran.x, mc.tran.y, mc.tran.z}; + PmCartesian pn = {normal[0], normal[1], normal[2]}; + PmCircle circle; + bool round = travel.placed && rotation != 0 + && pmCircleInit(&circle, &ps, &pe, &pc, &pn, rotation > 0 ? rotation - 1 : rotation) == 0; + identity_move(line_number, to, round ? &circle : nullptr); + return; + } + Interp *interp = nullptr; + KinematicsUserContext *ctx = travel_kins(&interp, -1); + if(!ctx) { travel.at = to; travel.placed = true; travel.have = false; return; } + setup_pointer s = &interp->_setup; + motion_planning::SegmentArc arc = {}; + if(!travel_start(ctx, s, &arc.ends.start)) { + travel.have = !travel.placed && travel_place(ctx, s, to); + travel.at = to; + travel.placed = true; + kinematicsUserSetType(ctx, s->kins_type); + if(travel.have) travel_check_end(line_number, s); + return; + } + kinematicsUserSetType(ctx, s->kins_type); + travel.have = true; + EmcPose mc = machine_pose(centre); + const EmcPose &st = arc.ends.start; + PmCartesian ps = {st.tran.x, st.tran.y, st.tran.z}; + PmCartesian pe = {to.tran.x, to.tran.y, to.tran.z}; + PmCartesian pc = {mc.tran.x, mc.tran.y, mc.tran.z}; + PmCartesian pn = {normal[0], normal[1], normal[2]}; + arc.scale = 1.0; + arc.line = rotation == 0; + if(!arc.line && pmCircleInit(&arc.circle, &ps, &pe, &pc, &pn, rotation > 0 ? rotation - 1 : rotation) != 0) { + arc.line = true; + } + double sweep = arc.line ? 0.0 : fabs(arc.circle.angle) * 180.0 / M_PI; + follow_move(line_number, -1, &arc.ends.start, to, &arc, sweep); +} + // One point of a NURBS curve, fed through the canon. The curve's two // components land in the active plane and every other axis holds the position // the interpreter last reported; a plane that carries no NURBS feeds nothing, @@ -502,6 +855,8 @@ void ARC_FEED(int line_number, double second_axis, int rotation, double axis_end_point, double a_position, double b_position, double c_position, double u_position, double v_position, double w_position) { + follow_arc(line_number, first_end, second_end, first_axis, second_axis, rotation, + axis_end_point, a_position, b_position, c_position, u_position, v_position, w_position); // XXX: set _pos_* parse_state.canon->arc_feed(line_number, ensure_inch(first_end), @@ -518,13 +873,22 @@ void ARC_FEED(int line_number, ensure_inch(P9_W, w_position)); } +static void draw_feed(int line_number, const Point9 &p) { + parse_state.pos = p; + parse_state.canon->straight_feed(line_number, ensure_inch(p)); +} + +static void draw_traverse(int line_number, const Point9 &p) { + parse_state.pos = p; + parse_state.canon->straight_traverse(line_number, ensure_inch(p)); +} + void STRAIGHT_FEED(int line_number, double x, double y, double z, double a, double b, double c, double u, double v, double w) { - parse_state.pos = {x, y, z, a, b, c, u, v, w}; - parse_state.canon->straight_feed(line_number, - ensure_inch({x, y, z, a, b, c, u, v, w})); + follow_move(line_number, -1, nullptr, machine_pose({x, y, z, a, b, c, u, v, w}), nullptr, 0.0); + draw_feed(line_number, {x, y, z, a, b, c, u, v, w}); } // the preview draws a joint interpolated move as the traverse between its @@ -534,19 +898,17 @@ void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, double x, double y, double z, double a, double b, double c, double u, double v, double w) { - (void)joints; - (void)have_joints; - STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); + follow_joints(line_number, joints, have_joints, machine_pose({x, y, z, a, b, c, u, v, w})); + draw_traverse(line_number, {x, y, z, a, b, c, u, v, w}); } void JOINT_FEED(int line_number, const double *joints, int have_joints, double x, double y, double z, double a, double b, double c, double u, double v, double w, double seconds) { - (void)joints; - (void)have_joints; (void)seconds; - STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); + follow_joints(line_number, joints, have_joints, machine_pose({x, y, z, a, b, c, u, v, w})); + draw_feed(line_number, {x, y, z, a, b, c, u, v, w}); } // a move the primary type interpolates under the machine frame type is a @@ -557,10 +919,10 @@ void KINS_TYPE_TRAVERSE(int line_number, int ktype, double x, double y, double z, double a, double b, double c, double u, double v, double w) { - (void)ktype; - (void)start; - (void)end; - (void)joints; + // the line on type ktype; the joints end where the interpreter solved them + follow_move(line_number, ktype, &start, end, nullptr, 0.0); + if(travel.have) for(int i = 0; i < EMCMOT_MAX_JOINTS; i++) travel.joints[i] = joints[i]; + travel.at = machine_pose({x, y, z, a, b, c, u, v, w}); parse_state.pos = {x, y, z, a, b, c, u, v, w}; parse_state.canon->part_traverse(line_number, ensure_inch({x, y, z, a, b, c, u, v, w})); @@ -570,9 +932,8 @@ void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, double u, double v, double w) { - parse_state.pos = {x, y, z, a, b, c, u, v, w}; - parse_state.canon->straight_traverse(line_number, - ensure_inch({x, y, z, a, b, c, u, v, w})); + follow_move(line_number, -1, nullptr, machine_pose({x, y, z, a, b, c, u, v, w}), nullptr, 0.0); + draw_traverse(line_number, {x, y, z, a, b, c, u, v, w}); } void SET_G5X_OFFSET(int g5x_index, @@ -618,6 +979,7 @@ void SET_G68_FRAME(double x, double y, double z, void USE_LENGTH_UNITS(CANON_UNITS u) { parse_state.metric = u == CANON_UNITS_MM; } void SELECT_PLANE(CANON_PLANE pl) { + travel.plane = pl; parse_state.canon->set_plane(static_cast(pl)); } @@ -1203,6 +1565,7 @@ static py::object parse_file(const char *f, py::handle canon, // the interpreter reads the offset in force as it starts, and the one // the last parse ended on is not the machine's: the GUI restates it parse_state.tool_offset = {}; + travel = Travel(); read_kins_types(); on_part = OnPart(); diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index fbeda6ef381..bf43d3d7a2f 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1929,6 +1929,22 @@ def run_warn(): if o.canon.max_extents_notool[i] > machine_limit_max[i]: warnings.append(_("Program exceeds machine maximum on axis %s") % "XYZABCUVW"[i]) + # the joints the preview followed through the kinematics, which the + # extents of X Y Z say nothing about on a machine with rotaries; a + # module that cannot be followed here (line -1) is not worth a + # dialog on every run + left = [m for m in getattr(o.canon, "joint_travel_left", []) if m[0] >= 0] + for line, joint, side, where, limit in left[:10]: + if side > 0: + warnings.append(_("Line %(line)d: joint %(joint)d would reach %(where).3f, " + "past its MAX_LIMIT of %(limit).3f") + % {"line": line, "joint": joint, "where": where, "limit": limit}) + else: + warnings.append(_("Line %(line)d: joint %(joint)d would reach %(where).3f, " + "past its MIN_LIMIT of %(limit).3f") + % {"line": line, "joint": joint, "where": where, "limit": limit}) + if len(left) > 10: + warnings.append(_("and %d more moves") % (len(left) - 10)) # A truncated preview only checked part of the program; warn rather # than imply a clean all-clear. if getattr(o.canon, "preview_incomplete", False): diff --git a/tests/kins-limits/preview/README b/tests/kins-limits/preview/README new file mode 100644 index 00000000000..cd4972fb771 --- /dev/null +++ b/tests/kins-limits/preview/README @@ -0,0 +1,15 @@ +The preview follows the joints along every move through the kinematics, +on the type each move runs on, against [JOINT_n] MIN_LIMIT and MAX_LIMIT, +and reports a move a joint would leave its travel along, which task +refuses at run time; AXIS lists them when Run is pressed. + +5axiskins with a 400 mm pivot under halrun, previewed with no machine: +the swing of tests/kins-limits/travel, the head laid over and C turning +round the tip, reported both ways round with a tool of 0.05, its peak +0.05 past joint 1's travel between two samples, and not with -0.05; an +arc whose top is past one way round and not the other, in XY and in ZX; +a joint move whose end is past; a move on the identity type; and, on a +trivkins gantry whose second Y joint has less travel than the axis, a line +ending past it and an arc whose top is, read off the axes with nothing +loaded; and a line ending past a three joint trivkins machine with no +coordinates= given. diff --git a/tests/kins-limits/preview/check.py b/tests/kins-limits/preview/check.py new file mode 100644 index 00000000000..5d8adf09857 --- /dev/null +++ b/tests/kins-limits/preview/check.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The preview follows the joints along each move through the kinematics and +# reports where one would leave its travel, the move task refuses at run +# time; a GUI warns before the run. Each case is a program and the moves +# reported, as (line, joint, side). +import gcode +import linuxcnc +import os +import sys +from rs274.interpret import Translated + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +class Canon(Translated): + # what a preview reads off a machine on its 5axiskins type, nothing homed + kinematics_type = linuxcnc.KINEMATICS_BOTH + def __init__(self): + self.parameter_file = "preview.var" + self.left = [] + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def get_tool(self, pocket): + return (-1,) + (0,) * 13 + def get_external_length_units(self): + return 1.0 + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return 0x137 # X Y Z B C W + def get_block_delete(self): + return 0 + def get_external_joint_positions(self): + return () + def get_kinematics_type(self): + return self.kinematics_type + def get_kins_type(self): + return 0 + def get_kins_type_flags(self): + return (linuxcnc.KINSTYPE_PRIMARY, + linuxcnc.KINSTYPE_IDENTITY | linuxcnc.KINSTYPE_MACHINE, 0) + (-1,) * 13 + def joint_travel(self, line, joint, side, where, limit): + self.left.append((line, joint, side, where, limit)) + +def case(name, text, want, canon=None): + with open("case.ngc", "w") as f: + f.write("G21 G90 G17\nG12.1 P0\n" + text + "\nM2\n") + canon = canon or Canon() + res, seq = gcode.parse("case.ngc", canon, "G21", "") + if res > gcode.MIN_ERROR: + error("%s: the preview stops at line %d: %s" % (name, seq, gcode.strerror(res).strip())) + return [] + got = [(line, joint, side) for line, joint, side, where, limit in canon.left] + if got != want: + error("%s: reported %s, not %s" % (name, canon.left, want)) + else: + print("%s: %s" % (name, canon.left)) + return canon.left + +# the head laid over to B90, the tip held while C turns: the carriage +# swings round the tip at the pivot plus the tool, to Y+ that at C-90, +# against joint 1's MAX_LIMIT of 400. A tool of 0.05 peaks 0.05 past it +# between two samples 1.2 inside, both ways round; -0.05 stays inside. +SWING = "G43.1 Z%s\nG0 X0 Y0 Z0 B90 C-5\nG0 C-175\nG0 C-5" +left = case("swing past", SWING % "0.05", [(5, 1, 1), (6, 1, 1)]) +if left and abs(left[0][3] - 400.05) > 1e-3: + error("the swing peaks at %.4f, not 400.05" % left[0][3]) +case("swing inside", SWING % "-0.05", []) +case("swing samples past", SWING % "100", [(5, 1, 1), (6, 1, 1)]) + +# a parse starts from nowhere the preview knows, the GUI's restating move +# going to where the machine stands: the first move is not followed from +# zero, only its end checked +case("first move", "G43.1 Z100\nG0 X0 Y0 Z0 B90 C-175", []) +case("first move past", "G43.1 Z100\nG0 X0 Y0 Z0 B90 C-90", [(4, 1, 1)]) + +# with the head upright Y is joint 1: an arc whose ends are inside and whose +# top is past, one way round, and the same arc the other way round, under +case("arc", "G0 X-100 Y350 Z0 B0 C0\nG2 X100 Y350 I100 J-50\nG0 X-100 Y350\n" + "G3 X100 Y350 I100 J-50", [(4, 1, 1)]) +# in the ZX plane, one way round over Z's MAX_LIMIT of 1000 +case("arc zx", "G18 G0 X0 Y0 Z900 B0 C0\nG2 X300 Z900 I150 K0\nG0 X0 Z900\n" + "G3 X300 Z900 I150 K0", [(6, 2, 1)]) +# a joint move goes straight in joint space: its end alone can be past +case("joint move", "G0 X0 Y0 Z0 B0 C0\nG53.7 G0 J1=450\nG53.7 G0 J1=0", [(4, 1, 1)]) +# on the identity type the joints are the axes +case("identity", "G0 X0 Y0 Z0 B0 C0\nG13.1\nG0 Y450\nG0 Y0", [(5, 1, 1)]) +# a machine whose kinematics is the identity, a gantry: each joint is its +# axis, nothing loaded; the second Y joint has the shorter travel, which the +# extents of Y do not show. A line is checked at its end, an arc also where +# the circle turns back, here its top, one way round and not the other. +os.environ["INI_FILE_NAME"] = "gantry.ini" +def gantry(name, text, want): + flat = Canon() + flat.kinematics_type = linuxcnc.KINEMATICS_IDENTITY + return case(name, text, want, flat) +gantry("gantry line", "G0 X0 Y0 Z0\nG0 Y390\nG0 Y0", [(4, 3, 1)]) +gantry("gantry arc", "G0 X-100 Y300 Z0\nG2 X100 Y300 I100 J0\nG0 X-100 Y300\n" + "G3 X100 Y300 I100 J0", [(4, 3, 1)]) +gantry("gantry inside", "G0 X0 Y0 Z0\nG0 Y370\nG2 X0 Y370 I0 J-5\nG0 Y0", []) +# three joints and no coordinates=: trivkins' nine letters cover them +os.environ["INI_FILE_NAME"] = "plain.ini" +gantry("three joints", "G0 X0 Y0 Z0\nG0 Y450\nG0 Y0", [(4, 1, 1)]) + +os.unlink("case.ngc") +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/preview/checkresult b/tests/kins-limits/preview/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/preview/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/preview/gantry.ini b/tests/kins-limits/preview/gantry.ini new file mode 100644 index 00000000000..887c3f82d77 --- /dev/null +++ b/tests/kins-limits/preview/gantry.ini @@ -0,0 +1,44 @@ +# An identity machine, a gantry whose second Y joint has less travel than +# the axis: the joints are read off the axes, nothing is loaded. + +[KINS] +KINEMATICS = trivkins coordinates=XYZY +JOINTS = 4 + +[TRAJ] +COORDINATES = XYZY +LINEAR_UNITS = mm +ANGULAR_UNITS = deg + +[AXIS_X] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[AXIS_Y] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[AXIS_Z] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 400 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_3] +TYPE = LINEAR +# the travel the gantry's Y is measured against +MIN_LIMIT = -1000 +MAX_LIMIT = 380 diff --git a/tests/kins-limits/preview/plain.ini b/tests/kins-limits/preview/plain.ini new file mode 100644 index 00000000000..2263ed12586 --- /dev/null +++ b/tests/kins-limits/preview/plain.ini @@ -0,0 +1,38 @@ +# An identity machine with three joints and trivkins' default coordinates, +# XYZABCUVW, mapped over all the joints motion has, as trivkins maps them. + +[KINS] +KINEMATICS = trivkins +JOINTS = 3 + +[TRAJ] +COORDINATES = XYZ +LINEAR_UNITS = mm +ANGULAR_UNITS = deg + +[AXIS_X] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[AXIS_Y] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[AXIS_Z] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 400 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 diff --git a/tests/kins-limits/preview/preview.hal b/tests/kins-limits/preview/preview.hal new file mode 100644 index 00000000000..f29fced7960 --- /dev/null +++ b/tests/kins-limits/preview/preview.hal @@ -0,0 +1,6 @@ +# the module the preview evaluates outside realtime, with the pin it reads: +# laid over to B90, the head swings the carriage round the tip at the +# pivot length plus the tool's +loadrt 5axiskins coordinates=xyzbcw +setp 5axiskins.pivot-length 400 +loadusr -w env INI_FILE_NAME=test.ini python3 check.py diff --git a/tests/kins-limits/preview/test.ini b/tests/kins-limits/preview/test.ini new file mode 100644 index 00000000000..287aec65228 --- /dev/null +++ b/tests/kins-limits/preview/test.ini @@ -0,0 +1,67 @@ +# The preview's interpreter reads the kinematics, the joints' travel and +# the axis kinds from here; nothing else runs. + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg + +[AXIS_X] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[AXIS_Y] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[AXIS_Z] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_1] +TYPE = LINEAR +# the travel the swings are measured against +MIN_LIMIT = -1000 +MAX_LIMIT = 400 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 diff --git a/tests/kins-limits/preview/test.sh b/tests/kins-limits/preview/test.sh new file mode 100755 index 00000000000..07c0c0a746b --- /dev/null +++ b/tests/kins-limits/preview/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e +# a crash in the test script prints where it was +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +rm -f preview.var preview.var.bak +halrun -f preview.hal +rm -f preview.var preview.var.bak From 0b466272b422ae1984aa5c55d95f7d91124172b7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 9 Nov 2026 19:24:15 +1100 Subject: [PATCH 41/44] interp: turn a coordinate system about X and Y as well as Z A part that does not sit square to the machine, a face of a fixture block on a rotary table or a part measured on a tilted surface, needs its work offset turned in space, not only about Z. G10 L2 now takes I, J and K, the turn about X, Y and Z of the coordinate system, K being the same value as R. They turn about the machine's axes through the origin of the system, X first, then Y, then Z: Rz(K) * Ry(J) * Rx(I). That is how G68.2 P1 takes its I J K (Q123), Heidenhain the 3-D basic rotation of a preset (SPA SPB SPC) and Siemens the rotations of a work offset (ROT X Y Z); Fanuc's workpiece setting error compensation (G54.4) stores rotations about X, Y and Z too. Heidenhain's manual shows a rotary axis offset gives the wrong orientation for a tilted plane on such a part and recommends the 3-D basic rotation, which is what this is; A, B and C values of a coordinate system stay axis offsets. A system turned about Z alone behaves and computes as R always has. The rotation sits where R did in the chain, world = TLO + G5x + Rg * (G92 + plane), so G92 and a G68.2 plane defined on the system turn with it, as G68.2 on a turned work offset does on those controls. The interpreter, canon, the preview renderer, its Python canon and arc segments, task's jog frame for the plane, halui's relative position and the DRO arithmetic of glcanon, hal_glib, drowidget, gscreen and touchy apply the full rotation; AXIS and the Qt and GTK gremlins list I and J beside R when they are set. I and J are stored with the system after R, at #5x11 and #5x12 (#5231 and #5232 for G54), and saved in the var file; a var file without them reads them as zero. Status has them as rotation_yz and rotation_zx beside rotation_xy. Canon hears them through a new SET_YZ_ZX_ROTATION, sent only when they change, so a program that never uses them talks to canon as it always did. G10 L20 keeps the rotation and sets the coordinates given along the system's turned axes, so one coordinate can move the origin along all three; it refuses I J K as it refuses R. G10 L10 and L11 refuse a system turned about X or Y, since a tool offset is along the machine's axes. When G10 L2 changes the rotation of the system in effect, the tool stays where it is and its coordinates go round the whole chain; before, a change of R with a G92 in effect left the current point off by the turned G92. tests/g5x-rotation runs an XYZ sim twice: the moves, an arc in the program's XY plane on a system turned about X, a G68.2 plane on top, G92, G10 L20 and the refusals, status, halui, the interpreter's point and both preview canons, then the restart reading the rotation back from the var file. tests/twp-jog-frame jogs its plane on a system turned about X, Y and Z, which a jog frame that knew only R fails, and tests/twp-table's last G53.1 case does so too: the tool along the turned normal and the move in the plane where the turned plane puts it. --- docs/src/config/python-interface.adoc | 8 + docs/src/gcode/g-code.adoc | 79 ++++- docs/src/gcode/overview.adoc | 9 + lib/python/common/hal_glib.py | 7 + lib/python/gladevcp/drowidget.py | 7 + lib/python/gladevcp/hal_gremlin.py | 4 + lib/python/rs274/glcanon.py | 15 + lib/python/rs274/glcanon_scene.py | 14 +- lib/python/rs274/interpret.py | 17 + src/emc/nml_intf/canon.hh | 9 + src/emc/nml_intf/emc.cc | 4 + src/emc/nml_intf/emc_nml.hh | 10 +- src/emc/nml_intf/emcops.cc | 2 + src/emc/rs274ngc/canonmodule.cc | 1 + src/emc/rs274ngc/gcode_renderer.cc | 32 +- src/emc/rs274ngc/gcode_renderer.hh | 4 +- src/emc/rs274ngc/gcodemodule.cc | 18 +- src/emc/rs274ngc/gcodemodule.hh | 16 + src/emc/rs274ngc/interp_array.cc | 18 +- src/emc/rs274ngc/interp_check.cc | 5 +- src/emc/rs274ngc/interp_convert.cc | 125 ++++--- src/emc/rs274ngc/interp_find.cc | 8 +- src/emc/rs274ngc/interp_internal.hh | 3 + src/emc/rs274ngc/interp_parameter_def.hh | 1 + src/emc/rs274ngc/interp_setup.cc | 3 + src/emc/rs274ngc/interp_workplane.cc | 127 +++++-- src/emc/rs274ngc/interpmodule.cc | 14 + src/emc/rs274ngc/rs274ngc_interp.hh | 7 +- src/emc/rs274ngc/rs274ngc_pre.cc | 6 +- src/emc/sai/saicanon.cc | 4 + src/emc/task/emccanon.cc | 80 ++++- src/emc/task/emctaskmain.cc | 34 +- src/emc/usr_intf/axis/extensions/emcmodule.cc | 4 + src/emc/usr_intf/axis/scripts/axis.py | 1 + src/emc/usr_intf/gremlin/qt5_graphics.py | 4 + src/emc/usr_intf/gscreen/emc_interface.py | 7 + src/emc/usr_intf/halui.cc | 16 +- src/emc/usr_intf/touchy/emc_interface.py | 7 + tests/g5x-rotation/README | 18 + tests/g5x-rotation/checkresult | 3 + tests/g5x-rotation/test-ui.py | 321 ++++++++++++++++++ tests/g5x-rotation/test.ini | 82 +++++ tests/g5x-rotation/test.sh | 11 + tests/g5x-rotation/tool.tbl | 1 + tests/twp-jog-frame/README | 11 +- tests/twp-jog-frame/test-ui.py | 20 +- tests/twp-table/README | 4 +- tests/twp-table/test-ui.py | 31 +- 48 files changed, 1054 insertions(+), 178 deletions(-) create mode 100644 tests/g5x-rotation/README create mode 100755 tests/g5x-rotation/checkresult create mode 100755 tests/g5x-rotation/test-ui.py create mode 100644 tests/g5x-rotation/test.ini create mode 100755 tests/g5x-rotation/test.sh create mode 100644 tests/g5x-rotation/tool.tbl diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index ffd0e25e08d..d3b9f871f67 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -357,6 +357,14 @@ see <> for an example. *rotation_xy*:: '(returns float)' - current XY rotation angle around Z axis. +*rotation_yz*:: '(returns float)' - + current rotation of the coordinate system around the X axis, in degrees + (G10 L2 I). The coordinate system turns about X, then Y, then Z. + +*rotation_zx*:: '(returns float)' - + current rotation of the coordinate system around the Y axis, in degrees + (G10 L2 J). + *settings*:: '(returns tuple of floats)' - current interpreter settings: + settings[0] = sequence number, + diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index ee50c3e338b..9c820dcfdc0 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -758,11 +758,14 @@ see the <> diagram. [source,ngc] ---- -G10 L2 P- +G10 L2 P- ---- * 'P' - coordinate system (0-9) * 'R' - rotation about the Z axis +* 'I' - rotation about the X axis +* 'J' - rotation about the Y axis +* 'K' - rotation about the Z axis, the same as 'R' G10 L2 offsets the origin of the axes in the coordinate system specified to the value of the axis word. The offset is from the machine origin @@ -793,6 +796,25 @@ Z axis. The direction of rotation is CCW as viewed from the positive end of the Z axis. +Optionally program I and J to turn the coordinate system about the X and +the Y axis as well, in degrees, for a part that does not sit square to the +machine: a face of a fixture block on a rotary table, or a part measured +on a tilted surface. K is the rotation about Z, the same value as R; give +one or the other. The three turn about the machine's axes through the +origin of the coordinate system, in the order X, then Y, then Z, which +is how 'G68.2 P1' takes its I J K (Q123), Heidenhain the 3-D basic +rotation of a preset (SPA SPB SPC) and Siemens the rotations of a work +offset (ROT X Y Z). A rotation only about Z behaves as R always has. +A tilted work plane (<>) defined on a turned +coordinate system is turned with it, as 'G68.2' on top of the work offset +rotation is on those controls. A, B and C values of the coordinate system +stay offsets of the rotary axes; they do not turn it. + +The values are kept with the coordinate system and saved in the parameter +file: for coordinate system n (1 for G54 to 9 for G59.3) R is +'#[5210 + 20 * n]', I '#[5211 + 20 * n]' and J '#[5212 + 20 * n]', so G54 +has R at #5230, I at #5231 and J at #5232. + All axis words are optional. Being in incremental distance mode (<>) has no @@ -808,7 +830,9 @@ Important Concepts: * If a 'G52' local offset or 'G92' origin offset was in effect before 'G10 L2', it will continue to be in effect afterwards. * When programming a coordinate system with R, any 'G52' or 'G92' will - be applied *after* the rotation. + be applied *after* the rotation; with I J K likewise. +* When the coordinate system in effect changes, the tool stays where it is + and its coordinates change to match. * The coordinate system whose origin is set by a 'G10' command may be active or inactive at the time the 'G10' is executed. If it is currently active, the new coordinates take effect immediately. @@ -817,6 +841,7 @@ It is an error if: * The P number does not evaluate to an integer in the range 0 to 9. * An axis is programmed that is not defined in the configuration. +* R and K are both given. .G10 L2 Example Line [source,ngc] @@ -838,6 +863,17 @@ G10 L2 P1 X0 Y0 Z0 (clear offsets for X,Y & Z axes in coordinate system 1) The above example sets the XYZ coordinates of the coordinate system 1 to the machine origin. +.G10 L2 Example Line +[source,ngc] +---- +G10 L2 P2 X100 Y0 Z50 I-90 (G55 on the face of a block standing on a horizontal A) +---- + +The above example puts the origin of coordinate system 2 at X100 Y0 Z50 +and turns it -90 degrees about X, so its Z points along machine -Y, out of +the face of a block turned so on the table: a program written for a part +lying flat runs on that face unchanged in 'G55'. + The coordinate system is described in the <> section. @@ -880,6 +916,9 @@ It is an error if: * The P number is unspecified * The P number is not a valid tool number from the tool table * The P number is 0 +* The coordinate system in effect is turned about X or Y (G10 L2 I J): a + tool offset is along the machine's axes; set it with + <>. [[gcode:g10-l11]] == G10 L11 Set Tool Table(((G10 L11 Set Tool Table))) @@ -916,6 +955,7 @@ It is an error if: * The P number is unspecified * The P number is not a valid tool number from the tool table * The P number is 0 +* G59.3 is turned about X or Y (G10 L2 I J). [[gcode:g10-l20]] == G10 L20 Set Coordinate System(((G10 L20 Set Coordinate System))) @@ -930,6 +970,9 @@ G10 L20 P- axes G10 L20 is similar to G10 L2 except that instead of setting the offset/entry to the given value, it is set to a calculated value that makes the current coordinates become the given value. +The rotation of the coordinate system stays as it is, and the coordinates +are along its turned axes: on a system turned with I or J, setting one of +X, Y and Z can move the origin along all three. .G10 L20 Example Line [source,ngc] @@ -943,6 +986,7 @@ It is an error if: * An axis is programmed that is not defined in the configuration. * '<>' or 'G43.5' is in effect; cancel it with 'G49' first. +* R, I, J or K is given; set the rotation with 'G10 L2'. [[gcode:g12.1-g13.1]] == G12.1, G13.1 Select Kinematics(((G12.1, G13.1 Select Kinematics))) @@ -2336,22 +2380,23 @@ It is an error if: * 'G59.2' - select coordinate system 8 * 'G59.3' - select coordinate system 9 -The coordinate systems store the axis values and the XY rotation angle -around the Z axis in the parameters shown in the following table. +The coordinate systems store the axis values and the rotation angles +around the Z, X and Y axes (<>) in the +parameters shown in the following table. .Coordinate System Parameters -[width="80%",options="header",cols="<,11*^"] +[width="90%",options="header",cols="<,13*^"] |=== -|Select |CS |X |Y |Z |A |B |C |U |V |W |R -|G54 |1 |5221 |5222 |5223 |5224 |5225 |5226 |5227 |5228 |5229 |5230 -|G55 |2 |5241 |5242 |5243 |5244 |5245 |5246 |5247 |5248 |5249 |5250 -|G56 |3 |5261 |5262 |5263 |5264 |5265 |5266 |5267 |5268 |5269 |5270 -|G57 |4 |5281 |5282 |5283 |5284 |5285 |5286 |5287 |5288 |5289 |5290 -|G58 |5 |5301 |5302 |5303 |5304 |5305 |5306 |5307 |5308 |5309 |5310 -|G59 |6 |5321 |5322 |5323 |5324 |5325 |5326 |5327 |5328 |5329 |5330 -|G59.1 |7 |5341 |5342 |5343 |5344 |5345 |5346 |5347 |5348 |5349 |5350 -|G59.2 |8 |5361 |5362 |5363 |5364 |5365 |5366 |5367 |5368 |5369 |5370 -|G59.3 |9 |5381 |5382 |5383 |5384 |5385 |5386 |5387 |5388 |5389 |5390 +|Select |CS |X |Y |Z |A |B |C |U |V |W |R |I |J +|G54 |1 |5221 |5222 |5223 |5224 |5225 |5226 |5227 |5228 |5229 |5230 |5231 |5232 +|G55 |2 |5241 |5242 |5243 |5244 |5245 |5246 |5247 |5248 |5249 |5250 |5251 |5252 +|G56 |3 |5261 |5262 |5263 |5264 |5265 |5266 |5267 |5268 |5269 |5270 |5271 |5272 +|G57 |4 |5281 |5282 |5283 |5284 |5285 |5286 |5287 |5288 |5289 |5290 |5291 |5292 +|G58 |5 |5301 |5302 |5303 |5304 |5305 |5306 |5307 |5308 |5309 |5310 |5311 |5312 +|G59 |6 |5321 |5322 |5323 |5324 |5325 |5326 |5327 |5328 |5329 |5330 |5331 |5332 +|G59.1 |7 |5341 |5342 |5343 |5344 |5345 |5346 |5347 |5348 |5349 |5350 |5351 |5352 +|G59.2 |8 |5361 |5362 |5363 |5364 |5365 |5366 |5367 |5368 |5369 |5370 |5371 |5372 +|G59.3 |9 |5381 |5382 |5383 |5384 |5385 |5386 |5387 |5388 |5389 |5390 |5391 |5392 |=== It is an error if: @@ -2523,7 +2568,7 @@ say, is untouched. Positions on the display, probe results and chain as always with one more stage, applied first: ---- -absolute = tool offset + G5x + XY rotation applied to (G92 + origin + rotation applied to (G52 in the plane + program)) +absolute = tool offset + G5x + G5x rotation applied to (G92 + origin + rotation applied to (G52 in the plane + program)) ---- .'G68.2 X50 Y30 Z40 J30': a plane tilted 30 degrees about X, on G54, on the machine coordinates @@ -2532,7 +2577,7 @@ image::images/g68-2-plane.svg["G68.2 tilted work plane",align="center"] 'X', 'Y' and 'Z' are the plane's origin ('P2' gives it by points, below) and the plane's rotation is built from the rest of the words, both in the coordinate system that is active when the plane is defined: the work offset -with 'G92' and the XY rotation in place, which is what the position display +with 'G92' and its rotation (R, or I J K) in place, which is what the position display shows at that moment. A rotary word does not pass through the plane, since on a TCP kinematics the rotary coordinates are the rotary joints and a plane does not change what a joint is. Words left out are zero. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index ae352b92262..adb83c400a3 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -330,22 +330,31 @@ example '##2' means the value of the parameter whose index is the * '5220' - Coordinate System number 1 - 9 for G54 - G59.3. Persistent. * '5221-5230' - Coordinate System 1, G54 for X, Y, Z, A, B, C, U, V, W & R. R denotes the XY rotation angle around the Z axis. Persistent. +* '5231-5232' - Coordinate System 1, the rotation about X and Y (G10 L2 I J). Persistent. * '5241-5250' - Coordinate System 2, G55 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5251-5252' - Coordinate System 2, the rotation about X and Y (G10 L2 I J). Persistent. * '5261-5270' - Coordinate System 3, G56 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5271-5272' - Coordinate System 3, the rotation about X and Y (G10 L2 I J). Persistent. * '5281-5290' - Coordinate System 4, G57 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5291-5292' - Coordinate System 4, the rotation about X and Y (G10 L2 I J). Persistent. * '5301-5310' - Coordinate System 5, G58 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5311-5312' - Coordinate System 5, the rotation about X and Y (G10 L2 I J). Persistent. * '5321-5330' - Coordinate System 6, G59 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5331-5332' - Coordinate System 6, the rotation about X and Y (G10 L2 I J). Persistent. * '5341-5350' - Coordinate System 7, G59.1 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5351-5352' - Coordinate System 7, the rotation about X and Y (G10 L2 I J). Persistent. * '5361-5370' - Coordinate System 8, G59.2 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5371-5372' - Coordinate System 8, the rotation about X and Y (G10 L2 I J). Persistent. * '5381-5390' - Coordinate System 9, G59.3 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +* '5391-5392' - Coordinate System 9, the rotation about X and Y (G10 L2 I J). Persistent. * '5399' - Result of M66 - Check or wait for input. Volatile. * '5400' - Tool Number. Volatile. * '5401-5409' - The loaded tool's stored offset for X, Y, Z, A, B, C, U, V diff --git a/lib/python/common/hal_glib.py b/lib/python/common/hal_glib.py index 26e3ba2f883..68155f464bc 100644 --- a/lib/python/common/hal_glib.py +++ b/lib/python/common/hal_glib.py @@ -1162,6 +1162,13 @@ def get_position(self): yr = x * math.sin(t) + y * math.cos(t) x = xr y = yr + # a system also turned about X or Y (G10 L2 I J): the turn about + # Y comes off next, then the one about X + if getattr(self.stat, 'rotation_zx', 0) or getattr(self.stat, 'rotation_yz', 0): + t = math.radians(-self.stat.rotation_zx) + x, z = x * math.cos(t) + z * math.sin(t), z * math.cos(t) - x * math.sin(t) + t = math.radians(-self.stat.rotation_yz) + y, z = y * math.cos(t) - z * math.sin(t), y * math.sin(t) + z * math.cos(t) x -= self.stat.g92_offset[0] y -= self.stat.g92_offset[1] diff --git a/lib/python/gladevcp/drowidget.py b/lib/python/gladevcp/drowidget.py index 5b446a080c3..4a270cfed60 100644 --- a/lib/python/gladevcp/drowidget.py +++ b/lib/python/gladevcp/drowidget.py @@ -277,6 +277,13 @@ def position(self): yr = x * math.sin(t) + y * math.cos(t) x = xr y = yr + # a system also turned about X or Y (G10 L2 I J): the turn about + # Y comes off next, then the one about X + if getattr(self.status, 'rotation_zx', 0) or getattr(self.status, 'rotation_yz', 0): + t = math.radians(-self.status.rotation_zx) + x, z = x * math.cos(t) + z * math.sin(t), z * math.cos(t) - x * math.sin(t) + t = math.radians(-self.status.rotation_yz) + y, z = y * math.cos(t) - z * math.sin(t), y * math.sin(t) + z * math.cos(t) x -= self.status.g92_offset[0] y -= self.status.g92_offset[1] diff --git a/lib/python/gladevcp/hal_gremlin.py b/lib/python/gladevcp/hal_gremlin.py index 10e3ed417a1..18697a2d92a 100644 --- a/lib/python/gladevcp/hal_gremlin.py +++ b/lib/python/gladevcp/hal_gremlin.py @@ -235,6 +235,10 @@ def dro_format(self,s,spd,dtg,limit,homed,positions,axisdtg,g5x_offset,g92_offse if s.axis_mask & (1< Any: return ctx.show_relative and ( s.g5x_offset[X] or s.g5x_offset[Y] or s.g5x_offset[Z] or s.g92_offset[X] or s.g92_offset[Y] or s.g92_offset[Z] or - s.rotation_xy) + s.rotation_xy or getattr(s, 'rotation_yz', 0) or + getattr(s, 'rotation_zx', 0)) def draw(self, ctx: FrameContext) -> None: s = ctx.stat @@ -1621,6 +1622,11 @@ def draw(self, ctx: FrameContext) -> None: ctx.mv.translate(*g5x_offset) ctx.mv.rotate(s.rotation_xy, 0, 0, 1) + # and before it the turns about Y and X (G10 L2 I J) + if getattr(s, 'rotation_zx', 0): + ctx.mv.rotate(s.rotation_zx, 0, 1, 0) + if getattr(s, 'rotation_yz', 0): + ctx.mv.rotate(s.rotation_yz, 1, 0, 0) if show_offsets and self.offsets.has_offset(g92_offset): self.offsets.draw_offset(ctx, g92_offset, "G92") @@ -2776,14 +2782,20 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": g92 = ctx.to_internal_units(s.g92_offset) g5x = ctx.to_internal_units(s.g5x_offset) t = math.radians(s.rotation_xy) + a = math.radians(getattr(s, 'rotation_yz', 0)) + b = math.radians(getattr(s, 'rotation_zx', 0)) except (AttributeError, TypeError): return None c, sn = math.cos(t), math.sin(t) + ca, sa, cb, sb = math.cos(a), math.sin(a), math.cos(b), math.sin(b) def through(x: float, y: float, z: float) -> tuple[float, float, float]: x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) + # the system's turn about X, then Y (G10 L2 I J), before Z + y, z = y*ca - z*sa, y*sa + z*ca + x, z = x*cb + z*sb, z*cb - x*sb return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) origin = through(0, 0, 0) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 9f86a99e1ac..bc7900902b8 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -24,6 +24,8 @@ class Translated: g5x_offset_a = g5x_offset_b = g5x_offset_c = 0 g5x_offset_u = g5x_offset_v = g5x_offset_w = 0 rotation_xy = 0 + g5x_tilt = (0.0, 0.0) + g5x_tilt_rotation = None g68_active = 0 g68_offset = (0.0, 0.0, 0.0) g68_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) @@ -46,6 +48,12 @@ def rotate_and_translate(self, x,y,z,a,b,c,u,v,w): v += self.g92_offset_v w += self.g92_offset_w + if self.g5x_tilt_rotation: + r = self.g5x_tilt_rotation + x, y, z = (r[0]*x + r[1]*y + r[2]*z, + r[3]*x + r[4]*y + r[5]*z, + r[6]*x + r[7]*y + r[8]*z) + if self.rotation_xy: rotx = x * self.rotation_cos - y * self.rotation_sin y = x * self.rotation_sin + y * self.rotation_cos @@ -93,6 +101,15 @@ def set_xy_rotation(self, theta): t = math.radians(theta) self.rotation_sin = math.sin(t) self.rotation_cos = math.cos(t) + def set_yz_zx_rotation(self, yz, zx): + # the turn about X, then Y, that comes before the one about Z + self.g5x_tilt = (yz, zx) + if yz or zx: + a, b = math.radians(yz), math.radians(zx) + ca, sa, cb, sb = math.cos(a), math.sin(a), math.cos(b), math.sin(b) + self.g5x_tilt_rotation = (cb, sb*sa, sb*ca, 0.0, ca, -sa, -sb, cb*sa, cb*ca) + else: + self.g5x_tilt_rotation = None def set_g68_frame(self, x, y, z, r0, r1, r2, r3, r4, r5, r6, r7, r8, active): self.g68_active = active self.g68_offset = (x, y, z) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 7428f4ff0df..0d10d055f1b 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -154,6 +154,9 @@ typedef struct { typedef struct CanonConfig_t { CanonConfig_t() : xy_rotation(0.0), + yz_rotation(0.0), + zx_rotation(0.0), + g5xRotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, rotary_unlock_for_traverse(-1), g5xOffset{}, g92Offset{}, @@ -179,6 +182,9 @@ typedef struct CanonConfig_t { {} double xy_rotation; + double yz_rotation; + double zx_rotation; + double g5xRotation[9]; // row major, the three above; used when yz or zx is not 0 int rotary_unlock_for_traverse; // jointnumber or -1 CANON_POSITION g5xOffset; @@ -252,6 +258,9 @@ extern void SET_G92_OFFSET(double x, double y, double z, double u, double v, double w); extern void SET_XY_ROTATION(double t); +/* The rotation of the coordinate system about X and about Y, in degrees, + applied before the one about Z: Rz(xy) * Ry(zx) * Rx(yz). */ +extern void SET_YZ_ZX_ROTATION(double yz, double zx); /* G28.2: trigger the machine homing cycle from G-code (bare form = all * joints, in HOME_SEQUENCE order). Maps to EMC_JOINT_HOME(-1). */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 7d4b1ea5f5f..7a487ab8307 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1375,6 +1375,8 @@ void EMC_TASK_STAT::update(CMS * cms) cms->update(g5x_index); EmcPose_update(cms, &g92_offset); cms->update(rotation_xy); + cms->update(rotation_yz); + cms->update(rotation_zx); EmcPose_update(cms, &g68_offset); cms->update(g68_rotation, 9); cms->update(g68_active); @@ -1663,6 +1665,8 @@ void EMC_TRAJ_SET_ROTATION::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); cms->update(rotation); + cms->update(rotation_yz); + cms->update(rotation_zx); } // cppcheck-suppress duplInheritedMember diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index febe03d99ad..5968576c3d3 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -855,7 +855,7 @@ class EMC_TRAJ_SET_ROTATION:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_ROTATION() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_ROTATION_TYPE, sizeof(EMC_TRAJ_SET_ROTATION)), - rotation(0.0) + rotation(0.0), rotation_yz(0.0), rotation_zx(0.0) {}; // For internal NML/CMS use only. @@ -863,7 +863,9 @@ class EMC_TRAJ_SET_ROTATION:public EMC_TRAJ_CMD_MSG { // cppcheck-suppress duplInheritedMember void update(CMS * cms); - double rotation; + double rotation; // about Z, degrees + double rotation_yz; // about X, applied first + double rotation_zx; // about Y, applied second }; // the tilted work plane frame (G68.2, G68.3, G68.4, G69): origin in user @@ -1585,7 +1587,9 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { EmcPose g5x_offset; // in user units, currently active int g5x_index; // index of active g5x system EmcPose g92_offset; // in user units, currently active - double rotation_xy; + double rotation_xy; // rotation of the coordinate system about Z, degrees + double rotation_yz; // about X, applied first + double rotation_zx; // about Y, applied second EmcPose g68_offset; // tilted work plane origin, in user units double g68_rotation[9]; // tilted work plane rotation, row major int g68_active; // a tilted work plane is in effect diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index d9a0f3e931d..c3963d36723 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -157,6 +157,8 @@ EMC_TASK_STAT::EMC_TASK_STAT() g5x_index(0), g92_offset{}, rotation_xy(0.0), + rotation_yz(0.0), + rotation_zx(0.0), g68_offset{}, g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, g68_active(0), diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 27f15580928..b06bb0650e1 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -232,6 +232,7 @@ BOOST_PYTHON_MODULE(emccanon) { def("SET_TOOL_TABLE_ENTRY",&SET_TOOL_TABLE_ENTRY); def("SET_TRAVERSE_RATE",&SET_TRAVERSE_RATE); def("SET_XY_ROTATION",&SET_XY_ROTATION); + def("SET_YZ_ZX_ROTATION",&SET_YZ_ZX_ROTATION); def("SPINDLE_RETRACT",&SPINDLE_RETRACT); def("SPINDLE_RETRACT_TRAVERSE",&SPINDLE_RETRACT_TRAVERSE); def("START_CUTTER_RADIUS_COMPENSATION",&START_CUTTER_RADIUS_COMPENSATION); diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 95edab13fd0..a0d5b7efad7 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -48,6 +48,11 @@ void GCodeRenderer::set_xy_rotation(double degrees) { unrot_sin_ = -rotation_sin_; // sin(back); } +void GCodeRenderer::set_yz_zx_rotation(double yz, double zx) { + if(parse_state.interp_error) return; + tilt_ = g5x_tilt(yz, zx); +} + // emccanon recomputes the CSS factor on every spindle-speed command, out of // the program units in force at that moment (emccanon.cc SPINDLE_SPEED_), so // this does too. S is surface feet per minute under G20 and metres per minute @@ -914,6 +919,7 @@ void GCodeRenderer::chain(const WorkFrame &frame, const Point9 &in, out = in; frame.apply(out); out += g92_; + tilt_.apply(out); if(rotation_xy_ != 0.0) { double rotx = out[P9_X] * rotation_cos_ - out[P9_Y] * rotation_sin_; out[P9_Y] = out[P9_X] * rotation_sin_ + out[P9_Y] * rotation_cos_; @@ -931,6 +937,7 @@ void GCodeRenderer::untransform(const Point9 &in, Point9 &out) const { out[P9_Y] = out[P9_X] * unrot_sin_ + out[P9_Y] * unrot_cos_; out[P9_X] = rotx; } + tilt_.remove(out); out -= g92_; } @@ -1003,7 +1010,7 @@ void GCodeRenderer::mark(int line_number, const Point9 &at, void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2, const Point9 &d1, const Point9 &d2) { // With no rotation to remove, the zero_rxy kinds are the plain ones. - bool rotated = rotation_xy_ != 0.0; + bool rotated = rotation_xy_ != 0.0 || tilt_.active; // A pair's box, and the same with the rotation taken out. struct Boxes { Box3 box, rot; }; auto boxes = [&](const Point9 &a, const Point9 &b) { @@ -1044,14 +1051,20 @@ void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2, } } -// The g5x XY rotation taken back out about the g5x origin, for the -// rotation-removed extents. Z is left alone. +// The g5x rotation taken back out about the g5x origin, for the +// rotation-removed extents. Z is left alone unless the system is also +// turned about X or Y. void GCodeRenderer::unrotate_xy(const Point9 &p, Point3 &out) const { double tx = p[P9_X] - g5x_[P9_X]; double ty = p[P9_Y] - g5x_[P9_Y]; - out[P3_X] = tx * unrot_cos_ - ty * unrot_sin_ + g5x_[P9_X]; - out[P3_Y] = tx * unrot_sin_ + ty * unrot_cos_ + g5x_[P9_Y]; - out[P3_Z] = p[P9_Z]; + Point9 t = {}; + t[P9_X] = tx * unrot_cos_ - ty * unrot_sin_; + t[P9_Y] = tx * unrot_sin_ + ty * unrot_cos_; + t[P9_Z] = p[P9_Z] - g5x_[P9_Z]; + tilt_.remove(t); + out[P3_X] = t[P9_X] + g5x_[P9_X]; + out[P3_Y] = t[P9_Y] + g5x_[P9_Y]; + out[P3_Z] = tilt_.active ? t[P9_Z] + g5x_[P9_Z] : p[P9_Z]; } void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, @@ -1371,7 +1384,7 @@ void GCodeRenderer::render_arc(int line_number, double first_end, double second_ if(suppress_ > 0) return; double radius = 0.0; arc_segments(lo_, plane_, rotation_cos_, rotation_sin_, - g5x_, g92_, frame_, first_end, second_end, + g5x_, g92_, frame_, tilt_, first_end, second_end, first_axis, second_axis, rotation, axis_end_point, a, b, c, u, v, w, arcdivision_, segs_, &radius); @@ -1426,7 +1439,7 @@ static void rotate(double &x, double &y, double c, double s) { int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, - const WorkFrame &frame, + const WorkFrame &frame, const WorkFrame &tilt, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -1452,6 +1465,7 @@ int arc_segments(const Point9 &lo, int plane, n[P9_W] = w; o -= g5xoffset; unrotate(o[P9_X], o[P9_Y], rotation_cos, rotation_sin); + tilt.remove(o); o -= g92offset; // the tilted work plane sits inside G92: off the last point on the way // in, back on every point on the way out @@ -1497,11 +1511,13 @@ int arc_segments(const Point9 &lo, int plane, for(int j = P9_A; j < P9_COUNT; j++) p[j] = o[j] + d[j] * f; frame.apply(p); p += g92offset; + tilt.apply(p); rotate(p[P9_X], p[P9_Y], rotation_cos, rotation_sin); p += g5xoffset; } frame.apply(n); n += g92offset; + tilt.apply(n); rotate(n[P9_X], n[P9_Y], rotation_cos, rotation_sin); n += g5xoffset; out[(size_t)steps - 1] = n; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 6f53886f0fc..6360287f483 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -246,7 +246,7 @@ void renderer_canon_register(pybind11::module_ &m); int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, - const WorkFrame &frame, + const WorkFrame &frame, const WorkFrame &tilt, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -415,6 +415,7 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; + void set_yz_zx_rotation(double yz, double zx) override; // The moves go through the frame canon applies; the plane is drawn as // the program defined it, which set_g68_drawn() brings. void set_g68_frame(const WorkFrame &frame) override { @@ -573,6 +574,7 @@ private: Point9 g92_ = {}; Point9 g5x_ = {}; double rotation_xy_ = 0.0; + WorkFrame tilt_; // the turn about X and Y before it double rotation_cos_ = 1.0; double rotation_sin_ = 0.0; double unrot_cos_ = 1.0; // the same rotation, negated, for the diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index ce96f8a2dd1..a7e725d86ba 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -368,6 +368,10 @@ class CallbackCanon final : public Canon { maybe_new_line(); forward("set_xy_rotation", degrees); } + void set_yz_zx_rotation(double yz, double zx) override { + maybe_new_line(); + forward("set_yz_zx_rotation", yz, zx); + } void set_g68_frame(const WorkFrame &f) override { maybe_new_line(); const std::array &r = f.rotation; @@ -955,6 +959,10 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; +void SET_YZ_ZX_ROTATION(double yz, double zx) { + parse_state.canon->set_yz_zx_rotation(yz, zx); +} + // The plane canon applies, which carries the moves onto the slides, and the // plane as defined, which the renderer draws on the part: under the machine // frame kinematics the one has the second feature coordinate system folded @@ -1816,10 +1824,18 @@ static py::list rs274_arc_to_segments(py::handle canon, for(size_t i=0; i<3; i++) frame.origin[i] = origin[i].cast(); for(size_t i=0; i<9; i++) frame.rotation[i] = rotation[i].cast(); } + // The coordinate system's turn about X and Y, likewise. + WorkFrame tilt; + if(py::hasattr(canon, "g5x_tilt")) { + py::sequence t = canon.attr("g5x_tilt").cast(); + if(py::len(t) != 2) + throw py::value_error("arc_to_segments: canon.g5x_tilt is two numbers"); + tilt = g5x_tilt(t[0].cast(), t[1].cast()); + } std::vector pts; int steps = arc_segments(o, plane, rotation_cos, rotation_sin, - g5xoffset, g92offset, frame, + g5xoffset, g92offset, frame, tilt, x1, y1, cx, cy, rot, z1, a, b, c, u, v, w, max_segments, pts); py::list segs(steps); diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index da381c5b39d..f2357dbd67c 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -123,6 +124,20 @@ struct WorkFrame { } }; +// The coordinate system turned about X and then Y (G10 L2 I J), which +// comes before its turn about Z: g5x + Rz * tilt * (g92 + ...). A frame with +// no origin, inactive when both angles are 0. +inline WorkFrame g5x_tilt(double yz, double zx) { + WorkFrame f; + double a = yz * (M_PI / 180.0), b = zx * (M_PI / 180.0); + double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b); + f.active = yz != 0.0 || zx != 0.0; + f.rotation = {cb, sb * sa, sb * ca, + 0.0, ca, -sa, + -sb, cb * sa, cb * ca}; + return f; +} + // --------------------------------------------------------------------------- // The canon protocol // --------------------------------------------------------------------------- @@ -182,6 +197,7 @@ public: virtual void set_g5x_offset(int index, const Point9 &offsets) = 0; virtual void set_g92_offset(const Point9 &offsets) = 0; virtual void set_xy_rotation(double degrees) = 0; + virtual void set_yz_zx_rotation(double yz, double zx) = 0; // The tilted work plane, or its cancellation; the origin already in // inches. virtual void set_g68_frame(const WorkFrame &frame) = 0; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 31ac55ecb39..52a2f1c4674 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -191,39 +191,39 @@ const int Interp::required_parameters[] = { 5221, 5222, 5223, /* coordinate system 1 */ 5224, 5225, 5226, /* A, B, & C */ 5227, 5228, 5229, /* U, V, & W */ - 5230, + 5230, 5231, 5232, /* R, I & J: the rotation about Z, X & Y */ 5241, 5242, 5243, /* coordinate system 2 */ 5244, 5245, 5246, /* A, B, & C */ 5247, 5248, 5249, /* U, V, & W */ - 5250, + 5250, 5251, 5252, /* R, I & J: the rotation about Z, X & Y */ 5261, 5262, 5263, /* coordinate system 3 */ 5264, 5265, 5266, /* A, B, & C */ 5267, 5268, 5269, /* U, V, & W */ - 5270, + 5270, 5271, 5272, /* R, I & J: the rotation about Z, X & Y */ 5281, 5282, 5283, /* coordinate system 4 */ 5284, 5285, 5286, /* A, B, & C */ 5287, 5288, 5289, /* U, V, & W */ - 5290, + 5290, 5291, 5292, /* R, I & J: the rotation about Z, X & Y */ 5301, 5302, 5303, /* coordinate system 5 */ 5304, 5305, 5306, /* A, B, & C */ 5307, 5308, 5309, /* U, V, & W */ - 5310, + 5310, 5311, 5312, /* R, I & J: the rotation about Z, X & Y */ 5321, 5322, 5323, /* coordinate system 6 */ 5324, 5325, 5326, /* A, B, & C */ 5327, 5328, 5329, /* U, V, & W */ - 5330, + 5330, 5331, 5332, /* R, I & J: the rotation about Z, X & Y */ 5341, 5342, 5343, /* coordinate system 7 */ 5344, 5345, 5346, /* A, B, & C */ 5347, 5348, 5349, /* U, V, & W */ - 5350, + 5350, 5351, 5352, /* R, I & J: the rotation about Z, X & Y */ 5361, 5362, 5363, /* coordinate system 8 */ 5364, 5365, 5366, /* A, B, & C */ 5367, 5368, 5369, /* U, V, & W */ - 5370, + 5370, 5371, 5372, /* R, I & J: the rotation about Z, X & Y */ 5381, 5382, 5383, /* coordinate system 9 */ 5384, 5385, 5386, /* A, B, & C */ 5387, 5388, 5389, /* U, V, & W */ - 5390, + 5390, 5391, 5392, /* R, I & J: the rotation about Z, X & Y */ RS274NGC_MAX_PARAMETERS }; diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 6db0f3c293d..4dde03d4f02 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -360,8 +360,9 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && - !plane_words), - _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); + !plane_words && + !(block->g_modes[GM_MODAL_0] == G_10 && (block->l_number == 2 || block->l_number == 20))), + _("K word with no G2, G3, G6.2, G10 L2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->l_number != -1) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 431134e1d64..084ead350eb 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -2470,7 +2470,7 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->u_origin_offset = USER_TO_PROGRAM_AX(AXIS_U, parameters[5207 + (origin * 20)]); settings->v_origin_offset = USER_TO_PROGRAM_AX(AXIS_V, parameters[5208 + (origin * 20)]); settings->w_origin_offset = USER_TO_PROGRAM_AX(AXIS_W, parameters[5209 + (origin * 20)]); - settings->rotation_xy = parameters[5210 + (origin * 20)]; + g5x_rotation_load(settings); if (settings->g68_active) { world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, &settings->current_y, &settings->current_z); @@ -2497,7 +2497,7 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->v_axis_offset, settings->w_axis_offset); - SET_XY_ROTATION(settings->rotation_xy); + g5x_rotation_send(settings); // canon's plane has the second feature coordinate system folded in, over // the offsets that just changed if (settings->g68_snap_active) { work_plane_send(settings); } @@ -4828,6 +4828,12 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { int to_fixture = block->l_number == 11; int destination_system = to_fixture? 9 : settings->origin_index; // maybe 9 (g59.3) should be user configurable? + // the tool offset is the machine's, along its axes: a system + // turned about X or Y would tip a length into X and Y + CHKS((g5x_tilted(settings, destination_system)), + _("Cannot use G10 L%d with coordinate system %d turned about X or Y (G10 L2 I J), " + "set the tool offset with G10 L1"), block->l_number, destination_system); + find_current_in_system_without_tlo(settings, destination_system, &tx, &ty, &tz, &ta, &tb, &tc, @@ -5032,7 +5038,6 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 double b; double c; double u, v, w; - double r; CHKS((settings->g68_active), _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); @@ -5045,7 +5050,11 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 double cx, cy, cz, ca, cb, cc, cu, cv, cw; - CHKS((block->i_flag || block->j_flag), _("I J words not allowed with G10 L2")); + // I J K turn the system about X, Y and Z, K the same as R + CHKS((block->l_number == 20 && (block->i_flag || block->j_flag || block->k_flag)), + _("I J K not allowed in G10 L20")); + CHKS((block->r_flag && block->k_flag), + _("G10 L2 takes the rotation about Z as R or as K, not both")); parameters = settings->parameters; p_int = (int) (block->p_number + 0.0001); @@ -5082,46 +5091,39 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 &ca, &cb, &cc, &cu, &cv, &cw); + // the tool stays where it is: its X Y Z go round the chain, before the + // change and back after it + bool in_use = p_int == settings->origin_index; + double wx = 0, wy = 0, wz = 0; + if (in_use) { + program_to_world_xyz(settings, settings->current_x, settings->current_y, + settings->current_z, &wx, &wy, &wz); + } + if (block->r_flag) { CHKS((block->l_number == 20), _("R not allowed in G10 L20")); - r = block->r_number; - parameters[5210 + (p_int * 20)] = r; - } else - r = parameters[5210 + (p_int * 20)]; + parameters[5210 + (p_int * 20)] = block->r_number; + } else if (block->k_flag) { + parameters[5210 + (p_int * 20)] = block->k_number; + } + if (block->i_flag) { parameters[5211 + (p_int * 20)] = block->i_number; } + if (block->j_flag) { parameters[5212 + (p_int * 20)] = block->j_number; } if (block->l_number == 20) { - // old position in rotated system - double oldx = cx, oldy = cy; - - // find new desired position in rotated system - x = cx; - y = cy; - - if (block->x_flag) { - x = block->x_number; - } - if (block->y_flag) { - y = block->y_number; - } - - // move old current position into the unrotated system - rotate(&oldx, &oldy, r); - // move desired position into the unrotated system - rotate(&x, &y, r); - - // find new offset - x = oldx + USER_TO_PROGRAM_LEN(parameters[5201 + (p_int * 20)]) - x; - y = oldy + USER_TO_PROGRAM_LEN(parameters[5202 + (p_int * 20)]) - y; - - // parameters are not rotated - parameters[5201 + (p_int * 20)] = PROGRAM_TO_USER_LEN(x); - parameters[5202 + (p_int * 20)] = PROGRAM_TO_USER_LEN(y); - - if (p_int == settings->origin_index) { - // let the code below fix up the current coordinates correctly - rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); - settings->rotation_xy = 0; - } + // the shift that puts the current point at the coordinates given, + // seen from the system and turned by its rotation; X Y Z not given + // keep theirs + double d[3] = { block->x_flag ? cx - block->x_number : 0.0, + block->y_flag ? cy - block->y_number : 0.0, + block->z_flag ? cz - block->z_number : 0.0 }; + + g5x_rotate(settings, p_int, &d[0], &d[1], &d[2]); + x = USER_TO_PROGRAM_LEN(parameters[5201 + (p_int * 20)]) + d[0]; + y = USER_TO_PROGRAM_LEN(parameters[5202 + (p_int * 20)]) + d[1]; + z = USER_TO_PROGRAM_LEN(parameters[5203 + (p_int * 20)]) + d[2]; + if (d[0] != 0.0) { parameters[5201 + (p_int * 20)] = PROGRAM_TO_USER_LEN(x); } + if (d[1] != 0.0) { parameters[5202 + (p_int * 20)] = PROGRAM_TO_USER_LEN(y); } + if (d[2] != 0.0) { parameters[5203 + (p_int * 20)] = PROGRAM_TO_USER_LEN(z); } } else { if (block->x_flag) { x = block->x_number; @@ -5135,15 +5137,14 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 } else { y = USER_TO_PROGRAM_LEN(parameters[5202 + (p_int * 20)]); } + if (block->z_flag) { + z = block->z_number; + parameters[5203 + (p_int * 20)] = PROGRAM_TO_USER_LEN(z); + } else { + z = USER_TO_PROGRAM_LEN(parameters[5203 + (p_int * 20)]); + } } - if (block->z_flag) { - z = block->z_number; - if (block->l_number == 20) z = cz + USER_TO_PROGRAM_LEN(parameters[5203 + (p_int * 20)]) - z; - parameters[5203 + (p_int * 20)] = PROGRAM_TO_USER_LEN(z); - } else - z = USER_TO_PROGRAM_LEN(parameters[5203 + (p_int * 20)]); - if (block->a_flag) { a = block->a_number; if (block->l_number == 20) a = ca + USER_TO_PROGRAM_AX(AXIS_A, parameters[5204 + (p_int * 20)]) - a; @@ -5186,13 +5187,7 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 } else w = USER_TO_PROGRAM_AX(AXIS_W, parameters[5209 + (p_int * 20)]); - if (p_int == settings->origin_index) { /* system is currently used */ - - rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); - - settings->current_x += settings->origin_offset_x; - settings->current_y += settings->origin_offset_y; - settings->current_z += settings->origin_offset_z; + if (in_use) { settings->AA_current += settings->AA_origin_offset; settings->BB_current += settings->BB_origin_offset; settings->CC_current += settings->CC_origin_offset; @@ -5210,9 +5205,6 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 settings->v_origin_offset = v; settings->w_origin_offset = w; - settings->current_x -= x; - settings->current_y -= y; - settings->current_z -= z; settings->AA_current -= a; settings->BB_current -= b; settings->CC_current -= c; @@ -5220,11 +5212,12 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 settings->v_current -= v; settings->w_current -= w; - SET_G5X_OFFSET(p_int, x, y, z, a, b, c, u, v, w); + g5x_rotation_load(settings); + world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, + &settings->current_y, &settings->current_z); - rotate(&settings->current_x, &settings->current_y, - r); - settings->rotation_xy = r; - SET_XY_ROTATION(settings->rotation_xy); + SET_G5X_OFFSET(p_int, x, y, z, a, b, c, u, v, w); + g5x_rotation_send(settings); } #ifdef DEBUG_EMC @@ -5556,7 +5549,7 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 settings->u_origin_offset = USER_TO_PROGRAM_AX(AXIS_U, settings->parameters[5227]); settings->v_origin_offset = USER_TO_PROGRAM_AX(AXIS_V, settings->parameters[5228]); settings->w_origin_offset = USER_TO_PROGRAM_AX(AXIS_W, settings->parameters[5229]); - settings->rotation_xy = settings->parameters[5230]; + g5x_rotation_load(settings); settings->current_x -= settings->origin_offset_x; settings->current_y -= settings->origin_offset_y; @@ -5579,7 +5572,7 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 settings->u_origin_offset, settings->v_origin_offset, settings->w_origin_offset); - SET_XY_ROTATION(settings->rotation_xy); + g5x_rotation_send(settings); } /*2*/ if (settings->plane != CANON_PLANE::XY) { @@ -6939,13 +6932,13 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu USE_TOOL_LENGTH_OFFSET(tool_offset); - // by the offset difference seen from the program: the XY rotation - // and the tilted work plane taken off it + // by the offset difference seen from the program: the rotation of + // the coordinate system and the tilted work plane taken off it dx = settings->tool_offset.tran.x - tool_offset.tran.x; dy = settings->tool_offset.tran.y - tool_offset.tran.y; dz = settings->tool_offset.tran.z - tool_offset.tran.z; - rotate(&dx, &dy, -settings->rotation_xy); + g5x_unrotate(settings, 0, &dx, &dy, &dz); g68_unrotate(settings, &dx, &dy, &dz); settings->current_x += dx; diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 758e0fd62dc..d6d6acb7c6a 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -615,7 +615,7 @@ int Interp::find_current_in_system(setup_pointer s, int system, *v += s->v_axis_offset; *w += s->w_axis_offset; - rotate(x, y, s->rotation_xy); + g5x_rotate(s, 0, x, y, z); *x += s->origin_offset_x; *y += s->origin_offset_y; @@ -637,7 +637,7 @@ int Interp::find_current_in_system(setup_pointer s, int system, *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5208 + system * 20]); *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5209 + system * 20]); - rotate(x, y, -p[5210 + system * 20]); + g5x_unrotate(s, system, x, y, z); if (p[5210]) { *x -= USER_TO_PROGRAM_LEN(p[5211]); @@ -684,7 +684,7 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *v += s->v_axis_offset; *w += s->w_axis_offset; - rotate(x, y, s->rotation_xy); + g5x_rotate(s, 0, x, y, z); *x += s->origin_offset_x; *y += s->origin_offset_y; @@ -716,7 +716,7 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5208 + system * 20]); *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5209 + system * 20]); - rotate(x, y, -p[5210 + system * 20]); + g5x_unrotate(s, system, x, y, z); if (p[5210]) { *x -= USER_TO_PROGRAM_LEN(p[5211]); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index b9ebed90451..39ca549092a 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -796,6 +796,9 @@ struct setup double origin_offset_y; // g5x offset y double origin_offset_z; // g5x offset z double rotation_xy; // rotation of coordinate system around Z, in degrees + double rotation_yz; // and around X, applied first + double rotation_zx; // and around Y, applied second + double rotation_sent[2]; // rotation_yz and rotation_zx as canon last heard them // the tilted work plane (G68.2): a frame inside G92, program units, // in the active coordinate system bool g68_active; diff --git a/src/emc/rs274ngc/interp_parameter_def.hh b/src/emc/rs274ngc/interp_parameter_def.hh index d0c81d3f872..45090595d00 100644 --- a/src/emc/rs274ngc/interp_parameter_def.hh +++ b/src/emc/rs274ngc/interp_parameter_def.hh @@ -83,6 +83,7 @@ enum InterpParameterIndex { // 5341-5350 - Coordinate System 7, G59.1 for X, Y, Z, A, B, C, U, V, W & R. Persistent. // 5361-5370 - Coordinate System 8, G59.2 for X, Y, Z, A, B, C, U, V, W & R. Persistent. // 5381-5390 - Coordinate System 9, G59.3 for X, Y, Z, A, B, C, U, V, W & R. Persistent. +// 5x11 and 5x12 after each of them - its rotation about X and about Y (G10 L2 I J). Persistent. ACTIVE_WORK_CSYS=5220, G54_X=5221, G54_Y, diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 0fa8e36257b..3f8c122ed68 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,9 @@ setup::setup() : origin_offset_y (0.0), origin_offset_z (0.0), rotation_xy (0.0), + rotation_yz (0.0), + rotation_zx (0.0), + rotation_sent{0.0, 0.0}, g68_active(false), g68_code(0), g68_offset{0.0, 0.0, 0.0}, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 7840e343194..177b6a64229 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -6,10 +6,11 @@ * * The chain, as canon applies it: * -* world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * (L + program)) +* world = TLO + G5x + Rg * (G92 + O + R * (L + program)) * +* Rg is the rotation of the coordinate system, G10 L2 I J K (R for K). * O and R are the plane's origin and rotation, expressed in the active -* coordinate system: G5x with G92 and the XY rotation in place, which +* coordinate system: G5x with G92 and its rotation in place, which * is what the operator sees on the display and what G68.2 X Y Z means * on every control. A change of G5x takes the plane along, as Fanuc's * parameter 3TW does. @@ -113,6 +114,94 @@ static void mat_from_axes(const double x[3], const double y[3], const double z[3 for (int i = 0; i < 3; i++) { m[i][0] = x[i]; m[i][1] = y[i]; m[i][2] = z[i]; } } +//---------------------------------------------------------------------- +// the rotation of a coordinate system +//---------------------------------------------------------------------- + +// G10 L2 I J K (or R for K) turn a coordinate system about X, then Y, then +// Z, fixed axes, as G68.2 P1 Q123 turns a plane: Rz(K) * Ry(J) * Rx(I). +// Heidenhain's 3-D basic rotation (SPA SPB SPC) and Siemens' work offset +// rotations (ROT X Y Z) are the same turn. System 0 is the rotation in +// effect, 1 to 9 the one stored for G54 to G59.3: R at #5x10, I at #5x11 +// and J at #5x12. +static void g5x_angles(setup_pointer s, int system, double *yz, double *zx, double *xy) +{ + if (system == 0) { + *yz = s->rotation_yz; + *zx = s->rotation_zx; + *xy = s->rotation_xy; + } else { + *yz = s->parameters[5211 + system * 20]; + *zx = s->parameters[5212 + system * 20]; + *xy = s->parameters[5210 + system * 20]; + } +} + +static void g5x_matrix(double yz, double zx, double xy, double m[3][3]) +{ + double rx[3][3], ry[3][3], rz[3][3]; + + mat_rotation(1, yz, rx); + mat_rotation(2, zx, ry); + mat_rotation(3, xy, rz); + mat_mul(ry, rx, m); + mat_mul(rz, m, m); +} + +bool Interp::g5x_tilted(setup_pointer s, int system) +{ + double yz, zx, xy; + + g5x_angles(s, system, &yz, &zx, &xy); + return yz != 0.0 || zx != 0.0; +} + +// Without I and J this is the rotation about Z alone, computed as it +// always was, so a program without them gives the same numbers. +void Interp::g5x_rotate(setup_pointer s, int system, double *x, double *y, double *z) +{ + double yz, zx, xy, m[3][3]; + + g5x_angles(s, system, &yz, &zx, &xy); + if (yz == 0.0 && zx == 0.0) { + rotate(x, y, xy); + return; + } + g5x_matrix(yz, zx, xy, m); + mat_apply(m, x, y, z); +} + +void Interp::g5x_unrotate(setup_pointer s, int system, double *x, double *y, double *z) +{ + double yz, zx, xy, m[3][3]; + + g5x_angles(s, system, &yz, &zx, &xy); + if (yz == 0.0 && zx == 0.0) { + rotate(x, y, -xy); + return; + } + g5x_matrix(yz, zx, xy, m); + mat_apply_transposed(m, x, y, z); +} + +// the rotation of the active system, from its parameters +void Interp::g5x_rotation_load(setup_pointer s) +{ + g5x_angles(s, s->origin_index, &s->rotation_yz, &s->rotation_zx, &s->rotation_xy); +} + +// Canon hears of I and J only when they change, so a program that never +// turns a system about X or Y talks to it as it always did. +void Interp::g5x_rotation_send(setup_pointer s) +{ + if (s->rotation_yz != s->rotation_sent[0] || s->rotation_zx != s->rotation_sent[1]) { + SET_YZ_ZX_ROTATION(s->rotation_yz, s->rotation_zx); + s->rotation_sent[0] = s->rotation_yz; + s->rotation_sent[1] = s->rotation_zx; + } + SET_XY_ROTATION(s->rotation_xy); +} + //---------------------------------------------------------------------- // the chain //---------------------------------------------------------------------- @@ -164,7 +253,7 @@ void Interp::program_to_world_xyz(setup_pointer s, x += s->axis_offset_x; y += s->axis_offset_y; z += s->axis_offset_z; - rotate(&x, &y, s->rotation_xy); + g5x_rotate(s, 0, &x, &y, &z); x += s->origin_offset_x + s->tool_offset.tran.x; y += s->origin_offset_y + s->tool_offset.tran.y; z += s->origin_offset_z + s->tool_offset.tran.z; @@ -194,7 +283,7 @@ void Interp::world_to_program_xyz(setup_pointer s, double y = wy - s->origin_offset_y - s->tool_offset.tran.y; double z = wz - s->origin_offset_z - s->tool_offset.tran.z; - rotate(&x, &y, -s->rotation_xy); + g5x_unrotate(s, 0, &x, &y, &z); x -= s->axis_offset_x; y -= s->axis_offset_y; z -= s->axis_offset_z; @@ -238,7 +327,7 @@ void Interp::work_plane_sent(setup_pointer s, double o[3], double flat[9]) x -= s->origin_offset_x + s->tool_offset.tran.x; y -= s->origin_offset_y + s->tool_offset.tran.y; z -= s->origin_offset_z + s->tool_offset.tran.z; - rotate(&x, &y, -s->rotation_xy); + g5x_unrotate(s, 0, &x, &y, &z); v[k][0] = x - s->axis_offset_x; v[k][1] = y - s->axis_offset_y; v[k][2] = z - s->axis_offset_z; @@ -1239,29 +1328,29 @@ int Interp::snap_after_switch(setup_pointer s, const double *joints, bool have) } // a direction the program gives, in world coordinates: through the -// tilted work plane's rotation where one is active, then the XY rotation +// tilted work plane's rotation where one is active, then the rotation // of the coordinate system it sits on -static void direction_in_world(setup_pointer s, const double v[3], double rotation_xy, PmCartesian *out) +static void direction_in_world(setup_pointer s, const double v[3], PmCartesian *out) { double x = v[0], y = v[1], z = v[2]; - double t = rotation_xy * M_PI / 180.0; if (s->g68_active) { x = s->g68_rotation[0][0] * v[0] + s->g68_rotation[0][1] * v[1] + s->g68_rotation[0][2] * v[2]; y = s->g68_rotation[1][0] * v[0] + s->g68_rotation[1][1] * v[1] + s->g68_rotation[1][2] * v[2]; z = s->g68_rotation[2][0] * v[0] + s->g68_rotation[2][1] * v[1] + s->g68_rotation[2][2] * v[2]; } - out->x = x * cos(t) - y * sin(t); - out->y = x * sin(t) + y * cos(t); + Interp::g5x_rotate(s, 0, &x, &y, &z); + out->x = x; + out->y = y; out->z = z; } // a direction of the plane in world coordinates -static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +static void plane_axis_in_world(setup_pointer s, int column, PmCartesian *out) { double v[3] = { column == 0 ? 1.0 : 0.0, column == 1 ? 1.0 : 0.0, column == 2 ? 1.0 : 0.0 }; - direction_in_world(s, v, rotation_xy, out); + direction_in_world(s, v, out); } static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) @@ -1343,15 +1432,15 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) pmCartCartCross(&zt, &x, &y); // from world directions to the system the plane is defined in: the - // XY rotation comes off + // rotation of the coordinate system comes off { PmCartesian cols[3] = { x, y, zt }; - double c = cos(-s->rotation_xy * M_PI / 180.0), sn = sin(-s->rotation_xy * M_PI / 180.0); for (int j = 0; j < 3; j++) { - rotation[0][j] = cols[j].x * c - cols[j].y * sn; - rotation[1][j] = cols[j].x * sn + cols[j].y * c; + rotation[0][j] = cols[j].x; + rotation[1][j] = cols[j].y; rotation[2][j] = cols[j].z; + g5x_unrotate(s, 0, &rotation[0][j], &rotation[1][j], &rotation[2][j]); } } origin[0] = block->x_flag ? block->x_number : 0.0; @@ -1419,8 +1508,8 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) int count = orienting_letters(ctx, now, orienting); solved = INTERP_OK; if (!identity && !(code == G_53_1 && by_pose)) { - plane_axis_in_world(s, 2, s->rotation_xy, &axis); - plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + plane_axis_in_world(s, 2, &axis); + plane_axis_in_world(s, 0, &xdir); solved = orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name); } kinematicsUserSetType(ctx, s->kins_type); @@ -1757,7 +1846,7 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotar axis.y = v[1]; axis.z = v[2]; } else { - direction_in_world(s, v, s->rotation_xy, &axis); + direction_in_world(s, v, &axis); } CHP(orient_solve(s, ctx, &axis, NULL, 0, 1, now, sol, "G43.5")); diff --git a/src/emc/rs274ngc/interpmodule.cc b/src/emc/rs274ngc/interpmodule.cc index 773a12e7065..ee70c37729f 100644 --- a/src/emc/rs274ngc/interpmodule.cc +++ b/src/emc/rs274ngc/interpmodule.cc @@ -533,6 +533,18 @@ static inline double get_rotation_xy (Interp &interp) { static inline void set_rotation_xy(Interp &interp, double value) { interp._setup.rotation_xy = value; } +static inline double get_rotation_yz (Interp &interp) { + return interp._setup.rotation_yz; +} +static inline void set_rotation_yz(Interp &interp, double value) { + interp._setup.rotation_yz = value; +} +static inline double get_rotation_zx (Interp &interp) { + return interp._setup.rotation_zx; +} +static inline void set_rotation_zx(Interp &interp, double value) { + interp._setup.rotation_zx = value; +} static inline double get_traverse_rate (Interp &interp) { return interp._setup.traverse_rate; } @@ -973,6 +985,8 @@ BOOST_PYTHON_MODULE(interpreter) { .add_property("program_z", &get_program_z, &set_program_z) .add_property("return_value", &get_return_value, &set_return_value) .add_property("rotation_xy", &get_rotation_xy, &set_rotation_xy) + .add_property("rotation_yz", &get_rotation_yz, &set_rotation_yz) + .add_property("rotation_zx", &get_rotation_zx, &set_rotation_zx) .add_property("traverse_rate", &get_traverse_rate, &set_traverse_rate) .add_property("u_axis_offset", &get_u_axis_offset, &set_u_axis_offset) .add_property("u_origin_offset", &get_u_origin_offset, &set_u_origin_offset) diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 6abfd18c8f4..f052ffd1cf1 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -424,6 +424,11 @@ public: void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); + static void g5x_rotate(setup_pointer settings, int system, double *x, double *y, double *z); + static void g5x_unrotate(setup_pointer settings, int system, double *x, double *y, double *z); + static bool g5x_tilted(setup_pointer settings, int system); + void g5x_rotation_load(setup_pointer settings); + void g5x_rotation_send(setup_pointer settings); void program_to_world_xyz(setup_pointer settings, double px, double py, double pz, double *wx, double *wy, double *wz); void world_to_program_xyz(setup_pointer settings, double wx, double wy, double wz, @@ -598,7 +603,7 @@ int read_dollar(char *line, int *counter, block_pointer block, double *parameters); int refresh_actual_position(setup_pointer settings); void get_abs_position(setup_pointer settings, double abs_pos[9]); - void rotate(double *x, double *y, double t); + static void rotate(double *x, double *y, double t); int set_probe_data(setup_pointer settings); int write_g_codes(block_pointer block, setup_pointer settings); int write_m_codes(block_pointer block, setup_pointer settings); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index f5bfaf9558c..2e840145843 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1218,8 +1218,10 @@ int Interp::init() _setup.v_axis_offset , _setup.w_axis_offset); - _setup.rotation_xy = pars[k+10]; - SET_XY_ROTATION(pars[k+10]); + g5x_rotation_load(&_setup); + // INIT_CANON above left canon with no rotation + _setup.rotation_sent[0] = _setup.rotation_sent[1] = 0.0; + g5x_rotation_send(&_setup); SET_FEED_REFERENCE(CANON_XYZ); //_setup.active_g_codes initialized below //_setup.active_m_codes initialized below diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index d8d4709f767..316638daef2 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -108,6 +108,10 @@ void SET_XY_ROTATION(double t) { ECHO_WITH_ARGS("{:.4f}", t); } +void SET_YZ_ZX_ROTATION(double yz, double zx) { + ECHO_WITH_ARGS("{:.4f}, {:.4f}", yz, zx); +} + void HOME_CYCLE(void) { ECHO_WITH_ARGS(""); } void HOME_CYCLE_JOINT(int joint) { ECHO_WITH_ARGS("{}", joint); } diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 492a2292bd4..33134a65601 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -235,14 +235,43 @@ static void g68_rotate(double &x, double &y, double &z) { z = r[6]*px + r[7]*py + r[8]*pz; } +// The rotation of the coordinate system: about Z alone as it always was, +// or the matrix where it is also turned about X or Y. +static bool g5x_tilted() { + return canon.yz_rotation != 0.0 || canon.zx_rotation != 0.0; +} + +static void g5x_rotate(double &x, double &y, double &z) { + if (!g5x_tilted()) { + rotate(x, y, canon.xy_rotation); + return; + } + const double *r = canon.g5xRotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz; + y = r[3]*px + r[4]*py + r[5]*pz; + z = r[6]*px + r[7]*py + r[8]*pz; +} + +static void g5x_unrotate(double &x, double &y, double &z) { + if (!g5x_tilted()) { + rotate(x, y, -canon.xy_rotation); + return; + } + const double *r = canon.g5xRotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[3]*py + r[6]*pz; + y = r[1]*px + r[4]*py + r[7]*pz; + z = r[2]*px + r[5]*py + r[8]*pz; +} + /** * Rotation of a direction vector into the world frame: the tilted work - * plane first, then the planar rotation about Z. - * The use of static "xy_rotation" is ugly here, but is at least consistent. + * plane first, then the rotation of the coordinate system. */ static void to_rotated(PM_CARTESIAN &vec) { g68_rotate(vec.x, vec.y, vec.z); - rotate(vec.x,vec.y,canon.xy_rotation); + g5x_rotate(vec.x, vec.y, vec.z); } #if 0 static void from_rotated(PM_CARTESIAN &vec) { @@ -255,7 +284,7 @@ static void rotate_and_offset(CANON_POSITION & pos) { pos += canon.g92Offset; - rotate(pos.x, pos.y, canon.xy_rotation); + g5x_rotate(pos.x, pos.y, pos.z); pos += canon.g5xOffset; @@ -268,7 +297,7 @@ static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) { xyz += canon.g92Offset.xyz(); - rotate(xyz.x, xyz.y, canon.xy_rotation); + g5x_rotate(xyz.x, xyz.y, xyz.z); xyz += canon.g5xOffset.xyz(); @@ -286,7 +315,7 @@ static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION& pos) { res -= canon.g5xOffset; - rotate(res.x, res.y, -canon.xy_rotation); + g5x_unrotate(res.x, res.y, res.z); res -= canon.g92Offset; @@ -308,7 +337,7 @@ static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, do v += canon.g92Offset.v; w += canon.g92Offset.w; - rotate(x, y, canon.xy_rotation); + g5x_rotate(x, y, z); x += canon.g5xOffset.x; y += canon.g5xOffset.y; @@ -534,12 +563,34 @@ static void send_g92_msg(void) { interp_list.append(std::move(set_g92_msg)); } -void SET_XY_ROTATION(double t) { +// Status has the three angles; canon the matrix they make, +// Rz(xy) * Ry(zx) * Rx(yz). +static void send_rotation() { + double a = D2R(canon.yz_rotation), b = D2R(canon.zx_rotation), c = D2R(canon.xy_rotation); + double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b), cc = cos(c), sc = sin(c); + double *r = canon.g5xRotation; + + r[0] = cc*cb; r[1] = cc*sb*sa - sc*ca; r[2] = cc*sb*ca + sc*sa; + r[3] = sc*cb; r[4] = sc*sb*sa + cc*ca; r[5] = sc*sb*ca - cc*sa; + r[6] = -sb; r[7] = cb*sa; r[8] = cb*ca; + auto sr = std::make_unique(); - sr->rotation = t; + sr->rotation = canon.xy_rotation; + sr->rotation_yz = canon.yz_rotation; + sr->rotation_zx = canon.zx_rotation; interp_list.append(std::move(sr)); +} +void SET_XY_ROTATION(double t) { canon.xy_rotation = t; + send_rotation(); +} + +void SET_YZ_ZX_ROTATION(double yz, double zx) { + flush_segments(); + canon.yz_rotation = yz; + canon.zx_rotation = zx; + send_rotation(); } @@ -3005,7 +3056,7 @@ void ARC_FEED(int line_number, // the naive cam detector works on the world XY projection of the arc, // which a tilted work plane takes out of the XY plane - if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS && !canon.g68Active) { + if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS && !canon.g68Active && !g5x_tilted()) { double mx, my; double lx, ly, lz; double unused = 0; @@ -3239,7 +3290,7 @@ void ARC_FEED(int line_number, double j2 = FROM_EXT_LEN(emcAxisGetMaxJerk(axis2)); double j_min = MIN(j1, j2); - if((canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) || canon.g68Active) { + if((canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) || canon.g68Active || g5x_tilted()) { // also consider the third plane's constraint, which may get // involved since we're rotated. @@ -3989,6 +4040,9 @@ void INIT_CANON() // initialize locals to original values canon.xy_rotation = 0.0; + canon.yz_rotation = 0.0; + canon.zx_rotation = 0.0; + for (int i = 0; i < 9; i++) { canon.g5xRotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } canon.g68Offset[0] = canon.g68Offset[1] = canon.g68Offset[2] = 0.0; for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } canon.g68Active = 0; @@ -4369,7 +4423,9 @@ int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]) if (fabs(g5x.tran.x - sg5x.tran.x) > 1e-9 || fabs(g5x.tran.y - sg5x.tran.y) > 1e-9 || fabs(g5x.tran.z - sg5x.tran.z) > 1e-9 || fabs(g92.tran.x - sg92.tran.x) > 1e-9 || fabs(g92.tran.y - sg92.tran.y) > 1e-9 || fabs(g92.tran.z - sg92.tran.z) > 1e-9 - || fabs(canon.xy_rotation - emcStatus->task.rotation_xy) > 1e-9) return 0; + || fabs(canon.xy_rotation - emcStatus->task.rotation_xy) > 1e-9 + || fabs(canon.yz_rotation - emcStatus->task.rotation_yz) > 1e-9 + || fabs(canon.zx_rotation - emcStatus->task.rotation_zx) > 1e-9) return 0; origin[0] = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->task.g68_offset.tran.x)); origin[1] = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->task.g68_offset.tran.y)); origin[2] = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->task.g68_offset.tran.z)); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index f95b813442c..8b5407469f0 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1714,24 +1714,30 @@ int emcTaskQueueCommand(std::unique_ptr &&cmd) } // The tilted work plane's axes in world coordinates, for motion to jog -// along: the plane's rotation turned by the XY rotation of the coordinate -// system it was defined in, as canon applies the two, and then by the -// second feature coordinate system where the plane is cut through one. -// Motion hears only of a change, so a program without a plane sends it -// nothing +// along: the plane's rotation turned by the rotation of the coordinate +// system it was defined in, Rz(xy) * Ry(zx) * Rx(yz) as canon applies it, +// and then by the second feature coordinate system where the plane is cut +// through one. Motion hears only of a change, so a program without a +// plane sends it nothing static int sendWorkPlane() { static double sent[9]; static int sent_active = 0; const double *g = emcStatus->task.g68_rotation; const double *k = emcStatus->task.g68_kins_rotation; - double t = emcStatus->task.rotation_xy * M_PI / 180.0, c = cos(t), s = sin(t), q[9], r[9]; + double q[9], r[9], m[9]; int active = emcStatus->task.g68_active, retval; + double a = emcStatus->task.rotation_yz * M_PI / 180.0, b = emcStatus->task.rotation_zx * M_PI / 180.0; + double c = emcStatus->task.rotation_xy * M_PI / 180.0; + double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b), cc = cos(c), sc = sin(c); - for (int j = 0; j < 3; j++) { - q[j] = c * g[j] - s * g[3 + j]; - q[3 + j] = s * g[j] + c * g[3 + j]; - q[6 + j] = g[6 + j]; + m[0] = cc*cb; m[1] = cc*sb*sa - sc*ca; m[2] = cc*sb*ca + sc*sa; + m[3] = sc*cb; m[4] = sc*sb*sa + cc*ca; m[5] = sc*sb*ca - cc*sa; + m[6] = -sb; m[7] = cb*sa; m[8] = cb*ca; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + q[3*i + j] = m[3*i] * g[j] + m[3*i + 1] * g[3 + j] + m[3*i + 2] * g[6 + j]; + } } for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -2064,10 +2070,14 @@ static int emcTaskIssueCommand(NMLmsg * cmd) break; } - case EMC_TRAJ_SET_ROTATION_TYPE: - emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; + case EMC_TRAJ_SET_ROTATION_TYPE: { + EMC_TRAJ_SET_ROTATION *rot = reinterpret_cast(cmd); + emcStatus->task.rotation_xy = rot->rotation; + emcStatus->task.rotation_yz = rot->rotation_yz; + emcStatus->task.rotation_zx = rot->rotation_zx; retval = sendWorkPlane(); break; + } case EMC_TRAJ_SET_G68_TYPE: { EMC_TRAJ_SET_G68 *g68 = reinterpret_cast(cmd); diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index c0b5a968ce9..4e7be5d2ad8 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1157,6 +1157,10 @@ static PyMemberDef Stat_members[] = { { "task_paused", T_INT, O(task.task_paused), READONLY, NULL}, { "input_timeout", T_BOOL, O(task.input_timeout), READONLY, NULL}, { "rotation_xy", T_DOUBLE, O(task.rotation_xy), READONLY, NULL}, + { "rotation_yz", T_DOUBLE, O(task.rotation_yz), READONLY, + "Rotation of the coordinate system about X, degrees, applied before rotation_zx and rotation_xy."}, + { "rotation_zx", T_DOUBLE, O(task.rotation_zx), READONLY, + "Rotation of the coordinate system about Y, degrees, applied after rotation_yz and before rotation_xy."}, { "g68_active", T_INT, O(task.g68_active), READONLY, "A tilted work plane (G68.2) is in effect."}, { "ini_filename", T_STRING_INPLACE, O(task.ini_filename), READONLY, NULL}, { "delay_left", T_DOUBLE, O(task.delayLeft), READONLY, NULL}, diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index bf43d3d7a2f..db8c376ac8d 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1618,6 +1618,7 @@ def next_line(*args): pass def set_g5x_offset(*args): pass def set_g92_offset(*args): pass def set_xy_rotation(*args): pass + def set_yz_zx_rotation(*args): pass def set_g68_frame(*args): pass def get_external_angular_units(self): return 1.0 def get_external_length_units(self): return 1.0 diff --git a/src/emc/usr_intf/gremlin/qt5_graphics.py b/src/emc/usr_intf/gremlin/qt5_graphics.py index c4e14cb2c13..2549c9341d4 100644 --- a/src/emc/usr_intf/gremlin/qt5_graphics.py +++ b/src/emc/usr_intf/gremlin/qt5_graphics.py @@ -787,6 +787,10 @@ def dro_format(self,s,spd,dtg,limit,homed,positions,axisdtg,g5x_offset,g92_offse if s.axis_mask & (1<task.rotation_xy * TO_RAD; double x = rx * cos(t) - ry * sin(t); double y = ry * cos(t) + rx * sin(t); + double z = rz; + // a system also turned about X or Y (G10 L2 I J): the turn about Y + // comes off next, then the one about X + if (emcStatus->task.rotation_zx != 0.0 || emcStatus->task.rotation_yz != 0.0) { + double u; + t = -emcStatus->task.rotation_zx * TO_RAD; + u = x * cos(t) + z * sin(t); + z = z * cos(t) - x * sin(t); + x = u; + t = -emcStatus->task.rotation_yz * TO_RAD; + u = y * cos(t) - z * sin(t); + z = y * sin(t) + z * cos(t); + y = u; + } rx = x - emcStatus->task.g92_offset.tran.x; ry = y - emcStatus->task.g92_offset.tran.y; - rz -= emcStatus->task.g92_offset.tran.z; + rz = z - emcStatus->task.g92_offset.tran.z; } if (emcStatus->task.g68_active) { const double *r = emcStatus->task.g68_rotation; diff --git a/src/emc/usr_intf/touchy/emc_interface.py b/src/emc/usr_intf/touchy/emc_interface.py index 25ba4e8f4e0..dc250fedf19 100644 --- a/src/emc/usr_intf/touchy/emc_interface.py +++ b/src/emc/usr_intf/touchy/emc_interface.py @@ -341,6 +341,13 @@ def periodic(self): yr = x * math.sin(t) + y * math.cos(t) x = xr y = yr + # a system also turned about X or Y (G10 L2 I J): the turn about + # Y comes off next, then the one about X + if getattr(self.emcstat, 'rotation_zx', 0) or getattr(self.emcstat, 'rotation_yz', 0): + t = math.radians(-self.emcstat.rotation_zx) + x, z = x * math.cos(t) + z * math.sin(t), z * math.cos(t) - x * math.sin(t) + t = math.radians(-self.emcstat.rotation_yz) + y, z = y * math.cos(t) - z * math.sin(t), y * math.sin(t) + z * math.cos(t) x -= self.emcstat.g92_offset[0] y -= self.emcstat.g92_offset[1] diff --git a/tests/g5x-rotation/README b/tests/g5x-rotation/README new file mode 100644 index 00000000000..e3861fdc31f --- /dev/null +++ b/tests/g5x-rotation/README @@ -0,0 +1,18 @@ +A coordinate system turned about X, Y and Z with G10 L2 I J K, on an XYZ +machine (trivkins): the turn is Rz(K) * Ry(J) * Rx(I), K being R. + +- G54 turned 90 about Y puts program X1 Y2 Z3 where the rotation says; + status has the three angles and halui's relative position reads the + program point back. +- Changing the rotation leaves the tool where it is, the interpreter's + current point follows, and a G92 turns with the system. +- K is R; both on one line are refused. +- An arc in the program's XY plane on a system turned -90 about X stays + on its circle in the machine's XZ plane. +- A G68.2 plane sits on the turned system. +- G10 L20 Z0 moves the origin along the system's Z, machine Y here; + G10 L20 refuses I J K, G10 L10 refuses a system turned about X or Y. +- The preview renderer and the Python canon draw the points the machine + goes to. +- The rotation is saved in the var file (#5250 R, #5251 I, #5252 J for + G55) and comes back on the next start. diff --git a/tests/g5x-rotation/checkresult b/tests/g5x-rotation/checkresult new file mode 100755 index 00000000000..9d2246a5fb7 --- /dev/null +++ b/tests/g5x-rotation/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures, once per run +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/g5x-rotation/test-ui.py b/tests/g5x-rotation/test-ui.py new file mode 100755 index 00000000000..f5b6584caa7 --- /dev/null +++ b/tests/g5x-rotation/test-ui.py @@ -0,0 +1,321 @@ +#!/usr/bin/env python3 +# A coordinate system turned about X, Y and Z (G10 L2 I J K): see README. + +import linuxcnc +import hal +import math +import os +import sys +import time +import gcode +import preview_helpers +from rs274 import glcanon_bake +from rs274.interpret import Translated + +PHASE = os.environ.get("G5X_PHASE", "set") + +ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def wait_for(what, cond, timeout=60): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if cond(): + return True + time.sleep(0.02) + error("timed out waiting for %s" % what) + return False + +def settled(): + last = [None] + def still(): + now = tuple(s.joint_position[:3]) + done = s.inpos and not s.queue and now == last[0] + last[0] = now + return done + wait_for("the move to end", still) + +def mdi(cmd, expect=None): + print("mdi:", cmd) + c.mdi(cmd) + c.wait_complete(60) + if expect: + # a refused line reaches the error channel by itself + wait_for("the error for %r" % cmd, lambda: s.interp_state == linuxcnc.INTERP_IDLE) + deadline = time.time() + 10 + while True: + m = e.poll() + if m: + print("channel:", m) + if expect in m[1]: + break + error("reported: %s" % m[1]) + elif time.time() > deadline: + error("%r: no error saying %r" % (cmd, expect)) + break + else: + time.sleep(0.02) + settled() + drain() + +def close(a, b, tol=1e-6): + return max(abs(p - q) for p, q in zip(a, b)) < tol + +def machine(): + s.poll() + return [s.actual_position[i] for i in range(3)] + +def program(): + # the current point as the interpreter has it, #5420 to #5422 + c.mdi("(debug,here #5420 #5421 #5422)") + c.wait_complete(60) + deadline = time.time() + 10 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.02) + continue + print("channel:", m) + if m[1].startswith("here "): + return [float(v) for v in m[1].split()[1:4]] + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + error("no current point from the interpreter") + return [math.nan] * 3 + +def rot(yz, zx, xy, v): + # Rz(xy) * Ry(zx) * Rx(yz) * v, as the documentation has it + x, y, z = v + a, b, t = math.radians(yz), math.radians(zx), math.radians(xy) + y, z = y * math.cos(a) - z * math.sin(a), y * math.sin(a) + z * math.cos(a) + x, z = x * math.cos(b) + z * math.sin(b), z * math.cos(b) - x * math.sin(b) + x, y = x * math.cos(t) - y * math.sin(t), x * math.sin(t) + y * math.cos(t) + return [x, y, z] + +def unrot(yz, zx, xy, v): + # the rotation back: Rx(-yz) * Ry(-zx) * Rz(-xy) * v + return rot(-yz, 0, 0, rot(0, -zx, 0, rot(0, 0, -xy, v))) + +def world(origin, angles, p): + return [o + d for o, d in zip(origin, rot(*angles, p))] + +def relative(): + # halui's relative position, the DRO a panel reads + return [hal.get_value("halui.axis.%s.pos-relative" % a) for a in "xyz"] + +def check_relative(name, want): + if not wait_for("halui at %s" % want, lambda: close(relative(), want), timeout=10): + error("%s: halui reads %s, not %s" % (name, relative(), want)) + +def angles(): + s.poll() + return (s.rotation_yz, s.rotation_zx, s.rotation_xy) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +wait_for("homing", lambda: all(s.homed[:3])) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +ORIGIN = [10.0, 20.0, 30.0] + +if PHASE == "restore": + # the var file the first run left: G55 turned I-90 J0 R15 + mdi("G55") + if not close(angles(), (-90, 0, 15)): + error("G55 comes back turned %s, not (-90, 0, 15)" % (angles(),)) + mdi("G0 X1 Y2 Z3") + want = world(ORIGIN, (-90, 0, 15), (1, 2, 3)) + if not close(machine(), want): + error("after the restart G55 X1 Y2 Z3 is at %s, not %s" % (machine(), want)) + print("Exiting with %d errors" % errors) + sys.exit(0) + +# a turn about Y: program X lies along machine -Z +mdi("G10 L2 P1 X10 Y20 Z30 I0 J90 K0") +mdi("G54") +if not close(angles(), (0, 90, 0)): + error("status has the rotation %s, not (0, 90, 0)" % (angles(),)) +mdi("G0 X1 Y2 Z3") +want = world(ORIGIN, (0, 90, 0), (1, 2, 3)) +if not close(machine(), want): + error("J90: X1 Y2 Z3 is at %s, not %s" % (machine(), want)) +check_relative("J90", [1, 2, 3]) +if not close(program(), [1, 2, 3]): + error("J90: the interpreter has the current point at %s" % program()) + +# the tool stays where it is when the rotation changes, and its +# coordinates follow; a G92 sits inside the rotation and turns with it +mdi("G92 X0 Y0 Z0") +before = machine() +mdi("G10 L2 P1 J0 I30 R40") +if not close(machine(), before): + error("G10 L2 moved the machine from %s to %s" % (before, machine())) +want = [a - b for a, b in zip(unrot(30, 0, 40, [p - o for p, o in zip(before, ORIGIN)]), (1, 2, 3))] +if not close(program(), want): + error("after G10 L2 the interpreter has the point at %s, not %s" % (program(), want)) +check_relative("G10 L2 under G92", want) +mdi("G0 X0 Y0 Z0") +want = world(ORIGIN, (30, 0, 40), (1, 2, 3)) +if not close(machine(), want): + error("G92 zero on the turned system is at %s, not %s" % (machine(), want)) +mdi("G92.1") + +# K is R +mdi("G10 L2 P1 I0 J0 K25") +if not close(angles(), (0, 0, 25)): + error("K25 gives the rotation %s" % (angles(),)) +mdi("G10 L2 P1 R5 K5", expect="as R or as K, not both") + +# an arc in the program's XY plane on a system turned -90 about X lies in +# the machine's XZ plane, around its centre there +mdi("G10 L2 P1 X10 Y20 Z30 I-90 J0 K0") +mdi("G0 X0 Y0 Z0") +mdi("F100") +c.mdi("G2 X2 Y0 I1 J0") +samples = [] +end = world(ORIGIN, (-90, 0, 0), (2, 0, 0)) +def sample(): + p = machine() + samples.append(p) + return s.inpos and not s.queue and close(p, end) +wait_for("the arc", sample) +c.wait_complete(60) +drain() +centre = world(ORIGIN, (-90, 0, 0), (1, 0, 0)) +off_plane = max(abs(p[1] - centre[1]) for p in samples) +off_circle = max(abs(math.hypot(p[0] - centre[0], p[2] - centre[2]) - 1) for p in samples) +deepest = max(abs(p[2] - centre[2]) for p in samples) +print("arc: %d samples, off the plane %g, off the circle %g, deepest %g" + % (len(samples), off_plane, off_circle, deepest)) +if off_plane > 1e-5 or off_circle > 1e-5: + error("the arc leaves its circle in the machine's XZ plane: %g off the plane, %g off the circle" + % (off_plane, off_circle)) +if deepest < 0.9: + error("the arc never bows out of the X axis, deepest %g" % deepest) + +# a tilted work plane sits on the turned system and turns with it +mdi("G68.2 X0 Y0 Z0 I0 J30 K0") +mdi("G0 X0 Y1 Z0") +want = world(ORIGIN, (-90, 0, 0), rot(30, 0, 0, (0, 1, 0))) +if not close(machine(), want): + error("G68.2 on the turned system: Y1 is at %s, not %s" % (machine(), want)) +mdi("G69") + +# G10 L20 sets the coordinate given along the system's turned axes: its Z +# is machine +Y, so the origin moves along Y alone +mdi("G0 X1 Y2 Z3") +before = machine() +s.poll() +g5x = list(s.g5x_offset[:3]) +mdi("G10 L20 P1 Z0") +s.poll() +moved = [a - b for a, b in zip(s.g5x_offset[:3], g5x)] +if not close(machine(), before): + error("G10 L20 moved the machine") +if not close(moved, [0, 3, 0]): + error("G10 L20 Z0 moved the origin by %s, not along Y by 3" % moved) +check_relative("G10 L20 Z0", [1, 2, 0]) +mdi("G10 L20 P1 I5", expect="I J K not allowed in G10 L20") + +# a tool offset is along the machine's axes +mdi("G10 L10 P1 Z1", expect="turned about X or Y") + +# the preview draws the same points: the renderer and the Python canon +class Machine: + def get_tool(self, pocket): + return tuple(s.tool_table[pocket]) if 0 <= pocket < len(s.tool_table) else (-1,) + (0,) * 13 + def get_external_length_units(self): + return s.linear_units + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return s.axis_mask + def get_block_delete(self): + return 0 + +class Rendered(Machine, gcode.RendererCanon): + def __init__(self): + self.program_geometry = glcanon_bake.ProgramGeometry(geometry="XYZ") + self.parameter_file = "preview.var" + def adopt_geometry(self, program): + self.program_geometry.adopt(program, {"dwell": (1, 0, 0, 1), "m1xx": (0, 0, 1, 1)}) + def comment(self, text): pass + def message(self, text): pass + def check_abort(self): return False + def next_line(self, state): pass + def change_tool(self, pocket): pass + +class Python(Machine, Translated): + def __init__(self): + self.parameter_file = "preview.var" + self.ends = [] + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def straight_feed(self, *a): + self.ends.append([25.4 * v for v in self.rotate_and_translate(*a)[:3]]) + +PROG = ("G21 G90 G17\nG10 L2 P2 X10 Y20 Z30 I-90 J0 R15\nG55\n" + "G1 X1 Y2 Z3 F100\nG2 X3 Y2 I1 J0\nM2\n") +with open("preview.ngc", "w") as f: + f.write(PROG) +for f in ("preview.var", "preview.var.bak"): + if os.path.exists(f): + os.unlink(f) +canon = Rendered() +res, seq = gcode.parse("preview.ngc", canon, preview_helpers.preview_initcodes(s, ini)) +if res > gcode.MIN_ERROR: + error("the renderer stops at line %d: %s" % (seq, gcode.strerror(res).strip())) +else: + pts = [[25.4 * v for v in p] for p in canon.program_geometry.positions()] + start = world(ORIGIN, (-90, 0, 15), (1, 2, 3)) + near = [i for i, p in enumerate(pts) if close(p, start, 1e-4)] + if not near: + error("the renderer draws no point at X1 Y2 Z3 of G55, %s" % start) + else: + centre = world(ORIGIN, (-90, 0, 15), (2, 2, 3)) + normal = rot(-90, 0, 15, (0, 0, 1)) + arc = pts[near[-1]:] + worst = max(abs(math.dist(p, centre) - 1) for p in arc) + tilt = max(abs(sum((a - b) * n for a, b, n in zip(p, centre, normal))) for p in arc) + print("renderer: %d points, arc off the radius %g, off the plane %g" % (len(pts), worst, tilt)) + if len(arc) < 3 or worst > 1e-4 or tilt > 1e-4: + error("the renderer's arc leaves its circle: %d points, %g off the radius, %g off the plane" + % (len(arc), worst, tilt)) +canon = Python() +res, seq = gcode.parse("preview.ngc", canon, preview_helpers.preview_initcodes(s, ini)) +if res > gcode.MIN_ERROR: + error("the Python canon stops at line %d: %s" % (seq, gcode.strerror(res).strip())) +elif not canon.ends or not close(canon.ends[0], world(ORIGIN, (-90, 0, 15), (1, 2, 3))): + error("the Python canon puts X1 Y2 Z3 of G55 at %s" % (canon.ends[:1],)) +for f in ("preview.ngc", "preview.var", "preview.var.bak"): + if os.path.exists(f): + os.unlink(f) + +# for the second run: G55 kept in the var file +mdi("G10 L2 P2 X10 Y20 Z30 I-90 J0 R15") +mdi("G54") +print("Exiting with %d errors" % errors) diff --git a/tests/g5x-rotation/test.ini b/tests/g5x-rotation/test.ini new file mode 100644 index 00000000000..dcd5ec82d76 --- /dev/null +++ b/tests/g5x-rotation/test.ini @@ -0,0 +1,82 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G94 G97 +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 1.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[HAL] +HALUI = halui +HALFILE = LIB:core_sim.hal +HALFILE = LIB:simulated_home.hal + +[TRAJ] +COORDINATES = X Y Z +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 +MAX_LINEAR_VELOCITY = 200.0 +NO_FORCE_HOMING = 1 + +[KINS] +KINEMATICS = trivkins coordinates=XYZ +JOINTS = 3 + +[AXIS_X] +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Y] +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Z] +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +HOME_SEQUENCE = 0 diff --git a/tests/g5x-rotation/test.sh b/tests/g5x-rotation/test.sh new file mode 100755 index 00000000000..c66256b1178 --- /dev/null +++ b/tests/g5x-rotation/test.sh @@ -0,0 +1,11 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +rm -f sim.var sim.var.bak preview.ngc preview.var preview.var.bak +G5X_PHASE="set" linuxcnc -r test.ini +# the rotation is kept in the var file, at #5251 and #5252 for G55 +grep -E '^5251[[:space:]]+-90\.0+$' sim.var +grep -E '^5252[[:space:]]+0\.0+$' sim.var +grep -E '^5250[[:space:]]+15\.0+$' sim.var +G5X_PHASE=restore linuxcnc -r test.ini +rm -f sim.var sim.var.bak diff --git a/tests/g5x-rotation/tool.tbl b/tests/g5x-rotation/tool.tbl new file mode 100644 index 00000000000..22e8070cca9 --- /dev/null +++ b/tests/g5x-rotation/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z0 D0 ; diff --git a/tests/twp-jog-frame/README b/tests/twp-jog-frame/README index 3b33b204d32..84e97e7f171 100644 --- a/tests/twp-jog-frame/README +++ b/tests/twp-jog-frame/README @@ -1,11 +1,12 @@ World jogs of X, Y and Z along the tilted work plane and along the tool. 5axiskins with a 100 mm pivot, the tool centre point type, Y slow and -short against X and Z. The plane frame: a plane defined under an XY -rotation, incremental jogs of each letter, each world move checked -against the plane's axes as status reports them turned by the XY -rotation, rotaries untouched; a machine jog afterwards starts where the -frame jogs left the machine. A plane whose X runs mostly along Y: the +short against X and Z. The plane frame: a plane defined on a coordinate +system turned about X, Y and Z, incremental jogs of each letter, each +world move checked against the plane's axes as status reports them +turned by the system's rotation, rotaries untouched; a machine jog +afterwards starts where the frame jogs left the machine. +A plane whose X runs mostly along Y: the jog along it runs at the speed Y allows. One frame axis moves at a time: a jog of plane Y asked for while plane X runs waits until X has stopped. A continuous jog stops where the turned axis meets the diff --git a/tests/twp-jog-frame/test-ui.py b/tests/twp-jog-frame/test-ui.py index 537a3fba3f9..a3215765819 100755 --- a/tests/twp-jog-frame/test-ui.py +++ b/tests/twp-jog-frame/test-ui.py @@ -142,6 +142,14 @@ def rot_z(deg, v): return [math.cos(t) * v[0] - math.sin(t) * v[1], math.sin(t) * v[0] + math.cos(t) * v[1], v[2]] +def rot_g5x(v): + # the coordinate system's turn from status, Rz Ry Rx + a, b = math.radians(s.rotation_yz), math.radians(s.rotation_zx) + x, y, z = v + y, z = y * math.cos(a) - z * math.sin(a), y * math.sin(a) + z * math.cos(a) + x, z = x * math.cos(b) + z * math.sin(b), z * math.cos(b) - x * math.sin(b) + return rot_z(s.rotation_xy, [x, y, z]) + def dot(a, b): return sum(p * q for p, q in zip(a, b)) @@ -154,10 +162,10 @@ def fmt(v): def plane_axes(): # the plane's axes in world coordinates, from status: the plane's own - # rotation turned by the XY rotation it was defined under + # rotation turned by the coordinate system it was defined on s.poll() r = s.g68_rotation - return [rot_z(s.rotation_xy, column(r, k)) for k in range(3)] + return [rot_g5x(column(r, k)) for k in range(3)] def tool_axes(): # the tool frame's axes in world coordinates, from status @@ -211,9 +219,9 @@ def cruise(axes, speed, what): if s.jog_frame != linuxcnc.JOG_FRAME_MACHINE: error("the jog frame starts at %d, not the machine's" % s.jog_frame) -# the plane, defined under an XY rotation of 30 degrees: its axes turned by -# both are what the jog must follow -mdi("G12.1 P0", "G10 L2 P1 R30", "G54", "G68.2 X0 Y0 Z0 I0 J30 K0") +# the plane, defined on a coordinate system turned about X, Y and Z: its +# axes turned by both are what the jog must follow +mdi("G12.1 P0", "G10 L2 P1 I10 J-15 R30", "G54", "G68.2 X0 Y0 Z0 I0 J30 K0") W = plane_axes() print("plane axes in world: X %s, Y %s, Z %s" % tuple(fmt(v) for v in W)) teleop() @@ -230,7 +238,7 @@ def cruise(axes, speed, what): # a plane whose X runs mostly along the slow Y: the jog along it goes no # faster than Y allows, and two frame axes jogged at once keep every world # axis inside its own speed -mdi("G69", "G10 L2 P1 R0", "G54", "G68.2 X0 Y0 Z0 I60 J0 K0") +mdi("G69", "G10 L2 P1 I0 J0 R0", "G54", "G68.2 X0 Y0 Z0 I60 J0 K0") W = plane_axes() teleop() frame(linuxcnc.JOG_FRAME_PLANE) diff --git a/tests/twp-table/README b/tests/twp-table/README index 92cf086a12f..764e76ec89f 100644 --- a/tests/twp-table/README +++ b/tests/twp-table/README @@ -4,7 +4,9 @@ A here, as Heidenhain's SEQ names its master axis. For each plane, from several starting poses: G53.1 with no P takes the nearer of the two poses, P1 always the one with A positive and P2 the one with A negative, the same pose from every start; each leaves the tool along the plane's normal; a -move in the plane after the turn ends where the plane puts it. +move in the plane after the turn ends where the plane puts it, also for a +plane on a coordinate system turned about X, Y and Z (G10 L2 I J K), +which turns the plane's normal with it. The planes are one in general position, one with J negative, and one whose C solutions sit near the +-180 wrap. twp-table-one-sided runs the diff --git a/tests/twp-table/test-ui.py b/tests/twp-table/test-ui.py index 16a801658e9..c53af0cf6fe 100755 --- a/tests/twp-table/test-ui.py +++ b/tests/twp-table/test-ui.py @@ -154,21 +154,38 @@ def travel(j, start): error("G53.1 on I%g J%g from A%g C%g went to A%.3f C%.3f, A%.3f C%.3f is nearer" % (i, jj, start[0], start[1], j[A], j[C], other[A], other[C])) -# after the turn, a move in the plane ends where the plane puts it -turns = () if one_sided else ((60, 30, "P1"), (60, 30, "P2"), (-90, 45, "P1")) -for i, jj, word in turns: - mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj), +# the coordinate system's turn, G10 L2 I J K: Rz(K) Ry(J) Rx(I) +def turned(g, v): + a, b, k = (math.radians(q) for q in g) + x, y, z = v + y, z = y*math.cos(a) - z*math.sin(a), y*math.sin(a) + z*math.cos(a) + x, z = x*math.cos(b) + z*math.sin(b), z*math.cos(b) - x*math.sin(b) + return (x*math.cos(k) - y*math.sin(k), x*math.sin(k) + y*math.cos(k), z) + +# after the turn, a move in the plane ends where the plane puts it; the +# last plane sits on a coordinate system turned about X, Y and Z, which +# turns the plane with it +turns = () if one_sided else ((60, 30, "P1", (0, 0, 0)), (60, 30, "P2", (0, 0, 0)), + (-90, 45, "P1", (20, -15, 30))) +for i, jj, word, g in turns: + mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G10 L2 P1 I%g J%g K%g" % g, "G54", + "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj), "G53.1 %s" % word, "G0 X10 Y5 Z20", "G1 X-10 F60000") s.poll() + off = angle(tool_axis(s.joint_position), turned(g, normal(i, jj))) + if off > 1e-6: + error("G53.1 %s on I%g J%g on a system turned %s left the tool %.6f degrees off the normal" + % (word, i, jj, g, off)) ri, rj = math.radians(i), math.radians(jj) px, py, pz = -10.0, 5.0, 20.0 y1 = py*math.cos(rj) - pz*math.sin(rj) z1 = py*math.sin(rj) + pz*math.cos(rj) - want = (px*math.cos(ri) - y1*math.sin(ri), px*math.sin(ri) + y1*math.cos(ri), z1) + want = turned(g, (px*math.cos(ri) - y1*math.sin(ri), px*math.sin(ri) + y1*math.cos(ri), z1)) got = s.position[:3] if max(abs(p - q) for p, q in zip(got, want)) > 1e-6: - error("after G53.1 %s on I%g J%g the tip is at %s, the plane puts it at %s" - % (word, i, jj, got, want)) + error("after G53.1 %s on I%g J%g on a system turned %s the tip is at %s, the plane puts it at %s" + % (word, i, jj, g, got, want)) +mdi("G69", "G10 L2 P1 I0 J0 K0") mdi("G69", "G0 X0 Y0 Z60 A0 C0") From 8c090e1543f753f108b49c8c72fc13af8a0cd082 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 14 Nov 2026 12:57:35 +1100 Subject: [PATCH 42/44] preview: restate the G52 and G92 offsets by value The preview started from the startup code, which often clears the G92 offsets with G92.1, and then restated the "G92 applied" mode as G92.3, which reads them back from the preview's copy of the parameters. A G52 set in MDI was lost, so the program was drawn without it, and with a tilted work plane active the G92.3 came after the plane, which refuses a change to the G92 offsets: the preview stopped at line 0 with "Cannot change G92 offsets while a tilted work plane (G68.2) is active". The preview now restates the offsets with G52 and the machine's values, ahead of the plane, which sits inside them. tests/workplane-preview checks a G52 alone and under a plane: both ended 7 mm off or refused before, the same end as the machine now. Reported by Sigma1912. --- lib/python/preview_helpers.py | 20 +++++++++++++------- tests/workplane-preview/expected | 2 ++ tests/workplane-preview/test-ui.py | 4 ++++ 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/lib/python/preview_helpers.py b/lib/python/preview_helpers.py index 0c44d06eb0d..94bf4c910cb 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -8,16 +8,16 @@ # (issue #269), M2, which blanks the next preview, and M6 or M61, which has # no Q to restate. Cutter radius compensation (gcodes[4]) is left out, because # the blocks after it would be refused while it is on. The tool length offset -# (gcodes[9]) and the tilted work plane (gcodes[12]) are restated with their -# values instead. -MODAL_GCODES = (3, 6, 14, 7, 5, 10, 16, 8, 11, 13, 15) +# (gcodes[9]), the tilted work plane (gcodes[12]) and the G92 offsets +# (gcodes[16]) are restated with their values instead. +MODAL_GCODES = (3, 6, 14, 7, 5, 10, 8, 11, 13, 15) MODAL_MCODES = (2, 4, 5, 6, 7, 8, 9) def preview_initcodes(s, inifile): """The machine's state as G-code for a preview interpreter to start - from: the startup code, the units, the tilted work plane, the tool - in the spindle, the position, the active modes and the tool length - offset in force.""" + from: the startup code, the units, the G92 offsets, the tilted work + plane, the tool in the spindle, the position, the active modes and the + tool length offset in force.""" wrapped = axis_kinds.wrapped(inifile) s.poll() codes = [] @@ -26,12 +26,18 @@ def preview_initcodes(s, inifile): if startup: codes.append(startup) codes.append("G%d" % (20 + (s.linear_units == 1))) + axes = [(i, "XYZABCUVW"[i]) for i in range(9) if s.axis_mask & (1 << i)] + # The G92 offsets as G52 sets them, by value: the startup code may clear + # them, and G92.3 would take them from the preview's parameters. They + # come in machine units, ahead of the plane, which sits inside them and + # refuses a change to them + if any(s.g92_offset[i] for i, axis in axes): + codes.append("G52" + "".join(" %s%.12f" % (axis, s.g92_offset[i]) for i, axis in axes)) plane = workplane_code(s) if plane: codes.append(plane) codes.append("G90") codes.append("T%d M6" % s.tool_in_spindle) - axes = [(i, "XYZABCUVW"[i]) for i in range(9) if s.axis_mask & (1 << i)] offset = "G43.1" + "".join(" %s%.8f" % (axis, s.tool_offset[i]) for i, axis in axes) # Under G43.4 or G43.5 the switch and the offset come ahead of the # position: the switch holds the joints and would move a point restated diff --git a/tests/workplane-preview/expected b/tests/workplane-preview/expected index 29ffdcc2685..0626cdac2fd 100644 --- a/tests/workplane-preview/expected +++ b/tests/workplane-preview/expected @@ -4,3 +4,5 @@ composed then G68.4: same end gimbal: same end no plane: preview line 1: G68.4 needs an active tilted work plane to build on; machine: G68.4 needs an active tilted work plane to build on unfinished MDI sequence: same end +G52: same end +G52 and a plane: same end diff --git a/tests/workplane-preview/test-ui.py b/tests/workplane-preview/test-ui.py index 6770f4145f0..d0492376efb 100755 --- a/tests/workplane-preview/test-ui.py +++ b/tests/workplane-preview/test-ui.py @@ -101,4 +101,8 @@ def case(name, prog, *setup): case("gimbal", "move.ngc", "G20", "G68.2 P1 X0.5 Y0.2 Z0.1 I20 J90 K30") case("no plane", "compose.ngc", "G20") case("unfinished MDI sequence", "move.ngc", "G20", "G68.2 P2 Q1 X0 Y0 Z0") +# G52 offsets reach the preview by value, under a plane as without one +case("G52", "move.ngc", "G21", "G52 X3 Y-2 Z7") +case("G52 and a plane", "move.ngc", "G21", "G52 X3 Y-2 Z7", "G68.2 X10 Y20 Z5 I10 J20 K30") +mdi("G69", "G52 X0 Y0 Z0") c.state(linuxcnc.STATE_OFF) From 57d9b56e6233a12450a21786dbb5e687fce5abe4 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 14 Nov 2026 12:57:35 +1100 Subject: [PATCH 43/44] gladevcp, qtvcp: show the relative position in the tilted work plane hal_glib's get_position(), which gmoccapy's and qtvcp's DROs read, and the gladevcp DRO widget left out the tilted work plane, which AXIS takes off after G92: after G68.2 Z30 and G0 X0 Y0 Z0 they showed Z30. Both now take the plane off as AXIS does. Reported by Sigma1912. --- lib/python/common/hal_glib.py | 8 ++++++++ lib/python/gladevcp/drowidget.py | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/lib/python/common/hal_glib.py b/lib/python/common/hal_glib.py index 68155f464bc..475a0e5a547 100644 --- a/lib/python/common/hal_glib.py +++ b/lib/python/common/hal_glib.py @@ -1179,6 +1179,14 @@ def get_position(self): u -= self.stat.g92_offset[6] v -= self.stat.g92_offset[7] w -= self.stat.g92_offset[8] + # the tilted work plane sits inside G92 + if getattr(self.stat, 'g68_active', 0): + r = self.stat.g68_rotation + o = self.stat.g68_offset + x, y, z = x - o[0], y - o[1], z - o[2] + x, y, z = (r[0] * x + r[3] * y + r[6] * z, + r[1] * x + r[4] * y + r[7] * z, + r[2] * x + r[5] * y + r[8] * z) relp = [x, y, z, a, b, c, u, v, w] return p,relp,dtg diff --git a/lib/python/gladevcp/drowidget.py b/lib/python/gladevcp/drowidget.py index 4a270cfed60..7feebdc60e9 100644 --- a/lib/python/gladevcp/drowidget.py +++ b/lib/python/gladevcp/drowidget.py @@ -294,6 +294,14 @@ def position(self): u -= self.status.g92_offset[6] v -= self.status.g92_offset[7] w -= self.status.g92_offset[8] + # the tilted work plane sits inside G92 + if getattr(self.status, 'g68_active', 0): + r = self.status.g68_rotation + o = self.status.g68_offset + x, y, z = x - o[0], y - o[1], z - o[2] + x, y, z = (r[0] * x + r[3] * y + r[6] * z, + r[1] * x + r[4] * y + r[7] * z, + r[2] * x + r[5] * y + r[8] * z) relp = [x, y, z, a, b, c, u, v, w] if self.display_units_mm != self.machine_units_mm: From 454b261df370268fd14f63753754914a94226e04 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 14 Nov 2026 14:14:39 +1100 Subject: [PATCH 44/44] gladevcp, qtvcp: show the relative position in the plane under G13.1 Under the machine frame kinematics the plane is cut through the machine's second feature coordinate system, which comes after the whole offset chain; AXIS takes it off first. hal_glib's get_position() and the gladevcp DRO widget left it out: in a plane under G13.1, G0 X1 Y2 Z3 showed X-4.1 Y-88.7 Z27.1. Both now take it off as AXIS does. To fold into the lane that taught AXIS the same, once #4841 has merged. --- lib/python/common/hal_glib.py | 31 ++++++++++++++++++++++--------- lib/python/gladevcp/drowidget.py | 31 ++++++++++++++++++++++--------- 2 files changed, 44 insertions(+), 18 deletions(-) diff --git a/lib/python/common/hal_glib.py b/lib/python/common/hal_glib.py index 475a0e5a547..8675119aa52 100644 --- a/lib/python/common/hal_glib.py +++ b/lib/python/common/hal_glib.py @@ -1146,15 +1146,28 @@ def get_position(self): mp = self.stat.position dtg = self.stat.dtg - x = p[0] - self.stat.g5x_offset[0] - self.stat.tool_offset[0] - y = p[1] - self.stat.g5x_offset[1] - self.stat.tool_offset[1] - z = p[2] - self.stat.g5x_offset[2] - self.stat.tool_offset[2] - a = p[3] - self.stat.g5x_offset[3] - self.stat.tool_offset[3] - b = p[4] - self.stat.g5x_offset[4] - self.stat.tool_offset[4] - c = p[5] - self.stat.g5x_offset[5] - self.stat.tool_offset[5] - u = p[6] - self.stat.g5x_offset[6] - self.stat.tool_offset[6] - v = p[7] - self.stat.g5x_offset[7] - self.stat.tool_offset[7] - w = p[8] - self.stat.g5x_offset[8] - self.stat.tool_offset[8] + # off the second feature coordinate system a plane is cut through + # under the machine frame kinematics, which comes after the whole + # offset chain + q = p + k = getattr(self.stat, 'g68_kins_rotation', None) if getattr(self.stat, 'g68_active', 0) else None + if k: + o = self.stat.g68_kins_offset + _x, _y, _z = p[0] - o[0], p[1] - o[1], p[2] - o[2] + q = list(p) + q[0] = k[0] * _x + k[3] * _y + k[6] * _z + q[1] = k[1] * _x + k[4] * _y + k[7] * _z + q[2] = k[2] * _x + k[5] * _y + k[8] * _z + + x = q[0] - self.stat.g5x_offset[0] - self.stat.tool_offset[0] + y = q[1] - self.stat.g5x_offset[1] - self.stat.tool_offset[1] + z = q[2] - self.stat.g5x_offset[2] - self.stat.tool_offset[2] + a = q[3] - self.stat.g5x_offset[3] - self.stat.tool_offset[3] + b = q[4] - self.stat.g5x_offset[4] - self.stat.tool_offset[4] + c = q[5] - self.stat.g5x_offset[5] - self.stat.tool_offset[5] + u = q[6] - self.stat.g5x_offset[6] - self.stat.tool_offset[6] + v = q[7] - self.stat.g5x_offset[7] - self.stat.tool_offset[7] + w = q[8] - self.stat.g5x_offset[8] - self.stat.tool_offset[8] if self.stat.rotation_xy != 0: t = math.radians(-self.stat.rotation_xy) diff --git a/lib/python/gladevcp/drowidget.py b/lib/python/gladevcp/drowidget.py index 7feebdc60e9..ca8e356fd1d 100644 --- a/lib/python/gladevcp/drowidget.py +++ b/lib/python/gladevcp/drowidget.py @@ -261,15 +261,28 @@ def position(self): p = self.status.position dtg = self.status.dtg - x = p[0] - self.status.g5x_offset[0] - self.status.tool_offset[0] - y = p[1] - self.status.g5x_offset[1] - self.status.tool_offset[1] - z = p[2] - self.status.g5x_offset[2] - self.status.tool_offset[2] - a = p[3] - self.status.g5x_offset[3] - self.status.tool_offset[3] - b = p[4] - self.status.g5x_offset[4] - self.status.tool_offset[4] - c = p[5] - self.status.g5x_offset[5] - self.status.tool_offset[5] - u = p[6] - self.status.g5x_offset[6] - self.status.tool_offset[6] - v = p[7] - self.status.g5x_offset[7] - self.status.tool_offset[7] - w = p[8] - self.status.g5x_offset[8] - self.status.tool_offset[8] + # off the second feature coordinate system a plane is cut through + # under the machine frame kinematics, which comes after the whole + # offset chain + q = p + k = getattr(self.status, 'g68_kins_rotation', None) if getattr(self.status, 'g68_active', 0) else None + if k: + o = self.status.g68_kins_offset + _x, _y, _z = p[0] - o[0], p[1] - o[1], p[2] - o[2] + q = list(p) + q[0] = k[0] * _x + k[3] * _y + k[6] * _z + q[1] = k[1] * _x + k[4] * _y + k[7] * _z + q[2] = k[2] * _x + k[5] * _y + k[8] * _z + + x = q[0] - self.status.g5x_offset[0] - self.status.tool_offset[0] + y = q[1] - self.status.g5x_offset[1] - self.status.tool_offset[1] + z = q[2] - self.status.g5x_offset[2] - self.status.tool_offset[2] + a = q[3] - self.status.g5x_offset[3] - self.status.tool_offset[3] + b = q[4] - self.status.g5x_offset[4] - self.status.tool_offset[4] + c = q[5] - self.status.g5x_offset[5] - self.status.tool_offset[5] + u = q[6] - self.status.g5x_offset[6] - self.status.tool_offset[6] + v = q[7] - self.status.g5x_offset[7] - self.status.tool_offset[7] + w = q[8] - self.status.g5x_offset[8] - self.status.tool_offset[8] if self.status.rotation_xy != 0: t = math.radians(-self.status.rotation_xy)