From aa3fdec6101f47d51bd4293e374a0fe0bc11d0f8 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 1/8] 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 35beda51f729f144ec9a36eea344fd7e6d93e639 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 2/8] 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 1c83d8ff56158d57d052f2b5ae4c408f499efd8b 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 3/8] 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 6a4c920c1e55a12c0942658d1dadd3ad9f3cd08b 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 4/8] 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 f130b061dba31405854bc0a9ebd28335f1a75767 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 5/8] 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 05dafd2e7d3175ffc249b44e8c7d8a65e141760b 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 6/8] 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 bfb6c0d8f75aba9877afa9f0c8d484ab959de8f4 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 7/8] 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 861b1d40f2a907a0563c0a5027fc787034d77a5f 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 8/8] 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 ;