From 7e1ec6c61996a297f830bcbd0e8f97df028cfd64 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 21 Oct 2026 19:40:57 +1100 Subject: [PATCH 01/24] maxkins, xyzab_tdr_kins: report the tool and work frames Neither module reported its frames, so everything that needs them refused or did without on these two machines: the tilted work plane's orienting codes, the preview's tool marker, the tool jog frame and the turn of an offset across the tool. maxkins tilts the head about y by B and turns the table about z by C. The table turns the work by +C into the pose, so the work frame is Rz(-C); the head's tool frame is Ry(con*B), con the sign the conventional-directions pin selects, since the pin flips the tilt in the forward and the frame has to follow it: the W joint runs the tip out along (-con*sin(B), 0, -cos(B)), the tool axis reversed. xyzab_tdr_kins carries the work on both rotaries, so its tool frame is the identity and its work frame the transpose of the forward's coefficients of the X, Y and Z joints, as the trt modules do it. tests/kins-frames takes xyzab_tdr_kins and maxkins with the pin either way, and its quill check reads the tool axis from the work, which on a head over a turning table is where the forward reports the W joint's run; on 5axiskins the work frame is the identity and the check is unchanged. tests/kins-params expects their orientation joints. The kinematics conventions chapter says which in-tree modules still report no frames: genserkins, scorbot-kins, genhexkins and pentakins. --- docs/src/motion/kinematics-conventions.adoc | 7 ++++ src/emc/kinematics/maxkins.c | 37 +++++++++++++++++++++ src/hal/components/xyzab_tdr_kins.comp | 24 +++++++++++++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 9 ++--- tests/kins-frames/test.sh | 17 ++++++++++ tests/kins-params/test.sh | 4 +-- 7 files changed, 93 insertions(+), 7 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 02446cf425a..4e27eee5d33 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -274,6 +274,13 @@ are tilted work plane handling, tool length compensation along a tilted axis, previews and simulation models, and probing routines that have to say which way the stylus faces. +The frames are optional. A module that leaves them out answers neither, and a +caller that needs them, a tilted work plane first of all, refuses under it. +Of the in-tree modules whose joints tilt the tool or the work, all report both +frames except genserkins, scorbot-kins, genhexkins and pentakins. genhexkins +and pentakins solve the forward iteratively, so the platform's orientation +needs the seed the forward keeps, which the frame calls do not take. + The joint values that reach a requested orientation, through `kinematicsToolFrameInverse()`, which is the inverse of the pair above and the question a tilted work plane asks when it has to orient the machine. diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index a32981d12cf..1f2e6e596aa 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -173,9 +173,46 @@ static int max_jacobian(const kins_params *p, const double *joints, return 0; } +// The C table turns the work by +C into the pose, so the work frame is Rz(-C). +// The head tilts the tool about machine y by B, the sense set by the pin: the +// W joint runs the tip out along (-con*sin(B), 0, -cos(B)), so the tool axis, +// tip towards holder, is the third column of Ry(con*B). +static int max_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)fflags; + const double sc = sin(d2r(joints[5])), cc = cos(d2r(joints[5])); + + rot->x.x = cc; rot->y.x = sc; rot->z.x = 0; + rot->x.y = -sc; rot->y.y = cc; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; + + return 0; +} + +static int max_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double con = CON(p); + const double sb = sin(d2r(joints[4])), cb = cos(d2r(joints[4])); + + rot->x.x = cb; rot->y.x = 0; rot->z.x = con * sb; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = -con * sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} + static const kins_ops max_ops = { .forward = max_forward, .inverse = max_inverse, + .work = max_work_frame, + .tool = max_tool_frame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = max_jacobian, }; diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 0444c68e1d1..853de4a8ea2 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -196,9 +196,33 @@ static int tdr_jacobian(const kins_params *p, const double *j, return 0; } // tdr_jacobian() +// both rotaries carry the work, so the tool frame is the shared identity one; +// the forward's coefficients of the X, Y and Z joints are the rotation from +// machine into work, so the work frame is their transpose +static int tdr_work_frame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)fflags; + double sa = sin(j[3]*TO_RAD); + double ca = cos(j[3]*TO_RAD); + double sb = sin(j[4]*TO_RAD); + double cb = cos(j[4]*TO_RAD); + + rot->x.x = cb; rot->y.x = sa*sb; rot->z.x = -ca*sb; + rot->x.y = 0; rot->y.y = ca; rot->z.y = sa; + rot->x.z = sb; rot->y.z = -cb*sa; rot->z.z = ca*cb; + + return 0; +} // tdr_work_frame() + static const kins_ops tdr_ops = { .forward = tdr_forward, .inverse = tdr_inverse, + .work = tdr_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = tdr_jacobian, .primary = 1, }; diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 48c09ee0d98..3c8e32879c9 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 6 ] \ +[ "$(grep -c 'frames agree' "$1")" = 9 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index ed6acf27a08..fb0ff214fd8 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -211,11 +211,12 @@ static void check(const double *j, int own_kinematics) if (quill >= 0) { /* the joint runs the tool out along its own axis, away from the - holder, so the tip moves along the tool axis reversed: the one - tie between the reported frame and the forward transform on a - machine that turns nothing but the tool */ + holder, so the tip moves along the tool axis reversed, seen from + the work: the one tie between the reported tool frame and the + forward transform on a machine whose head turns the tool */ response(j, quill, &d); - expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + expect(close3(&d, -dot(&work.x, &tool.z), -dot(&work.y, &tool.z), + -dot(&work.z, &tool.z)), "the quill runs out along the tool axis", j); } diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 109e69821dc..236bf357351 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,6 +49,23 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4 fixed_spindle=1 ktype=1" + +# a head that tilts and a table that turns, the tilt's sense set by a pin: +# the W joint ties the tool frame to the forward either way +run "maxkins" "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 quill=8" +run "maxkins" "setp maxkins.pivot-length 100 +setp maxkins.conventional-directions 1" \ + "joints=9 r1=4 r2=5 quill=8" + run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 1e623690fa4..4dd6c2d4843 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -92,7 +92,7 @@ setp matrixkins.C_zz 1.001" \ # tables and heads, offsets set so no term drops out run "maxkins" \ "setp maxkins.pivot-length 100" \ - "joints=9 jnt=10,20,30,0,15,25,7,0,3" + "joints=9 jnt=10,20,30,0,15,25,7,0,3 orient=- posejoint=4" run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" 1 @@ -123,7 +123,7 @@ setp xyzab_tdr_kins.tool-offset-z 7 setp xyzab_tdr_kins.x-rot-point 1 setp xyzab_tdr_kins.y-rot-point 2 setp xyzab_tdr_kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" 1 + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=4" 1 run "xyzacb_trsrn" \ "setp xyzacb_trsrn_kins.nut-angle 45 From 40517a1265f986654bd07da7c7cee610d89d6cf1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 22 Oct 2026 00:36:51 +1100 Subject: [PATCH 02/24] genserkins: report the tool frame genserkins reported no frames, so an arm described by its DH chain got none of the orientation codes, the tool marker or the tool jog frame, where pumakins, the same arm in closed form, had them all. The arm carries the tool and nothing turns the work, so the work frame is the identity. The tool frame is the flange frame the chain ends in, z along the last joint axis and out of the flange, declared with the same half turn to the convention as pumakins. The forward already reports that frame as roll, pitch and yaw in A B C, so the tool frame is that rotation, written out rather than taken through gomath or a posemath call the module does not link. scorbot-kins stays without: its controlled point is the wrist centre and A and B are the wrist joints passed through, so there is no tool frame to report. The kinematics conventions chapter says so. tests/kins-frames takes genserkins as it takes pumakins, and both gain a check that the tool frame turned back by the half turn is the rotation the forward's A B C describe; with the defaults, which are the PUMA of pumakins, the two modules give the same tool frame at the same joints. tests/kins-params expects genserkins to report a frame. --- docs/src/motion/kinematics-conventions.adoc | 8 +++--- src/emc/kinematics/genserfuncs.c | 27 +++++++++++++++++++++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 26 ++++++++++++++++++++ tests/kins-frames/test.sh | 6 ++++- tests/kins-params/test.sh | 4 +-- 6 files changed, 66 insertions(+), 7 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 4e27eee5d33..685820d4693 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -277,9 +277,11 @@ the stylus faces. The frames are optional. A module that leaves them out answers neither, and a caller that needs them, a tilted work plane first of all, refuses under it. Of the in-tree modules whose joints tilt the tool or the work, all report both -frames except genserkins, scorbot-kins, genhexkins and pentakins. genhexkins -and pentakins solve the forward iteratively, so the platform's orientation -needs the seed the forward keeps, which the frame calls do not take. +frames except scorbot-kins, genhexkins and pentakins. scorbot-kins controls the +wrist centre and passes the wrist joints through as A and B, so it has no tool +frame to report. genhexkins and pentakins solve the forward iteratively, so the +platform's orientation needs the seed the forward keeps, which the frame calls +do not take. The joint values that reach a requested orientation, through `kinematicsToolFrameInverse()`, which is the inverse of the pair above and the diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index c43156490c9..d0c918e0f7a 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -668,9 +668,36 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, return GO_RESULT_ERROR; } +// The arm carries the tool and nothing turns the work. The tool frame is the +// flange frame the DH chain ends in, z along the last joint axis and out of +// the flange as on pumakins, which the forward reports as roll, pitch and yaw +// in A B C; the half turn to the convention is the native rotation. +static int genser_tool_frame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose world; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double ca, sa, cb, sb, cc, sc; + + memset(&world, 0, sizeof(world)); + if (genser_forward(p, NULL, joint, &world, fflags, &iflags) != 0) { return -1; } + ca = cos(world.a * PM_PI / 180); sa = sin(world.a * PM_PI / 180); + cb = cos(world.b * PM_PI / 180); sb = sin(world.b * PM_PI / 180); + cc = cos(world.c * PM_PI / 180); sc = sin(world.c * PM_PI / 180); + // Rz(C) Ry(B) Rx(A), by columns + rot->x.x = cc * cb; rot->y.x = cc * sb * sa - sc * ca; rot->z.x = cc * sb * ca + sc * sa; + rot->x.y = sc * cb; rot->y.y = sc * sb * sa + cc * ca; rot->z.y = sc * sb * ca - cc * sa; + rot->x.z = -sb; rot->y.z = cb * sa; rot->z.z = cb * ca; + return 0; +} + const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, + .work = kinsIdentityFrame, + .tool = genser_tool_frame, + .native = &TOOL_FRAME_FLANGE, .jacobian = genser_jacobian, .primary = 1, }; diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 3c8e32879c9..e92522018b1 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 9 ] \ +[ "$(grep -c 'frames agree' "$1")" = 10 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index fb0ff214fd8..8ca5ce37ca0 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -55,6 +55,9 @@ RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 fo static int quill = -1; RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); +static int abc_flange = 0; +RTAPI_MP_INT(abc_flange, "1 when the forward reports the flange orientation as roll, pitch and yaw in A B C"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -220,6 +223,29 @@ static void check(const double *j, int own_kinematics) "the quill runs out along the tool axis", j); } + if (abc_flange) { + /* an arm's forward reports the flange frame of ISO 9787, z out of + the interface towards the work, as roll, pitch and yaw in A B C; + the tool frame is that frame turned a half turn about its x, so + turned back it has to be the rotation A B C describe: + Rz(C) Ry(B) Rx(A) */ + EmcPose w; + KINEMATICS_INVERSE_FLAGS inf = 0; + double ca, sa, cb, sb, cc, sc; + + memset(&w, 0, sizeof(w)); + kinematicsForward(j, &w, &ff, &inf); + ca = cos(w.a * TO_RAD); sa = sin(w.a * TO_RAD); + cb = cos(w.b * TO_RAD); sb = sin(w.b * TO_RAD); + cc = cos(w.c * TO_RAD); sc = sin(w.c * TO_RAD); + expect(close3(&tool.x, cc * cb, sc * cb, -sb), + "tool x is the flange x A B C give", j); + expect(close3(&tool.y, -(cc * sb * sa - sc * ca), -(sc * sb * sa + cc * ca), -(cb * sa)), + "tool y is the flange y A B C give, reversed", j); + expect(close3(&tool.z, -(cc * sb * ca + sc * sa), -(sc * sb * ca - cc * sa), -(cb * ca)), + "tool axis is the flange z A B C give, reversed", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 236bf357351..3a18f726919 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -70,4 +70,8 @@ run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" run "pumakins" "setp pumakins.A2 300" \ - "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" + "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0 abc_flange=1" + +# the same arm described by its DH chain +run "genserkins" "" \ + "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0 abc_flange=1" diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 4dd6c2d4843..e2693212c1b 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -170,8 +170,8 @@ run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" -run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" -run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0 orient=-" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0 orient=-" # parallel machines, from a pose the forward can be seeded with run "tripodkins" \ From c92628ea74253ded5634bc0a16cee017838fd33a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 18:46:45 +1000 Subject: [PATCH 03/24] tests: put the nutating head kinematics next to the tilted work plane maths The two trsrn configs carry their orientation maths in python, written apart from the kinematics modules, and nothing had compared the two. tests/kins-twp loads each module in realtime with a small component answering frame and inverse requests over HAL pins and drives it from python holding the twp functions. Over a grid of primary, secondary and table angles the module's tool frame equals the python transformation matrix to 1e-9; for a set of requested tool axes with the table held as the remap holds it, the joint pairs the module finds are the python's candidates and the spin it reports is the python's virtual rotation. With nothing held the module may turn the table, which the python never does, so there each side is judged by the other's maths. --- tests/kins-twp/README | 9 ++ tests/kins-twp/check.py | 273 ++++++++++++++++++++++++++++++++++ tests/kins-twp/checkresult | 3 + tests/kins-twp/skip | 4 + tests/kins-twp/test.sh | 30 ++++ tests/kins-twp/twp-xyzacb.ini | 16 ++ tests/kins-twp/twp-xyzbca.ini | 16 ++ tests/kins-twp/twpcheck.c | 165 ++++++++++++++++++++ 8 files changed, 516 insertions(+) create mode 100644 tests/kins-twp/README create mode 100755 tests/kins-twp/check.py create mode 100755 tests/kins-twp/checkresult create mode 100755 tests/kins-twp/skip create mode 100755 tests/kins-twp/test.sh create mode 100644 tests/kins-twp/twp-xyzacb.ini create mode 100644 tests/kins-twp/twp-xyzbca.ini create mode 100644 tests/kins-twp/twpcheck.c diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..b9b3b7275fa --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,9 @@ +The C kinematics against the tilted work plane maths. + +The two nutating-head configs carry their orientation maths in python, +remap_funcs_twp.py, written independently of the kinematics modules. +This test loads each module in realtime and puts its tool frame next to +the python transformation matrix over a grid of head angles, and its +tool frame inverse next to the python candidate joint angles and virtual +rotation for a set of requested tool axes. Where they disagree, one of +the two has the sign or the order of a rotation wrong. diff --git a/tests/kins-twp/check.py b/tests/kins-twp/check.py new file mode 100755 index 00000000000..2b8d52dd683 --- /dev/null +++ b/tests/kins-twp/check.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +# The python half of the tilted work plane cross-check. +# +# Imports the machine's remap_funcs_twp.py, the maths the tilted work +# plane remap orients the head with, and drives twpcheck, loaded after +# the kinematics module, to get the module's answers to the same +# questions. Two comparisons: +# +# frames over a grid of primary angle, secondary angle, virtual +# rotation and table angle, the module's tool frame in +# machine coordinates against the python transformation +# matrix Rp * Rs * Rtc +# inverse for a set of requested tool axes, with the table held as +# the remap holds it, the joint angle pairs the module's +# kinematicsToolFrameInverse() finds against the pairs the +# python candidate search keeps, and the spin about the tool +# the module reports for the python's horizontal tool x +# against the python's own virtual rotation; then with nothing +# held, where the module may turn the table, each side judged +# by the other's maths +# +# Usage: check.py MACHINE CONFIGDIR INIFILE +# MACHINE is xyzacb or xyzbca; CONFIGDIR holds remap_funcs_twp.py; +# INIFILE is what that file reads its letters and limits from. + +import os +import sys +import time +from math import radians, degrees, pi, sin, cos, atan2 + +import numpy as np +import hal + +machine, cfgdir, inifile = sys.argv[1:4] +os.environ["INI_FILE_NAME"] = inifile +sys.path.insert(0, cfgdir) +import remap_funcs_twp as twp + +# joint numbers: the table, the secondary and the primary rotary +TABLE, SECONDARY, PRIMARY = {"xyzacb": (3, 4, 5), "xyzbca": (4, 3, 5)}[machine] +PREROT = "%s_trsrn_kins.pre-rot" % machine +TOL = 1e-9 +ANGLE_TOL = 1e-6 # degrees + +failures = 0 +def fail(what): + global failures + failures += 1 + print("kins-twp: FAIL %s: %s" % (machine, what)) + +class Log: + def debug(self, *a, **k): pass + def error(self, *a, **k): print("kins-twp: python error:", a[0] % tuple(a[1:]) if len(a) > 1 else a[0]) +log = Log() + +# ---- driving twpcheck + +request = 0 +def ask(j, axis=None, xdir=None, held=0): + """set the joints and the request, wait for the answer""" + global request + for i, v in enumerate(j): + hal.set_p("twpcheck.j-%d" % i, str(v)) + hal.set_p("twpcheck.held", str(held)) + for i, c in enumerate("xyz"): + hal.set_p("twpcheck.axis-%s" % c, str(axis[i] if axis is not None else 0.0)) + hal.set_p("twpcheck.xdir-%s" % c, str(xdir[i] if xdir is not None else 0.0)) + hal.set_p("twpcheck.have-x", "1" if xdir is not None else "0") + request += 1 + hal.set_p("twpcheck.request", str(request)) + deadline = time.time() + 5 + while hal.get_value("twpcheck.done") != request: + if time.time() > deadline: + print("kins-twp: FAIL twpcheck did not answer") + sys.exit(1) + time.sleep(0.002) + +def read_matrix(name): + return np.array([[hal.get_value("twpcheck.%s-%d%d" % (name, r, c)) for c in range(3)] + for r in range(3)]) + +def read_solutions(): + n = hal.get_value("twpcheck.nsol") + sols = [] + for k in range(max(n, 0)): + sols.append(([hal.get_value("twpcheck.sol-%d-%d" % (k, i)) for i in range(6)], + hal.get_value("twpcheck.spin-%d" % k), + hal.get_value("twpcheck.free-%d" % k))) + return n, sols + +def joints_at(table, secondary, primary): + j = [10.0, 20.0, 30.0, 0.0, 0.0, 0.0] + j[TABLE], j[SECONDARY], j[PRIMARY] = table, secondary, primary + return j + +# ---- the python's answers + +def py_matrix(primary_deg, secondary_deg, tc): + m = twp.kins_calc_transformation_matrix(radians(primary_deg), radians(secondary_deg), tc, + np.asmatrix(np.identity(4)), 'inv') + return np.array(m)[:3, :3] + +def py_pairs(z): + """the (primary, secondary) pairs in degrees the remap would keep for a + tool axis, following remap.py: every combination of the candidate + lists, kept where it reaches the axis""" + t1, t2 = twp.kins_calc_possible_joint_angles(log, np.array(z), None) + if t1 is None or t2 is None: + return [] + pairs = [] + for a in set(t1): + for b in set(t2): + m = py_matrix(degrees(a), degrees(b), 0.0) + if np.allclose(m[:, 2], z, atol=1e-6): + pairs.append((degrees(a), degrees(b))) + return pairs + +def same_angle(a, b): + d = (a - b + 180.0) % 360.0 - 180.0 + return abs(d) < ANGLE_TOL + +def same_pair(p, q): + return same_angle(p[0], q[0]) and same_angle(p[1], q[1]) + +def fmt(m): + return np.array2string(m, precision=6, suppress_small=True) + +# ---- frames +# +# The module's frame is the head's rotation from its joints alone, so it +# is compared with the python matrix at zero virtual rotation; whether the +# frame should carry the virtual rotation too is a convention question the +# test does not settle. + +frames = 0 +hal.set_p(PREROT, "0") +for table in (0.0, 20.0): + for primary in (0.0, 30.0, -25.0, 90.0, 180.0, -135.0): + for secondary in (0.0, 30.0, -25.0, 90.0, -90.0, 180.0): + ask(joints_at(table, secondary, primary)) + if hal.get_value("twpcheck.frame-rc") != 0: + fail("no frame at primary %g secondary %g" % (primary, secondary)) + continue + tool = read_matrix("tool") + want = py_matrix(primary, secondary, 0.0) + frames += 1 + if not np.allclose(tool, want, atol=TOL): + fail("tool frame differs at primary %g secondary %g table %g\n module:\n%s\n python:\n%s" + % (primary, secondary, table, fmt(tool), fmt(want))) + +# ---- inverse, the table held +# +# The remap holds the table and orients the head, so ask the module the +# same: the joint pairs must then be the python's, and the spin about the +# tool for the python's horizontal tool x must be the python's virtual +# rotation. + +def rz(a): + return np.array([[cos(a), -sin(a), 0.0], [sin(a), cos(a), 0.0], [0.0, 0.0, 1.0]]) + +def frames_at(j): + ask(j) + return read_matrix("work"), read_matrix("tool") + +def in_work(work, tool): + return work.T @ tool + +HOLD_TABLE = 1 << TABLE +REQUESTS = ((30.0, 30.0), (-25.0, 60.0), (120.0, -45.0), (180.0, 90.0), + (0.0, 0.0), (90.0, 135.0), (45.0, 170.0), (-100.0, -20.0)) + +requests = 0 +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + if not any(same_pair(p, (primary, secondary)) for p in pairs): + fail("the python does not find the pair (%g, %g) the axis was made from" % (primary, secondary)) + + ask(seed, axis=z, held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("with the table held, the module finds no solution for " + where) + continue + found = [(s[0][PRIMARY], s[0][SECONDARY]) for s in sols] + for s in sols: + j, spin, free = s + if abs(j[TABLE] - seed[TABLE]) > 1e-12: + fail("the held table moved for " + where) + if free != 0 and (primary, secondary) != (0.0, 0.0): + fail("with the table held a solution is still a family for " + where) + for p in pairs: + if not any(same_pair(p, f) for f in found): + fail("python pair (%.6f, %.6f) not among the module's %s for %s" + % (p[0], p[1], ["(%.6f, %.6f)" % f for f in found], where)) + for f in found: + if not any(same_pair(p, f) for p in pairs): + fail("module pair (%.6f, %.6f) not among the python's %s for %s" + % (f[0], f[1], ["(%.6f, %.6f)" % p for p in pairs], where)) + + # tool x as the python's virtual rotation places it, horizontal: the + # module, holding the table, must answer the same pair with that spin + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + if abs(full[2, 0]) > 1e-9: + fail("python virtual rotation %g leaves tool x off horizontal for pair (%.6f, %.6f)" % (tc, p[0], p[1])) + ask(seed, axis=z, xdir=full[:, 0], held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + match = [s for s in sols if same_pair((s[0][PRIMARY], s[0][SECONDARY]), p)] + if not match: + fail("with tool x given and the table held, pair (%.6f, %.6f) is gone from the module's answers" % p) + continue + spin = match[0][1] + if abs((spin - tc + pi) % (2 * pi) - pi) > 1e-6: + fail("module spin %.9f and python virtual rotation %.9f differ for pair (%.6f, %.6f)" + % (spin, tc, p[0], p[1])) + +# ---- inverse, nothing held +# +# The module may now turn the table, since it turns the tool against the +# work as surely as the head does, and reports one member of the family +# that results. Not the python's answer, so each is judged by the other's +# maths: a module solution must reach the axis through the python head +# matrix composed with the module's table frame, and with tool x given it +# must reach the whole frame. + +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + + ask(seed, axis=z) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution for " + where) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + if not np.allclose(tool, py_matrix(j[PRIMARY], j[SECONDARY], 0.0), atol=TOL): + fail("module frame at its own solution differs from the python head matrix for " + where) + if not np.allclose(in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0))[:, 2], z, atol=1e-6): + fail("module solution %s does not reach %s by the python head matrix" % (fmt(np.array(j)), where)) + for i in (0, 1, 2): + if abs(j[i] - seed[i]) > 1e-9: + fail("solution moved linear joint %d for %s" % (i, where)) + + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + ask(seed, axis=z, xdir=full[:, 0]) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution with tool x given for pair (%.6f, %.6f)" % p) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + achieved = in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0) @ rz(spin)) + if not np.allclose(achieved, full, atol=1e-6): + fail("module solution %s spin %.6f does not reach the python frame for pair (%.6f, %.6f)\n achieved:\n%s\n wanted:\n%s" + % (fmt(np.array(j)), spin, p[0], p[1], fmt(achieved), fmt(full))) + +if failures: + sys.exit(1) +print("kins-twp: %s agrees, %d frames, %d requests" % (machine, frames, requests)) diff --git a/tests/kins-twp/checkresult b/tests/kins-twp/checkresult new file mode 100755 index 00000000000..011ea9232ad --- /dev/null +++ b/tests/kins-twp/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'kins-twp: .* agrees' "$1")" = 2 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-twp/skip b/tests/kins-twp/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-twp/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh new file mode 100755 index 00000000000..86b77f6ff44 --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -e + +# RIP layout: $HEADERS is $TOPDIR/include +TOPDIR=$(dirname "$HEADERS") +CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating + +${SUDO} halcompile --install twpcheck.c >/dev/null + +# One hal file per machine. twpcheck answers frame and inverse requests +# from check.py over HAL pins; check.py holds the python maths. +run() { + local machine=$1 hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s_trsrn\n' "$machine" + printf 'setp %s_trsrn_kins.nut-angle 45\n' "$machine" + printf 'loadrt twpcheck joints=6 ktype=1\n' + printf 'loadrt threads name1=t1 period1=1000000\n' + printf 'addf twpcheck t1\n' + printf 'start\n' + printf 'loadusr -w python3 check.py %s %s/%s-trsrn_twp %s/twp-%s.ini\n' \ + "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + } > "$hal" + echo "=== $machine" + halrun -f "$hal" + rm -f "$hal" +} + +run xyzacb +run xyzbca diff --git a/tests/kins-twp/twp-xyzacb.ini b/tests/kins-twp/twp-xyzacb.ini new file mode 100644 index 00000000000..7d276d86bf8 --- /dev/null +++ b/tests/kins-twp/twp-xyzacb.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzacb_trsrn + +[TWP] +PRIMARY = C +SECONDARY = B + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_B] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twp-xyzbca.ini b/tests/kins-twp/twp-xyzbca.ini new file mode 100644 index 00000000000..b6bfd198e60 --- /dev/null +++ b/tests/kins-twp/twp-xyzbca.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzbca_trsrn + +[TWP] +PRIMARY = C +SECONDARY = A + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_A] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twpcheck.c b/tests/kins-twp/twpcheck.c new file mode 100644 index 00000000000..7113dff88b5 --- /dev/null +++ b/tests/kins-twp/twpcheck.c @@ -0,0 +1,165 @@ +/* + * twpcheck: the realtime half of the tilted work plane cross-check. + * + * Loaded after a kinematics module, it answers requests made over HAL + * pins: for the joint values on its inputs it reports the module's tool + * frame and work frame, and for the tool axis (and optionally tool x) + * on its inputs it reports what kinematicsToolFrameInverse() finds, the + * joint sets and the spin about the tool each needs, with the joints + * named on the held pin kept where they are. check.py drives it and + * holds the python maths the answers are compared with. + * + * A request is made by raising the request pin; done follows it when + * the answers are on the pins. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + */ +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 6; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); + +static int comp_id = -1; + +#define NSOL TOOL_FRAME_MAX_SOLUTIONS + +static struct { + hal_real_t j[EMCMOT_MAX_JOINTS]; + hal_real_t axis[3]; + hal_real_t xdir[3]; + hal_bool_t have_x; + hal_uint_t held; /* bit per joint the inverse may not move */ + hal_uint_t request; + hal_uint_t done; + hal_real_t tool[3][3]; /* [row][column], columns are the frame's axes */ + hal_real_t work[3][3]; + hal_sint_t frame_rc; + hal_sint_t nsol; + hal_real_t sol[NSOL][EMCMOT_MAX_JOINTS]; + hal_real_t spin[NSOL]; + hal_sint_t free[NSOL]; +} *pins; + +static void publish(hal_real_t out[3][3], const PmRotationMatrix *m) +{ + hal_set_real(out[0][0], m->x.x); hal_set_real(out[0][1], m->y.x); hal_set_real(out[0][2], m->z.x); + hal_set_real(out[1][0], m->x.y); hal_set_real(out[1][1], m->y.y); hal_set_real(out[1][2], m->z.y); + hal_set_real(out[2][0], m->x.z); hal_set_real(out[2][1], m->y.z); hal_set_real(out[2][2], m->z.z); +} + +static void update(void *arg, long period) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, work; + PmCartesian axis, xdir; + double j[EMCMOT_MAX_JOINTS]; + double sols[NSOL * EMCMOT_MAX_JOINTS]; /* rows of joints doubles, packed */ + double spin[NSOL]; + int freed[NSOL]; + int i, k, n, rc; + (void)arg; + (void)period; + + if (hal_get_ui32(pins->request) == hal_get_ui32(pins->done)) { return; } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = i < joints ? hal_get_real(pins->j[i]) : 0.0; + } + + rc = kinematicsToolFrame(j, &tool, &ff); + if (!rc) { rc = kinematicsWorkFrame(j, &work, &ff); } + hal_set_si32(pins->frame_rc, rc); + if (!rc) { + publish(pins->tool, &tool); + publish(pins->work, &work); + } + + axis.x = hal_get_real(pins->axis[0]); + axis.y = hal_get_real(pins->axis[1]); + axis.z = hal_get_real(pins->axis[2]); + xdir.x = hal_get_real(pins->xdir[0]); + xdir.y = hal_get_real(pins->xdir[1]); + xdir.z = hal_get_real(pins->xdir[2]); + n = -1; + if (axis.x != 0 || axis.y != 0 || axis.z != 0) { + n = kinematicsToolFrameInverse(&axis, hal_get_bool(pins->have_x) ? &xdir : NULL, + j, hal_get_ui32(pins->held), sols, NSOL, + freed, spin); + } + hal_set_si32(pins->nsol, n); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + hal_set_real(pins->sol[k][i], k < n ? sols[k * joints + i] : 0.0); + } + hal_set_real(pins->spin[k], k < n ? spin[k] : 0.0); + hal_set_si32(pins->free[k], k < n ? freed[k] : 0); + } + + hal_set_ui32(pins->done, hal_get_ui32(pins->request)); +} + +int rtapi_app_main(void) +{ + static const char letter[3] = { 'x', 'y', 'z' }; + int i, k, r, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("twpcheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->j[i], 0.0, "twpcheck.j-%d", i); + } + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->axis[i], 0.0, "twpcheck.axis-%c", letter[i]); + res += hal_pin_new_real(comp_id, HAL_IN, &pins->xdir[i], 0.0, "twpcheck.xdir-%c", letter[i]); + } + res += hal_pin_new_bool(comp_id, HAL_IN, &pins->have_x, 0, "twpcheck.have-x"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->held, 0, "twpcheck.held"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->request, 0, "twpcheck.request"); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &pins->done, 0, "twpcheck.done"); + for (r = 0; r < 3; r++) { + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->tool[r][i], 0.0, "twpcheck.tool-%d%d", r, i); + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->work[r][i], 0.0, "twpcheck.work-%d%d", r, i); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->frame_rc, 0, "twpcheck.frame-rc"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->nsol, 0, "twpcheck.nsol"); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->sol[k][i], 0.0, "twpcheck.sol-%d-%d", k, i); + } + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->spin[k], 0.0, "twpcheck.spin-%d", k); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->free[k], 0, "twpcheck.free-%d", k); + } + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + if (hal_export_funct("twpcheck", update, NULL, 0, comp_id)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } From 7095a1b7893e89c6d5ce0a28ce538c5f639f4743 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:22:08 +1000 Subject: [PATCH 04/24] motion: add the point-to-point move, interpolated in joint space A segment whose endpoint is Cartesian but whose interpolation is in joint space: the inverse runs once, at the endpoint, when the move is queued, and the planner runs every joint from where the queue ends to there together, the slowest setting the pace. Its length is the joint space distance with the tightest per-joint velocity, acceleration and jerk limits scaled onto it. Nothing blends into or out of it. While it runs the servo thread takes the joints from the planner and reports the tool from the forward kinematics; a forward that fails, as an iterating one can at the singularity the move is there to cross, leaves the last solved position and carte_pos_cmd_ok cleared. This is how a program crosses a singularity or flips a head without leaving its coordinate system. The move is a rapid with the rapid override or a feed that takes a given time with the feed override; the endpoint can also be given as joints, whose world position motion finds with the forward. Canon JOINT_TRAVERSE and JOINT_FEED, NML EMC_TRAJ_JOINT_MOVE and EMCMOT_SET_JOINT_LINE carry it; the preview draws the straight line between its ends. An external offset cannot ride on a joint segment: the queue refuses the segment while one is applied, and a request arriving while one is queued waits until the last joint segment is done. A queue reset forgets the joint-space bookkeeping with the queue. --- src/emc/motion/command.c | 158 +++++++++++++++++++++++++++ src/emc/motion/control.c | 34 +++++- src/emc/motion/motion.h | 8 ++ src/emc/nml_intf/canon.hh | 18 ++++ src/emc/nml_intf/emc.cc | 15 +++ src/emc/nml_intf/emc.hh | 2 + src/emc/nml_intf/emc_nml.hh | 21 ++++ src/emc/rs274ngc/gcodemodule.cc | 22 ++++ src/emc/sai/saicanon.cc | 43 ++++++++ src/emc/task/emccanon.cc | 41 +++++++ src/emc/task/emctaskmain.cc | 12 +++ src/emc/task/taskintf.cc | 17 +++ src/emc/tp/tc.c | 71 ++++++++++++- src/emc/tp/tc.h | 1 + src/emc/tp/tc_types.h | 20 +++- src/emc/tp/tp.c | 183 +++++++++++++++++++++++++++++++- src/emc/tp/tp.h | 9 ++ src/emc/tp/tp_types.h | 11 ++ 18 files changed, 681 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index fd9760e59ac..3666ffde43e 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1122,6 +1122,164 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } break; + case EMCMOT_SET_JOINT_LINE: { + /* a move interpolated in joint space to a Cartesian endpoint: the + inverse runs once here, at the endpoint, and the planner takes + the joints from there; or the endpoint is given as joints and + the forward says where that is */ + double start[EMCMOT_MAX_JOINTS], target[EMCMOT_MAX_JOINTS]; + EmcPose end = emcmotCommand->pos; + double length = 0.0, vmax = 0.0, amax = 0.0, jmax = 0.0; + int moving = 0, jerk_limited = 0, bad = 0, axis_num; + + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_LINE"); + if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { + reportError(_("need to be enabled, in coord mode for joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; + SET_MOTION_ERROR_FLAG(1); + break; + } + if (!limits_ok()) { + reportError(_("can't do joint interpolated move with limits exceeded")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + /* an external offset is applied to the world position on the way + to the inverse every cycle, which a joint interpolated segment + does not go through */ + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + if (axis_get_ext_offset_curr_pos(axis_num) != 0.0) { bad = 1; } + } + if (bad) { + reportError(_("can't do joint interpolated move on line %d with an external offset applied"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* where the queue ends in joint space */ + if (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + EmcPose goal; + tpGetGoalPos(&emcmotInternal->coord_tp, &goal); + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; + } + if (kinematicsInverse(&goal, start, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { target[joint_num] = start[joint_num]; } + if (emcmotCommand->have_joint_target) { + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + target[joint_num] = emcmotCommand->joint_target[joint_num]; + } + if (kinematicsForward(target, &end, &fflags, &iflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsForward"), + emcmotCommand->id); + 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")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + if (kinematicsInverse(&end, target, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + /* the endpoint must be inside the joint limits, and every joint + that moves needs limits to move within; the segment length is + the joint space distance and each joint's limits are scaled + onto it so that the slowest joint sets the pace */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + double d = target[joint_num] - start[joint_num]; + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint)) { continue; } + if (!isfinite(target[joint_num])) { + reportError(_("joint interpolated move on line %d gave non-finite joint location on joint %d"), + emcmotCommand->id, joint_num); + bad = 1; + } else if (target[joint_num] > joint->max_pos_limit || target[joint_num] < joint->min_pos_limit) { + reportError(_("joint interpolated move on line %d would exceed joint %d's limit"), + emcmotCommand->id, joint_num); + bad = 1; + } + length += d * d; + } + length = sqrt(length); + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS && !bad; joint_num++) { + double d = fabs(target[joint_num] - start[joint_num]); + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint) || d < TP_POS_EPSILON) { continue; } + if (joint->vel_limit <= 0.0 || joint->acc_limit <= 0.0) { + reportError(_("joint interpolated move on line %d: joint %d has no velocity or acceleration limit"), + emcmotCommand->id, joint_num); + bad = 1; + break; + } + if (!moving || joint->vel_limit * length / d < vmax) { vmax = joint->vel_limit * length / d; } + if (!moving || joint->acc_limit * length / d < amax) { amax = joint->acc_limit * length / d; } + if (joint->jerk_limit > 0.0) { + if (!jerk_limited || joint->jerk_limit * length / d < jmax) { jmax = joint->jerk_limit * length / d; } + jerk_limited = 1; + } + moving = 1; + } + if (bad) { + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* a feed asks for a time; the joint limits still cap it */ + double vreq = vmax; + if (emcmotCommand->joint_seconds > 0.0 && length / emcmotCommand->joint_seconds < vmax) { + vreq = length / emcmotCommand->joint_seconds; + } + tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id); + int res_addjoint = tpAddJointLine(&emcmotInternal->coord_tp, + start, target, NO_OF_KINS_JOINTS, end, + emcmotCommand->motion_type, + vreq, vmax, amax, jmax, + emcmotStatus->enables_new, + emcmotCommand->tag); + if (res_addjoint < 0) { + reportError(_("can't add joint interpolated move at line %d, error code %d"), + emcmotCommand->id, res_addjoint); + emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } else if (res_addjoint == 0) { + SET_MOTION_ERROR_FLAG(0); + rehomeAll = 1; + } + break; + } + case EMCMOT_SET_CIRCLE: /* emcmotInternal->coord_tp up a circular move */ /* requires coordinated mode, enable on, not on limits */ diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 7c9a35c237b..87dcb467305 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1430,10 +1430,41 @@ static void get_pos_cmds(long period) emcmotStatus->syncOverrunSpindle = 0; SET_MOTION_ERROR_FLAG(1); } + + if (tpGetJointPos(&emcmotInternal->coord_tp, positions) > 0) { + /* a joint interpolated segment: the planner hands out the + joints and the forward kinematics says where the tool is, + for status and for the display; nothing is inverted, and + the planner's own position is the chord between the ends. + The joints are commanded either way; a forward that fails, + as an iterating one can at a singularity, leaves the last + solved position reported rather than an unsolved one */ + EmcPose pose = emcmotStatus->carte_pos_cmd; + if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { + emcmotStatus->carte_pos_cmd = pose; + emcmotStatus->carte_pos_cmd_ok = 1; + } else { + emcmotStatus->carte_pos_cmd_ok = 0; + } + result = 0; + } else { + /* a joint interpolated segment that ended this cycle is gone + from the queue: its end joints seed the inverse, since the + modules that read their rotary angles from the seed would + otherwise get last cycle's */ + tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); - if (axis_update_coord_with_bound(pcmd_p, servo_period)) { + if (tpJointSegmentsQueued(&emcmotInternal->coord_tp)) { + /* an external offset cannot ride on a joint interpolated + segment: its joints were solved without one, and the + queue refused the segment while one was applied. A + request that arrives while one is queued waits here, + unplanned, and ramps in at its own limits once the last + joint segment is done, instead of landing as a step at + the segment's ends */ + } else if (axis_update_coord_with_bound(pcmd_p, servo_period)) { ext_offset_coord_limit = 1; } else { ext_offset_coord_limit = 0; @@ -1442,6 +1473,7 @@ static void get_pos_cmds(long period) /* OUTPUT KINEMATICS - convert to joints in local array */ result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags); + } if(result == 0) { /* copy to joint structures and spline them up */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1a48aa02f57..d5ce5fe936a 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -117,6 +117,7 @@ extern "C" { EMCMOT_SET_LINE, /* queue up a linear move */ EMCMOT_SET_CIRCLE, /* queue up a circular move */ + EMCMOT_SET_JOINT_LINE, /* queue up a joint interpolated move */ EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */ EMCMOT_PROBE, /* go to pos, stop if probe trips, record trip pos */ @@ -278,6 +279,13 @@ extern "C" { struct state_tag_t tag; int switchkins_type; /* switchkins type requested by G12.1 */ + + /* a joint interpolated move: either pos is the endpoint and the joints + come from the inverse, or these are the joints and pos comes from + the forward */ + double joint_target[EMCMOT_MAX_JOINTS]; + int have_joint_target; + double joint_seconds; /* 0 for a rapid, else the time the move is to take */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 9a0a4de911c..421ce6ea50f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -298,6 +298,24 @@ extern void STRAIGHT_TRAVERSE(int lineno, double x, double y, double z, double a, double b, double c, double u, double v, double w); + +/* A traverse interpolated in joint space. The endpoint x..w is in program + coordinates like STRAIGHT_TRAVERSE's; motion runs the inverse once there + and interpolates the joints to it. With have_joints the joints given + (machine units, one per joint) are the endpoint instead and x..w say + where the interpreter believes that is. Nothing blends into or out of + it. */ +extern void JOINT_TRAVERSE(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); +/* The same move at feed: it is to take 'seconds' seconds at the programmed + feed, the feed override applies, and the joint limits still cap it. */ +extern void JOINT_FEED(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds); /* Move at traverse rate so that at any time during the move, all axes diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index dee2d5699fc..6805097bc73 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -295,6 +295,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_G68_TYPE: ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -510,6 +513,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_ROTATION"; case EMC_TRAJ_SET_G68_TYPE: return "EMC_TRAJ_SET_G68"; + case EMC_TRAJ_JOINT_MOVE_TYPE: + return "EMC_TRAJ_JOINT_MOVE"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1660,6 +1665,16 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) cms->update(active); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index d251e08abc6..269bfaf4fad 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -109,6 +109,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) #define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) +#define EMC_TRAJ_JOINT_MOVE_TYPE ((NMLTYPE) 240) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -375,6 +376,7 @@ extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale, int indexer_jnum); +extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index f71030156bb..9c1787e3b67 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -935,6 +935,27 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { unsigned char probe_type; }; +// a move interpolated in joint space: to the world endpoint, whose joints +// motion finds with the inverse; or to the joints given, whose world +// position motion finds with the forward +class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_JOINT_MOVE() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), + end{}, joints{}, have_joints(0), seconds(0.0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose end; + double joints[EMCMOT_MAX_JOINTS]; + int have_joints; + double seconds; /* 0 for a rapid, else the time it is to take */ +}; + class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_RIGID_TAP() diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f725a708dff..886b194c5bb 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -526,6 +526,28 @@ void STRAIGHT_FEED(int line_number, ensure_inch({x, y, z, a, b, c, u, v, w})); } +// the preview draws a joint interpolated move as the traverse between its +// ends: the path between them depends on the kinematics, which the +// preview does not have +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)joints; + (void)have_joints; + STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, double seconds) { + (void)joints; + (void)have_joints; + (void)seconds; + STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 11ea4dfae7e..5eceebc3cce 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -222,6 +222,49 @@ void SET_TRAVERSE_RATE(double rate) _sai._traverse_rate = rate; } +void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + +void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/, + double seconds) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c, seconds); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c, seconds); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void STRAIGHT_TRAVERSE( int /*line_number*/, double x, double y, double z , double a /*AA*/ diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 5ad736c59e8..75c6ac54888 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1146,6 +1146,47 @@ void generate_fast_move(double x, double y, double z, canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } +static void joint_move(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + auto msg = std::make_unique(); + + flush_segments(); + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = have_joints ? 1 : 0; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { + msg->joints[i] = (have_joints && joints) ? joints[i] : 0.0; + } + msg->seconds = seconds; + interp_list.set_line_number(line_number); + tag_and_send(std::move(msg), _tag); + + canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); +} + +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, 0.0); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, seconds); +} + void generate_move(double vel,double x, double y, double z, double a, double b, double c, double u, double v, double w) diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 049c640f1af..8461c06baba 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -527,6 +527,9 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) case EMC_TRAJ_LINEAR_MOVE_TYPE: break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + break; + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: break; @@ -1564,6 +1567,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) break; case EMC_TRAJ_LINEAR_MOVE_TYPE: + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: case EMC_TRAJ_SET_ACCELERATION_TYPE: @@ -1931,6 +1935,13 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajLinearMoveMsg->indexer_jnum); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: { + EMC_TRAJ_JOINT_MOVE *jm = reinterpret_cast(cmd); + emcTrajUpdateTag(jm->tag); + retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds); + break; + } + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: emcTrajUpdateTag((reinterpret_cast(cmd))->tag); emcTrajCircularMoveMsg = reinterpret_cast(cmd); @@ -2578,6 +2589,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_SYSTEM_CMD; break; + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_LINEAR_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 2a429b0f66b..9416079d437 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1476,6 +1476,23 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } +int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds) +{ + int i; + + emcmotCommand.command = EMCMOT_SET_JOINT_LINE; + emcmotCommand.pos = end; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.motion_type = seconds > 0.0 ? EMC_MOTION_TYPE_FEED : EMC_MOTION_TYPE_TRAVERSE; + emcmotCommand.joint_seconds = seconds; + emcmotCommand.have_joint_target = have_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + emcmotCommand.joint_target[i] = (have_joints && joints) ? joints[i] : 0.0; + } + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcTrajSetOffset(const EmcPose& tool_offset) { emcmotCommand.command = EMCMOT_SET_OFFSET; diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 25953a8e01c..72976edb34b 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -200,6 +200,7 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou tcCircleStartAccelUnitVector(tc,out); break; case TC_SPHERICAL: + case TC_JOINT: return -1; default: return -1; @@ -207,6 +208,24 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * The world direction of a joint interpolated segment, end minus start, + * for the status fields that want a direction. The path between them is + * not straight, so this is the chord, and there is none when only the + * rotaries move. + */ +static int tcJointChordUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian d; + double mag; + + pmCartCartSub(&tc->coords.joint.world_end.tran, &tc->coords.joint.world_start.tran, &d); + pmCartMag(&d, &mag); + if (mag < TP_POS_EPSILON) { return -1; } + pmCartScalMult(&d, 1.0 / mag, out); + return 0; +} + /** * Get the acceleration direction unit vector for blend velocity calculations. * This calculates the direction of acceleration at the end of a segment. @@ -325,6 +344,8 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -348,6 +369,8 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -403,6 +426,8 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -526,6 +551,30 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_JOINT: { + // the ends are exact; between them this is the chord, a proxy + // for the status fields, and the servo thread reports the + // real position from the forward kinematics + const PmJointLine *jl = &tc->coords.joint; + double f = (tc->target > 0.0) ? progress / tc->target : 0.0; + EmcPose d; + + emcPoseSub(&jl->world_end, &jl->world_start, &d); + pos->tran.x = jl->world_start.tran.x + f * d.tran.x; + pos->tran.y = jl->world_start.tran.y + f * d.tran.y; + pos->tran.z = jl->world_start.tran.z + f * d.tran.z; + pos->a = jl->world_start.a + f * d.a; + pos->b = jl->world_start.b + f * d.b; + pos->c = jl->world_start.c + f * d.c; + pos->u = jl->world_start.u + f * d.u; + pos->v = jl->world_start.v + f * d.v; + pos->w = jl->world_start.w + f * d.w; + if (of_point == TC_GET_ENDPOINT) { *pos = jl->world_end; } + return TP_ERR_OK; + } + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetPosReal!\n", tc->motion_type); + return TP_ERR_FAIL; } if (res_fit == TP_ERR_OK) { @@ -536,6 +585,26 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) } +/** + * The joints of a joint interpolated segment at its progress. + * Returns the joint count, or 0 for any other segment. + */ +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) +{ + const PmJointLine *jl; + double f; + int i; + + if (!tc || tc->motion_type != TC_JOINT) { return 0; } + jl = &tc->coords.joint; + f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; + if (f > 1.0) { f = 1.0; } + for (i = 0; i < jl->num_joints; i++) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } + return jl->num_joints; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. @@ -621,7 +690,7 @@ int tcIsBlending(TC_STRUCT * const tc) { //FIXME Disabling blends for rigid tap cycle until changes can be verified. int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; + tc->motion_type != TC_RIGIDTAP && tc->motion_type != TC_JOINT; //Latch up the blending_next status here, so that even if the prev conditions //aren't necessarily true we still blend to completion once the blend diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index dcafd088e31..d6072f824c2 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -37,6 +37,7 @@ int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints); int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index c7406ecf801..53e11266d31 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -33,7 +33,8 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_JOINT = 5 } tc_motion_type_t; typedef enum { @@ -117,6 +118,19 @@ typedef struct { RIGIDTAP_STATE state; } PmRigidTap; +/* A segment interpolated in joint space: every joint runs from start to + * end together, the longest one setting the pace. The world poses at the + * two ends are what the segments around it see; the position along the way + * is not a line in world space and the servo thread reports it from the + * forward kinematics. */ +typedef struct { + double start[EMCMOT_MAX_JOINTS]; + double end[EMCMOT_MAX_JOINTS]; + int num_joints; + EmcPose world_start; + EmcPose world_end; +} PmJointLine; + typedef struct { double cycle_time; //Position stuff @@ -161,11 +175,13 @@ typedef struct { PmCircle9 circle; PmRigidTap rigidtap; Arc9 arc; + PmJointLine joint; } coords; int motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or - // TC_RIGIDTAP (coords.rigidtap) + // TC_RIGIDTAP (coords.rigidtap) or + // TC_JOINT (coords.joint) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? int term_cond; // gcode requests continuous feed at the end of diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 2b4024c9ab1..8302a2793fb 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -158,6 +158,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; + case TC_JOINT: + return true; default: tp_debug_print("Unknown motion type!\n"); return false; @@ -439,12 +441,23 @@ STATIC void tpReleaseQueuedPlanners(TP_STRUCT * const tp) * intended to put the motion queue in the state it would be if all queued * motions finished at the current position. */ +/* What the planner knows in joint space belongs to the queue: with the + queue reset, the queue end is no longer at those joints, no segment is + left to hand its end joints out, and no joint segment is waiting. */ +STATIC void tpForgetJoints(TP_STRUCT * const tp) +{ + tp->queue_end_joints_valid = 0; + tp->joint_end_valid = 0; + tp->joint_segments_queued = 0; +} + int tpClear(TP_STRUCT * const tp) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + tpForgetJoints(tp); // Clear out status ID's tp->nextId = 0; tp->execId = 0; @@ -1651,6 +1664,8 @@ int tpAddRigidTap(TP_STRUCT * const tp, acc, ini_maxjerk); + tp->queue_end_joints_valid = 0; + // Setup rigid tap geometry pmRigidTapInit(&tc.coords.rigidtap, &tp->goalPos, @@ -2073,6 +2088,11 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp_debug_print(" queue empty\n"); return NO_BLEND; } + if (prev_tc->motion_type == TC_JOINT) { + // nothing blends with a joint interpolated segment + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + return NO_BLEND; + } if (prev_tc->progress > prev_tc->target / 2.0) { tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); return NO_BLEND; @@ -2111,6 +2131,80 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { return blend_used; } +/** + * Add a joint interpolated segment to the tc queue. + * + * The joints run from start to end together over a "length" that is the + * joint space distance between them. vel and acc are already the tightest + * per-joint limits scaled onto that length, so no joint exceeds its own. + * Nothing blends into or out of it: the segment before it is made to stop + * and so is this one, since the path between the two world poses is not a + * line and the next segment has to start from rest at world_end. + */ +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag) +{ + TC_STRUCT tc = {0}; + PmJointLine *jl = &tc.coords.joint; + TC_STRUCT *prev_tc; + double length = 0.0; + int i; + + if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return TP_ERR_MISSING_INPUT; + } + if (tp->aborting) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); + return TP_ERR_FAIL; + } + + tcInit(&tc, TC_JOINT, canon_motion_type, tp->cycleTime, enables, 0); + tc.tag = tag; + tpSetupSyncedIO(tp, &tc); + tcSetupState(&tc, tp); + // a joint move has no path to synchronise to a spindle along + tc.synchronized = TC_SYNC_NONE; + tc.uu_per_rev = 0.0; + tcSetupMotion(&tc, vel, ini_maxvel, acc, ini_maxjerk); + + jl->num_joints = num_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + jl->start[i] = (i < num_joints) ? start[i] : 0.0; + jl->end[i] = (i < num_joints) ? end[i] : 0.0; + length += (jl->end[i] - jl->start[i]) * (jl->end[i] - jl->start[i]); + } + jl->world_start = tp->goalPos; + jl->world_end = world_end; + + tc.target = pmSqrt(length); + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + // it runs in joint units, so the max velocity slider does not cap it + tc.vlimit_scale = 0.0; + tc.indexer_jnum = -1; + tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + + prev_tc = tcqLast(&tp->queue); + if (prev_tc) { + tcSetTermCond(prev_tc, &tc, TC_TERM_COND_STOP); + tcFinalizeLength(prev_tc); + } + + int retval = tpAddSegmentToQueue(tp, &tc, true); + if (retval == TP_ERR_OK) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->queue_end_joints[i] = jl->end[i]; } + tp->queue_end_joints_valid = 1; + tp->joint_segments_queued++; + } + tpRunOptimization(tp); + return retval; +} + //TODO final setup steps as separate functions // /** @@ -2153,6 +2247,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, ini_maxjerk); tc.vlimit_scale = vlimit_scale; // Setup line geometry + tp->queue_end_joints_valid = 0; pmLine9Init(&tc.coords.line, &tp->goalPos, &end); @@ -2225,6 +2320,7 @@ int tpAddCircle(TP_STRUCT * const tp, tp->cycleTime, enables, atspeed); + tp->queue_end_joints_valid = 0; tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); @@ -3320,6 +3416,7 @@ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3375,6 +3472,16 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, return TP_ERR_FAIL; } + // a joint interpolated segment leaves its end joints behind for the + // servo thread: it asks after the segment is gone, and would otherwise + // invert the end position with the previous cycle's joints as seed + if (tc->motion_type == TC_JOINT) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = tc->coords.joint.end[i]; } + tp->joint_end_valid = 1; + if (tp->joint_segments_queued > 0) { tp->joint_segments_queued--; } + } + //Clear status flags associated since segment is done //TODO stuff into helper function? tc->active = 0; @@ -3421,6 +3528,7 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3518,7 +3626,8 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_MISSING_INPUT; } - if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_JOINT + || tc->synchronized != TC_SYNC_NONE)) { //Can't activate a segment with synced motion in reverse return TP_ERR_REVERSE_EMPTY; } @@ -4476,6 +4585,72 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) return TP_ERR_OK; } +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos) +{ + if (0 == tp) { + ZERO_EMC_POSE((*pos)); + return TP_ERR_FAIL; + } + *pos = tp->goalPos; + return TP_ERR_OK; +} + +/** + * The joints the active segment commands, when it is a joint interpolated + * one: the servo thread takes these instead of inverting the position. + * Returns the joint count, or 0 when the active segment is any other kind. + */ +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints) +{ + TC_STRUCT const *tc; + + if (!tp || !joints) { return 0; } + tc = tcqItem((TC_QUEUE_STRUCT *)&tp->queue, 0); + if (!tc || !tc->active) { return 0; } + return tcGetJointPos(tc, joints); +} + +/** + * The end joints of a joint interpolated segment that completed this + * cycle, once: the seed for the servo thread's inverse of the position the + * planner is now at, which is that segment's end and whatever a following + * segment added in the rest of the cycle. Returns 1 and fills the joints, + * or 0. + */ +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->joint_end_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->joint_end[i]; } + tp->joint_end_valid = 0; + return 1; +} + +/** + * How many joint interpolated segments the queue holds, active one + * included. An external offset cannot ride on one, so its planning waits + * while any is queued. + */ +int tpJointSegmentsQueued(TP_STRUCT const * const tp) +{ + return tp ? tp->joint_segments_queued : 0; +} + +/** + * Where the queue ends in joint space, if the last segment queued was a + * joint interpolated one. Returns 1 and fills the joints, or 0 when the + * answer is the inverse of the goal position. + */ +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->queue_end_joints_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->queue_end_joints[i]; } + return 1; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { @@ -4568,6 +4743,12 @@ EXPORT_SYMBOL(tpAbort); EXPORT_SYMBOL(tpActiveDepth); EXPORT_SYMBOL(tpAddCircle); EXPORT_SYMBOL(tpAddLine); +EXPORT_SYMBOL(tpAddJointLine); +EXPORT_SYMBOL(tpGetGoalPos); +EXPORT_SYMBOL(tpGetJointPos); +EXPORT_SYMBOL(tpTakeJointEnd); +EXPORT_SYMBOL(tpJointSegmentsQueued); +EXPORT_SYMBOL(tpGetQueueEndJoints); EXPORT_SYMBOL(tpAddRigidTap); EXPORT_SYMBOL(tpClear); EXPORT_SYMBOL(tpCreate); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 5aed4015707..f11955e0442 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -65,6 +65,15 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, double vlimit_scale, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag); +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints); +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints); +int tpJointSegmentsQueued(TP_STRUCT const * const tp); +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 610f629fbcb..05e95316a53 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -117,6 +117,17 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + /* where the queue ends in joint space, known when the last segment + queued was a joint interpolated one; a world segment after it makes + the answer the inverse of goalPos again */ + double queue_end_joints[EMCMOT_MAX_JOINTS]; + int queue_end_joints_valid; + /* the end joints of a joint interpolated segment that completed this + cycle, for the servo thread to seed its inverse with: the segment is + gone from the queue by the time it asks */ + double joint_end[EMCMOT_MAX_JOINTS]; + int joint_end_valid; + int joint_segments_queued; /* joint interpolated segments in the queue */ int queueSize; double cycleTime; From a164a3733741aaa6893e49b11160765fbd5feb99 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:33:42 +1000 Subject: [PATCH 05/24] interpreter: the point-to-point moves and the tool orientation codes, G53.1 to G53.6, and G68.3 The interpreter evaluates the kinematics module ahead of motion through the loader in kinematics_userspace, opened on first use on a HAL component of its own, so G53.n can ask the tool frame inverse where the rotaries go, G68.3 can read the tool direction, and the point-to-point codes can tell where their joints put the tool. G53.4, G53.5 and G53.7 are point-to-point moves, non-modal modifiers of G0 and G1 like G53: the path is interpolated in joint space, only the endpoint is defined. G53.4 takes a program point through the offsets. G53.5 reads the axis letters as a point in the machine frame with the orientation left out, XYZ the carriage in machine coordinates and the rotary letters the rotary joints, no offset applied: the world of the type the module flags KINSTYPE_MACHINE, the identity type where the slides line up with the frame, so there a letter names the joints the module's identity mapping gives it, and on a module with a slanted or composite slide the point goes through that module's own machine frame maths, a letter not given holding its machine coordinate. Where that type is a plain identity it is refused on a machine whose letters name joints of the other unit class, as a serial robot's X; a module that declares no types is read as letters = joints. G53.7 takes joint values as J= words, a form the lexer reads when the J value is followed by an equals sign, the joint's own position in its own units, which works on every machine. With G1 the move takes the time the straight move would at the programmed feed. The inverse ahead of motion is run to a fixed point, because a module may read the joints it is handed. G53.1 moves the rotaries to the plane's normal with the linear joints where they are; G53.6 does the same holding the tool centre point, a Cartesian move; G53.3 X Y Z goes to a point in the plane with the tool oriented. P picks among the poses: nearest by default, P1 and P2 by the sign of the secondary rotary, the one whose axis the other carries, found by turning each rotary through the module's tool frame, Heidenhain's SEQ+ and SEQ-. Q says whether the joints that carry the work take part: Q0 holds them, COORD ROT, falling back to everything free; Q1 frees them, TABLE ROT. G68.3 takes the plane from the tool, Z the tool axis as the joints have it, X the default tool X of the conventions chapter. tests/twp-native runs the xyzacb nutating head through all of it against the python maths in tests/kins-twp; tests/ptp-robot puts pumakins through the point-to-point codes; tests/ptp-machine-frame builds a module with a slanted Y slide out of tree and checks G53.5 holds machine X across a Y move, which the slides would not. Reversing the joint interpolation fails three checks in twp-native. --- docs/src/gcode/coordinates.adoc | 5 + docs/src/gcode/g-code.adoc | 250 +++++++- docs/src/gcode/overview.adoc | 6 +- docs/src/motion/kinematics-conventions.adoc | 40 +- docs/src/motion/kinematics.adoc | 22 + docs/src/motion/switchkins.adoc | 5 +- src/emc/motion/command.c | 24 +- src/emc/rs274ngc/Submakefile | 2 +- src/emc/rs274ngc/interp_array.cc | 4 +- src/emc/rs274ngc/interp_check.cc | 59 +- src/emc/rs274ngc/interp_convert.cc | 36 +- src/emc/rs274ngc/interp_internal.cc | 6 +- src/emc/rs274ngc/interp_internal.hh | 22 + src/emc/rs274ngc/interp_read.cc | 18 +- src/emc/rs274ngc/interp_setup.cc | 6 + src/emc/rs274ngc/interp_workplane.cc | 588 ++++++++++++++++++ src/emc/rs274ngc/rs274ngc_interp.hh | 17 + src/emc/rs274ngc/rs274ngc_pre.cc | 17 + tests/kins-twp/README | 6 +- tests/kins-twp/test.sh | 8 +- .../kins-twp/xyzacb}/remap_funcs_twp.py | 0 .../kins-twp/xyzbca}/remap_funcs_twp.py | 0 tests/ptp-machine-frame/README | 16 + tests/ptp-machine-frame/checkresult | 3 + tests/ptp-machine-frame/sim.hal | 16 + tests/ptp-machine-frame/skip | 4 + tests/ptp-machine-frame/slantkins.comp | 170 +++++ tests/ptp-machine-frame/test-ui.py | 150 +++++ tests/ptp-machine-frame/test.ini | 114 ++++ tests/ptp-machine-frame/test.sh | 5 + tests/ptp-machine-frame/tool.tbl | 1 + tests/ptp-robot/README | 6 + tests/ptp-robot/checkresult | 3 + tests/ptp-robot/sim.hal | 16 + tests/ptp-robot/skip | 4 + tests/ptp-robot/test-ui.py | 99 +++ tests/ptp-robot/test.ini | 134 ++++ tests/ptp-robot/test.sh | 4 + tests/ptp-robot/tool.tbl | 1 + tests/twp-native/README | 13 + tests/twp-native/abort.ngc | 11 + tests/twp-native/sim.hal | 16 + tests/twp-native/skip | 4 + tests/twp-native/test-ui.py | 489 +++++++++++++++ tests/twp-native/test.ini | 147 +++++ tests/twp-native/test.sh | 4 + tests/twp-native/tool.tbl | 1 + tests/twp-table/README | 7 + tests/twp-table/checkresult | 3 + tests/twp-table/sim.hal | 15 + tests/twp-table/test-ui.py | 140 +++++ tests/twp-table/test.ini | 122 ++++ tests/twp-table/test.sh | 4 + tests/twp-table/tool.tbl | 1 + 54 files changed, 2823 insertions(+), 41 deletions(-) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp => tests/kins-twp/xyzacb}/remap_funcs_twp.py (100%) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp => tests/kins-twp/xyzbca}/remap_funcs_twp.py (100%) create mode 100644 tests/ptp-machine-frame/README create mode 100755 tests/ptp-machine-frame/checkresult create mode 100644 tests/ptp-machine-frame/sim.hal create mode 100755 tests/ptp-machine-frame/skip create mode 100644 tests/ptp-machine-frame/slantkins.comp create mode 100755 tests/ptp-machine-frame/test-ui.py create mode 100644 tests/ptp-machine-frame/test.ini create mode 100755 tests/ptp-machine-frame/test.sh create mode 100644 tests/ptp-machine-frame/tool.tbl create mode 100644 tests/ptp-robot/README create mode 100755 tests/ptp-robot/checkresult create mode 100644 tests/ptp-robot/sim.hal create mode 100755 tests/ptp-robot/skip create mode 100755 tests/ptp-robot/test-ui.py create mode 100644 tests/ptp-robot/test.ini create mode 100755 tests/ptp-robot/test.sh create mode 100644 tests/ptp-robot/tool.tbl create mode 100644 tests/twp-native/README create mode 100644 tests/twp-native/abort.ngc create mode 100644 tests/twp-native/sim.hal create mode 100755 tests/twp-native/skip create mode 100755 tests/twp-native/test-ui.py create mode 100644 tests/twp-native/test.ini create mode 100755 tests/twp-native/test.sh create mode 100644 tests/twp-native/tool.tbl create mode 100644 tests/twp-table/README create mode 100755 tests/twp-table/checkresult create mode 100644 tests/twp-table/sim.hal create mode 100755 tests/twp-table/test-ui.py create mode 100644 tests/twp-table/test.ini create mode 100755 tests/twp-table/test.sh create mode 100644 tests/twp-table/tool.tbl diff --git a/docs/src/gcode/coordinates.adoc b/docs/src/gcode/coordinates.adoc index f35a7cdd324..ffe321c982e 100644 --- a/docs/src/gcode/coordinates.adoc +++ b/docs/src/gcode/coordinates.adoc @@ -53,6 +53,11 @@ to clear the work area and access the workpiece in the vise. G53 is a non modal command. It must be used in every block where a move in machine coordinate system is desired. +On a machine with rotary axes the machine coordinates are those of the +kinematics in force, which can turn with the table or follow the tool, so +G53 does not always put the slides at the position given; G53.5 does, see +<>. + == Coordinate Systems [[fig:coordinate-systems-example]] diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 46386681cce..874ba24d893 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -89,11 +89,13 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates +|<> |Orient the Tool to the Tilted Work Plane +|<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance -|<> |Tilted Work Plane +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1842,6 +1844,229 @@ It is an error if: * G53 is used without G0 or G1 being active, * or G53 is used while cutter compensation is on. +[[gcode:g53.1]] +== G53.1, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) + +[source,ngc] +---- +G53.1 +G53.3 X- Y- Z- +G53.6 +---- + +Each of these moves the rotary joints so that the tool axis is normal to the +active <>, the plane's Z. They differ in what +happens to the tool tip on the way: + +* 'G53.1' moves the rotaries alone. The linear joints stay where they are, + and the tool tip swings to wherever that carries it. It is a + point-to-point move, like <>. +* 'G53.6' keeps the tool centre point where it is. It is a Cartesian move of + the rotary words, so the kinematics compensates the linear joints all along. +* 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the + plane, in one point-to-point move. A word left out keeps the present value. + +The kinematics module answers where the rotaries have to go, with its tool +frame inverse (see the kinematics conventions chapter), so no configuration +carries the formula. That answer has more than one solution on most +machines, two on a five-axis one, and often a choice of which joints to use. + +'P' picks which of them. Without 'P', or with 'P0', the machine takes the +one nearest where its rotaries are standing, which is the shortest move and +depends on where that is. 'P1' and 'P2' name the pose instead, so a program +reaches the same one wherever it starts from: on a five axis machine the two +poses lean the head or the table opposite ways, and they differ in the sign +of the secondary rotary, the one whose axis the other carries. 'P1' is the +pose with that rotary positive and 'P2' the pose with it negative. The +interpreter works out which rotary that is by asking the kinematics module, +so nothing is configured for it. This is the choice Heidenhain writes as +`SEQ+` and `SEQ-`. + +The two poses become one where the tool direction asked for lies along the +primary rotary's axis, straight up on a vertical mill, and there every form +gives the same answer. A machine that is not of this shape, a robot among +them, has no such sign to name, and there only the nearest form is +available. + +'Q' says whether the joints that carry the work, the table, take part. + +* 'Q0', the default, holds them and lets the head do the work. On a machine + whose head has only two rotaries the plane's X is then not something the + joints can place, and it is left to the coordinate system, which the plane + already carries; nothing else is done with it. This is what Heidenhain + calls `COORD ROT`. If nothing is reachable with the table held, the move + is tried again with every joint free, the plane's X still left to the + coordinate system; on a machine whose rotaries are all in the table, that + is how its two poses are reached. +* 'Q1' frees the table from the start. With the plane's X asked for as well, + a machine with the joints for it turns the table so that the plane's X is + reached by the machine, Heidenhain's `TABLE ROT`. + +Which joints carry the work is read off the kinematics module's work frame, +so 'Q' means the same thing on every module and needs no INI entry. + +The orientation is evaluated by the interpreter, ahead of the moves before +it, on the kinematics type the program is in. It needs a kinematics type +whose frames describe the machine: on a switchable module that is its TCP +kinematics, selected with <>, and not the identity +kinematics the module starts in. + +.G53.1 Example +[source,ngc] +---- +G12.1 P1 (the TCP kinematics) +G68.2 X50 Y50 Z0 I30 J20 K0 (a plane) +G53.1 (rotaries to its normal, the head does it) +G0 X0 Y0 Z10 (rapid to a point in the plane, above its origin) +G1 Z-2 F200 (down, along the plane's normal) +G69 +---- + +It is an error if: + +* No tilted work plane is active. +* The kinematics type in force is the identity kinematics, the module cannot + be evaluated by the interpreter, or it reports no frames, so it cannot say + where its joints point the tool. +* The plane's normal cannot be reached by the rotary joints. +* 'P' is anything but 0, 1 or 2, no reachable pose has the rotary that + names it the way 'P' asks for, or the machine has no pair of poses a + tilting joint tells apart. +* 'Q' is anything but 0 or 1. +* Axis words are used with 'G53.1' or 'G53.6', or words other than 'X', 'Y' + and 'Z' with 'G53.3'. +* Cutter compensation is on. + +[[gcode:g53.4]] +== G53.4, G53.5, G53.7 Point-to-Point Move(((G53.4 Point-to-Point Move))) + +[source,ngc] +---- +G53.4 G0 +G53.4 G1 F- +G53.5 G0 +G53.5 G1 F- +G53.7 G0 J= ... +G53.7 G1 F- J= ... +---- + +A point-to-point move defines its two ends and leaves the path between them +to the joints. The inverse kinematics runs once, at the destination, and every +joint then 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; on a +machine with rotary joints it swings. The joints are the motion controller's, +<>, the ones the kinematics module maps the axes +to; what HAL connects behind each of them is not part of the interpolation. + +All three are non-modal, like 'G53': they apply to the one block they are +written in, which must have 'G0' or 'G1' in force or on the line. Each strips +one more layer of interpretation from the destination than the one before: + +* 'G53.4' takes a destination in program coordinates, through the offsets and + the tilted work plane like any other move. This is the move a program uses + to cross a kinematic singularity, or to turn a rotary head right round, + without leaving its coordinate system and without the trajectory planner + trying to hold the tool tip on a line the joints cannot follow at speed. +* 'G53.5' takes the same axis letters as a position in the machine frame + with the orientation left out: 'X', 'Y' and 'Z' are the carriage, the + point the rotary joints do not move, in machine coordinates, and the + rotary letters are the rotary joints themselves. On a machine whose slides + line up with its frame, which every shipped module is, that is the slides: + each letter names the joints the module's identity mapping gives it, which + the `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, + and the two joints of a gantry take one value together. A module whose + carriage does not line up, a slanted slide or a composite Y on a mill-turn, + declares a machine frame type of its own (see the + <> chapter) and 'G53.5' + goes through it, so that 'Y10' is machine Y whichever slides make it, and + a letter not given holds its machine coordinate rather than a joint. + Values are in program units, so 'G20' scales them like any other axis + word. No offset, tool length, rotation or work plane applies. On a mill + whose joint 2 carries the Z slide, 'G53.5 G0 Z0' puts that slide at zero + whatever the head and the tool are doing, where under a tool centre point + kinematics 'G53 G0 Z0' puts the point a tool length above the tip at + machine Z zero, which moves with the head angle and the tool. + This is the move for parking, tool change and home positions, + where the slides matter and the part does not. It is not a joint jog: the + machine stays in world mode and the position on the screen stays true + through the move. ++ +An axis letter carries a unit class and a joint does not, so where the +machine frame is the joints 'G53.5' is refused on a machine whose letters +name joints of the other kind. A serial robot answers X with its first +rotary joint, which turns in degrees, and there the whole code is refused +rather than that one letter. Which joints turn is what `[JOINT_n] TYPE` +declares. + +* 'G53.7' takes joint values, one word per joint, and works on every machine: + 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the + `[JOINT_n]` section and the `joint.n` HAL pins, and the value after '=' is + the joint's own position, what `joint.n.pos-cmd` shows, in the joint's own + units. Nothing is converted, not even 'G20' and 'G21'. A joint left out + keeps its position, and the two joints of a gantry pair must both be given, + with one value. This is the form for a robot, and for any machine where the + letters do not name the joints they look like. + +With 'G0' the speed comes from the joint limits in the INI file, +`[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint +exceeds its own; the rapid override applies. With 'G1' the move takes the time +the straight move to the same destination would take at the programmed feed: +under G94 the F word applies to the distance between the two ends by the same +rule as a straight move, XYZ if any of them move, else UVW, else the rotary +words; under G93 the move takes 1/F minutes exactly, which is the form to use +when the ends coincide or only the joints move. The feed override applies, and +the joint limits still cap it. Nothing blends into or out of a point-to-point +move: the move before it comes to a stop and so does the move itself. + +'G53.4' is also the move <> and <> make. + +.G53.4 Example +[source,ngc] +---- +G0 X0 Y0 Z100 A0 C0 +G53.4 G0 A90 C180 (swing the rotaries round, the tip goes where the joints take it) +G0 X0 Y0 Z100 (and back on a straight line) +---- + +.G53.5 Example +[source,ngc] +---- +G53.5 G0 Z0 (the Z slide to its zero, whatever the head's tilt) +G53.5 G0 X0 Y0 B0 C0 (park X, Y and both head joints; the Z slide stays) +G93 G53.5 G1 F0.5 C180 (turn the C joint to 180 in two minutes) +G94 +---- + +.G53.7 Example +[source,ngc] +---- +G53.7 G0 J2=0 (the Z slide, joint 2, to its zero) +G53.7 G0 J0=0 J1=0 J4=0 J5=0 (park those four joints; joint 2 stays) +G93 G53.7 G1 F0.5 J5=180 (turn joint 5 to 180 in two minutes) +G94 +---- + +It is an error if: + +* Neither 'G0' nor 'G1' is in force. +* An axis letter is without a real value, or one is used that is not + configured. +* With 'G53.5', no axis word is given, a letter is used that is not a joint of + the kinematics, the machine's letters name joints of the other kind, or the + machine frame kinematics cannot reach the point. +* With 'G53.7', an axis word or a plain 'J' word is used, no joint word is + given, a joint number is not a whole number from 0 to 15 or is beyond the + joints of the kinematics, or one joint of a gantry pair is given without the + other or with a different value. +* With 'G53.5' or 'G53.7', polar coordinates are used, incremental distance + mode is in force, or the kinematics module cannot be evaluated by the + interpreter. +* A joint word, 'J=', is used without 'G53.7'. +* With 'G1', the feed is zero, or under G94 the two ends coincide, or feed per + revolution (G95) is in force. +* Cutter compensation is on. +* An external offset is applied when the move reaches motion. + [[gcode:g54-g59.3]] == G54-G59.3 Select Coordinate System(((G54-G59.3 Select Coordinate System))) @@ -2017,7 +2242,7 @@ G64 P0.015 Q2 image::images/G64_Heart_Q2.png["G64 Heart",align="center"] [[gcode:g68.2]] -== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) +== G68.2, G68.3, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) [source,ngc] ---- @@ -2029,6 +2254,7 @@ G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side; R on one block) G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.3 X- Y- Z- (the plane from the tool direction) G68.4 ... (any G68.2 form, on the active plane) G69 (cancel) ---- @@ -2091,6 +2317,14 @@ dropped when a program is opened. A definition with a plane already active replaces it, with the words in the coordinate system underneath, not in the old plane. +'G68.3' takes the plane from the tool: Z is the tool axis as the rotary +joints have it at that moment, X is the default tool X of the kinematics +conventions, tool X turned about the tool axis by the smaller angle that +makes it parallel to the machine XY plane, and machine X when the tool is +vertical; 'R' turns the plane from there. It asks the kinematics module for +the tool direction, so it needs a kinematics type whose frames describe the +machine, as <> does. + 'G68.4' takes any 'G68.2' form and composes it onto the active plane: the words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. @@ -2133,7 +2367,8 @@ does not set one itself. With the setting on: Nothing about the plane is written to the parameter file, so a restart always cancels it. -Defining the plane does not move anything. +Defining the plane does not move anything. To bring the tool normal to it +use <>. While a plane is active the codes that change the coordinate system the plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G10 L2', @@ -2150,7 +2385,7 @@ its own axes, as Fanuc's local coordinate system does in a tilted working plane: 'G52 X10' moves the program's zero 10 along the plane's X. It is not written to the 'G92' parameters; 'G52 X0 Y0 Z0' takes it out, and 'G69' drops it. A 'G52' given before the plane stays in 'G92', under the plane. A -new plane, by 'G68.2' or 'G68.4', is refused while a 'G52' in the plane is +new plane, by 'G68.2', 'G68.3' or 'G68.4', is refused while a 'G52' in the plane is in effect, as on Fanuc. The active plane is reported in the modal G-code display as the code that @@ -2163,6 +2398,7 @@ plane coordinates or draw the plane. ---- G54 G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G53.1 (tool normal to it) G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) G1 Z-3 F150 (a hole 3 deep, straight into the plane) G0 Z5 @@ -2184,7 +2420,11 @@ It is an error if: with any form. * 'R' is given on more than one block of a 'P2' definition. * 'G68.4' is used with no plane active. -* 'G68.2' or 'G68.4' is used while a 'G52' given in the plane is in effect. +* 'G68.2', 'G68.3' or 'G68.4' is used while a 'G52' given in the plane is in effect. +* 'G68.3' is used where the kinematics cannot be evaluated, is the identity + kinematics, or reports no frames. +* 'G68.3' is given 'I', 'J', 'K', 'P' or 'Q', or 'G69' any of those or 'R': + a word the code does not use is an error, not ignored. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 3aeb7ba083e..c418ec1b665 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -122,7 +122,7 @@ The table includes N and O for completeness, even though, as defined above, line |G | General function (See table <>) |H | Tool length offset index |I | X offset for arcs and G87 canned cycles -|J | Y offset for arcs and G87 canned cycles +|J | Y offset for arcs and G87 canned cycles; as J=, the position of joint n for <> .2+|K | Z offset for arcs and G87 canned cycles. <| Spindle-Motion Ratio for G33 synchronized movements. |L | generic parameter word for G10, M66 and others @@ -965,7 +965,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 @@ -975,7 +975,7 @@ The modal groups are shown in the following Table. |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 |Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 -|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 +|Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 685820d4693..d46b3dd1b63 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -63,6 +63,28 @@ against, and the two halves of this chapter use different pairs deliberately. Positions are measured in the work frame, which is what makes a program independent of how the table is set. +With every rotary joint at zero, the work frame position is what the linear +joints read. The shipped head and table modules fold the pivot lengths and +the offsets in, so that the tool tip and the slides agree at the zero pose +and the pivot shows only as the compensation, `L(1 - cos B)` on a head, +once a rotary leaves zero. A module with a switchable identity type keeps +this of necessity, since the identity and the geometric type must agree at +the zero pose or a switch would jump the reported position; a module without +one keeps it too, so that `G53.5` machine frame positions and `G53` positions +coincide with the rotaries at zero. + +The machine frame is the frame with the orientation left out: XYZ is the +pivot, the point the rotary joints do not move, in machine coordinates, and +the rotary letters are the rotary joints as they are. On a machine whose +slides line up with its frame that is the identity type, and it is what +`G13.1` and `G49` select and `G53.5` moves in. A machine whose carriage does +not line up, a slanted slide, a composite Y made by two slides, an offset +pivot, declares a machine frame type of its own next to its identity type, +and its identity type then keeps the plain meaning, joints are axes, which a +consumer uses to skip the maths. The zero-pose rule holds for the machine +frame type as it does for the identity: the working transform and the machine +frame type agree with the rotaries at zero. + Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine coordinates. A consumer that wants the tool in workpiece coordinates composes @@ -338,6 +360,13 @@ its X points, and `G53.1` moves the rotaries to align the tool axis. Neither refuses a program for naming both directions on a five-axis machine, and neither should this. +In tree the G-code side of that is `G68.2`, which defines the plane, and +`G53.1`, `G53.3` and `G53.6`, which ask this inverse where the rotaries go, +with the plane's normal and its X as the request. Their `Q` word is the +`held` mask: `Q0` holds the joints the work frame survey finds and takes the +reported turn as the coordinate rotation it is, `Q1` holds nothing. See the +G-code chapter. + Some requests still do not pin the machine down. A five-axis machine asked to point the tool along the axis its primary rotary turns about can hold any primary angle; a machine with three orientation joints asked only for a tool @@ -536,8 +565,10 @@ the optional work and tool frames with the native rotation that relates the tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks -the no-transform type `G13.1` cancels to, `primary` the working transform -`G43.4` switches to (see the Switchable Kinematics chapter). A module with +the no-transform type, joints are axes; `primary` the working transform +`G43.4` switches to; `machine` the machine frame type `G13.1` and `G49` +cancel to and `G53.5` moves in, which a module leaves unset when its +identity type is that frame (see the Switchable Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. @@ -590,6 +621,11 @@ Frames:: separately, each against the machine frame, so that a consumer placing both bodies can. +Zero pose:: + With every rotary joint at zero the work frame position equals the linear + joints. Pivots and offsets live in the module and show only as compensation + once a rotary moves. + Signs:: Positive A, B and C are counterclockwise about work X, Y and Z viewed from the positive end, describing the motion of the tool relative to the diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 32b87d51a4a..c5c88f77c38 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -48,6 +48,28 @@ typically refer to the usual Cartesian coordinates. The A B C axes refer to rotational coordinates about the X Y Z axes respectively. The U V W axes refer to additional coordinates that are commonly made colinear to the X Y Z axes respectively. +[[sec:joint-space]] +=== Joint Space + +The joints the motion controller works with are the ones the kinematics +module defines: the numbers `kinematicsInverse()` produces and +`kinematicsForward()` reads, numbered as the `[JOINT_n]` sections of the INI +file and the `joint.N` HAL pins. Their zero is where homing put it and their +limits are the `[JOINT_n]` limits. _Joint space_ is the set of these +positions taken together, one coordinate per joint, as opposed to the +Cartesian coordinates of the axes. + +What sits behind a joint is a HAL matter. On most machines +`joint.N.motor-pos-cmd` drives one motor, so a joint and an actuator are the +same thing, but a joint can also drive two motors, or one motor through a +ratio, or feed a linkage computed in HAL, and the motion controller does not +know the difference. A point-to-point move, <>, runs each +of the controller's joints on a straight line from its start to its end +position at a rate within that joint's limits; the actuators behind them +follow through whatever HAL puts in between. With switchable kinematics the +joints do not change when the kinematics type does, only the mapping between +them and the axes. + == Trivial Kinematics The simplest machines are those in which which each joint is placed diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index edb93924ae2..81c5ac2ef49 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -586,8 +586,9 @@ int switchkinsDeclare(int ktype, int flags); ---- G-code reads these declarations: 'G13.1' and 'G49' cancel to the -kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype -declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose +kinstype declared KINSTYPE_MACHINE, 'G53.5' moves in its world, and +'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever +their numbers, so a module whose kinematics are not in the conventional order still gets working spellings. The machine frame type is the machine with the orientation left out, XYZ the pivot in machine coordinates and the rotary letters diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 3666ffde43e..747940f41ab 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -120,6 +120,26 @@ void emcmotApplyPendingPlannerType(void) } /* ===== END PLANNER_SWITCH_DEFER ==================================================== */ +/* the inverse once, for an endpoint, run to a fixed point: some modules + read the joints they are handed (a nutating head takes its rotary angles + from them), so one pass from a stale seed answers for the wrong angles, + and running again from its own answer settles it */ +static int inverse_settled(EmcPose *pos, double *joints, + KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int pass, j; + + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { prev[j] = joints[j]; } + if (kinematicsInverse(pos, joints, iflags, fflags) != 0) { return -1; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { worst = fmax(worst, fabs(joints[j] - prev[j])); } + if (worst < 1e-9) { break; } + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -1168,7 +1188,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; } - if (kinematicsInverse(&goal, start, &iflags, &fflags) != 0) { + if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), emcmotCommand->id); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; @@ -1199,7 +1219,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } - if (kinematicsInverse(&end, target, &iflags, &fflags) != 0) { + if (inverse_settled(&end, target, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d fails kinematicsInverse"), emcmotCommand->id); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 49226eb60e3..801542ad8f5 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -42,7 +42,7 @@ TARGETS += ../lib/librs274.so ../lib/librs274.so.0 ../lib/librs274.so.0: $(patsubst %.cc,objects/%.o,$(LIBRS274SRCS)) \ ../lib/liblinuxcncini.so ../lib/libpyplugin.so ../lib/liblinuxcnchal.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking $(notdir $@) @mkdir -p ../lib @rm -f $@ diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 14dd80bf570..58d0f12806b 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0,-1, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9, 9, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index b2c8bfc80b8..925096c8d99 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -110,6 +110,30 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { + } else if (mode0 == G_53_1 || mode0 == G_53_6) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Cannot use axis words with G53.1 or G53.6")); + } else if (mode0 == G_53_3) { + CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Only X, Y and Z words can be used with G53.3")); + } else if (mode0 == G_53_4 || mode0 == G_53_5 || mode0 == G_53_7) { + CHKS(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)), + _("G53.4, G53.5 and G53.7 need G0 or G1")); + if (mode0 == G_53_5 || mode0 == G_53_7) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G53.5 or G53.7")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G53.5 or G53.7 in incremental distance mode")); + } + if (mode0 == G_53_7) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("G53.7 takes joint words, J=, not axis words; G53.5 takes the axis words")); + CHKS((block->j_flag), _("Cannot use a J word with G53.7; a joint is J=")); + } } else if (mode0 == G_12_1){ // kins-switch CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); @@ -291,28 +315,39 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("H word with no G43, G43.4 or G76 to use it")); } + // G68.2 and G68.4 take I J K, P and Q; G68.3 only R, X Y Z; G69 nothing + int plane_words = block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4; + int plane_r = plane_words || block->g_modes[GM_WORK_PLANE] == G_68_3; + if (block->i_flag) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->j_flag) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); + !plane_words), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G68.4, G76 or G87 to use it")); + } + + for (int n = 0; n < EMCMOT_MAX_JOINTS; n++) { + if (block->joint_flag[n]) { + CHKS((block->g_modes[GM_MODAL_0] != G_53_7), _("J%d= word with no G53.7 to use it"), n); + break; + } } if (block->k_flag) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -330,7 +365,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -343,7 +379,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -361,12 +397,13 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (motion != G_6) && (motion != G_6_2) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -377,7 +414,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_r && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 379b21eaba6..151215790a4 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4521,7 +4521,11 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_axis_offsets(code, block, settings)); } else if ((code == G_5_3)||(code == G_6_3)) { // jjf CHP(convert_nurbs(code, block, settings)); - } else if ((code == G_4) || (code == G_53)); // handled elsewhere + } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) + || (code == G_53_7)); // handled elsewhere + else if ((code == G_53_1) || (code == G_53_3) || (code == G_53_6)) { + CHP(convert_orient_tool(code, block, settings)); + } else if ((code == G_12_1) || (code == G_13_1)) { // The flag makes the interpreter wait for motion to drain, so that no // motion is planned across a change of kinematics. Reading runs far @@ -5744,6 +5748,11 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { + // the words name joints, by letter or by number: nothing below applies + CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); + return INTERP_OK; + } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); @@ -5757,7 +5766,28 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 // Create a state tag and dump it to canon write_canon_state_tag(block, settings); - if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ + if (block->g_modes[GM_MODAL_0] == G_53_4) { + // point-to-point: the endpoint is this Cartesian point, the path to it + // is whatever the joints make of it + CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use G53.4 with cutter radius compensation on")); + tag_straight(block,end_x, end_y); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end); + } else { + double seconds; + CHP(ptp_seconds(block, settings, end_x, end_y, end_z, + AA_end, BB_end, CC_end, u_end, v_end, w_end, &seconds)); + JOINT_FEED(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end, seconds); + } + settings->current_x = end_x; + settings->current_y = end_y; + settings->current_z = end_z; + } else if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ (settings->cutter_comp_radius > 0.0)) { /* radius always is >= 0 */ CHKS((block->g_modes[GM_MODAL_0] == G_53), @@ -6626,7 +6656,7 @@ static int kins_type_info_available() // the type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test -static int flagged_kins_type(int flag) +int flagged_kins_type(int flag) { int k, f; diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index bcccebe27c2..7596c990b8f 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,7 +174,7 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode1 = block->g_modes[GM_MOTION]; mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) - || (mode0 == G_52) || (mode0 == G_92)); + || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); @@ -286,6 +286,10 @@ int Interp::init_block(block_pointer block) //!< pointer to a block to be i block->h_number = -1; block->i_flag = false; block->j_flag = false; + for (n = 0; n < EMCMOT_MAX_JOINTS; n++) { + block->joint_flag[n] = false; + block->joint_value[n] = 0.0; + } block->k_flag = false; block->l_number = -1; block->l_flag = false; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index ebe9d0d0d49..0447465f87b 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -265,6 +265,12 @@ enum GCodes G_51 = 510, G_52 = 520, G_53 = 530, + G_53_1 = 531, + G_53_3 = 533, + G_53_4 = 534, + G_53_5 = 535, + G_53_6 = 536, + G_53_7 = 537, G_54 = 540, G_55 = 550, G_56 = 560, @@ -275,6 +281,7 @@ enum GCodes G_59_2 = 592, G_59_3 = 593, G_68_2 = 682, + G_68_3 = 683, G_68_4 = 684, G_69 = 690, G_61 = 610, @@ -514,6 +521,9 @@ struct block_struct bool dollar_flag{}; + bool joint_flag[EMCMOT_MAX_JOINTS]{}; // J= words, for G53.5 + double joint_value[EMCMOT_MAX_JOINTS]{}; + double radius{}; double theta{}; int radius_flag{}; @@ -796,6 +806,14 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r + // the kinematics, for G68.3 and the orientation moves: loaded on first + // use through the non-realtime loader, on a HAL component of our own + void *kins_ctx; // KinematicsUserContext + int kins_comp_id; + char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it + int kins_joints; // [KINS] JOINTS + int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer @@ -1109,4 +1127,8 @@ struct scoped_locale { }; #define FORCE_LC_NUMERIC_C scoped_locale force_lc_numeric_c(LC_NUMERIC, "C") + +// the kinematics type carrying a KINSTYPE_ flag, or -1 when the module +// declares none (interp_convert.cc) +int flagged_kins_type(int flag); #endif // INTERP_INTERNAL_HH diff --git a/src/emc/rs274ngc/interp_read.cc b/src/emc/rs274ngc/interp_read.cc index 6e58d1635dc..d0a34a21b8a 100644 --- a/src/emc/rs274ngc/interp_read.cc +++ b/src/emc/rs274ngc/interp_read.cc @@ -888,11 +888,15 @@ Returned Value: int NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED 2. A j_coordinate has already been inserted in the block. NCE_MULTIPLE_J_WORDS_ON_ONE_LINE + 3. The value is followed by '=' and is not a joint number, or that joint + already has a value in the block. Side effects: counter is reset. The j_flag in the block is turned on. A j_coordinate setting is inserted in the block. + For the J= form, joint_flag[n] is turned on and joint_value[n] + is set instead; j_flag is left alone. Called by: read_one_item @@ -918,8 +922,20 @@ int Interp::read_j(char *line, //!< string: line of RS274 code being processed CHKS((line[*counter] != 'j'), NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED); *counter = (*counter + 1); - CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); CHP(read_real_value(line, counter, &value, parameters)); + if (line[*counter] == '=') { + // J=: a joint value for G53.5, n is the joint number + int n = (int)value; + CHKS((value != (double)n || n < 0 || n >= EMCMOT_MAX_JOINTS), + _("The joint number in a J= word must be a whole number from 0 to %d"), EMCMOT_MAX_JOINTS - 1); + CHKS((block->joint_flag[n]), _("Multiple J%d= words on one line"), n); + *counter = (*counter + 1); + CHP(read_real_value(line, counter, &value, parameters)); + block->joint_flag[n] = true; + block->joint_value[n] = value; + return INTERP_OK; + } + CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); block->j_flag = true; block->j_number = value; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index bd513053de2..1e4e03c6776 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -114,6 +114,12 @@ setup::setup() : g68_seq_have(0), g68_seq_r_q(-1), g68_seq_word{}, + kins_ctx(nullptr), + kins_comp_id(0), + kins_module{}, + kins_joints(0), + kins_angular_joints(0), + kins_seed{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 6290218c72a..19fedadd513 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -535,6 +535,10 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) _("Cannot cancel a tilted work plane with cutter radius compensation on")); return work_plane_cancel(s, true); } + if (g_code == G_68_3) { + CHKS((s->g68_seq_code != 0), _("G68.3 cannot interrupt a G68.2 sequence")); + return convert_work_plane_from_tool(block, s); + } CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -561,3 +565,587 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } return work_plane_set(s, g_code, origin, rotation); } + +//---------------------------------------------------------------------- +// The kinematics. G68.3 and the orientation moves need the frames and +// the tool frame inverse of the module motion runs, evaluated here, ahead +// of motion, through the loader in kinematics_userspace/. The loader +// binds its pins to a HAL component, so the interpreter makes one, named +// by its process, the first time it is asked. +//---------------------------------------------------------------------- + +#include +#include +#include "units.h" +#include +#include + +#define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + if (!s->kins_ctx) { + char name[HAL_NAME_LEN + 1]; + int comp; + + CHKS((!s->kins_module[0]), + _("the INI file names no [KINS] KINEMATICS, so the kinematics cannot be evaluated here")); + CHKS((s->kins_joints < 1), + _("the INI file gives no [KINS] JOINTS, so the kinematics cannot be evaluated here")); + snprintf(name, sizeof(name), "interp.%d", (int)getpid()); + comp = hal_init(name); + CHKS((comp < 0), + _("cannot connect to HAL to evaluate the kinematics (is realtime running?)")); + s->kins_comp_id = comp; + ctx = kinematicsUserInitString(s->kins_module, s->kins_joints, comp, name); + hal_ready(comp); + CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); + s->kins_ctx = ctx; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + } + ctx = KINS_CTX(s); + CHKS((kinematicsUserIsRtOnly(ctx)), + _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); + CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), + _("kinematics type %d is not available outside realtime"), s->kins_type); + *out = ctx; + return INTERP_OK; +} + +void Interp::kins_release(setup_pointer s) +{ + if (s->kins_ctx) { + kinematicsUserFree(KINS_CTX(s)); + s->kins_ctx = NULL; + } + if (s->kins_comp_id > 0) { + hal_exit(s->kins_comp_id); + s->kins_comp_id = 0; + } +} + +// the current point as the machine sees it: the absolute frame, in the +// machine's units, which is what the kinematics works in +void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) +{ + double abs_pos[9]; + + get_abs_position(s, abs_pos); + pose->tran.x = PROGRAM_TO_USER_LEN(abs_pos[0]); + pose->tran.y = PROGRAM_TO_USER_LEN(abs_pos[1]); + pose->tran.z = PROGRAM_TO_USER_LEN(abs_pos[2]); + pose->a = PROGRAM_TO_USER_ANG(abs_pos[3]); + pose->b = PROGRAM_TO_USER_ANG(abs_pos[4]); + pose->c = PROGRAM_TO_USER_ANG(abs_pos[5]); + pose->u = PROGRAM_TO_USER_LEN(abs_pos[6]); + pose->v = PROGRAM_TO_USER_LEN(abs_pos[7]); + pose->w = PROGRAM_TO_USER_LEN(abs_pos[8]); +} + +// and back: a machine pose as program coordinates, through the chain +void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, double prog[9]) +{ + world_to_program_xyz(s, + USER_TO_PROGRAM_LEN(pose->tran.x), + USER_TO_PROGRAM_LEN(pose->tran.y), + USER_TO_PROGRAM_LEN(pose->tran.z), + &prog[0], &prog[1], &prog[2]); + prog[3] = USER_TO_PROGRAM_ANG(pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_ANG(pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_ANG(pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_LEN(pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_LEN(pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; +} + +// the joints the machine is at, as far as the interpreter can know ahead of +// motion: the seed while it still explains the current point, since a point +// does not name one joint set, else the joints the machine stands in +int Interp::current_joints(setup_pointer s, void *vctx, double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + int pass, i; + + current_machine_pose(s, &pose); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("the kinematics cannot invert the current position")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; +} + +// a direction of the plane in world coordinates: the plane's rotation +// then the XY rotation of the coordinate system it sits on +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double x = s->g68_rotation[0][column]; + double y = s->g68_rotation[1][column]; + double z = s->g68_rotation[2][column]; + double t = rotation_xy * M_PI / 180.0; + + out->x = x * cos(t) - y * sin(t); + out->y = x * sin(t) + y * cos(t); + out->z = z; +} + +static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) +{ + // Rodrigues, for a unit axis + PmCartesian c; + double d = axis->x*v->x + axis->y*v->y + axis->z*v->z; + + pmCartCartCross(axis, v, &c); + v->x = v->x*cos(rad) + c.x*sin(rad) + axis->x*d*(1 - cos(rad)); + v->y = v->y*cos(rad) + c.y*sin(rad) + axis->y*d*(1 - cos(rad)); + v->z = v->z*cos(rad) + c.z*sin(rad) + axis->z*d*(1 - cos(rad)); +} + +// G68.3: the plane from the tool. Z is the tool axis as the joints have it, +// X the default tool X of the conventions chapter, R turns the plane from +// there; X Y Z are the origin, as G68.2's +int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + PmRotationMatrix work, tool, in_work; + PmCartesian zt, xt, yt, zm, xm, x, y; + double origin[3], rotation[3][3], r, t, along; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((s->g68_local[0] != 0.0 || s->g68_local[1] != 0.0 || s->g68_local[2] != 0.0), + _("Cancel the G52 in the tilted work plane (G52 X0 Y0 Z0) before G68.3")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, joints)); + CHKS((kinematicsUserWorkFrame(ctx, joints, &work) != 0 + || kinematicsUserToolFrame(ctx, joints, &tool) != 0), + _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); + toolFrameInWork(&work, &tool, &in_work); + zt = in_work.z; + xt = in_work.x; + yt = in_work.y; + // machine Z and X seen from the work: the rows of the work frame + zm.x = work.x.z; zm.y = work.y.z; zm.z = work.z.z; + xm.x = work.x.x; xm.y = work.y.x; xm.z = work.z.x; + + pmCartCartCross(&zt, &zm, &x); + if (sqrt(x.x*x.x + x.y*x.y + x.z*x.z) < 1e-9) { + // vertical: tool x is machine x, less whatever of it lies along + // the tool axis, which is rounding + along = xm.x*zt.x + xm.y*zt.y + xm.z*zt.z; + x.x = xm.x - along*zt.x; x.y = xm.y - along*zt.y; x.z = xm.z - along*zt.z; + pmCartUnitEq(&x); + } else { + // the turn about the tool axis that takes tool x into the + // machine XY plane: (cos t x + sin t y) . zm = 0, the root nearer + // to no turn at all + double xz = xt.x*zm.x + xt.y*zm.y + xt.z*zm.z; + double yz = yt.x*zm.x + yt.y*zm.y + yt.z*zm.z; + + t = atan2(-xz, yz); + if (t > M_PI / 2) { t -= M_PI; } + if (t < -M_PI / 2) { t += M_PI; } + x = xt; + rotate_about(&zt, t, &x); + } + r = block->r_flag ? block->r_number : 0.0; + rotate_about(&zt, r * M_PI / 180.0, &x); + pmCartCartCross(&zt, &x, &y); + + // from world directions to the system the plane is defined in: the + // XY rotation comes off + { + PmCartesian cols[3] = { x, y, zt }; + double c = cos(-s->rotation_xy * M_PI / 180.0), sn = sin(-s->rotation_xy * M_PI / 180.0); + + for (int j = 0; j < 3; j++) { + rotation[0][j] = cols[j].x * c - cols[j].y * sn; + rotation[1][j] = cols[j].x * sn + cols[j].y * c; + rotation[2][j] = cols[j].z; + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + return work_plane_set(s, G_68_3, origin, rotation); +} + +// G53.1, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 turns +// the rotaries alone, in joint space; G53.6 keeps the tool centre point, a +// Cartesian move; G53.3 goes to X Y Z in the plane. P picks the pose, +// nearest first or by the sign of the tilting joint; Q0 holds the joints that +// carry the work (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS]; + double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + double distance[TOOL_FRAME_MAX_SOLUTIONS]; + int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + unsigned int held = 0; + int p, q, n, i, j, chosen, njoints; + const double *sol; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot orient the tool with cutter radius compensation on")); + p = block->p_flag ? (int)round(block->p_number) : 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), + _("P word with %s must be 0, 1 or 2"), name); + q = block->q_flag ? (int)round(block->q_number) : 0; + CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), + _("Q word with %s must be 0 or 1"), name); + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + njoints = kinematicsUserGetNumJoints(ctx); + CHP(current_joints(s, ctx, now)); + + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + if (q == 0) { + if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } + } + n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + if (n == 0 && held) { + // nothing reachable with the work held still: let it move, still + // leaving the plane's X to the coordinate system, or a table that + // can place it pins the pose and P has nothing to choose from + held = 0; + n = kinematicsUserToolFrameInverse(ctx, &axis, NULL, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + } + CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + + // nearest first, by rotary travel in joint units + for (i = 0; i < n; i++) { + distance[i] = 0.0; + for (j = 0; j < njoints; j++) { distance[i] += fabs(solutions[i*njoints + j] - now[j]); } + order[i] = i; + } + for (i = 1; i < n; i++) { + int k = order[i]; + for (j = i; j > 0 && distance[order[j-1]] > distance[k]; j--) { order[j] = order[j-1]; } + order[j] = k; + } + if (p == 0) { + chosen = order[0]; + } else { + // P names the pose rather than its rank, so that the same program + // reaches the same pose from wherever the machine is standing + int sign_joint; + CHKS((kinematicsUserPoseJoint(ctx, now, &sign_joint) != 0), + _("%s P%d: the poses of this machine cannot be told apart by a tilting" + " joint, so leave P out and take the nearest"), name, p); + chosen = -1; + for (i = 0; i < n; i++) { + double value = solutions[order[i]*njoints + sign_joint]; + if ((p == 1 && value > 1e-9) || (p == 2 && value < -1e-9)) { + chosen = order[i]; + break; + } + } + CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), + name, p, sign_joint, (p == 1) ? "positive" : "negative"); + } + sol = solutions + chosen * njoints; + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + { + double full[EMCMOT_MAX_JOINTS]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } + CHKS((kinematicsUserForward(ctx, full, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = full[i]; } + } + machine_pose_to_program(s, &end_pose, end_prog); + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + +// how long a point-to-point feed is to take: the time the same straight +// move would take at the programmed feed +int Interp::ptp_seconds(block_pointer block, setup_pointer s, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds) +{ + if (s->feed_mode == FEED_MODE::INVERSE_TIME) { + CHKS((block->f_number <= 0.0), _("F must be positive with G93")); + *seconds = 60.0 / block->f_number; + } else if (s->feed_mode == FEED_MODE::UNITS_PER_MINUTE) { + double length = find_straight_length(x, y, z, a, b, c, u, v, w, + s->current_x, s->current_y, s->current_z, + s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current); + CHKS((length <= 0.0), + _("a point-to-point feed with no displacement has nothing to apply F to in G94 mode; use G93 or G0")); + *seconds = 60.0 * length / s->feed_rate; + } else { + ERS(_("Cannot use feed per revolution with a point-to-point move")); + } + return INTERP_OK; +} + +// The axis letters read as machine frame coordinates, the words the module's +// machine frame type answers to: the pivot in machine coordinates, the +// rotaries as joints, in program units. The joints are read in on the type +// in force and come out as the joints of the machine frame point, the letters +// not given standing where they are. Where the machine frame type is a plain +// identity the letters name joints, and a letter carries a unit class where +// a joint does not: on a serial robot the first joint answers to X and turns +// in degrees, and the whole machine is refused rather than that one letter, +// since a mapping that lies about X is telling nothing useful about A. A +// module that declares no types at all is read as letters = joints through +// its identity mapping, gantry pairs sharing the value. +int Interp::slide_joints(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + const int machine = flagged_kins_type(KINSTYPE_MACHINE); + int a, given = 0, status; + + for (a = 0; a < 9; a++) { given += flags[a] ? 1 : 0; } + CHKS((given == 0), _("%s needs at least one axis word"), name); + if (machine < 0) { + return slide_words(name, s, ctx, p, njoints, 1, flags, words, joints); + } + CHKS((kinematicsUserSetType(ctx, machine) != 0), + _("%s: kinematics type %d is not available outside realtime"), name, machine); + status = slide_words(name, s, ctx, p, njoints, 0, flags, words, joints); + // back on the type in force whatever happened: the caller's context + kinematicsUserSetType(ctx, s->kins_type); + return status; +} + +// the words on the context as set: the module's machine frame type, or the +// type in force with letters = joints where the module declares none +int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, int undeclared, const int flags[9], const double words[9], + double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + int a, j; + + if (undeclared || kinematicsUserIsIdentity(ctx)) { + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("%s: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + name, letters[a], j, turns ? "angular" : "linear", j); + } + } + } + if (!undeclared) { + return machine_frame_joints(name, s, ctx, flags, words, joints); + } + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + double value; + if (!flags[a]) { continue; } + CHKS((p->joints_of_axis[a] == 0), + _("%s: %c is not a joint of this kinematics"), name, letters[a]); + value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } + } + } + return INTERP_OK; +} + +// The joints of a machine frame point, on the machine frame type the +// context is set to: where the joints handed in stand in that frame, the +// letters given replacing their coordinate, and the inverse iterated to a +// fixed point as current_joints() does, since an iterative inverse takes +// its seed from the joints handed in. +int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, + const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double *coord[9]; + int a, i, pass; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the machine frame kinematics cannot place the current joints"), name); + coord[0] = &pose.tran.x; coord[1] = &pose.tran.y; coord[2] = &pose.tran.z; + coord[3] = &pose.a; coord[4] = &pose.b; coord[5] = &pose.c; + coord[6] = &pose.u; coord[7] = &pose.v; coord[8] = &pose.w; + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + if (!flags[a]) { continue; } + *coord[a] = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("%s: the machine frame kinematics cannot reach that point"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + return INTERP_OK; +} + +// The two point-to-point codes below G53.4: G53.5 by axis letter in the +// machine frame, the module's machine frame type, refused where that type is +// a plain identity whose letters name joints of the other unit class; G53.7 +// by J= in the joint's own units. A letter or joint left out keeps +// its position. +int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + EmcPose pose; + double prog[9]; + const int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + const char *name = (code == G_53_5) ? "G53.5" : "G53.7"; + int a, j, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (code == G_53_7) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + if (!block->joint_flag[j]) { continue; } + CHKS((j >= njoints), _("G53.7: this kinematics has no joint %d"), j); + joints[j] = block->joint_value[j]; + given++; + } + CHKS((given == 0), _("G53.7 needs at least one J= joint word")); + } else { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + } + + // the joints of a gantry pair move together: both given, one value + if (code == G_53_7) { + for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + int first = -1; + if (!(bits & (bits - 1))) { continue; } + for (j = 0; j < njoints; j++) { + if (!(bits & (1 << j))) { continue; } + if (first < 0) { first = j; continue; } + CHKS((block->joint_flag[j] != block->joint_flag[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give both"), first, j); + CHKS((block->joint_flag[j] && block->joint_value[j] != block->joint_value[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); + } + } + } + + // where that puts the tool, and what the program calls it + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 5e4dce0d250..b7cd601f65a 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -376,6 +376,23 @@ public: int work_plane_check_sequence(block_pointer block, setup_pointer settings); int work_plane_local(block_pointer block, setup_pointer settings); int work_plane_restore(setup_pointer settings); + int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); + int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, const int flags[9], const double words[9], double *joints); + int slide_words(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, int undeclared, const int flags[9], const double words[9], double *joints); + int machine_frame_joints(const char *name, setup_pointer settings, void *ctx, + const int flags[9], const double words[9], double *joints); + int ptp_seconds(block_pointer block, setup_pointer settings, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds); + int kins_context(setup_pointer settings, void **ctx); + void kins_release(setup_pointer settings); + void current_machine_pose(setup_pointer settings, EmcPose *pose); + void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); + int current_joints(setup_pointer settings, void *ctx, double *joints); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index f420af7b336..da4ad31a332 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -178,6 +178,7 @@ InterpBase *makeInterp() } Interp::~Interp() { + kins_release(&_setup); if(log_file) { if(log_file != stderr) fclose(log_file); @@ -923,6 +924,22 @@ int Interp::init() _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); + // the kinematics, for the codes that ask it something + if (auto kins = inifile.findString("KINEMATICS", "KINS")) { + snprintf(_setup.kins_module, sizeof(_setup.kins_module), "%s", kins->c_str()); + } + _setup.kins_joints = inifile.findIntV("JOINTS", "KINS", 0); + // which joints turn rather than slide, so that an axis letter is + // refused where it would name a joint of the other kind + _setup.kins_angular_joints = 0; + for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { + char section[16]; + snprintf(section, sizeof(section), "JOINT_%d", jno); + if (auto type = inifile.findString("TYPE", section)) { + if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } + } + } + _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); _setup.naivecam_tolerance_default = inifile.findRealV("G64_DEFAULT_NAIVETOLERANCE", "RS274NGC", 0.0); diff --git a/tests/kins-twp/README b/tests/kins-twp/README index b9b3b7275fa..d114c3a2afc 100644 --- a/tests/kins-twp/README +++ b/tests/kins-twp/README @@ -1,7 +1,9 @@ The C kinematics against the tilted work plane maths. -The two nutating-head configs carry their orientation maths in python, -remap_funcs_twp.py, written independently of the kinematics modules. +The tilted work plane maths for the two nutating-head machines was first +written in python, remap_funcs_twp.py, independently of the kinematics +modules, for the remap the configs used before the interpreter learned +the codes. It lives on here, one copy per machine, as the oracle. This test loads each module in realtime and puts its tool frame next to the python transformation matrix over a grid of head angles, and its tool frame inverse next to the python candidate joint angles and virtual diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh index 86b77f6ff44..d50c9bea68e 100755 --- a/tests/kins-twp/test.sh +++ b/tests/kins-twp/test.sh @@ -1,10 +1,6 @@ #!/bin/bash set -e -# RIP layout: $HEADERS is $TOPDIR/include -TOPDIR=$(dirname "$HEADERS") -CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating - ${SUDO} halcompile --install twpcheck.c >/dev/null # One hal file per machine. twpcheck answers frame and inverse requests @@ -18,8 +14,8 @@ run() { printf 'loadrt threads name1=t1 period1=1000000\n' printf 'addf twpcheck t1\n' printf 'start\n' - printf 'loadusr -w python3 check.py %s %s/%s-trsrn_twp %s/twp-%s.ini\n' \ - "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + printf 'loadusr -w python3 check.py %s %s/%s %s/twp-%s.ini\n' \ + "$machine" "$PWD" "$machine" "$PWD" "$machine" } > "$hal" echo "=== $machine" halrun -f "$hal" diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzacb/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzacb/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzbca/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzbca/remap_funcs_twp.py diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README new file mode 100644 index 00000000000..c7e76a61ea1 --- /dev/null +++ b/tests/ptp-machine-frame/README @@ -0,0 +1,16 @@ +The point-to-point moves on a machine whose slides do not line up with +its frame. + +slantkins.comp is built here, out of tree the way the switchkinscomp +template builds: XYZBC on five joints, joint 1 a slide slanted 30 degrees +in the XY plane, so machine X and Y are made by joints 0 and 1 together. +Type 0 is the carriage in machine coordinates, declared the machine frame +type and not an identity; type 1 the tool tip with the head tilted about Y +and the tool length from the tool table; type 2 a plain identity. + +The test checks that G13.1 and G49 select the machine frame type rather +than the identity, that G53.5 puts the carriage at a machine frame point, +the letters not given holding their machine coordinate and not their +joint, from the machine frame type and from under the tilted tip +kinematics alike, and that a plain move after a machine frame move starts +from the right place. diff --git a/tests/ptp-machine-frame/checkresult b/tests/ptp-machine-frame/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-machine-frame/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-machine-frame/sim.hal b/tests/ptp-machine-frame/sim.hal new file mode 100644 index 00000000000..fbd3d125327 --- /dev/null +++ b/tests/ptp-machine-frame/sim.hal @@ -0,0 +1,16 @@ +loadrt switchkins_core +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-machine-frame/skip b/tests/ptp-machine-frame/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-machine-frame/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-machine-frame/slantkins.comp b/tests/ptp-machine-frame/slantkins.comp new file mode 100644 index 00000000000..65b8daa55a3 --- /dev/null +++ b/tests/ptp-machine-frame/slantkins.comp @@ -0,0 +1,170 @@ +component slantkins "Test kinematics whose machine frame is not its joints"; + +description +""" +A switchable kinematics for the ptp-machine-frame test: XYZBC on five +joints, where joint 1 is a slide slanted in the XY plane by the 'slant' +angle, so that machine X and Y are made by joints 0 and 1 together, the way +a mill-turn with a composite Y slide is built. + +type0 is the machine frame: the carriage in machine coordinates, the two +rotaries as joints, declared as the machine frame type and not an identity. + +type1 is the working transform: the tool tip, with the head tilted about Y +by B and the tool length from the tool table along the tool axis, folded so +that tip and carriage agree at B zero. + +type2 is a plain identity, the joints as the axes, declared identity. +"""; +pin out si32 dummy=0 "one pin, which halcompile requires"; +option period no; +option extra_setup; +license "GPL"; +author "Luca Toniolo"; +;; + +#include +// out of tree, the way the switchkinscomp template builds: the switchkins +// implementation is switchkins_core, loaded ahead of this module +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +static const kins_param_desc slant_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "slant", KINS_PARAM_FLOAT, KINS_IN, 0, 30.0 }, +}; +enum { P_TOOL, P_SLANT }; + +static void unused_axes(EmcPose *pos) +{ + pos->a = 0; + pos->u = 0; + pos->v = 0; + pos->w = 0; +} + +// the carriage: joint 1 runs at the slant angle from X, in the XY plane +static int frame_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)fflags; (void)iflags; + + pos->tran.x = j[0] + j[1] * cos(th); + pos->tran.y = j[1] * sin(th); + pos->tran.z = j[2]; + pos->b = j[3]; + pos->c = j[4]; + unused_axes(pos); + return 0; +} + +static int frame_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)iflags; (void)fflags; + + j[1] = pos->tran.y / sin(th); + j[0] = pos->tran.x - j[1] * cos(th); + j[2] = pos->tran.z; + j[3] = pos->b; + j[4] = pos->c; + return 0; +} + +// the tip: the head tilts about Y by B and carries the tool length below +// the pivot, folded so that the tip is the carriage at B zero +static void tip_offset(const kins_params *p, double b, double *dx, double *dz) +{ + double L = p->tool.tran.z; + double rb = b * PM_PI / 180.0; + + *dx = -L * sin(rb); + *dz = L * (1.0 - cos(rb)); +} + +static int tip_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dx, dz; + + frame_forward(p, s, j, pos, fflags, iflags); + tip_offset(p, j[3], &dx, &dz); + pos->tran.x += dx; + pos->tran.z += dz; + return 0; +} + +static int tip_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose carriage = *pos; + double dx, dz; + + tip_offset(p, pos->b, &dx, &dz); + carriage.tran.x -= dx; + carriage.tran.z -= dz; + return frame_inverse(p, s, &carriage, j, iflags, fflags); +} + +static const kins_ops frame_ops = { + .forward = frame_forward, + .inverse = frame_inverse, + .machine = 1, +}; + +static const kins_ops tip_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "slantkins"; + kp->halprefix = "slantkins"; + kp->required_coordinates = "xyzbc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = slant_params; + kp->nparams = sizeof(slant_params)/sizeof(slant_params[0]); + + switchkinsRegisterOps(0, &frame_ops); + switchkinsRegisterOps(1, &tip_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + return 0; +} + +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} + +EXPORT_SYMBOL(kinsDescribe); + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } + return switchkinsInit(comp_id, &kp, coordinates); +} diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py new file mode 100755 index 00000000000..28021cc33f5 --- /dev/null +++ b/tests/ptp-machine-frame/test-ui.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +# The point-to-point moves on a machine whose slides do not line up with its +# frame: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math + +JOINTS = 5 +SLANT = math.radians(30.0) +TOOL = 50.0 # T1 in tool.tbl + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def show(what, j): + print("%-34s %s" % (what, " ".join("%.4f" % v for v in j))) + +def expect(what, j, want, tol=1e-4): + show(what, j) + if max(abs(a - b) for a, b in zip(j, want)) > tol: + error("%s: expected %s" % (what, " ".join("%.4f" % v for v in want))) + +# the joints of a carriage position: joint 1 is the slanted slide +def carriage(x, y, z, b=0.0, cc=0.0): + j1 = y / math.sin(SLANT) + return [x - j1 * math.cos(SLANT), j1, z, b, cc] + +def expect_type(what, want): + k = kins_type() + print("%-34s kins-type %d" % (what, k)) + if k != want: + error("%s: kins-type %d, expected %d" % (what, k, want)) + +# --- G13.1 and G49 select the machine frame type, not the identity --------- +mdi("G12.1 P2") +expect_type("G12.1 P2, the plain identity", 2) +mdi("G13.1") +expect_type("G13.1", 0) +mdi("G43.4 H1") +expect_type("G43.4", 1) +mdi("G49") +expect_type("G49", 0) +mdi("G12.1 P1", "G13.1") +expect_type("G12.1 P1 then G13.1", 0) +drain() + +# --- G53.5 is the machine frame, not the slides ------------------------------ +# on the machine frame type G53 and G53.5 agree, and a Y word alone moves +# both slides: X is held, which the slanted slide would otherwise pull +j = mdi("G53 G0 X5 Y0 Z0 B0 C0") +expect("G53 to (5, 0, 0)", j, carriage(5, 0, 0)) +j = mdi("G53.5 G0 Y10") +expect("G53.5 Y10 holds machine X", j, carriage(5, 10, 0)) +j = mdi("G53.5 G0 X0") +expect("G53.5 X0 holds machine Y", j, carriage(0, 10, 0)) + +# under the tip kinematics with the head tilted and a tool on, G53.5 still +# means the carriage: the tilt and the tool length are left out. The tilt +# holds the tip, so the carriage has moved by the tool's swing, and that is +# where X is held +mdi("G43.4 H1") +j = mdi("G0 B30") +expect_type("G43.4 with the head at B30", 1) +swing_x = TOOL * math.sin(math.radians(30)) +swing_z = TOOL * (1 - math.cos(math.radians(30))) +expect("the tilt holds the tip", j, carriage(swing_x, 10, -swing_z, 30)) +j = mdi("G53.5 G0 Y0 Z0") +expect("G53.5 Y0 Z0 under the tip, tilted", j, carriage(swing_x, 0, 0, 30)) +j = mdi("G53.5 G0 X20 Y10 Z-5") +expect("G53.5 X20 Y10 Z-5 under the tip", j, carriage(20, 10, -5, 30)) +j = mdi("G53.5 G0 B0") +expect("G53.5 B0 is the joint", j, carriage(20, 10, -5, 0)) +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +# --- the program position agrees with the move ------------------------------ +# the interpreter reports the tip where the joints put it, so a plain move +# after a machine frame move starts from the right place +mdi("G43.4 H1", "G0 B30", "G53.5 G0 X20 Y10 Z0") +s.poll() +before = list(s.position[:3]) +j = mdi("G91 G0 X1", "G90") +after = carriage(20, 10, 0, 30) +after[0] += 1.0 +expect("G91 X1 after G53.5", j, after) +s.poll() +d = [s.position[i] - before[i] for i in range(3)] +print("%-34s dx %.4f dy %.4f dz %.4f" % ("the tip moved", d[0], d[1], d[2])) +if abs(d[0] - 1.0) > 1e-4 or abs(d[1]) > 1e-4 or abs(d[2]) > 1e-4: + error("the tip did not move by X1 alone") +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-machine-frame/test.ini b/tests/ptp-machine-frame/test.ini new file mode 100644 index 00000000000..20053c9a2db --- /dev/null +++ b/tests/ptp-machine-frame/test.ini @@ -0,0 +1,114 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = slantkins +JOINTS = 5 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-machine-frame/test.sh b/tests/ptp-machine-frame/test.sh new file mode 100755 index 00000000000..48c8eebedaf --- /dev/null +++ b/tests/ptp-machine-frame/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash -e +${SUDO} halcompile --install slantkins.comp >/dev/null +# a failed run leaves the var file behind, and it carries stored positions +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-machine-frame/tool.tbl b/tests/ptp-machine-frame/tool.tbl new file mode 100644 index 00000000000..67f23122e9e --- /dev/null +++ b/tests/ptp-machine-frame/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D6 diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README new file mode 100644 index 00000000000..8fda2b1b350 --- /dev/null +++ b/tests/ptp-robot/README @@ -0,0 +1,6 @@ +The point-to-point moves on a serial robot. + +pumakins maps X, Y and Z to its first three joints, which turn rather +than slide, so G53.5 refuses the machine and says which joint gives it +away. G53.7 names joints by number and works, and G53.4 still takes a +Cartesian destination. diff --git a/tests/ptp-robot/checkresult b/tests/ptp-robot/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-robot/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/ptp-robot/sim.hal b/tests/ptp-robot/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-robot/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/ptp-robot/skip b/tests/ptp-robot/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-robot/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py new file mode 100755 index 00000000000..92362c901a7 --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# A serial robot has no axis letter that names the joint it looks like, so +# the letter form of the point-to-point move is refused here and the joint +# form is the one that works. +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 settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def drain(): + while e.poll(): + pass + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def refused(cmd, expect): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + return + if expect not in m[1]: + error("%s said %r, which does not mention %r" % (cmd, m[1].strip(), expect)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +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 joint form moves the joints it names and leaves the rest alone +before = mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +after = mdi("G53.7 G0 J1=-20 J4=35") +print("G53.7 G0 J1=-20 J4=35 %s" % " ".join("%.4f" % v for v in after)) +drain() +if abs(after[1] + 20) > 1e-6 or abs(after[4] - 35) > 1e-6: + error("G53.7 left joints 1 and 4 at %.6f and %.6f" % (after[1], after[4])) +for j in (0, 2, 3, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) + +# the letter form is refused whichever letter is used, because X names the +# first rotary joint here; the message says so and points at G53.7 +refused("G53.5 G0 X10", "joint 0") +refused("G53.5 G0 A10", "G53.7") +refused("G53.5 G0 Z0", "angular") + +# and the code that takes a Cartesian target still works: the point the +# robot is standing on is reachable by definition, so ask for it +s.poll() +here = list(s.position[:3]) +mdi("G53.7 G0 J1=0 J4=0") +# a serial robot reaches one point with more than one set of joints, so +# only the point is checked, not the pose it comes back in +back = mdi("G53.4 G0 X%.6f Y%.6f Z%.6f" % (here[0], here[1], here[2])) +drain() +s.poll() +if max(abs(a - b) for a, b in zip(s.position[:3], here)) > 1e-3: + error("G53.4 landed at %s, not at %s" + % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) +print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-robot/test.ini b/tests/ptp-robot/test.ini new file mode 100644 index 00000000000..4570f1389e8 --- /dev/null +++ b/tests/ptp-robot/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] +KINEMATICS = pumakins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp pumakins.A2 300 +HALCMD = setp pumakins.A3 50 +HALCMD = setp pumakins.D3 70 +HALCMD = setp pumakins.D4 400 +HALCMD = setp pumakins.D6 80 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-robot/test.sh b/tests/ptp-robot/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-robot/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/ptp-robot/tool.tbl b/tests/ptp-robot/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-robot/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/twp-native/README b/tests/twp-native/README new file mode 100644 index 00000000000..9a49937006d --- /dev/null +++ b/tests/twp-native/README @@ -0,0 +1,13 @@ +The tilted work plane on the xyzacb nutating-head sim, natively. + +G12.1 P1 selects the TCP kinematics, G68.2 defines a plane, and the +orientation moves are checked against the python maths in +tests/kins-twp/xyzacb: G53.1 lands the head on one of the oracle's +angle pairs and leaves the linear joints where they were all through the +move; G53.6 leaves the tool tip where it was; G53.3 ends at the point +asked for in the plane; a move along plane X goes along plane X in the +world; G68.3 reads the plane back off the oriented tool; G69 cancels. +Q1 lets the table take part. The point-to-point moves are checked too: +G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with +the head tilted, G53.4 G1 taking the time the straight move would in G94 and +in G93, and what they refuse. diff --git a/tests/twp-native/abort.ngc b/tests/twp-native/abort.ngc new file mode 100644 index 00000000000..217d67e407a --- /dev/null +++ b/tests/twp-native/abort.ngc @@ -0,0 +1,11 @@ +(a plane, then a long move to be stopped in the middle) +(the cancel at the end is one the read ahead reaches long before the machine does) +G21 G90 G94 +G12.1 P1 +G0 X0 Y0 Z0 A0 B0 C0 +G68.2 P1 Q123 I30 J20 K0 +G53.1 +G1 F300 Z-40 +G1 Z0 +G69 +M2 diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-native/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/twp-native/skip b/tests/twp-native/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/twp-native/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py new file mode 100755 index 00000000000..e5369dc42b1 --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,489 @@ +#!/usr/bin/env python3 +# The tilted work plane on the xyzacb nutating-head sim, natively: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math +import numpy as np + +TOPDIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")) +sys.path.insert(0, os.path.join(TOPDIR, "tests", "kins-twp", "xyzacb")) +import remap_funcs_twp as twp + +JOINTS = 6 +TABLE, SECONDARY, PRIMARY = 3, 4, 5 # A, B, C +I4 = np.asmatrix(np.identity(4)) + +class Log: + def debug(self, *a): pass + def error(self, *a): print("oracle:", a) +log = Log() + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +# run one command and sample joints and positions on the way +# status is a task-cycle snapshot with the feedback a servo cycle behind the +# command, so two equal polls inside one task cycle do not mean the move is +# over: the last sample is taken once the move has settled +def sampled(cmd): + c.mdi(cmd) + samples = [] + t0 = time.time() + while time.time() - t0 < 60: + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + if s.inpos and not s.queue and len(samples) > 20 and samples[-1] == samples[-2]: + break + time.sleep(0.005) + c.wait_complete(60) + end = settled() + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + return end, samples + +# a point-to-point move runs every joint on a straight line in joint space, +# all together: the fraction of the way each moving joint has gone is the +# same for all of them at every sample, never goes back, and reaches one +def joint_line(what, samples, start, end): + moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] + if not moving: + error("%s: no joint moved" % what) + return + last = 0.0 + for n, (j, p) in enumerate(samples): + fs = [(j[i] - start[i]) / (end[i] - start[i]) for i in moving] + f = sum(fs) / len(fs) + if max(abs(x - f) for x in fs) > 1e-3: + error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) + return + if f < last - 1e-6: + error("%s: the joint path ran backwards at sample %d of %d (%.4f after %.4f)" % (what, n, len(samples), f, last)) + return + if f < -1e-6 or f > 1 + 1e-6: + error("%s: a joint left its segment at sample %d of %d (fraction %.4f)" % (what, n, len(samples), f)) + return + last = f + if last < 1 - 1e-6: + error("%s: the last sample is short of the end (fraction %.4f)" % (what, last)) + print("%s: %d joints on one line in joint space through %d samples" % (what, len(moving), len(samples))) + +def show(what, j): + print("%-26s %s" % (what, " ".join("%.4f" % v for v in j))) + +def wrap(d): + return (d + 180.0) % 360.0 - 180.0 + +def close(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +# the pairs the oracle would keep for a tool axis, in machine coordinates, +# as (b, c) in degrees, and which is nearest to the head's present angles +def oracle_pairs(z): + t1s, t2s = twp.kins_calc_possible_joint_angles(log, list(z), None) + pairs = [] + for t1 in t1s or []: + for t2 in t2s or []: + m = twp.kins_calc_transformation_matrix(t1, t2, 0, I4, 'inv') + got = [m[0, 2], m[1, 2], m[2, 2]] + if close(got, z, 1e-6): + pairs.append((math.degrees(t2), math.degrees(t1))) + return pairs + +def nearest_pair(pairs, b_now, c_now): + return min(pairs, key=lambda p: abs(wrap(p[0] - b_now)) + abs(wrap(p[1] - c_now))) + +def tool_axis(joints): + # the tool axis in work coordinates: the head as the oracle models it, + # brought into the table's frame the way the module reports the work + m = twp.kins_calc_transformation_matrix(math.radians(joints[PRIMARY]), + math.radians(joints[SECONDARY]), 0, I4, 'inv') + zm = np.array([m[0, 2], m[1, 2], m[2, 2]]) + a = math.radians(joints[TABLE]) + W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) + return W.T.dot(zm) + +def plane_axes(): + s.poll() + r = s.g68_rotation + return ([r[0], r[3], r[6]], [r[1], r[4], r[7]], [r[2], r[5], r[8]]) + +def rot_x(d): + r = math.radians(d) + return np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]]) + +def rot_y(d): + r = math.radians(d) + return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) + +# --- the plane, and G53.1 with the table held --------------------------- +start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") +show("start", start) +R = rot_y(20).dot(rot_x(30)) +mdi("G68.2 P1 Q123 I30 J20 K0") +after, samples = sampled("G53.1") +show("G53.1", after) +drain() +s.poll() +if not s.g68_active: + error("G68.2 did not leave a plane active") +if not close(plane_axes()[2], list(R[:, 2]), 1e-9): + error("status reports a different plane normal than the program defined") +pairs = oracle_pairs(list(R[:, 2])) +print("oracle pairs (b, c):", ["(%.4f, %.4f)" % p for p in pairs]) +want = nearest_pair(pairs, start[SECONDARY], start[PRIMARY]) +if not pairs or abs(wrap(after[SECONDARY] - want[0])) > 1e-3 or abs(wrap(after[PRIMARY] - want[1])) > 1e-3: + error("G53.1 landed on (%.4f, %.4f), the oracle's nearest pair is (%.4f, %.4f)" + % (after[SECONDARY], after[PRIMARY], want[0], want[1])) +if abs(after[TABLE] - start[TABLE]) > 1e-9: + error("G53.1 moved the table with Q0") +worst = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) +print("linear joints moved at most %.9f through G53.1" % worst) +if worst > 1e-6: + error("G53.1 moved a linear joint") +if not close(tool_axis(after), list(R[:, 2]), 1e-6): + error("the tool axis after G53.1 is not the plane normal") +joint_line("G53.1", samples, start, after) + +# P names the pose rather than its rank: P1 is the one with the secondary +# rotary positive and P2 the one with it negative, from wherever the +# machine is standing, while no P is the nearest and so does depend on it +poses = {} +for where in ("G0 A0 B0 C0", "G0 A0 B-40 C170"): + for word in ("", "P1", "P2"): + mdi("G69") + mdi(where) + mdi("G68.2 P1 Q123 I30 J20 K0") + got = mdi("G53.1 %s" % word) + poses.setdefault(word, []).append(got) + drain() +for word, sign in (("P1", 1), ("P2", -1)): + a, b = poses[word] + rot = lambda j: [j[TABLE], j[SECONDARY], j[PRIMARY]] + if max(abs(wrap(x - y)) for x, y in zip(rot(a), rot(b))) > 1e-4: + error("G53.1 %s landed differently from two starting poses: %s and %s" + % (word, rot(a), rot(b))) + if sign * a[SECONDARY] <= 0: + error("G53.1 %s put the secondary rotary at %.4f" % (word, a[SECONDARY])) +if abs(poses["P1"][0][SECONDARY] - poses["P2"][0][SECONDARY]) < 1e-6: + error("G53.1 P1 and P2 chose the same pose") +if abs(poses[""][0][SECONDARY] - poses[""][1][SECONDARY]) < 1e-6: + error("G53.1 with no P gave the same pose from both starts, so it is not the nearest") +print("G53.1 P1 %.4f, P2 %.4f, no P %.4f then %.4f (secondary rotary)" + % (poses["P1"][0][SECONDARY], poses["P2"][0][SECONDARY], + poses[""][0][SECONDARY], poses[""][1][SECONDARY])) +c.mdi("G53.1 P3") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("G53.1 P3 was accepted") +drain() +mdi("G69") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +mdi("G68.2 P1 Q123 I30 J20 K0") +after = mdi("G53.1") + +# --- moves in the plane go along the plane's axes in the world ---------- +# G53.1 swung the tool tip, so it is somewhere in the plane; a move to X10 +# travels along the plane's X by the difference +def in_plane(): + s.poll() + return R.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])), list(s.position[:3]) +q0, p0 = in_plane() +mdi("G0 X10") +q1, p1 = in_plane() +d = [p1[i] - p0[i] for i in range(3)] +want = list((10 - q0[0]) * R[:, 0]) +if not close(d, want, 1e-3) or abs(q1[0] - 10) > 1e-3: + error("G0 X10 in the plane moved the tool by %s, expected %s" % (d, want)) +mdi("G0 Z5") +q2, p2 = in_plane() +d = [p2[i] - p1[i] for i in range(3)] +want = list((5 - q1[2]) * R[:, 2]) +if not close(d, want, 1e-3) or abs(q2[2] - 5) > 1e-3: + error("G0 Z5 in the plane moved the tool by %s, expected %s" % (d, want)) + +# --- G53.6 keeps the tool centre point -------------------------------- +mdi("G69") +R2 = rot_y(20).dot(rot_x(-30)) +mdi("G68.2 P1 Q123 I-30 J20 K0") +s.poll(); before = list(s.position) +after, samples = sampled("G53.6") +show("G53.6", after) +drain() +worst = max(abs(smp[1][i] - before[i]) for smp in samples for i in range(3)) +print("tool tip moved at most %.6f through G53.6" % worst) +if worst > 1e-3: + error("G53.6 moved the tool tip") +if not close(tool_axis(after), list(R2[:, 2]), 1e-6): + error("the tool axis after G53.6 is not the plane normal") + +# --- G53.3 goes to a point in the plane with the tool oriented ---------- +before = mdi("G69") +R3 = rot_y(-25).dot(rot_x(35)) +mdi("G68.2 P1 Q123 I35 J-25 K0") +after, samples = sampled("G53.3 X5 Y5 Z5") +show("G53.3", after) +drain() +joint_line("G53.3", samples, before, after) +s.poll() +prog = R3.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])) +if not close(list(prog), [5, 5, 5], 1e-3): + error("G53.3 ended at %s in the plane, not 5 5 5" % list(prog)) +if not close(tool_axis(after), list(R3[:, 2]), 1e-6): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool --------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +drain() +s.poll() +x, y, z = plane_axes() +if not s.g68_active or not close(list(s.g68_offset[:3]), [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(z, list(R3[:, 2]), 1e-6): + error("G68.3's normal %s is not the tool axis %s" % (z, list(R3[:, 2]))) +if abs(x[2]) > 1e-6: + error("G68.3's X is not parallel to the machine XY plane: %s" % x) +mdi("G69", "G68.3 R90") +xr, yr, zr = plane_axes() +if not close(xr, y, 1e-6): + error("G68.3 R90 did not turn the plane about its normal") + +# the words a plane code does not use are refused, not ignored +for cmd in ("G68.3 I1 J0 K0", "G68.3 P1", "G68.3 Q123", "G69 R45", "G69 I1", "G69 P1"): + mdi("G69") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + +# --- Q1 lets the table take part --------------------------------------- +mdi("G69", "G0 X0 Y0 Z0 A0 B0 C0") +R4 = rot_x(30) +mdi("G68.2 P1 Q123 I30 J0 K0") +after = mdi("G53.1 Q1") +show("G53.1 Q1", after) +drain() +if not close(tool_axis(after), list(R4[:, 2]), 1e-6): + error("the tool axis after G53.1 Q1 is not the plane normal") +# with the plane X requested as well and the table free, three joints +# place three constraints: the plane's X is reached by the machine rather +# than by the frame +m = twp.kins_calc_transformation_matrix(math.radians(after[PRIMARY]), + math.radians(after[SECONDARY]), 0, I4, 'inv') +a = math.radians(after[TABLE]) +W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) +xm = np.array([m[0, 0], m[1, 0], m[2, 0]]) +if not close(list(W.T.dot(xm)), list(R4[:, 0]), 1e-6): + error("with Q1 the machine did not place the plane's X") + +# --- G53.4, G53.5 and G69 ------------------------------------------------ +before = mdi("G69") +after, samples = sampled("G53.4 G0 X0 Y0 Z0 A0 B0 C0") +show("G53.4 G0", after) +drain() +joint_line("G53.4 G0", samples, before, after) +s.poll() +if s.g68_active: + error("G69 left the plane active") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("G69 left a rotation in status") +if not close(list(s.position[:3]), [0, 0, 0], 1e-6) or any(abs(after[j]) > 1e-6 for j in (TABLE, SECONDARY, PRIMARY)): + error("G53.4 G0 did not bring the tool and the rotaries back to zero") + +# G53.7 takes joint values by joint number: with the head tilted, J2=-5 +# puts joint 2 at -5 whatever that does to the tool tip, and touches no +# other joint; the value is the joint's own, untouched by G20 +before = mdi("G0 B30") +show("before G53.7", before) +after = mdi("G20 G53.7 G0 J2=-5") +mdi("G21") +show("G53.7 G0 J2=-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.7 J2=-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 J2=-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G53.7 G0 J2=0 J[2+2]=0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.7 J2=0 J4=0 did not put joints 2 and 4 at zero") + +# G53.5 takes the same destination by axis letter, in program units: Z-5 +# is joint 2 in millimetres, and under G20 the same words are inches +before = mdi("G0 B30") +after = mdi("G53.5 G0 Z-5") +show("G53.5 G0 Z-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.5 Z-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.5 Z-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G20 G53.5 G0 Z-1") +mdi("G21") +show("G20 G53.5 G0 Z-1", after) +if abs(after[2] + 25.4) > 1e-6: + error("an inch of G53.5 Z left joint 2 at %.6f, not -25.4" % after[2]) +after = mdi("G53.5 G0 Z0 B0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") + +# a point-to-point feed takes the time the straight move would: 10 mm at +# F600 is one second, and F30 in G93 is two +def timed(cmd): + c.mdi(cmd) + first = last = None + t0 = time.time() + s.poll(); start = [s.joint_position[i] for i in range(JOINTS)] + while time.time() - t0 < 60: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if now != start: + if first is None: + first = time.time() + last = time.time() + start = now + elif first is not None and s.inpos and not s.queue and time.time() - last > 0.3: + break + time.sleep(0.005) + c.wait_complete(60) + settled() + return (last - first) if first else 0.0 +mdi("G0 X0 Y0 Z0 A0 B0 C0") +took = timed("G53.4 G1 X10 F600") +print("G53.4 G1 X10 F600 took %.3f s" % took) +if not 0.7 < took < 1.5: + error("a 10 mm point-to-point feed at F600 took %.3f s, not about one" % took) +took = timed("G93 G53.4 G1 X0 F30") +mdi("G94") +print("G93 G53.4 G1 X0 F30 took %.3f s" % took) +if not 1.6 < took < 2.6: + error("a point-to-point feed at G93 F30 took %.3f s, not about two" % took) +drain() + +# what the point-to-point codes refuse +for cmd in ("G53.4 G2 X1 I1", "G91 G53.7 G0 J0=1", "G53.7 G0 J9=1", "G53.7 G0 X1", + "G53.7 G0 J1", "G53.7 G0", "G0 J0=1", "G53.7 G0 J0.5=1", "G53.7 G0 J0=1 J0=2", + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1"): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) +mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") + +# --- a plane refuses what would move the ground under it --------------- +# each refusal is an interpreter error, and the abort that follows cancels +# the plane, so it is defined afresh before every one +for cmd in ("G92 X1", "G52 A1", "G10 L2 P1 X1"): + mdi("G68.2 P1 Q123 I30 J0 K0") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted while a plane is active" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + s.poll() + if s.g68_active: + error("the abort after %s left the plane active" % cmd) +mdi("G69") + +# --- stopping a program that has a plane --------------------------------- +# The read ahead runs the program's own G69 long before the machine gets +# there, so the cancel is sitting in the queue when the stop button throws +# the queue away. The plane in status has to end up cancelled all the same, +# and a G69 typed afterwards has to be able to say so again. +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete(30) +c.program_open("abort.ngc") +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if s.g68_active and s.current_line >= 8: + break + time.sleep(0.02) +s.poll() +if not s.g68_active: + error("the program never reported a plane to stop in the middle of") +c.abort() +c.wait_complete(30) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() +s.poll() +if s.g68_active: + error("stopping the program left the plane active in status") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("stopping the program left a rotation in status") +mdi("G69") +s.poll() +if s.g68_active: + error("G69 after the stop did not clear the plane") +print("a stopped program leaves no plane behind") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini new file mode 100644 index 00000000000..a3a6bcf9e1a --- /dev/null +++ b/tests/twp-native/test.ini @@ -0,0 +1,147 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = xyzacb_trsrn +JOINTS = 6 + +# what the python oracle reads +[TWP] +PRIMARY = C +SECONDARY = B + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzacb_trsrn_kins.nut-angle 45 +HALCMD = setp xyzacb_trsrn_kins.y-pivot 100 +HALCMD = setp xyzacb_trsrn_kins.z-pivot 200 +HALCMD = setp xyzacb_trsrn_kins.x-offset 5 +HALCMD = setp xyzacb_trsrn_kins.y-offset 7 +HALCMD = setp xyzacb_trsrn_kins.y-rot-axis 300 +HALCMD = setp xyzacb_trsrn_kins.z-rot-axis 400 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -320 +MAX_LIMIT = 320 +HOME_SEQUENCE = 0 diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/twp-native/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/twp-native/tool.tbl b/tests/twp-native/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-native/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/twp-table/README b/tests/twp-table/README new file mode 100644 index 00000000000..29d0082bc38 --- /dev/null +++ b/tests/twp-table/README @@ -0,0 +1,7 @@ +G53.1 on a table-table machine, xyzac-trt-kins: both rotaries carry the +work, so the pose is named by the sign of the one that carries the other, +A here, as Heidenhain's SEQ names its master axis. For each plane, from +several starting poses: G53.1 with no P takes the nearer of the two poses, +P1 always the one with A positive and P2 the one with A negative, the same +pose from every start; each leaves the tool along the plane's normal; a +move in the plane after the turn ends where the plane puts it. diff --git a/tests/twp-table/checkresult b/tests/twp-table/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-table/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-table/sim.hal b/tests/twp-table/sim.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/twp-table/sim.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-table/test-ui.py b/tests/twp-table/test-ui.py new file mode 100755 index 00000000000..bc24aaf278d --- /dev/null +++ b/tests/twp-table/test-ui.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# G53.1 P1 and P2 on the xyzac-trt sim: see README. + +import linuxcnc +import sys +import os +import time +import math + +JOINTS = 5 +A, C = 3, 4 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + j = settled() + drain() + return j + +# the tool axis in the work frame, conventional-directions 0: the table +# turns C then A, so the tool is Rz(-C) Rx(-A) along Z seen from the part +def tool_axis(j): + a, cc = math.radians(j[A]), math.radians(j[C]) + return (math.sin(cc)*math.sin(a), math.cos(cc)*math.sin(a), math.cos(a)) + +# the plane's Z for G68.2 I J with K0, Euler Z X Z +def normal(i, j): + i, j = math.radians(i), math.radians(j) + return (math.sin(i)*math.sin(j), -math.cos(i)*math.sin(j), math.cos(j)) + +def angle(u, v): + return math.degrees(math.acos(max(-1.0, min(1.0, sum(p*q for p, q in zip(u, v)))))) + +def travel(j, start): + return abs(j[A] - start[0]) + abs(j[C] - start[1]) + +mdi("G12.1 P1", "G0 X0 Y0 Z60 A0 C0") + +planes = [(0, 30), (60, 30), (-90, 45), (45, -30), (150, 20)] +starts = [(0, 0), (-40, 200), (40, -100)] +for i, jj in planes: + nearest = [] + for word, sign in (("", 0), ("P1", 1), ("P2", -1)): + landed = [] + for start in starts: + mdi("G69", "G0 X0 Y0 Z60 A%g C%g" % start, + "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj)) + j = mdi("G53.1 %s" % word) + off = angle(tool_axis(j), normal(i, jj)) + if off > 1e-6: + error("G53.1 %s on I%g J%g from A%g C%g left the tool %.6f degrees off the normal" + % (word, i, jj, start[0], start[1], off)) + if sign and j[A] * sign <= 0: + error("G53.1 %s on I%g J%g from A%g C%g put A at %.4f" + % (word, i, jj, start[0], start[1], j[A])) + landed.append(j) + if not sign: + nearest.append((j, start)) + if sign: + # one pose, C on whichever turn is nearest the start + for j in landed[1:]: + d = (j[C] - landed[0][C]) % 360.0 + if abs(j[A] - landed[0][A]) > 1e-6 or min(d, 360.0 - d) > 1e-6: + error("G53.1 %s on I%g J%g landed on different poses: %s and %s" + % (word, i, jj, landed[0], j)) + print("I%g J%g G53.1 %s: %s" % (i, jj, word, + ", ".join("A%.3f C%.3f" % (j[A], j[C]) for j in landed))) + # no P: the nearer of the two poses, by rotary travel + for (j, start) in nearest: + mdi("G69", "G0 X0 Y0 Z60 A%g C%g" % start, "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj)) + other = mdi("G53.1 %s" % ("P2" if j[A] > 0 else "P1")) + if travel(other, start) < travel(j, start) - 1e-6: + error("G53.1 on I%g J%g from A%g C%g went to A%.3f C%.3f, A%.3f C%.3f is nearer" + % (i, jj, start[0], start[1], j[A], j[C], other[A], other[C])) + +# after the turn, a move in the plane ends where the plane puts it +for i, jj, word in ((60, 30, "P1"), (60, 30, "P2"), (-90, 45, "P1")): + mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj), + "G53.1 %s" % word, "G0 X10 Y5 Z20", "G1 X-10 F2000") + s.poll() + ri, rj = math.radians(i), math.radians(jj) + px, py, pz = -10.0, 5.0, 20.0 + y1 = py*math.cos(rj) - pz*math.sin(rj) + z1 = py*math.sin(rj) + pz*math.cos(rj) + want = (px*math.cos(ri) - y1*math.sin(ri), px*math.sin(ri) + y1*math.cos(ri), z1) + got = s.position[:3] + if max(abs(p - q) for p, q in zip(got, want)) > 1e-6: + error("after G53.1 %s on I%g J%g the tip is at %s, the plane puts it at %s" + % (word, i, jj, got, want)) + +mdi("G69", "G0 X0 Y0 Z60 A0 C0") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-table/test.ini b/tests/twp-table/test.ini new file mode 100644 index 00000000000..44fb0768e9f --- /dev/null +++ b/tests/twp-table/test.ini @@ -0,0 +1,122 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzac-trt-kins.x-rot-point 100 +HALCMD = setp xyzac-trt-kins.y-rot-point -50 +HALCMD = setp xyzac-trt-kins.z-rot-point -80 + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +HOME_SEQUENCE = 0 diff --git a/tests/twp-table/test.sh b/tests/twp-table/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/twp-table/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/twp-table/tool.tbl b/tests/twp-table/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-table/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 From ac3e6d53b4c36621a5011876aaa7f244230a019b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:58:37 +1000 Subject: [PATCH 06/24] configs: the nutating-head sims on the native tilted work plane The two trsrn_twp configs drop the python remap of G68.2, G68.3, G68.4, G69, G53.1, G53.3 and G53.6, its ngc wrappers, the abort handler that unwound it and the twp-status analog pin: the interpreter does all of it now, with nothing to restore on abort. The helper component that feeds the vismach model reads the plane from status. The demos select the TCP kinematics up front, since the orientation codes need it, and put kinematics 0 back at the end; square.ngc loses its G52, which a plane refuses. --- .../README | 33 +- .../demos/incremental_repetition.ngc | 2 + .../incremental_repetition_back_and_forth.ngc | 2 + .../demos/incremental_repetition_g533.ngc | 4 +- .../demos/simple_example.ngc | 2 + .../demos/square.ngc | 1 - .../python/remap.py | 1614 ----------------- .../python/toplevel.py | 20 - .../python/twp-helper-comp.py | 94 +- .../python/util.py | 67 - .../remap_subs/g531remap.ngc | 14 - .../remap_subs/g533remap.ngc | 20 - .../remap_subs/g536remap.ngc | 14 - .../remap_subs/g69remap.ngc | 11 - .../remap_subs/on_abort_with_twp_reset.ngc | 15 - .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 39 - .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 40 +- tests/twp-native/checkresult | 3 + tests/twp-native/test-ui.py | 12 +- 19 files changed, 63 insertions(+), 1944 deletions(-) delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc create mode 100755 tests/twp-native/checkresult diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README index fea7c5d4882..ba7fafdbe6d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README @@ -1,27 +1,20 @@ This is a simulation configuration for a 6 axis machine with one table rotary and two spindle rotary joints -This simulation also includes a python remap of Gcodes for tilted workplane (TWP) functionality. -Both the kinematic and the twp remap support nutation of the secondary rotary joints (ie A or B ) form 0 to 90°. -Hence this also works for the 'usual' orthogonal spindle rotary-tilt type machines by setting the nutation angle to 90°. +This simulation uses the interpreter's tilted work plane codes, documented in the G-code section of the manual: -Implemented TWP functionality: -G68.2 : defines twp using euler-angles, pitch-roll-yaw, 2-vectors, 3 points, optionally with offset in XYZ and rotation in XY -G68.3 : defines twp from current tool orientation, optionally with offset in XYZ and rotation in XY -G68.4 : same as G68.2 but as an incremental definition from an active TWP plane -G69 : cancels the current twp (resets all parameters, moves to G54 and sets Identity kinematics) -G53.1 (P) : spindle orientation without tcp, switches to G59 and activates tool kinematics -G53.3 (P XYZ) : same as G53.1 but with simultaneous move the the XYZ coords on the twp plane -G53.6 (P) : same as G53.1 but spindle orientation with tcp +G68.2 : defines the plane by three angles, three points or two vectors, with an origin in XYZ and a turn R about the plane's Z +G68.3 : defines the plane from the current tool direction, with an origin in XYZ and a turn R +G68.4 : any G68.2 form, composed onto the active plane +G69 : cancels the plane +G53.1 (P Q) : orients the tool to the plane, rotaries only, the linear joints stay where they are +G53.3 (P Q XYZ) : orients the tool and moves to XYZ in the plane, interpolated in joint space +G53.6 (P Q) : orients the tool with the tool centre point held -- Spindle is C primary, A secondary or B secondary as defined in the [TWP] section of the ini file -- All G53.x commands will respect axis limits as set in the ini file for the respective primary and secondary spindle joints. -- The P word sets the orientation strategy: 0(default)=shortest distance, - 1=positive rotation only, - 2=negative rotation only - (this applies to the primary rotary, the secondary moves the shortest distance) +The orientation codes need the TCP kinematics (G12.1 P1). P picks the solution, nearest to the present rotary position first. Q0 holds the table and lets the head do it, Q1 lets the table take part as well. + +The kinematic supports nutation of the secondary rotary joint (A or B) from 0 to 90 degrees, so this also covers the usual orthogonal spindle rotary-tilt machines by setting the nutation angle to 90 degrees. + +The python maths this configuration used to carry as a remap lives on in tests/kins-twp, where the kinematics module is checked against it. For more: https://forum.linuxcnc.org/show-your-stuff/49103-kinematic-model-for-a-5axis-mill-with-universal-nutating-head?start=0#271334 - -Full Documentation can be found at: -https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/table-rotary_spindle-rotary-nutating/Documentation diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc index 31feddcff12..4dc18624cf2 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -13,5 +14,6 @@ o100 REPEAT[100] g0 y50 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc index 37b6f85032d..e1d5bb5d34b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc @@ -1,4 +1,5 @@ g69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g68.2 q121 i0 j5 @@ -20,4 +21,5 @@ o100 REPEAT[1000] o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc index f01324916b5..0a2e4731539 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -7,7 +8,7 @@ o100 REPEAT[100] g68.4 q131 i-35 j-35 k0 ;g53.3 p0 x50y50z150 g53.6 - x50y50z150 + g0 x50y50z150 g0 z100 g0 x-50 g0 y-50 @@ -16,5 +17,6 @@ o100 REPEAT[100] g0 x0y0z120 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc index 7586f2f1d3c..bc2f46c6b6b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc @@ -1,4 +1,6 @@ g69 +g13.1 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g0 x0y0z100 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc index c7e263ce601..c1f2bbf2abe 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc @@ -5,5 +5,4 @@ osub g0 x50 g0 y50 g0 x0y0z120 - g52 x0y0z0 oendsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py deleted file mode 100755 index 7431b8ad635..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ /dev/null @@ -1,1614 +0,0 @@ -# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' -# G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 -# -# Copyright ()c) 2025 David Mueller -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -''' -The remap does the following: - -- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). -- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). -- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). -- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. -- Sets the kinematic modes -- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. -- Used MDI commands to: - - Move the rotary joints to the calculated positions - - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates -- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode -''' - - -import sys -import traceback -import numpy as np -from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs -from interpreter import * -import emccanon -from util import lineno, call_pydevd -import hal - -# logging -import logging -# this name will be printed first on each log message -log = logging.getLogger('remap.py; TWP') -# we have to setup a handler to be able to set the log level for this module -handler = logging.StreamHandler() -formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') -handler.setFormatter(formatter) -log.addHandler(handler) - -# set up parsing of the inifile -import os -import linuxcnc -# get the path for the ini file used to start this config -inifile = os.environ.get("INI_FILE_NAME") - -# adding the remap_funcs folder to the system path. The machine specific -# functions live beside the ini file, which is the working directory, and the -# parent is searched too so a config may keep them one level up and share them -# between variants. -cwd = os.getcwd() -parent = os.path.abspath(os.path.join(cwd, os.pardir)) -sys.path.insert(0, parent) -sys.path.insert(0, cwd) -from remap_funcs_twp import * - -# instantiate the LinuxCNC ini-parser -config = linuxcnc.ini(inifile) - -# debug setting -try: - debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) - if debug_setting > 4: debug_setting = 4 - if debug_setting < 0: debug_setting = 0 -except Exception as error: - debug_setting = 1 - log.warning("Unable to parse debug setting given in INI. Setting it to 1.") -debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) -log.setLevel(debug_levels[debug_setting]) - -## ROTARY JOINT LETTERS -# primary rotary joint (independent of the secondary joint) -joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# secondary rotary joint (dependent on the primary joint) -joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() - -if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): - log.error("Unable to parse joint letters given in INI [TWP].") -elif joint_letter_primary == joint_letter_secondary: - log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") -else: - # get the MIN/MAX limits of the respective rotary joint letters - category = 'AXIS_' + joint_letter_primary - primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', - joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) - category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', - joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) - - -## CONNECTIONS TO THE HELPER COMPONENT -twp_comp = 'twp-helper-comp.' -twp_is_defined = twp_comp + 'twp-is-defined' -twp_is_active = twp_comp + 'twp-is-active' - -# Which rotary joint should be prioritized when calculating optimal joint rotation angles -try: - optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) -except Exception as error: - log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") - optimization_priority = 1 - -# raise InterpreterException if execute() or read() fail -throw_exceptions = 1 - -## VALUE INITIALIZATION -# we start with the identity matrix (ie the twp is equal to the world coordinates) -twp_matrix = np.asmatrix(np.identity(4)) - -# some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for G53.n -# [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] -# note that we use string since boolean True == 1, which gives wrong results if we want -# to count the elements that are True because it is counted as integer '1' -# eg: twp_flag = [0, 1, 'empty'] -twp_flag = [] -# we need a place to store the twp-build-parameters if the mode needs more than one call -twp_build_params = {} -# container to store the current work offset during twp operations -current_work_offset_number = 1 -saved_work_offset = [0,0,0] -# orientation mode refers to the strategy used to choose from the different rotary angles for a given -# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being -# the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) -orient_mode = 0 - - - -# define the basic rotation matrices -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) - - - -def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians - # returns the rotation matrices for given order and angles - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - debug_msg = (f' Euler order {order} requested with angles: ' - f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') - log.debug(debug_msg) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - #log.debug(' Returning euler rotation as matrix: \n %s', matrix) - return matrix - - -def calc_joint_angles(z_vector_req, x_vector_req): - # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector - # returns an empty list if no valid position could be found - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(' z_vector_requested: %s', z_vector_req) - log.debug(' x_vector_requested: %s', x_vector_req) - # set the tolerance value - epsilon = 0.0001 - # create np.array so we can easily calculate differences and check elements - z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) - # calculate joint values using kinematic specific formula - try: - (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) - except Exception as error: - log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) - - # remove any duplicate values from the results - theta_1_calcd = tuple(set(theta_1_calcd)) - theta_2_calcd = tuple(set(theta_2_calcd)) - log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) - log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) - if theta_1_calcd == None or theta_2_calcd == None: - return [] - angle_pairs_list = [] - # create a list of paired combinations of returned angles (theta_1 , theta_2) - for i in range(len(theta_1_calcd)): - for j in range(len(theta_2_calcd)): - angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) - angle_pairs_list = list(set(angle_pairs_list)) - # iterate through the list and check if a particular pair actually produces the requested z-vector orientation - joint_angles_list = [] - for i in range(len(angle_pairs_list)): - debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' - f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') - log.debug(debug_msg) - # we start with an identity matrix (ie oriented to world) - matrix_in = np.asmatrix(np.identity(4)) - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - try: - matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) - except Exception as error: - log.error('kins_calc_transformation_matrix, %s', error) - # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column - z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) - # calculate the difference of the respective elements - z_vector_diff = z_vector_req - z_vector_would_be - log.debug(' z_vector_diff: %s', z_vector_diff) - # and check if all elements are within [-epsilon,epsilon] - match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) - log.debug(' Is the z-vector-vector close enough ? %s', match_z) - if match_z: - joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) - for (theta_1, theta_2) in joint_angles_list: - log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') - return joint_angles_list # returns radians - - -def calc_shortest_distance(pos, trgt, mode): - pos = degrees(pos) - trgt = degrees(trgt) - # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation we may need to return the long distance instead - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - dist_short = (trgt - pos + 180) % 360 - 180 - # calculate short and long distance - if dist_short >= 0: # ie dist_long should be negative - dist_long = -(360 - dist_short) - else: - dist_long = 360 + dist_short - log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') - if mode == 1: # positive rotation only, ie we want a positive distance - if dist_short >= 0: # ie we want this one - dist = dist_short - else: # ie we need to go the other way - dist = dist_long - elif mode == 2: # negative rotation only ie we want a positive distance - if dist_short > 0: # ie we need to go the other way - dist = dist_long - else: # ie we want this one - dist = dist_short - else: # mode = 0 ie we want the shortest distance either way - dist = dist_short - log.debug(f'Returning distance: {dist:.4f}°') - return radians(dist) - - -def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians - # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] - # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance - # from the given position to that target angle - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') - # calculate the shortest distance from position to target for the strategy given by - # the operator (ie shortest (= default), positive rotation only, negative rotation only ) - dist = calc_shortest_distance(pos, trgt, mode) - # check that the result is within the rotary axis limits defined in the ini file - if dist >= 0: # shortest way is in the positive direction - if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer way in the other direction - log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') - if mode == 0: - dist = dist - 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - else: # shortest way is in the negative direction - if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if negative limits would be exceeded we need to go the longer way int the other direction - log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') - if mode == 0: - dist = dist + 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - if theta is not None: - log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') - # we also attach the distance for this particular move and mode - return theta, dist # returns radians - - -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians - # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves - # in (min_limit, max_linit) from the current joint positions using the orient_mode set by - # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit - global orient_mode - # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians - # we want to return a list of angles that are optimized for the orient_mode and the - # rotary axes limits as set in the ini file - target_dist_list= [] - for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # For the priortized joint we apply the orient mode requested by the operator - # the other we optimize for shortest move - if optimization_priority == 2: - primary_strategy = 0 - secondary_strategy = orient_mode - else: - primary_strategy = orient_mode - secondary_strategy = 0 - # primary joint - prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, - primary_max_limit, primary_min_limit, - primary_strategy) - # secondary joint - sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, - secondary_max_limit, secondary_min_limit, - secondary_strategy) - # if a solution has been found for this particular pair then we add it to the list - if not (prim_move == None) and not (sec_move == None): - target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: - debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' - f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') - log.debug(debug_msg) - return target_dist_list # returns radians - - -def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): - # find the optimal joint move from current to target positions in the list - # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only - # For orient_mode=(1,2): If no move can be found within joint limits we return None - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global orient_mode - # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that - # will result in correct tool orientation, stay within the rotary axis limits and respect the - # orient_mode if set by the operator - valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) - if len(valid_joint_moves_and_distances) < 1: - log.error(f' No valid joint moves found.') - return (None, None) - # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the prioritized joint - (theta_1, theta_2) = (None, None) - joint = optimization_priority - 1 - dist = 10 # some large initial value - for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - if theta_1 is not None: - debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' - f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') - log.debug(debug_msg) - return theta_1, theta_2 # returns radians - - -def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians - # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested - # orientation after rotation - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # tolerance setting for check if x-vector-vector needs to be rotated at all - epsilon = 0.00000001 - log.info(" x-vector-requested: %s", x_vector_req) - debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' - f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') - log.debug(debug_msg) - # run matrix_in through the kinematic transformation in the requested direction - # using the given joint angles and zero virtual-rotation - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the x-vector for the given machine joint rotations is found directly in the first column - x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(" X-vector after machine rotation would be: %s", x_vector_is) - # we calculate the angular difference between the two vectors so we can add a virtual rotation - # around z-vector or work-z to match the requested x orientation after machine rotation - # just to be sure we normalize the two vectors - x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) - x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) - # check if the x-vector is already in the required orientation (ie parallel) - log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) - if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: - log.info(" X-vector already oriented, setting virtual-rotation = 0") - # if we are already parallel then we don't need to add a virtual rotation - virtual_rot = 0 - else: - # we can use the cross product to determine the direction we need to rotate - cross = np.cross(x_vector_req, x_vector_is) - log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) - virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) - log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') - # To find out which quadrant we need the angle to be in we create a list of them all - virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] - log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) - # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector - for virtual_rot in virtual_rot_list: - log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') - zeta = 0.0001 - # run the identity matrix through the kinematic transformation in the requested direction - # using the given joint angles and virtual-rotation angle in the list - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the oriented x-vector is found directly in the first column - x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(' x_vector_would_be: %s', x_vector_would_be) - # calculate the difference of the respective elements - x_vector_diff = x_vector_req - x_vector_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) - log.debug(' Is the X-vector close enough ? %s', match) - if match: - # if we have a match we leave the loop and use this angle - break - log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') - return virtual_rot # returns radians - - -def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians - # transforms a 4x4 input matrix using the current transformation matrix - # (forward or inverse) using the kinematic model of the machine - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global kins_virtual_rotation - # read current spindle rotary angles (radians) - theta_1, theta_2 = get_current_rotary_positions(self) - # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector - log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") - # run matrix_in through the kinematic transformation in the requested direction - # using the current joint angles and virtual-rotation as requested - try: - twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_twp_matrix_from_joint_position, %s', error) - return twp_matrix - - -def gui_update_twp(): - # The tilted-work-plane is created in identity mode and must NOT be updated after a switch - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, saved_work_offset - # twp origin as vector (in world coords) from current work-offset to the origin of the twp - try: - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - # publish the twp offset coordinates in world coordinates (ie identity) - [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) - # this is used to translate the rotated twp to the correct position - # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created in the origin, - # then rotated according to the rotary joint position and then translated. - # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then - # translated by the offset values of the identity mode. - try: - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - - -# NOTE: Due to easier abort handling we currently restrict the use of twp to G54 -# as LinuxCNC seems to revert to G54 as the default system -def get_current_work_offset(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # get which offset is active (g54=1 .. g59.3=9) - active_offset = int(self.params[5220]) - current_work_offset_number = active_offset - # set the relevant parameter numbers that hold the active offset values - # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) - work_offset_x = (active_offset-1)*20 + 5221 - work_offset_y = work_offset_x + 1 - work_offset_z = work_offset_x + 2 - co_x = self.params[work_offset_x] - co_y = self.params[work_offset_y] - co_z = self.params[work_offset_z] - current_work_offset = (co_x, co_y, co_z) - return [current_work_offset_number, current_work_offset] - - -def get_current_rotary_positions(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global joint_letter_primary, joint_letter_secondary - if joint_letter_primary == 'A': - theta_1 = radians(self.AA_current) - elif joint_letter_primary == 'B': - theta_1 = radians(self.BB_current) - elif joint_letter_primary == 'C': - theta_1 = radians(self.CC_current) - log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') - # read current spindle rotary angles and convert to radians - if joint_letter_secondary == 'A': - theta_2 = radians(self.AA_current) - elif joint_letter_secondary == 'B': - theta_2 = radians(self.BB_current) - elif joint_letter_secondary == 'C': - theta_2 = radians(self.CC_current) - log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') - return theta_1, theta_2 - - -def reset_twp_params(): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global virtual_rot, twp_matrix, twp_flag, twp_build_params - virtual_rot = 0 - # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_virtual_rot(0)! - twp_flag = [] - twp_build_params = {} - log.info(" Resetting TWP-matrix") - twp_matrix = np.asmatrix(np.identity(4)) - - -def g53n_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) - # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the - # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from - # an ngc reamp that contains a quebuster before calling this code. - # IMPORTANT: - # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called - # (ie do it in the ngc remap mentioned above!) - global saved_work_offset, twp_matrix, twp_flag, virtual_rot - global joint_letter_primary, joint_letter_secondary, twp_error_status - global orient_mode - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - if not hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: No TWP defined." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - elif hal.get_value(twp_is_active): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: TWP already active" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # Check if any words have been passed with the respective G53.n command - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - x = c.i_number if c.i_flag else None - y = c.j_number if c.j_flag else None - z = c.k_number if c.k_flag else None - log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) - - if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params() - msg = "G53.n : unrecognised P-Word found." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - orient_mode = p - z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation - try: - # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians - except Exception as error: - log.error('calc_joint_angles, %s', error) - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - if possible_prim_sec_angle_pairs == []: - # reset the twp parameters - log.error('G53.n: No possible primary/secondary angle pairs found.') - reset_twp_params() - msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - try: - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians - except Exception as error: - log.error('G53.n: Calculation of optimal joint move failed, %s', error) - if theta_1 == None or theta_2 == None: - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # get the particular conditions to be met for the kinematic at hand - try: - (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, - z_vector_requested, - twp_matrix) - except Exception as error: - log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) - # calculate the virtual-rotation needed - virtual_rot = calc_virtual_rotation(theta_1, - theta_2, - x_vector_requested, - z_vector_requested, - matrix_in, - direction) # returns radians - log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") - - # mark twp-flag as active - twp_flag = [0, 'active'] - gui_update_twp() - - # set the virtual-rotation value in the kinematic component - debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' - f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') - log.debug(debug_msg) - try: - kins_set_values(theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: kins_set_values failed, %s', error) - - # calculate the work offset in transformed-coordinatess - log.debug(" G53.n: Saved work offset: %s", saved_work_offset) - twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - try: - new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) - debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' - f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') - log.debug(debug_msg) - # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - - log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") - if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool and the requested work plane - self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # switch to the dedicated TWP work offsets - self.execute("G59", lineno()) - # activate TWP kinematics - self.execute("G12.1 P2") - if (x,y,z) != (None,None,None): - log.debug(' G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % - (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # set twp-state to 'active' (2) - self.execute("M68 E2 Q2") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# Cancel an active TWP definition and reset the parameters to zero -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc that contains a quebuster before calling this code -def g69_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_flag, saved_work_offset_number, saved_work_offset - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - log.info('G69 called') - # reset the twp parameters - reset_twp_params() - gui_update_twp() - # set twp-state to 'undefined' (0) - self.execute("M68 E2 Q0") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation -def g683(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg =("G68.3 ERROR: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - # get which offset is active (g54=1 .. g59.3=9) - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.3 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - c = self.blocks[self.remap_level] - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of x-vector around the origin - r = radians(c.r_number) if c.r_flag else 0 - - twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) # radians - # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand - try: - virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) - except Exception as error: - log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) - log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) - # then we need to calculate the transformation matrix of the current orientation with the including the - # calculated virtual-rotation. - # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the - # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle - # passed in the R word of the G68.3 command - start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) - # the required transformation direction may depend on the kinematic at hand - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) - log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) - # put the requested origin into the twp_matrix - (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) - # update the build state of the twp call - twp_flag[2] = 'done' - log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset = offsets - saved_work_offset_number = n - log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g682(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): # ie TWP has already been defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: TWP already defined.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset_number = n - saved_work_offset = offsets - log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional - twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs three calls, plus an optional Q0 first - if q == 0 and 'done' in twp_flag[2:]: - q = -1 # Q0 after the points, refused below - if q == 0: # shift of the origin from point 1, along the plane's axes - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - twp_build_params['q0'] = [x,y,z] - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[2] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[3] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[4] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # R may come on any one block of the definition - if c.r_flag: - if 'r_q' in twp_build_params: - msg = ("G68.2 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) - reset_twp_params() - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - twp_build_params['r_q'] = q - twp_build_params['r'] = radians(c.r_number) - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z] = twp_build_params['q0'][0:3] - r = twp_build_params.get('r', 0) - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.2 P2: Point 1: %s",p1) - log.debug(" G68.2 P2: Point 2: %s",p2) - log.debug(" G68.2 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.2 P2 (v2): %s",v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - # the origin is point 1, moved by Q0's x y z along the plane's own axes - twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - log.info(' first call') - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q1':[], 'q2':[]} - log.debug(' twp_build_params: %s', twp_build_params) - if q == 1: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 2: # define second vector (the normal vector of the twp) - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q2'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] - r = twp_build_params['q1'][6] - (i, j, k) = twp_build_params['q1'][3:6] - (i1, j1, k1) = twp_build_params['q2'] - log.debug("(x, y, z): %s", (x, y, z)) - log.debug("(i, j, k): %s", (i, j, k)) - log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # X is kept as given and Z is projected onto the plane normal - # to it, as Fanuc does; vectors 5 degrees or more off square, - # or a zero vector, are refused - twp_vect_x = np.array([i, j, k], dtype=float) - twp_vect_z = np.array([i1, j1, k1], dtype=float) - len_x = np.linalg.norm(twp_vect_x) - len_z = np.linalg.norm(twp_vect_z) - orth = 1.0 - if len_x > 1e-12 and len_z > 1e-12: - twp_vect_x = twp_vect_x / len_x - twp_vect_z = twp_vect_z / len_z - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - if abs(orth) >= sin(radians(5)): - reset_twp_params() - msg = ("G68.2 P3: The vectors are zero or 5 degrees or more off square.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - twp_vect_z = twp_vect_z - orth * twp_vect_x - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.2: twp_flag: %s", twp_flag) - log.debug(" G68.2: calls required: %s", twp_flag.count('done')) - log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) - log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.2: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.2: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.2: twp vector-z: %s", twp_vect_z) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g684(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if not hal.get_value(twp_is_defined): # ie there is currently no TWP defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No TWP defined to increment from. Run G68.2 or G68.3 first.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - n = get_current_work_offset(self)[0] - # Must be in one of the dedicated offset systems for TWP - if False: #n < 6: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # store the current TWP to - twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional - twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs three calls, plus an optional Q0 first - if q == 0 and 'done' in twp_flag[2:]: - q = -1 # Q0 after the points, refused below - if q == 0: # shift of the origin from point 1, along the plane's axes - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - twp_build_params['q0'] = [x,y,z] - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[2] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[3] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[4] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # R may come on any one block of the definition - if c.r_flag: - if 'r_q' in twp_build_params: - msg = ("G68.4 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) - reset_twp_params() - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - twp_build_params['r_q'] = q - twp_build_params['r'] = radians(c.r_number) - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z] = twp_build_params['q0'][0:3] - r = twp_build_params.get('r', 0) - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.4 P2: Point 1: %s",p1) - log.debug(" G68.4 P2: Point 2: %s",p2) - log.debug(" G68.4 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.4 P2: (v2) %s", v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - # the origin is point 1, moved by Q0's x y z along the plane's own axes - twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q1':[], 'q2':[]} - if q == 1: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 2: # define second vector (the normal vector of the twp) - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q2'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] - r = twp_build_params['q1'][6] - (i, j, k) = twp_build_params['q1'][3:6] - (i1, j1, k1) = twp_build_params['q2'] - log.debug("(x, y, z) %s", (x, y, z)) - log.debug("(i, j, k) %s", (i, j, k)) - log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # X is kept as given and Z is projected onto the plane normal - # to it, as Fanuc does; vectors 5 degrees or more off square, - # or a zero vector, are refused - twp_vect_x = np.array([i, j, k], dtype=float) - twp_vect_z = np.array([i1, j1, k1], dtype=float) - len_x = np.linalg.norm(twp_vect_x) - len_z = np.linalg.norm(twp_vect_z) - orth = 1.0 - if len_x > 1e-12 and len_z > 1e-12: - twp_vect_x = twp_vect_x / len_x - twp_vect_z = twp_vect_z / len_z - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - if abs(orth) >= sin(radians(5)): - reset_twp_params() - msg = ("G68.4 P3: The vectors are zero or 5 degrees or more off square.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - twp_vect_z = twp_vect_z - orth * twp_vect_x - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.4: twp_flag: %s", twp_flag) - log.debug(" G68.4: calls required: %s", twp_flag.count('done')) - log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) - log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info(" G68.4: calculating new twp_matrix...") - twp_matrix_new = twp_matrix_current * twp_matrix - log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.4: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.4: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.4: twp vector-z: %s", twp_vect_z) - log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) - twp_matrix = twp_matrix_new - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py deleted file mode 100755 index c7ce432a045..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py +++ /dev/null @@ -1,20 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2012, 2013 Dewey Garrett , -# Michael Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import remap - diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py index 44748312c4e..ee6994e7196 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py @@ -1,87 +1,47 @@ #!/usr/bin/env python3 +# Publishes the tilted work plane for the vismach model: the origin and the +# X and Z directions in the coordinate system the plane was defined in, +# read from status where the interpreter keeps them (G68.2, G68.3, G68.4, +# G69), and the active work offset the model translates the plane by. import hal import linuxcnc +import time h = hal.component("twp-helper-comp") -# this pin reflects the machine.analog pin used for the -# twp-status -h.newpin("twp-status", hal.Type.REAL, hal.Dir.IN) -# these pins are created here from 'twp-status'' -h.newpin("twp-is-redefined", hal.Type.BOOL, hal.Dir.OUT) +h.newpin("twp-status", hal.Type.REAL, hal.Dir.OUT) # 0 undefined, 1 defined h.newpin("twp-is-defined", hal.Type.BOOL, hal.Dir.OUT) h.newpin("twp-is-active", hal.Type.BOOL, hal.Dir.OUT) -# twp origin vector -h.newpin("twp-ox-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz", hal.Type.REAL, hal.Dir.OUT) -# twp x-orientation vector -h.newpin("twp-xx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xz", hal.Type.REAL, hal.Dir.OUT) -# twp z-orientation vector -h.newpin("twp-zx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zz", hal.Type.REAL, hal.Dir.OUT) -# twp origin vector in machine coordinate system -h.newpin("twp-ox-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz-world", hal.Type.REAL, hal.Dir.OUT) +for name in ("twp-ox", "twp-oy", "twp-oz", + "twp-xx", "twp-xy", "twp-xz", + "twp-zx", "twp-zy", "twp-zz", + "twp-ox-world", "twp-oy-world", "twp-oz-world"): + h.newpin(name, hal.Type.REAL, hal.Dir.OUT) h.ready() -# create a connection to the status channel s = linuxcnc.stat() try: while 1: - # publish twp-status - if h['twp-status'] == 1: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 0 - elif h['twp-status'] == 2: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 1 - else: - h['twp-is-defined'] = 0 - h['twp-is-active'] = 0 - - # passthrough the twp arguments - h['twp-ox'] = h['twp-ox-in'] - h['twp-oy'] = h['twp-oy-in'] - h['twp-oz'] = h['twp-oz-in'] - h['twp-xx'] = h['twp-xx-in'] - h['twp-xy'] = h['twp-xy-in'] - h['twp-xz'] = h['twp-xz-in'] - h['twp-zx'] = h['twp-zx-in'] - h['twp-zy'] = h['twp-zy-in'] - h['twp-zz'] = h['twp-zz-in'] - - # we only want to expose offsets when twp is not defined - if not h['twp-is-defined']: - s.poll() # get current values - g5x_offset = s.g5x_offset - h['twp-ox-world'] = g5x_offset[0] - h['twp-oy-world'] = g5x_offset[1] - h['twp-oz-world'] = g5x_offset[2] - else : # use the values from the remap - h['twp-ox-world'] = h['twp-ox-world-in'] - h['twp-oy-world'] = h['twp-oy-world-in'] - h['twp-oz-world'] = h['twp-oz-world-in'] + s.poll() + active = 1 if s.g68_active else 0 + h['twp-status'] = active + h['twp-is-defined'] = active + h['twp-is-active'] = active + + o = s.g68_offset + r = s.g68_rotation + h['twp-ox'], h['twp-oy'], h['twp-oz'] = o[0], o[1], o[2] + # columns of the rotation: the plane's X and Z + h['twp-xx'], h['twp-xy'], h['twp-xz'] = r[0], r[3], r[6] + h['twp-zx'], h['twp-zy'], h['twp-zz'] = r[2], r[5], r[8] + + g5x = s.g5x_offset + h['twp-ox-world'], h['twp-oy-world'], h['twp-oz-world'] = g5x[0], g5x[1], g5x[2] + time.sleep(0.05) except KeyboardInterrupt: raise SystemExit diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py deleted file mode 100755 index 59b012058ad..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py +++ /dev/null @@ -1,67 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2013 Dewey Garrett , Michael -# Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import inspect -import emccanon - -# O-word procedure to trap into the Pydevd debugger -# start debug server in Eclipse, then -# call as 'O call' from MDI - -# example setup for debugging embedded Python code -# see http://pydev.org/manual_adv_remote_debugger.html -# if this points to a valid directory, - -def call_pydevd(): - """ trap into the pydevd debugger""" - - import os,sys - - pydevdir= '/home/mah/.eclipse/org.eclipse.platform_3.5.0_155965261/plugins/org.python.pydev.debug_2.0.0.2011040403/pysrc/' - - # the 'emctask' module is present only in the milltask instance, otherwise both the UI and - # milltask would try to connect to the debug server. - - if os.path.isdir(pydevdir) and 'emctask' in sys.builtin_module_names: - sys.path.append(pydevdir) - sys.path.insert(0,pydevdir) - try: - import pydevd - emccanon.MESSAGE("pydevd imported, connecting to Eclipse debug server...") - pydevd.settrace() - except: - emccanon.MESSAGE("no pydevd module found") - pass - - - -def lineno(): - """ return line number in the current Python script """ - return inspect.currentframe().f_back.f_lineno - -def error_stack(self): - """ print the Interpreters error stack (function names) """ - print("error stack level=%d" % (self.stack_index)) - for s in self.stack(): - print("--'%s'" % (s)) - -def callstack(self): - """ print the O-Word call stack """ - for i in range(self.call_level): - c = self.sub_context[i] - print("%d: pos=%d seq=%d filename=%s sub=%s" % (i,c.position, c.sequence_number,c.filename,c.subname)) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc deleted file mode 100755 index 4b2fd293c61..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.1 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

= 0 ;if no P word has been passed we use the default (0) -o100 endif -G13.1 ;back to identity kinematic -M66 L0 E0 -M530 P#

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc deleted file mode 100755 index 16d9687cbe8..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc +++ /dev/null @@ -1,20 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.3 with simultaneous move to XYZ (in tool coordinates) - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [[EXISTS [#]] AND [EXISTS [#]] AND [EXISTS [#]]] - G13.1 ;back to identity kinematic -o100 else - (abort, G53.3: X,Y and Z words are required) ;it is an error if X,Y or Z word is missing -o100 endif -o105 if [EXISTS [#

]] ;check if a P word has been passed - ;(print, P=#

) -o105 else - #

= 0 ;if no P word has been passed we use the default (0) -o105 endif -M66 L0 E0 -;Note we can not pass XYZ words to an m-code so we send coords as ijk and handle it in remap.py -M530 P#

I# J# K# ;orient the spindle with P word and xyz as ijk -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc deleted file mode 100755 index a8b628a930c..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using TCP kinematics G53.6 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

= 0 ;if no P word has been passed we use the default (0) -o100 endif -G12.1 P1 ;switch to tcp kinematic -M66 L0 E0 -M530 P#

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc deleted file mode 100755 index 9efd1b7db29..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ /dev/null @@ -1,11 +0,0 @@ -; this is the wrapper remap to cancel TWP - -osub -M66 L0 E0 ; force sync, stop read ahead -M469 ; call the python G69_core code -G13.1 ; back to identity kins -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G54 ; switch to G54 -M66 L0 E0 ; force sync, stop read ahead -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc deleted file mode 100755 index 1cbf3d41db7..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ /dev/null @@ -1,15 +0,0 @@ -;This is a workaround for a bug that leads to stat.gcodes and parameter[5250] -;to get out of sync after some program aborts -;in [RS274NGC] section of the ini add: ON_ABORT_COMMAND = o call -;save this to a path specified in SUBROUTINE_PATH = -;NOTE: we cannot run remapped codes here only custom Mcodes (ie M100..M199) - -o sub -;(msg, on_abort START) -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G13.1 ; back to identity kins -G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events -G54 ; switch to G54 -(msg, on_abort END) -o endsub -M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 2be585e6ef8..0c4389ca1a9 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -29,46 +29,18 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 ON_ABORT_COMMAND = o call -#ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzacb-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzacb_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = B - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl @@ -76,11 +48,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal #HALCMD = loadusr ../python/feed_zero.py - -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-y xyzacb_trsrn_kins.y-rot-axis @@ -93,8 +60,6 @@ HALCMD = net :offset-y xyzacb_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -150,7 +115,6 @@ HALCMD = net twp-status xyzacb-trsrn-gui.twp HALCMD = net twp-is-defined xyzacb-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzacb-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -220,7 +184,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -231,7 +194,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -252,7 +214,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - #table rotary [JOINT_3] TYPE = ANGULAR diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d3032855aee..5223bc375f6 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -28,56 +28,24 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 -#ON_ABORT_COMMAND = o call -ON_ABORT_COMMAND = o call +ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzbca-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzbca_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = A - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-x xyzbca_trsrn_kins.x-rot-axis @@ -90,8 +58,6 @@ HALCMD = net :offset-y xyzbca_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -147,7 +113,6 @@ HALCMD = net twp-status xyzbca-trsrn-gui.twp HALCMD = net twp-is-defined xyzbca-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzbca-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -217,7 +182,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -228,7 +192,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -249,7 +212,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - # spindle secondary joint [JOINT_3] TYPE = ANGULAR diff --git a/tests/twp-native/checkresult b/tests/twp-native/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-native/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index e5369dc42b1..6e4f953d7d2 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -182,8 +182,16 @@ def rot_y(d): % (after[SECONDARY], after[PRIMARY], want[0], want[1])) if abs(after[TABLE] - start[TABLE]) > 1e-9: error("G53.1 moved the table with Q0") -worst = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) -print("linear joints moved at most %.9f through G53.1" % worst) +worst = 0.0 +where = (0, 0, start[0], len(samples)) +for n, smp in enumerate(samples): + for i in range(3): + d = abs(smp[0][i] - start[i]) + if d > worst: + worst = d + where = (n, i, smp[0][i], len(samples)) +print("linear joints moved at most %.9f through G53.1 (sample %d of %d, joint %d at %.9f); ended at %s" + % ((worst,) + (where[0], where[3], where[1], where[2]) + (" ".join("%.9f" % v for v in after[:3]),))) if worst > 1e-6: error("G53.1 moved a linear joint") if not close(tool_axis(after), list(R[:, 2]), 1e-6): From 0c303f74f3f76ab5b3a96bda08a57a4c1b64a024 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 16:04:42 +1000 Subject: [PATCH 07/24] tests: give the tilted work plane a plane that tells the forms apart Every plane in the frame test was one rotation of ninety degrees about a single axis, written four ways. With two angles zero, composing about the frame as it turns and about the fixed axes give the same matrix, so the difference between G68.2 P0 and P1, the whole reason they are separate codes, went unobserved. One plane at 25, 40 and -15 degrees instead, still written four ways, with the points and vectors derived from it; G68.4 and R take odd angles too, and G53 inside the plane lands somewhere that is not a permutation of the axes. Caught now: P0 composing the way P1 does, R composed before the plane rotation, the three-point form taking Y as X cross Z. --- tests/interp/g68-frame/expected | 151 +++++++++++++++++--------------- tests/interp/g68-frame/g68.ngc | 28 +++--- 2 files changed, 95 insertions(+), 84 deletions(-) diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected index dc9016af4d2..0d844086ec2 100644 --- a/tests/interp/g68-frame/expected +++ b/tests/interp/g68-frame/expected @@ -11,76 +11,81 @@ 11 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) 12 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 13 N..... SET_XY_ROTATION(0.0000) - 14 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") - 15 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 16 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 17 N..... COMMENT("the same plane by fixed axis angles about X") - 18 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 19 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 20 N..... COMMENT("the same plane by three points, the first one its origin") - 21 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 22 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 23 N..... COMMENT("Q0 moves the origin along the plane's own axes: to 11 17 32") - 24 N..... SET_G68_FRAME(11.0000, 17.0000, 32.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 25 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 26 N..... COMMENT("and after R: to 8 17 31") - 27 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 28 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 29 N..... COMMENT("R on a point block turns the plane the same way: to 8 17 31") - 30 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 31 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 32 N..... COMMENT("the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it") - 33 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 34 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 35 N..... COMMENT("R turns the plane about its own Z") - 36 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 37 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 38 N..... COMMENT("an arc in the plane") - 39 N..... SET_FEED_RATE(100.0000) - 40 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 41 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) - 42 N..... COMMENT("G53 inside the plane goes to absolute coordinates") - 43 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) - 44 N..... COMMENT("and #5021 reports them") - 45 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") - 46 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") - 47 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") - 48 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 49 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") - 50 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 51 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") - 52 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) - 53 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 54 N..... COMMENT("G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1") - 55 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 56 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 57 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 58 N..... MESSAGE(" prog 0.000000 2.000000 1.000000") - 59 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 60 N..... COMMENT("G52 X0 Y0 Z0 takes it out again") - 61 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 62 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 63 N..... COMMENT("a change of coordinate system takes the plane along, turned by its XY rotation; the tool stays put") - 64 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 65 N..... SET_G5X_OFFSET(3, 50.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 66 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 67 N..... SET_XY_ROTATION(90.0000) - 68 N..... MESSAGE(" abs 110.000000 220.000000 330.000000") - 69 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 70 N..... MESSAGE(" at the plane origin on G56: abs 30.000000 10.000000 30.000000, expect 30 10 30") - 71 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) - 72 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 73 N..... SET_XY_ROTATION(0.0000) - 74 N..... COMMENT("G69 drops a G52 given in the plane") - 75 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 76 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) - 77 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 78 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 79 N..... SET_XY_ROTATION(0.0000) - 80 N..... SET_FEED_MODE(0, 0) - 81 N..... SET_FEED_RATE(0.0000) - 82 N..... STOP_SPINDLE_TURNING(0) - 83 N..... SET_SPINDLE_MODE(0 0.0000) - 84 N..... PROGRAM_END() - 85 N..... ON_RESET() - 86 N..... ON_RESET() + 14 N..... COMMENT("one plane, four ways. Three distinct angles, none of them right, so that") + 15 N..... COMMENT("the order the rotations compose in shows in the answer") + 16 N..... COMMENT("P0 turns about the frame as it goes, ZXZ by default") + 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 19 N..... COMMENT("the same plane about the fixed axes of the system it sits in, XYZ") + 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 22 N..... COMMENT("the same plane by three points: the first is the origin, the second on +X, the third on the +Y side") + 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("Q0 moves the origin along the plane's own axes: to 11 17 32") + 26 N..... SET_G68_FRAME(11.0000, 17.0000, 32.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 27 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("and after R: to 8 17 31") + 29 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 30 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 31 N..... COMMENT("R on a point block turns the plane the same way: to 8 17 31") + 32 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 33 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("the same plane by two vectors") + 35 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 36 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 37 N..... COMMENT("two vectors: X is kept and Z, 3 degrees off square, is squared to it") + 38 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 39 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 40 N..... COMMENT("R turns the plane about its own Z") + 41 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.7134, -0.6459, 0.2717, 0.6561, 0.4797, -0.5826, 0.2460, 0.5939, 0.7660], 1) + 42 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 43 N..... COMMENT("an arc in the plane") + 44 N..... SET_FEED_RATE(100.0000) + 45 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 46 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 47 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 48 N..... STRAIGHT_TRAVERSE(-27.6365, -20.9503, -14.0466, 0.0000, 0.0000, 0.0000) + 49 N..... COMMENT("and #5021 reports them") + 50 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -27.636497 -20.950346 -14.046603") + 51 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 52 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 53 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 54 N..... MESSAGE(" prog -29.358386 -25.107354 -19.408914") + 55 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 56 N..... COMMENT("G68.4 composes onto the plane, in its own axes") + 57 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.6477, -0.5920, 0.4796, 0.4862, -0.1635, -0.8584, 0.5865, 0.7892, 0.1820], 1) + 58 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 59 N..... COMMENT("G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1") + 60 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 61 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 62 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 63 N..... MESSAGE(" prog 0.000000 2.000000 1.000000") + 64 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 65 N..... COMMENT("G52 X0 Y0 Z0 takes it out again") + 66 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 67 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 68 N..... COMMENT("a change of coordinate system takes the plane along, turned by its XY rotation; the tool stays put") + 69 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 70 N..... SET_G5X_OFFSET(3, 50.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 71 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 72 N..... SET_XY_ROTATION(90.0000) + 73 N..... MESSAGE(" abs 110.000000 220.000000 330.000000") + 74 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 75 N..... MESSAGE(" at the plane origin on G56: abs 30.000000 10.000000 30.000000, expect 30 10 30") + 76 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 77 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 78 N..... SET_XY_ROTATION(0.0000) + 79 N..... COMMENT("G69 drops a G52 given in the plane") + 80 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 81 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) + 82 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 83 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 84 N..... SET_XY_ROTATION(0.0000) + 85 N..... SET_FEED_MODE(0, 0) + 86 N..... SET_FEED_RATE(0.0000) + 87 N..... STOP_SPINDLE_TURNING(0) + 88 N..... SET_SPINDLE_MODE(0 0.0000) + 89 N..... PROGRAM_END() + 90 N..... ON_RESET() + 91 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc index 08afdee8683..673da944992 100644 --- a/tests/interp/g68-frame/g68.ngc +++ b/tests/interp/g68-frame/g68.ngc @@ -4,16 +4,18 @@ g21 g90 g10 l2 p2 x100 y200 z300 r0 g10 l2 p3 x50 y0 z0 r90 g55 -(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) -g68.2 x10 y20 z30 i0 j90 k0 +(one plane, four ways. Three distinct angles, none of them right, so that) +(the order the rotations compose in shows in the answer) +(P0 turns about the frame as it goes, ZXZ by default) +g68.2 x10 y20 z30 i25 j40 k-15 g0 x1 y2 z3 -(the same plane by fixed axis angles about X) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +(the same plane about the fixed axes of the system it sits in, XYZ) +g68.2 p1 q123 x10 y20 z30 i39.025043525 j9.576578516 k13.400523974 g0 x1 y2 z3 -(the same plane by three points, the first one its origin) +(the same plane by three points: the first is the origin, the second on +X, the third on the +Y side) g68.2 p2 q1 x10 y20 z30 -g68.2 p2 q2 x15 y20 z30 -g68.2 p2 q3 x10 y20 z35 +g68.2 p2 q2 x14.796086535 y21.142635331 z29.168171623 +g68.2 p2 q3 x9.765570001 y22.339990858 z31.862655459 g0 x1 y2 z3 (Q0 moves the origin along the plane's own axes: to 11 17 32) g68.2 p2 q0 x1 y2 z3 @@ -33,12 +35,16 @@ g68.2 p2 q1 x10 y20 z30 g68.2 p2 q2 x15 y20 z30 g68.2 p2 q3 x10 y20 z35 r90 g0 x0 y0 z0 -(the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it) +(the same plane by two vectors) +g68.2 p3 q1 x10 y20 z30 i0.959217579 j0.228526484 k-0.166364909 +g68.2 p3 q2 i0.271653782 j-0.582563416 k0.766044443 +g0 x1 y2 z3 +(two vectors: X is kept and Z, 3 degrees off square, is squared to it) g68.2 p3 q1 x10 y20 z30 i1 j0 k0 g68.2 p3 q2 i0.05 j-1 k0 g0 x1 y2 z3 (R turns the plane about its own Z) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g68.2 x10 y20 z30 i25 j40 k-15 r37.5 g0 x1 y2 z3 (an arc in the plane) g1 f100 x0 y0 z0 @@ -52,8 +58,8 @@ g53 g0 x100 y200 z300 g43.1 z7 (debug, prog #5420 #5421 #5422) g49 -(G68.4 composes: a further 90 about the plane's X) -g68.4 p1 q123 i90 j0 k0 +(G68.4 composes onto the plane, in its own axes) +g68.4 p1 q123 i35 j-20 k10 g0 x1 y2 z3 (G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1) g68.2 x10 y20 z30 i0 j90 k0 From 69916ed85efb7837ad012500f22dbc98975dbc7e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 18:07:36 +1000 Subject: [PATCH 08/24] docs: say where the tilted work plane conventions come from The chapter and the G-code reference described the two angle forms but never said whose conventions they are: P0 is Heidenhain's PLANE EULER, P1 its PLANE SPATIAL, the numbering is Fanuc's, and Fanuc's fifth form is not implemented. Two differences from the control they borrow from are stated: P1 and P2 refuse where a control offering SEQ falls back to the nearer pose, and P names a pose only where a rotary turns the tool, so a machine whose rotaries all carry the work has only the nearest form; the G53.1 section read as though a tilting table had a pose to name. --- docs/src/gcode/g-code.adoc | 49 +++++++++++++-------- docs/src/motion/kinematics-conventions.adoc | 28 ++++++++++++ 2 files changed, 58 insertions(+), 19 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 874ba24d893..a6c1f88772d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1874,19 +1874,25 @@ machines, two on a five-axis one, and often a choice of which joints to use. 'P' picks which of them. Without 'P', or with 'P0', the machine takes the one nearest where its rotaries are standing, which is the shortest move and depends on where that is. 'P1' and 'P2' name the pose instead, so a program -reaches the same one wherever it starts from: on a five axis machine the two -poses lean the head or the table opposite ways, and they differ in the sign -of the secondary rotary, the one whose axis the other carries. 'P1' is the -pose with that rotary positive and 'P2' the pose with it negative. The -interpreter works out which rotary that is by asking the kinematics module, -so nothing is configured for it. This is the choice Heidenhain writes as -`SEQ+` and `SEQ-`. - -The two poses become one where the tool direction asked for lies along the -primary rotary's axis, straight up on a vertical mill, and there every form -gives the same answer. A machine that is not of this shape, a robot among -them, has no such sign to name, and there only the nearest form is -available. +reaches the same one wherever it starts from: the two poses tilt opposite +ways, and they differ in the sign of the first rotary met going from the +tool to the work. On a machine with two head rotaries that is the one whose +axis the other carries; with two table rotaries, the one that carries the +other, A on a table that turns C in an A trunnion; with one of each, the +head rotary. 'P1' is the pose with that rotary positive and 'P2' the pose +with it negative. The interpreter works out which rotary that is by asking +the kinematics module, so nothing is configured for it. This is the choice +Heidenhain writes as `SEQ+` and `SEQ-` on its master axis, with one +difference: a control offering `SEQ` falls back to the nearer pose when both +lie the same side of home, where 'P1' and 'P2' refuse and say so. + +The two poses become one where that rotary stands at zero, the tool along +the other rotary's axis, straight up on a vertical mill, and there every +form gives the same answer. + +'P' names a pose only on a machine that orients the tool with two rotaries. +On a machine that is not of this shape, a robot among them, only the nearest +form is available. 'Q' says whether the joints that carry the work, the table, take part. @@ -2304,12 +2310,17 @@ does not change what a joint is. Words left out are zero. 'R' turns the plane about its own Z after everything else, in degrees. -The forms follow the Fanuc tilted working plane commands, with a few -extensions: 'R' on 'P0', 'P1' and 'P3' (Fanuc gives it with 'P2' only), -'Q' on 'P0' (Fanuc's Euler angles are always Z, X, Z), 'P1' orders that -repeat an axis, such as 'Q121', besides the six that name each axis once, -and all three angles zero, which is the plane of the coordinate system -underneath rather than an error, as on a Fanuc with parameter ATW set. +The forms are numbered as Fanuc numbers them, and the two angle forms are the +conventions Heidenhain writes as `PLANE EULER` and `PLANE SPATIAL`. Fanuc has +a fifth form, projection angles, that is not implemented. See the +<> chapter. + +Beyond the Fanuc commands, the forms accept 'R' on 'P0', 'P1' and 'P3' +(Fanuc gives it with 'P2' only), 'Q' on 'P0' (Fanuc's Euler angles are +always Z, X, Z), 'P1' orders that repeat an axis, such as 'Q121', besides +the six that name each axis once, and all three angles zero, which is the +plane of the coordinate system underneath rather than an error, as on a +Fanuc with parameter ATW set. The blocks of a 'P2' or 'P3' definition have to follow one another; any other block in between is an error. A definition left unfinished in MDI is diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index d46b3dd1b63..fc4d94398bd 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -678,6 +678,34 @@ Mount orientation is not this:: determinant, rather than as three angles whose ordering convention is written down nowhere. +== Relation to Other Controls + +The tilted work plane borrows its shape from the controls that had one first, +so a program or a post moving either way reads the same. + +The two angle forms are the two those controls define. `G68.2 P0`, three +angles each about an axis of the plane as rotated so far, is what Heidenhain +calls `PLANE EULER`: a precession about Z, a nutation about the X the +precession has already turned, and a rotation about the tilted Z. `G68.2 P1`, +three angles about the axes of the system underneath, is `PLANE SPATIAL`, +whose three angles are each about a non-tilted axis. The numbering of the +forms follows Fanuc's `G68.2`, where P0 is Euler angles, P1 roll, pitch and +yaw, P2 three points and P3 two vectors. Fanuc has a fifth form, projection +angles, that is not implemented. + +Selecting one of the two poses that reach a plane, `P1` and `P2` on +<>, is Heidenhain's `SEQ+` and `SEQ-`, and takes the same +reference: the sign of the rotary measured from its home position. Two +differences are worth knowing. A control offering `SEQ` falls back to the +nearer pose when both lie the same side of home, where these refuse and say +so. And `SEQ` is offered on a machine whose rotaries all carry the work, +keyed to the tilting one, where `P` here names a pose only when a rotary +turns the tool, so on a table-table machine only the nearest form is +available. + +Whether the joints carrying the work take part, `Q0` and `Q1`, is +Heidenhain's `COORD ROT` and `TABLE ROT`. + == References * <>, for the axis nomenclature From 407777f0bbfcf0367c66aafbd35956a003882f44 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 20:23:32 +1000 Subject: [PATCH 09/24] interpreter, motion: hold the joints a point-to-point move asks for A point does not name one joint set: a robot wrist reaches it again with the forearm turned half a revolution, and both ends of a point-to-point move read the joints back from the point. The interpreter keeps its seed while it still explains where the machine is, instead of inverting the point every time and reporting joints the machine is not standing in, which had the next G53.7 refused on a joint limit. Motion holds the joints a joint segment ended on while the planner stays there, instead of inverting again the next cycle and undoing the move. tests/ptp-robot drives a wrist joint through zero and back and checks the joints not named stay put. --- src/emc/motion/control.c | 50 +++++++++++++++++++++++++--- src/emc/rs274ngc/interp_workplane.cc | 17 +++++++++- tests/ptp-robot/test-ui.py | 16 +++++++++ 3 files changed, 78 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 87dcb467305..1e7004bb699 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -932,6 +932,25 @@ static void check_for_faults(void) } } +/* The joints a joint interpolated segment ended on, held while the + planner stays at that point: a module's inverse answers with its own + joint set, which a robot wrist reaches with the forearm turned half a + revolution from the one asked for. The hold ends when the point moves. */ +static int joint_hold_valid = 0; +static double joint_hold[EMCMOT_MAX_JOINTS]; +static EmcPose joint_hold_pose; + +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + 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; +} + static void set_operating_mode(void) { int joint_num; @@ -1452,7 +1471,7 @@ static void get_pos_cmds(long period) from the queue: its end joints seed the inverse, since the modules that read their rotary angles from the seed would otherwise get last cycle's */ - tpTakeJointEnd(&emcmotInternal->coord_tp, positions); + int joint_end_fresh = tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); @@ -1470,9 +1489,32 @@ static void get_pos_cmds(long period) ext_offset_coord_limit = 0; } - /* OUTPUT KINEMATICS - convert to joints in local array */ - result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, - &iflags, &fflags); + /* OUTPUT KINEMATICS - convert to joints in local array, or + hold the joints a joint interpolated segment ended on while + the planner stays at the point they put the machine on */ + if (joint_end_fresh) { + EmcPose at = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 0; + if (kinematicsForward(positions, &at, &fflags, &iflags) == 0 + && same_carte_pos(&at, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = positions[joint_num]; + } + joint_hold_pose = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 1; + } + } + if (joint_hold_valid + && same_carte_pos(&joint_hold_pose, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + positions[joint_num] = joint_hold[joint_num]; + } + result = 0; + } else { + joint_hold_valid = 0; + result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, + &iflags, &fflags); + } } if(result == 0) { diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 19fedadd513..cb88c761733 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -662,17 +662,32 @@ void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, doubl prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; } +// whether two machine points are the same, to a hair either way +static bool same_pose(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + 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 the machine is at, as far as the interpreter can know ahead of // motion: the seed while it still explains the current point, since a point // does not name one joint set, else the joints the machine stands in int Interp::current_joints(setup_pointer s, void *vctx, double *joints) { KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; - EmcPose pose; + EmcPose pose, seeded; int pass, i; current_machine_pose(s, &pose); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + return INTERP_OK; + } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index 92362c901a7..c013a42f2ff 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -74,6 +74,22 @@ def refused(cmd, expect): if abs(after[j] - before[j]) > 1e-6: error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# a wrist reaches the same point with the forearm turned half a revolution +# and the wrist joints reversed, so a joint driven through zero must not +# come back as the other set: the joints not named stay where they are +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +held = mdi("G53.7 G0 J4=90") +for value in (-10, 90, -45): + now = mdi("G53.7 G0 J4=%d" % value) + print("G53.7 G0 J4=%-4d %s" % (value, " ".join("%.4f" % v for v in now))) + drain() + if abs(now[4] - value) > 1e-6: + error("G53.7 J4=%d left joint 4 at %.6f" % (value, now[4])) + for j in (0, 1, 2, 3, 5): + if abs(now[j] - held[j]) > 1e-6: + error("G53.7 J4=%d moved joint %d from %.6f to %.6f" + % (value, j, held[j], now[j])) + # the letter form is refused whichever letter is used, because X names the # first rotary joint here; the message says so and points at G53.7 refused("G53.5 G0 X10", "joint 0") From 3711442927833d18736cf9b1600e265fa7b7a207 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 23:13:58 +1000 Subject: [PATCH 10/24] interpreter: seed the kinematics from the joints the machine stands in The interpreter works out the joints behind a point by inverting it, and had nothing to start the first inverse from but zeros. A module that answers the inverse by iterating cannot be started anywhere: genserkins takes the Jacobian at the seed, and a serial arm with every joint at zero stands in a singular pose, so on a robot every G53.5 and G53.7 was refused with "the kinematics cannot invert the current position". Canon now reports the joint positions, and the interpreter falls back on them when its own seed no longer explains where the machine is. They also name the arm the machine is standing in, which the inverse of a point cannot: a robot reaches the same point again with the elbow the other way up. --- src/emc/nml_intf/canon.hh | 6 ++++++ src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 12 +++++++++++- src/emc/sai/saicanon.cc | 5 +++++ src/emc/task/emccanon.cc | 11 +++++++++++ 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 421ce6ea50f..259d43b6795 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -1024,6 +1024,12 @@ extern double GET_EXTERNAL_POSITION_V(); // returns the current w-axis position extern double GET_EXTERNAL_POSITION_W(); +// Copies up to max of the joint positions the machine stands in and +// returns how many were written. A point does not name one joint set, so +// an iterative inverse needs somewhere to start. Zero when the caller +// has no machine to ask. +extern int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max); + // Returns the position of the specified axis at the last probe trip, // in the current work coordinate system. diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 886b194c5bb..0b95420404b 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -775,6 +775,7 @@ double GET_EXTERNAL_POSITION_C() { return parse_state.pos[P9_C]; } double GET_EXTERNAL_POSITION_U() { return parse_state.pos[P9_U]; } double GET_EXTERNAL_POSITION_V() { return parse_state.pos[P9_V]; } double GET_EXTERNAL_POSITION_W() { return parse_state.pos[P9_W]; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index cb88c761733..617f3cbfcce 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -679,8 +679,9 @@ static bool same_pose(const EmcPose *a, const EmcPose *b) int Interp::current_joints(setup_pointer s, void *vctx, double *joints) { KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double standing[EMCMOT_MAX_JOINTS]; EmcPose pose, seeded; - int pass, i; + int pass, i, n; current_machine_pose(s, &pose); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } @@ -688,6 +689,15 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { return INTERP_OK; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + for (i = 0; i < n; i++) { joints[i] = standing[i]; } + if (n > 0) { + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; + } + } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 5eceebc3cce..59c48d410fa 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -938,6 +938,11 @@ double GET_EXTERNAL_POSITION_W() return 0.; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) +{ + return 0; +} + double GET_EXTERNAL_PROBE_POSITION_U() { return 0.; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 75c6ac54888..6f5580cbd39 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3959,6 +3959,17 @@ double GET_EXTERNAL_POSITION_W(void) return position.w; } +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) +{ + int n = emcStatus->motion.traj.joints; + + if (n > max) { n = max; } + for (int i = 0; i < n; i++) { + joints[i] = emcStatus->motion.joint[i].output; + } + return n; +} + double GET_EXTERNAL_PROBE_POSITION_X(void) { CANON_POSITION position; From 7de55ba5083952383c8abc2896e79f5c95ef057c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 11:55:19 +1000 Subject: [PATCH 11/24] preview: let the canon report where the joints stand The preview has a machine to ask, through the status buffer it already reads for the tool table and the offsets, so it can answer for the joints as well. Without them every inverse started from zeros, which previewed a point-to-point move from a pose the machine is not standing in, and on a serial arm could not be inverted at all. The one method on the mixin covers AXIS, gremlin and the Qt screens, which all take their canon from it. --- lib/python/rs274/interpret.py | 3 +++ src/emc/rs274ngc/gcodemodule.cc | 25 ++++++++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 3c83b5502a8..0662adc90bc 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -193,5 +193,8 @@ def get_axis_mask(self): def get_block_delete(self): return self.s.block_delete + def get_external_joint_positions(self): + return tuple(self.s.joint_actual_position[:self.s.joints]) + # vim:ts=8:sts=4:et: diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 0b95420404b..d81e6552989 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -775,7 +775,30 @@ double GET_EXTERNAL_POSITION_C() { return parse_state.pos[P9_C]; } double GET_EXTERNAL_POSITION_U() { return parse_state.pos[P9_U]; } double GET_EXTERNAL_POSITION_V() { return parse_state.pos[P9_V]; } double GET_EXTERNAL_POSITION_W() { return parse_state.pos[P9_W]; } -int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } + +// Where the machine's joints stand. A point does not name one joint set, so +// an iterative inverse has to start somewhere: a canon that watches the +// status buffer says where. One that cannot, or that answers something that +// is not a sequence of numbers, answers nothing, and the interpreter works +// from the point alone. +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) { + int n = 0; + if(parse_state.interp_error) return 0; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_external_joint_positions")) return 0; + try { + py::sequence seq = canon.attr("get_external_joint_positions")().cast(); + for(py::handle value : seq) { + if(n == max) break; + joints[n++] = value.cast(); + } + } catch(py::error_already_set &) { + return 0; // the error goes with the exception + } catch(py::builtin_exception &) { + return 0; + } + return n; +} void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) From 3bc832d0fb57c840db7155a5f02cec46dd1e720c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 15:56:21 +1000 Subject: [PATCH 12/24] tests: the point-to-point moves and their preview on an iterative module pumakins answers the inverse in closed form and never looks at the joints it is handed, so tests/ptp-robot says nothing about the seed. genserkins takes the Jacobian at those joints: the machine here homes well away from all zeros, so the seed has to come from where the machine stands, and all zeros is the singular pose it cannot come from at all. The same program goes through the interpreter and through the preview, and the two have to agree on where it ends up. Then the arm is parked in the singular pose, where the module reports through the HAL library it prints with: the preview refuses, rather than the process going down. --- tests/ptp-iterative/README | 9 ++ tests/ptp-iterative/checkresult | 3 + tests/ptp-iterative/sim.hal | 16 +++ tests/ptp-iterative/test-ui.py | 175 ++++++++++++++++++++++++++++++++ tests/ptp-iterative/test.ini | 140 +++++++++++++++++++++++++ tests/ptp-iterative/test.ngc | 3 + tests/ptp-iterative/test.sh | 4 + tests/ptp-iterative/tool.tbl | 1 + tests/ptp-robot/skip | 4 - tests/twp-native/skip | 4 - 10 files changed, 351 insertions(+), 8 deletions(-) create mode 100644 tests/ptp-iterative/README create mode 100755 tests/ptp-iterative/checkresult create mode 100644 tests/ptp-iterative/sim.hal create mode 100755 tests/ptp-iterative/test-ui.py create mode 100644 tests/ptp-iterative/test.ini create mode 100644 tests/ptp-iterative/test.ngc create mode 100755 tests/ptp-iterative/test.sh create mode 100644 tests/ptp-iterative/tool.tbl delete mode 100755 tests/ptp-robot/skip delete mode 100755 tests/twp-native/skip diff --git a/tests/ptp-iterative/README b/tests/ptp-iterative/README new file mode 100644 index 00000000000..456921354f4 --- /dev/null +++ b/tests/ptp-iterative/README @@ -0,0 +1,9 @@ +The point-to-point moves on a kinematics module that answers the inverse +by iterating, and the preview of them. + +genserkins takes the Jacobian at the joints it is handed, so the seed +decides whether there is an answer at all: this machine homes to a pose +that is nowhere near all zeros, and all zeros is the singular pose the +inverse cannot start from. The interpreter reads the joints the machine +stands in, and the preview reads them from the status buffer the way a GUI +does, so the same program previews and runs to the same place. diff --git a/tests/ptp-iterative/checkresult b/tests/ptp-iterative/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-iterative/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/ptp-iterative/sim.hal b/tests/ptp-iterative/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-iterative/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/ptp-iterative/test-ui.py b/tests/ptp-iterative/test-ui.py new file mode 100755 index 00000000000..7fb988a976e --- /dev/null +++ b/tests/ptp-iterative/test-ui.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# A module that answers the inverse by iterating has to be started +# somewhere, and the pose this machine homes to is not the one it would be +# started from by default. The moves are checked twice: through the +# interpreter, which reads the joints from motion, and through the preview, +# which reads them from the status buffer the way a GUI does. +import gcode +import linuxcnc +import preview_helpers +import os +import sys +import time +from rs274.interpret import StatMixin + +JOINTS = 6 +PROGRAM = "test.ngc" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + + +def drain(): + while e.poll(): + pass + + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + + +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.points = [] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + + def straight_traverse(self, *pos): + self.points.append(pos) + + def straight_feed(self, *pos): + self.points.append(pos) + + +# the canon protocol carries lengths in the interpreter's own units, which +# a GUI turns into the machine's; the angles are already there +def in_machine_units(pos): + s.poll() + scale = (s.linear_units or 1) * 25.4 + return [v * scale for v in pos[:3]] + list(pos[3:6]) + + +def preview(program=PROGRAM): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return None, "line %d: %s" % (line, gcode.strerror(result)) + return canon.points, None + + +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() + +home = settled() +print("homed at %s" % " ".join("%.4f" % v for v in home)) +if abs(home[1] + 90) > 1e-6 or abs(home[4] - 90) > 1e-6: + error("the machine did not home to the pose the test is written for") + +# the preview runs first, from the pose the machine stands in, and its last +# point is where the program ends up +points, refused = preview() +if refused: + error("the preview refused %s, %s" % (PROGRAM, refused)) +elif not points: + error("the preview of %s reported no move at all" % PROGRAM) +previewed = points[-1] if points else None + +# the interpreter takes the same program, one line at a time +for line in open(PROGRAM): + line = line.strip() + if not line or line.startswith("m2"): + continue + reached = mdi(line) + print("%-20s %s" % (line, " ".join("%.4f" % v for v in reached))) + drain() + +if abs(reached[0] - 10) > 1e-6 or abs(reached[4] - 80) > 1e-6: + error("the program left joints 0 and 4 at %.6f and %.6f" + % (reached[0], reached[4])) +for j in (1, 2, 3, 5): + if abs(reached[j] - home[j]) > 1e-6: + error("the program moved joint %d from %.6f to %.6f" + % (j, home[j], reached[j])) + +# and both agree on where that is +s.poll() +if previewed: + for name, i, got in zip("XYZABC", range(6), in_machine_units(previewed)): + if abs(got - s.position[i]) > 1e-3: + error("the preview put %s at %.6f, the machine at %.6f" + % (name, got, s.position[i])) + print("preview and machine agree on %s" + % " ".join("%.4f" % v for v in in_machine_units(previewed))) + +# a preview taken now starts where the machine stands, so a program that +# names the joints it is already in asks for no move at all +after, refused = preview() +if refused: + error("the second preview refused %s, %s" % (PROGRAM, refused)) +if after and max(abs(a - b) for a, b in zip(in_machine_units(after[0]), s.position[:6])) > 1e-3: + error("the second preview started at %s, not at %s" + % (["%.4f" % v for v in in_machine_units(after[0])], + ["%.4f" % v for v in s.position[:6]])) + +# a point out of the arm's reach: the module says so through the HAL +# library it prints with, which has to be within reach of the process the +# preview runs in, or the answer is the process going down +# all joints at zero is the pose this arm cannot be inverted from, and the +# module says so through the HAL library it prints with. That library has +# to be within reach of the process the preview runs in: a GUI has it only +# underneath the interpreter it loaded, and out of reach the answer is the +# process going down rather than a refusal. +mdi("g53.7 g0 j0=0 j1=0 j2=0 j3=0 j4=0 j5=0") +out, refused = preview() +if not refused: + error("the preview answered from the pose the arm cannot be inverted from") +elif "invert" not in refused: + error("the preview said %r, which does not mention the inverse" % refused) +else: + print("the preview refused from the singular pose: %s" % refused) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-iterative/test.ini b/tests/ptp-iterative/test.ini new file mode 100644 index 00000000000..bc942288e36 --- /dev/null +++ b/tests/ptp-iterative/test.ini @@ -0,0 +1,140 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = genserkins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# the modified DH parameters of the RV-6SDL, as the melfa-sim config has them +HALCMD = setp genserkins.A-1 85 +HALCMD = setp genserkins.A-2 380 +HALCMD = setp genserkins.A-3 100 +HALCMD = setp genserkins.ALPHA-1 -1.570796326 +HALCMD = setp genserkins.ALPHA-3 -1.570796326 +HALCMD = setp genserkins.ALPHA-4 1.570796326 +HALCMD = setp genserkins.ALPHA-5 -1.570796326 +HALCMD = setp genserkins.D-0 350 +HALCMD = setp genserkins.D-3 425 +HALCMD = setp genserkins.D-5 235 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = -90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-iterative/test.ngc b/tests/ptp-iterative/test.ngc new file mode 100644 index 00000000000..0afcad5d314 --- /dev/null +++ b/tests/ptp-iterative/test.ngc @@ -0,0 +1,3 @@ +g53.7 g0 j4=80 +g53.7 g0 j0=10 +m2 diff --git a/tests/ptp-iterative/test.sh b/tests/ptp-iterative/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-iterative/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/ptp-iterative/tool.tbl b/tests/ptp-iterative/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-iterative/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/ptp-robot/skip b/tests/ptp-robot/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/ptp-robot/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# Builds a realtime component with halcompile, which needs the build -# tools present. Skip when testing installed packages. -[ -z "$SYSTEM_BUILD" ] diff --git a/tests/twp-native/skip b/tests/twp-native/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/twp-native/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# Builds a realtime component with halcompile, which needs the build -# tools present. Skip when testing installed packages. -[ -z "$SYSTEM_BUILD" ] From 09d6b91b2fe6cc89fcfde2ea3127ac513e95359f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:08:54 +1000 Subject: [PATCH 13/24] genserkins: hold the inverse to a step the Jacobian is good for The inverse took the whole Newton step the Jacobian asked for, and the Jacobian holds only near the estimate: an endpoint far from the seed asked for many radians, the arm landed somewhere unrelated and converged on a pose reached through whole turns. Seeded at 90 -90 0 0 90 -17.7 and asked for X450 Y-200 Z150 it answered joint 0 at -3263.962, the right pose nine turns out, which the joint limits refuse on a move the arm could make. Cap the step at GENSER_MAX_ANGLE_STEP and scale the whole vector so its direction survives, measured on the rotary links; a servo cycle asks for far less and is untouched. --- src/emc/kinematics/genserfuncs.c | 21 +++++++++++++++++++++ src/emc/kinematics/genserkins.h | 2 ++ 2 files changed, 23 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index d0c918e0f7a..75bafb520f6 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -631,6 +631,27 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); + /* The Jacobian holds only near the estimate, and a far pose asks + for a step of many radians: the arm lands somewhere unrelated + and converges by way of whole turns its limits refuse. Cap the + step, keeping its direction, and let the iteration walk there. */ + { + double worst = 0.0; + + for (link = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_ANGLE == linkout[link].quantity + && fabs(dj[link]) > worst) { + worst = fabs(dj[link]); + } + } + if (worst > GENSER_MAX_ANGLE_STEP) { + double scale = GENSER_MAX_ANGLE_STEP / worst; + for (link = 0; link < genser->link_num; link++) { + dj[link] *= scale; + } + } + } + //pass through 678 as uvw if (p->max_joints > 6) joints[6] = world->u; if (p->max_joints > 7) joints[7] = world->v; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c50cddde3e3..52f369a958e 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -45,6 +45,8 @@ #define GENSER_MAX_JOINTS 6 #define GENSER_DEFAULT_MAX_ITERATIONS 100 +/* the most a rotary joint moves in one pass of the inverse, in radians */ +#define GENSER_MAX_ANGLE_STEP 0.2 #define PI_2 GO_PI_2 From 8bf0faaef8ddd53e07263ed0ab9e3ba9449d5adb Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:09:03 +1000 Subject: [PATCH 14/24] motion: check a move against the joints the queue leaves behind The joint limit check inverted a move's endpoint from the joints the machine stands in. The reading runs far ahead, so every endpoint of a short program was checked from the starting pose, and an iterating inverse answers nearest whatever it is handed: park a robot at J4=-90, run a program that takes it to J4=90 and asks for a point, and the point is refused because it was inverted from the parked pose with the other wrist. Seed from the end of the queue instead: a joint segment knows its own end and the rest carry the answer the last endpoint came out with; with nothing queued the machine is the reference. The joint interpolated path already worked this way and shares the helper. --- src/emc/motion/command.c | 48 +++++++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 747940f41ab..30af51ae540 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -140,6 +140,30 @@ static int inverse_settled(EmcPose *pos, double *joints, return 0; } +/* Where the queue leaves the joints, which is the seed an iterative + inverse wants: the reading runs ahead of the machine, so the joints to + hand it are not the ones the machine is standing in. A joint + interpolated segment knows its own end; the rest take the answer the + last endpoint checked came out with. Returns 0, and the joints the + machine stands in, when there is nothing queued to ask. */ +static double planned_joints[EMCMOT_MAX_JOINTS]; +static int planned_joints_ok = 0; + +static int queue_end_joints(double *joints_out) +{ + int j; + + if (tpGetQueueEndJoints(&emcmotInternal->coord_tp, joints_out)) { return 1; } + if (planned_joints_ok && tpQueueDepth(&emcmotInternal->coord_tp) > 0) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { joints_out[j] = planned_joints[j]; } + return 1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + joints_out[j] = (j < ALL_JOINTS) ? joints[j].pos_cmd : 0.0; + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -288,17 +312,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) /* Now, check that the endpoint puts the joints within their limits too */ - /* fill in all joints with 0 */ - for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { - joint = &joints[joint_num]; - joint_pos[joint_num] = joint->pos_cmd; - } + /* start the inverse from where the queue leaves the joints */ + queue_end_joints(joint_pos); /* now fill in with real values, for joints that are used */ - if (kinematicsInverse(&pos, joint_pos, &iflags, &fflags) != 0) + if (inverse_settled(&pos, joint_pos, &iflags, &fflags) != 0) { reportError(_("%s move on line %d fails kinematicsInverse"), move_type, id); + planned_joints_ok = 0; return 0; } @@ -329,6 +351,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) move_type, id, joint_num, joint->min_pos_limit); } } + + /* an endpoint on its way to the queue is where the next one starts + from; a refused one leaves the queue as it was */ + if (in_range) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + planned_joints[joint_num] = joint_pos[joint_num]; + } + planned_joints_ok = 1; + } return in_range; } @@ -1182,12 +1213,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } /* where the queue ends in joint space */ - if (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + if (!queue_end_joints(start)) { EmcPose goal; tpGetGoalPos(&emcmotInternal->coord_tp, &goal); - for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { - start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; - } if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), emcmotCommand->id); From 9f17668ccd25536b996953d56db47827c16570b3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 17 Sep 2026 23:16:09 +1000 Subject: [PATCH 15/24] interpreter: add G53.2, solve the tool orientation without moving G53.2 asks where the rotaries have to go for the tool to be normal to the active plane, like G53.1, and moves nothing: it publishes the pose on #<_orient_x> and kin and on #5071 to #5079, with #<_orient_valid> and #5080 saying a pose is held, so the program reaches it with a move of its own, for instance one G0 naming the linear and rotary words together, a single Cartesian move under TCP. Heidenhain's STAY. Reading the parameters before the first G53.2 is an error. Two fixes found on the way: a G68.3 block with axis words and a modal G0 emitted a stray move; and the solver's answers in (-180, 180] are unwrapped onto the turn nearest the present rotary position before the nearest-first ranking, or a rotary crossing 180 swings the long way round. --- docs/src/gcode/g-code.adoc | 25 ++++++++--- docs/src/gcode/overview.adoc | 14 +++++- docs/src/motion/kinematics-conventions.adoc | 4 +- src/emc/rs274ngc/interp_array.cc | 3 +- src/emc/rs274ngc/interp_check.cc | 12 ++--- src/emc/rs274ngc/interp_convert.cc | 2 +- src/emc/rs274ngc/interp_internal.cc | 3 +- src/emc/rs274ngc/interp_internal.hh | 4 ++ src/emc/rs274ngc/interp_namedparams.cc | 32 ++++++++++++++ src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_workplane.cc | 38 +++++++++++++--- tests/remap/introspect/expected | 4 +- tests/twp-native/test-ui.py | 49 +++++++++++++++++++++ 13 files changed, 165 insertions(+), 27 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index a6c1f88772d..634a04a80c8 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -89,7 +89,7 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates -|<> |Orient the Tool to the Tilted Work Plane +|<> |Orient the Tool to the Tilted Work Plane |<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode @@ -1845,18 +1845,19 @@ It is an error if: * or G53 is used while cutter compensation is on. [[gcode:g53.1]] -== G53.1, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) +== G53.1, G53.2, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) [source,ngc] ---- G53.1 +G53.2 G53.3 X- Y- Z- G53.6 ---- -Each of these moves the rotary joints so that the tool axis is normal to the -active <>, the plane's Z. They differ in what -happens to the tool tip on the way: +All four solve where the rotary joints have to go so that the tool axis is +normal to the active <>, the plane's Z. They +differ in what happens then: * 'G53.1' moves the rotaries alone. The linear joints stay where they are, and the tool tip swings to wherever that carries it. It is a @@ -1865,6 +1866,16 @@ happens to the tool tip on the way: the rotary words, so the kinematics compensates the linear joints all along. * 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the plane, in one point-to-point move. A word left out keeps the present value. +* 'G53.2' moves nothing. It only publishes the solved pose on the named + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', + '#<_orient_b>' and '#<_orient_c>', and on the numbered parameters + '#5071' to '#5079' ('X Y Z A B C U V W'), in program units in the plane, + and sets '#<_orient_valid>' and '#5080' to 1. The program can then reach + the pose with a move of its own making, for instance a single 'G0' that + names 'X Y Z' and the rotary words together, so the turn and the travel + are one Cartesian move under the TCP kinematics. This is what Heidenhain + calls `STAY`. Reading '#<_orient_x>' and kin before the first 'G53.2' is + an error; '#<_orient_valid>' and '#5080' say whether they hold a pose. The kinematics module answers where the rotaries have to go, with its tool frame inverse (see the kinematics conventions chapter), so no configuration @@ -1939,8 +1950,8 @@ It is an error if: names it the way 'P' asks for, or the machine has no pair of poses a tilting joint tells apart. * 'Q' is anything but 0 or 1. -* Axis words are used with 'G53.1' or 'G53.6', or words other than 'X', 'Y' - and 'Z' with 'G53.3'. +* Axis words are used with 'G53.1', 'G53.2' or 'G53.6', or words other than + 'X', 'Y' and 'Z' with 'G53.3'. * Cutter compensation is on. [[gcode:g53.4]] diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index c418ec1b665..8144448f898 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -305,6 +305,11 @@ example '##2' means the value of the parameter whose index is the which the G38 took place. Volatile. * '5070' - <> probe result: 1 if success, 0 if probe failed to close. Used with G38.3 and G38.5. Volatile. +* '5071-5079' - Pose last solved by <> for X, Y, Z, A, + B, C, U, V & W, in program units in the tilted work plane. Same values + as `#<_orient_x>` and kin. Read-only, volatile. +* '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. + Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, Z, A, B, C, U, V & W, in the current program units. Set by `G43`/`G43.1`/`G43.2`, and 0 when `G49` is in effect. These report the @@ -508,6 +513,13 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last + solved, in program units in the tilted work plane. '#<_orient_valid>' is + 1 once a 'G53.2' has run; reading the others before that is an error. The + same values are on the numbered parameters '#5071' to '#5080'. See + <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] @@ -965,7 +977,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index fc4d94398bd..d97d7c8d01e 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -361,8 +361,8 @@ refuses a program for naming both directions on a five-axis machine, and neither should this. In tree the G-code side of that is `G68.2`, which defines the plane, and -`G53.1`, `G53.3` and `G53.6`, which ask this inverse where the rotaries go, -with the plane's normal and its X as the request. Their `Q` word is the +`G53.1`, `G53.2`, `G53.3` and `G53.6`, which ask this inverse where the +rotaries go, with the plane's normal and its X as the request. Their `Q` word is the `held` mask: `Q0` holds the joints the work frame survey finds and takes the reported turn as the coordinate rotation it is, `Q1` holds nothing. See the G-code chapter. diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 58d0f12806b..c3518d7cad4 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0,-1, 0, 0, 0, 0, 0,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0, 0, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -229,6 +229,7 @@ const int Interp::required_parameters[] = { const int Interp::readonly_parameters[] = { 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y ... W, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 925096c8d99..25a81f4e17b 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -110,10 +110,10 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { - } else if (mode0 == G_53_1 || mode0 == G_53_6) { + } else if (mode0 == G_53_1 || mode0 == G_53_2 || mode0 == G_53_6) { CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), - _("Cannot use axis words with G53.1 or G53.6")); + _("Cannot use axis words with G53.1, G53.2 or G53.6")); } else if (mode0 == G_53_3) { CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), _("Only X, Y and Z words can be used with G53.3")); @@ -366,7 +366,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && !plane_words && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -379,7 +379,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.2 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -398,12 +398,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && !plane_words && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G53.1, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.2, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 151215790a4..aec6ba979f3 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4523,7 +4523,7 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_nurbs(code, block, settings)); } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) || (code == G_53_7)); // handled elsewhere - else if ((code == G_53_1) || (code == G_53_3) || (code == G_53_6)) { + else if ((code == G_53_1) || (code == G_53_2) || (code == G_53_3) || (code == G_53_6)) { CHP(convert_orient_tool(code, block, settings)); } else if ((code == G_12_1) || (code == G_13_1)) { diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 7596c990b8f..7229f324eaa 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -176,7 +176,8 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way - if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 + || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); mode_zero_covets_axes = 1; } diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 0447465f87b..181f4bbf560 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -266,6 +266,7 @@ enum GCodes G_52 = 520, G_53 = 530, G_53_1 = 531, + G_53_2 = 532, G_53_3 = 533, G_53_4 = 534, G_53_5 = 535, @@ -806,6 +807,9 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r + // the pose G53.2 last solved, in program words, for #<_orient_a> and kin + bool orient_valid; + double orient_pose[6]; // x y z a b c // the kinematics, for G68.3 and the orientation moves: loaded on first // use through the non-realtime loader, on a HAL component of our own void *kins_ctx; // KinematicsUserContext diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 6efa911a465..01574b1fef5 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,13 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_ORIENT_VALID, + NP_ORIENT_X, + NP_ORIENT_Y, + NP_ORIENT_Z, + NP_ORIENT_A, + NP_ORIENT_B, + NP_ORIENT_C, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -546,6 +553,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose + *value = _setup.orient_valid; + break; + + case NP_ORIENT_X: // _orient_x and kin: the pose G53.2 last solved + case NP_ORIENT_Y: + case NP_ORIENT_Z: + case NP_ORIENT_A: + case NP_ORIENT_B: + case NP_ORIENT_C: + if (!_setup.orient_valid) { + ERS(_("no G53.2 has solved an orientation yet")); + } + *value = _setup.orient_pose[cmd - NP_ORIENT_X]; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -923,6 +946,15 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the pose G53.2 last solved: 1.0 once one has been, and its words + init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); + init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); + init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); + init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); + init_readonly_param("_orient_a", NP_ORIENT_A, PA_USE_LOOKUP); + init_readonly_param("_orient_b", NP_ORIENT_B, PA_USE_LOOKUP); + init_readonly_param("_orient_c", NP_ORIENT_C, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 1e4e03c6776..f3b84654c09 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -114,6 +114,8 @@ setup::setup() : g68_seq_have(0), g68_seq_r_q(-1), g68_seq_word{}, + orient_valid(false), + orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, kins_ctx(nullptr), kins_comp_id(0), kins_module{}, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 617f3cbfcce..927365c2595 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -810,11 +810,12 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } -// G53.1, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 turns -// the rotaries alone, in joint space; G53.6 keeps the tool centre point, a -// Cartesian move; G53.3 goes to X Y Z in the plane. P picks the pose, -// nearest first or by the sign of the tilting joint; Q0 holds the joints that -// carry the work (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 +// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, +// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the +// pose on #<_orient_x> and kin (Heidenhain STAY). P picks the pose, nearest +// first or by the sign of the tilting joint; Q0 holds the joints that carry +// the work (COORD ROT), Q1 frees them (TABLE ROT). int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) { void *vctx; @@ -830,7 +831,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) unsigned int held = 0; int p, q, n, i, j, chosen, njoints; const double *sol; - const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_3) ? "G53.3" : "G53.6"; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -867,6 +868,18 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + // the solver reports each answer in (-180, 180], but the machine stands + // somewhere in turn space: unwrap every angular joint onto the turn + // nearest the present position, or the nearest pose is not the nearest + // move and a free rotary swings the long way round + for (i = 0; i < n; i++) { + for (j = 0; j < njoints; j++) { + double *v = &solutions[i*njoints + j]; + if (!(s->kins_angular_joints & (1 << j))) { continue; } + *v += 360.0 * floor((now[j] - *v) / 360.0 + 0.5); + } + } + // nearest first, by rotary travel in joint units for (i = 0; i < n; i++) { distance[i] = 0.0; @@ -912,6 +925,19 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } machine_pose_to_program(s, &end_pose, end_prog); + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + write_canon_state_tag(block, s); if (code == G_53_1) { // the rotaries alone: the linear joints are where they are, since diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 2f33b4bbe08..206bc4d96af 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 6e4f953d7d2..3eadf138911 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -162,6 +162,13 @@ def rot_y(d): return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) # --- the plane, and G53.1 with the table held --------------------------- +# no G53.2 has run yet, so the pose parameters must refuse to be read +c.mdi("G0 X#<_orient_x>") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading #<_orient_x> before the first G53.2 was accepted") +drain() start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") show("start", start) R = rot_y(20).dot(rot_x(30)) @@ -271,6 +278,48 @@ def in_plane(): if not close(tool_axis(after), list(R2[:, 2]), 1e-6): error("the tool axis after G53.6 is not the plane normal") +# --- G53.2 solves without moving, the pose lands on the parameters ---- +# same plane as G53.6 above, but from a pose that is not the answer: +# G53.2 must not move, and the pose it publishes must put the tool on the +# plane normal, which is where G53.6 already stands, so the nearest +# solution is the present rotary position +stay = after +after2, samples = sampled("G53.2") +drain() +if not close(after2, stay, 1e-9): + error("G53.2 moved the machine: %s became %s" % (stay, after2)) +# read the pose back through the parameters: a move to the published +# rotary words is a move to the present B and C, with the table held +before3 = mdi("G0 B0 C0") +after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") +show("G0 to #<_orient_b/c>", after3) +drain() +if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): + error("the tool axis at the pose G53.2 published is not the plane normal") +# the numbered parameters carry the same pose +before4 = mdi("G0 B0 C0") +after4, samples = sampled("G0 B#5075 C#5076") +show("G0 to #5075/#5076", after4) +drain() +if abs(wrap(after4[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after4[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#5075 #5076 held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after4[SECONDARY], after4[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +c.mdi("#5075 = 0") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("writing #5075 was accepted; the G53.2 pose is not read-only") +drain() +c.mdi("G0 A#<_orient_a>") +c.wait_complete(30) +m = e.poll() +if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("#<_orient_a> after G53.2: %s" % m[1]) +drain() + # --- G53.3 goes to a point in the plane with the tool oriented ---------- before = mdi("G69") R3 = rot_y(-25).dot(rot_x(35)) From c5029f309588ebdebefd9b12a45ed4fffed44b80 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 11:21:29 +1000 Subject: [PATCH 16/24] interpreter: keep the orientation poses inside the rotary travel The orientation codes took each pose the solver reported onto the turn nearest where the rotaries stood and ranked them by travel, without asking whether that turn was inside the joint's limits. A program that winds a rotary, a G68.4 incremental plane in a loop with G53.6, walked it past its limit, and motion refused the move in the middle of the program: "Linear move on line 9 would exceed joint 5's positive limit". The tilted work plane remap it replaces checked the limits and went the long way round. The interpreter reads [JOINT_n] MIN_LIMIT and MAX_LIMIT beside TYPE. Each angular joint of each pose goes onto the turn nearest where it stands that lies inside its travel, which is the long way round where the short way runs out, and a pose no turn brings inside is dropped before the ranking, so P0 takes the next nearest and P1 or P2 refuse. Under Q0 the table is freed before a pose is refused, as when nothing reaches the direction with it held. Where every pose that reaches the direction is outside the travel, the code refuses with that reason, at plan time rather than in motion. tests/twp-native stands C at 300 with a travel of 320, picks a plane whose nearest pose is the turn past the limit, and checks that G53.6 ends on the nearest pose inside the travel, with C inside it all the way. --- docs/src/gcode/g-code.adoc | 2 + src/emc/rs274ngc/interp_internal.hh | 4 ++ src/emc/rs274ngc/interp_setup.cc | 6 ++ src/emc/rs274ngc/interp_workplane.cc | 104 +++++++++++++++++++++++---- src/emc/rs274ngc/rs274ngc_pre.cc | 11 ++- tests/twp-native/README | 2 +- tests/twp-native/test-ui.py | 57 +++++++++++++++ tests/twp-table/README | 5 ++ tests/twp-table/checkresult | 4 +- tests/twp-table/test-ui.py | 19 ++++- tests/twp-table/test.sh | 7 ++ 11 files changed, 203 insertions(+), 18 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 634a04a80c8..cbdf99864c7 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1901,6 +1901,8 @@ The two poses become one where that rotary stands at zero, the tool along the other rotary's axis, straight up on a vertical mill, and there every form gives the same answer. +Every form stays inside the travel `[JOINT_n] MIN_LIMIT` and `MAX_LIMIT` give the rotary joints, and inside the `[AXIS_n]` travel of the rotary axes they drive, unless the axis is a wrapped or modulo rotary. A rotary reaches its angle by the turn nearest where it stands, and where that turn is outside its travel, by the nearest turn inside it, so it goes the long way round rather than run out of travel. A pose no turn brings inside the travel is not taken: without 'P' the next nearest is, and with 'P1' or 'P2' the code refuses. Under 'Q0' the table is freed before a pose is refused, as when no pose reaches the direction with it held. + 'P' names a pose only on a machine that orients the tool with two rotaries. On a machine that is not of this shape, a robot among them, only the nearest form is available. diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 181f4bbf560..cd635f539ae 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -817,6 +817,8 @@ struct setup char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it int kins_joints; // [KINS] JOINTS int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_joint_min[EMCMOT_MAX_JOINTS]; // [JOINT_n] MIN_LIMIT + double kins_joint_max[EMCMOT_MAX_JOINTS]; // [JOINT_n] MAX_LIMIT double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index @@ -900,6 +902,8 @@ struct setup AxisKinds axis_kinds; // [AXIS_] TYPE, [TRAJ] FEED_AXES int axis_wrapped[9]; // by AxisIndex; angular axes only int axis_rotary_modulo[9]; // angular axes only + double axis_min[9]; // [AXIS_n] MIN_LIMIT of a bounded angular axis, else -1e99 + double axis_max[9]; // [AXIS_n] MAX_LIMIT of a bounded angular axis, else 1e99 int rotary_modulo_literal; // M26 = shortest path (default), M27 = literal absolute int axis_indexer_jnum[9]; // -1 where the axis has no locking indexer diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index f3b84654c09..26ba4049f32 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -121,6 +121,8 @@ setup::setup() : kins_module{}, kins_joints(0), kins_angular_joints(0), + kins_joint_min{}, + kins_joint_max{}, kins_seed{}, parameters{0}, @@ -202,6 +204,8 @@ setup::setup() : axis_kinds(axisKindsDefault()), axis_wrapped{}, axis_rotary_modulo{}, + axis_min{}, + axis_max{}, rotary_modulo_literal(0), axis_indexer_jnum{-1, -1, -1, -1, -1, -1, -1, -1, -1}, @@ -227,6 +231,8 @@ setup::setup() : init_once(CANON_STOPPED) { std::fill(parameters, parameters + interp_param_global::RS274NGC_MAX_PARAMETERS, 0); + std::fill(axis_min, axis_min + 9, -1e99); + std::fill(axis_max, axis_max + 9, 1e99); } setup::~setup() { diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 927365c2595..a0426d228c9 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -810,6 +810,89 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } +// An angular axis letter of a machine pose, 3 A to 8 W. +static double pose_letter(const EmcPose *pose, int n) +{ + const double v[6] = {pose->a, pose->b, pose->c, pose->u, pose->v, pose->w}; + return v[n - 3]; +} + +// The solver reports each answer in (-180, 180], but the machine stands +// somewhere in turn space: every angular joint of each pose goes onto the +// turn nearest where it stands, or the nearest pose is not the nearest move +// and a free rotary swings the long way round. Of the turns, only those +// inside the joint's [JOINT_n] travel count, and inside the [AXIS_n] travel +// of the letter a whole turn of the joint turns with it, so a rotary that +// would run out of travel the short way goes the long way, and a pose no +// turn brings inside is dropped, as is one the forward kinematics puts an +// angular letter outside its travel at. seed is where the forward starts. +// Returns how many poses are left, packed at the front. +static int orient_fit(setup_pointer s, KinematicsUserContext *ctx, const EmcPose *seed, + double *solutions, int n, int njoints, const double *now) +{ + int i, j, l, kept = 0; + + for (i = 0; i < n; i++) { + double *pose = &solutions[i*njoints]; + double q[EMCMOT_MAX_JOINTS] = {}; + EmcPose base = *seed, turned; + bool inside = true, placed; + + for (j = 0; j < njoints; j++) { q[j] = pose[j]; } + placed = (kinematicsUserForward(ctx, q, &base) == 0); + for (j = 0; j < njoints; j++) { + if (!(s->kins_angular_joints & (1 << j))) { continue; } + double turns = floor((now[j] - pose[j]) / 360.0 + 0.5); + double first = -1e99, last = 1e99; + if (j < s->kins_joints) { + first = ceil((s->kins_joint_min[j] - pose[j]) / 360.0 - 1e-9); + last = floor((s->kins_joint_max[j] - pose[j]) / 360.0 + 1e-9); + } + // the letter a whole turn of the joint carries a whole turn + // along, if any, and the turns its own travel leaves + turned = base; + q[j] += 360.0; + if (placed && kinematicsUserForward(ctx, q, &turned) == 0) { + for (l = 3; l < 9; l++) { + double step = pose_letter(&turned, l) - pose_letter(&base, l); + if (fabs(fabs(step) - 360.0) > 1e-6) { continue; } + double at = pose_letter(&base, l), lo, hi; + if (step > 0) { + lo = ceil((s->axis_min[l] - at) / 360.0 - 1e-9); + hi = floor((s->axis_max[l] - at) / 360.0 + 1e-9); + } else { + lo = ceil((at - s->axis_max[l]) / 360.0 - 1e-9); + hi = floor((at - s->axis_min[l]) / 360.0 + 1e-9); + } + first = fmax(first, lo); + last = fmin(last, hi); + break; + } + } + q[j] = pose[j]; + if (first > last) { + inside = false; + break; + } + turns = fmin(fmax(turns, first), last); + pose[j] += 360.0 * turns; + q[j] = pose[j]; + } + if (inside && placed && kinematicsUserForward(ctx, q, &base) == 0) { + for (l = 3; l < 9; l++) { + double at = pose_letter(&base, l); + if (at < s->axis_min[l] - 1e-9 || at > s->axis_max[l] + 1e-9) { inside = false; } + } + } + if (!inside) { continue; } + if (kept != i) { + for (j = 0; j < njoints; j++) { solutions[kept*njoints + j] = pose[j]; } + } + kept++; + } + return kept; +} + // G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 // turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, // a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the @@ -830,6 +913,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) double end_prog[9]; unsigned int held = 0; int p, q, n, i, j, chosen, njoints; + bool reached; const double *sol; const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; @@ -849,6 +933,8 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); njoints = kinematicsUserGetNumJoints(ctx); CHP(current_joints(s, ctx, now)); + EmcPose seed = {}; + current_machine_pose(s, &seed); plane_axis_in_world(s, 2, s->rotation_xy, &axis); plane_axis_in_world(s, 0, s->rotation_xy, &xdir); @@ -857,6 +943,8 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + reached = (n > 0); + if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } if (n == 0 && held) { // nothing reachable with the work held still: let it move, still // leaving the plane's X to the coordinate system, or a table that @@ -864,22 +952,14 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) held = 0; n = kinematicsUserToolFrameInverse(ctx, &axis, NULL, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + reached = reached || (n > 0); + if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } } CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0 && reached), + _("%s: every pose that reaches the plane's normal puts a rotary outside its travel"), name); CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); - // the solver reports each answer in (-180, 180], but the machine stands - // somewhere in turn space: unwrap every angular joint onto the turn - // nearest the present position, or the nearest pose is not the nearest - // move and a free rotary swings the long way round - for (i = 0; i < n; i++) { - for (j = 0; j < njoints; j++) { - double *v = &solutions[i*njoints + j]; - if (!(s->kins_angular_joints & (1 << j))) { continue; } - *v += 360.0 * floor((now[j] - *v) / 360.0 + 0.5); - } - } - // nearest first, by rotary travel in joint units for (i = 0; i < n; i++) { distance[i] = 0.0; diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index da4ad31a332..d0dad43072e 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -920,6 +920,12 @@ int Interp::init() if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", section)) { _setup.axis_indexer_jnum[n] = *inival; } + // the travel a rotary is turned within by the orientation + // codes; a wrapped or modulo rotary has none + if (!_setup.axis_wrapped[n] && !_setup.axis_rotary_modulo[n]) { + _setup.axis_min[n] = inifile.findRealV("MIN_LIMIT", section, -1e99); + _setup.axis_max[n] = inifile.findRealV("MAX_LIMIT", section, 1e99); + } } _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); @@ -930,7 +936,8 @@ int Interp::init() } _setup.kins_joints = inifile.findIntV("JOINTS", "KINS", 0); // which joints turn rather than slide, so that an axis letter is - // refused where it would name a joint of the other kind + // refused where it would name a joint of the other kind, and the + // travel of each, so that a rotary is taken the way that stays in it _setup.kins_angular_joints = 0; for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { char section[16]; @@ -938,6 +945,8 @@ int Interp::init() if (auto type = inifile.findString("TYPE", section)) { if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } } + _setup.kins_joint_min[jno] = inifile.findRealV("MIN_LIMIT", section, -1e99); + _setup.kins_joint_max[jno] = inifile.findRealV("MAX_LIMIT", section, 1e99); } _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); diff --git a/tests/twp-native/README b/tests/twp-native/README index 9a49937006d..868da2c4f14 100644 --- a/tests/twp-native/README +++ b/tests/twp-native/README @@ -7,7 +7,7 @@ angle pairs and leaves the linear joints where they were all through the move; G53.6 leaves the tool tip where it was; G53.3 ends at the point asked for in the plane; a move along plane X goes along plane X in the world; G68.3 reads the plane back off the oriented tool; G69 cancels. -Q1 lets the table take part. The point-to-point moves are checked too: +G53.6 standing near the end of C's travel reaches the plane by the nearest pose inside it, never running C past its limit. Q1 lets the table take part. The point-to-point moves are checked too: G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with the head tilted, G53.4 G1 taking the time the straight move would in G94 and in G93, and what they refuse. diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 3eadf138911..b365a0c2ac5 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -320,6 +320,63 @@ def in_plane(): error("#<_orient_a> after G53.2: %s" % m[1]) drain() +# --- the orientation stays inside the rotary travel -------------------- +# the primary rotary C travels -320 to 320: standing at C300, a plane whose +# nearest pose puts C on the turn past 320 is reached the long way round or +# by the other pose, whichever is nearer inside the travel, and never by +# running C out of it +C_MIN, C_MAX, B_MIN, B_MAX = -320.0, 320.0, -185.0, 185.0 + +def turn_near(v, now, lo=None, hi=None): + turns = math.floor((now - v) / 360.0 + 0.5) + if lo is not None: + turns = min(max(turns, math.ceil((lo - v) / 360.0)), math.floor((hi - v) / 360.0)) + return v + 360.0 * turns + +def pose_near(pairs, b_now, c_now, limited): + best = None + for b, cc in pairs: + if limited: + bb, cc = turn_near(b, b_now, B_MIN, B_MAX), turn_near(cc, c_now, C_MIN, C_MAX) + else: + bb, cc = turn_near(b, b_now), turn_near(cc, c_now) + d = abs(bb - b_now) + abs(cc - c_now) + if best is None or d < best[0]: + best = (d, bb, cc) + return best[1], best[2] + +mdi("G69") +stand = mdi("G0 A0 B0 C300") +plane = None +for i in range(-60, 61, 5): + for j in range(-60, 61, 5): + z = list(rot_y(j).dot(rot_x(i))[:, 2]) + pairs = oracle_pairs(z) + if pairs and pose_near(pairs, stand[SECONDARY], stand[PRIMARY], False)[1] > C_MAX: + plane = (i, j, z, pose_near(pairs, stand[SECONDARY], stand[PRIMARY], True)) + break + if plane: + break +if not plane: + error("no plane on the grid puts the nearest C past the travel") +else: + i, j, z, want = plane + mdi("G68.2 P1 Q123 I%d J%d K0" % (i, j)) + after, samples = sampled("G53.6") + show("G53.6 at the C limit", after) + drain() + top = max(smp[0][PRIMARY] for smp in samples) + bottom = min(smp[0][PRIMARY] for smp in samples) + print("plane I%d J%d from C300: C went %.4f to %.4f, the nearest pose inside the travel is B %.4f C %.4f" + % (i, j, bottom, top, want[0], want[1])) + if top > C_MAX + 1e-6 or bottom < C_MIN - 1e-6: + error("G53.6 ran C out of its travel") + if abs(after[SECONDARY] - want[0]) > 1e-3 or abs(after[PRIMARY] - want[1]) > 1e-3: + error("G53.6 ended at B %.4f C %.4f, not the nearest pose inside the travel" + % (after[SECONDARY], after[PRIMARY])) + if not close(tool_axis(after), z, 1e-6): + error("the tool axis after G53.6 at the C limit is not the plane normal") + # --- G53.3 goes to a point in the plane with the tool oriented ---------- before = mdi("G69") R3 = rot_y(-25).dot(rot_x(35)) diff --git a/tests/twp-table/README b/tests/twp-table/README index 29d0082bc38..f9b61239e6e 100644 --- a/tests/twp-table/README +++ b/tests/twp-table/README @@ -5,3 +5,8 @@ several starting poses: G53.1 with no P takes the nearer of the two poses, P1 always the one with A positive and P2 the one with A negative, the same pose from every start; each leaves the tool along the plane's normal; a move in the plane after the turn ends where the plane puts it. + +Then again with A tilting one way only, [AXIS_A] 0 to 100 while [JOINT_3] +runs on to 9999: G53.1 to each side plane goes to A90 with C carrying the +side round, never to A270, a whole turn the joint has travel for and the +axis has not. diff --git a/tests/twp-table/checkresult b/tests/twp-table/checkresult index 9d48d3f180e..a9e60354d4f 100755 --- a/tests/twp-table/checkresult +++ b/tests/twp-table/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 configuration +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/twp-table/test-ui.py b/tests/twp-table/test-ui.py index bc24aaf278d..3f4c6f95207 100755 --- a/tests/twp-table/test-ui.py +++ b/tests/twp-table/test-ui.py @@ -76,7 +76,21 @@ def travel(j, start): mdi("G12.1 P1", "G0 X0 Y0 Z60 A0 C0") -planes = [(0, 30), (60, 30), (-90, 45), (45, -30), (150, 20)] +# half.ini: A tilts one way only, its [AXIS_A] travel 0 to 100 while +# [JOINT_3] runs on to 9999, so a pose turned whole turns along to fit the +# joint must still fit the axis: each side plane goes to A90, C carrying it +one_sided = linuxcnc.ini(os.environ["INI_FILE_NAME"]).find("AXIS_A", "MIN_LIMIT") == "0" +if one_sided: + for i in (0, 90, -90, 180): + mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J90 K0" % i) + j = mdi("G53.1") + off = angle(tool_axis(j), normal(i, 90)) + if off > 1e-6 or abs(j[A] - 90.0) > 1e-6: + error("G53.1 on I%g J90 went to A%.3f C%.3f, %.6f degrees off the normal" + % (i, j[A], j[C], off)) + print("I%g J90 G53.1 on the one-sided A: A%.3f C%.3f" % (i, j[A], j[C])) + +planes = [] if one_sided else [(0, 30), (60, 30), (-90, 45), (45, -30), (150, 20)] starts = [(0, 0), (-40, 200), (40, -100)] for i, jj in planes: nearest = [] @@ -114,7 +128,8 @@ def travel(j, start): % (i, jj, start[0], start[1], j[A], j[C], other[A], other[C])) # after the turn, a move in the plane ends where the plane puts it -for i, jj, word in ((60, 30, "P1"), (60, 30, "P2"), (-90, 45, "P1")): +turns = () if one_sided else ((60, 30, "P1"), (60, 30, "P2"), (-90, 45, "P1")) +for i, jj, word in turns: mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj), "G53.1 %s" % word, "G0 X10 Y5 Z20", "G1 X-10 F2000") s.poll() diff --git a/tests/twp-table/test.sh b/tests/twp-table/test.sh index 765cf14fed6..1a5d8c386c2 100755 --- a/tests/twp-table/test.sh +++ b/tests/twp-table/test.sh @@ -2,3 +2,10 @@ # a failed run leaves the var file behind, and it carries offsets rm -f sim.var sim.var.bak linuxcnc -r test.ini +# A tilting one way only, its joint left free to turn on +sed -e '/^\[AXIS_A\]/,/^\[/s/^MIN_LIMIT = .*/MIN_LIMIT = 0/' -e '/^\[AXIS_A\]/,/^\[/s/^MAX_LIMIT = .*/MAX_LIMIT = 100/' \ + -e '/^\[JOINT_3\]/,/^\[/s/^MIN_LIMIT = .*/MIN_LIMIT = 0/' -e '/^\[JOINT_3\]/,/^\[/s/^MAX_LIMIT = .*/MAX_LIMIT = 9999/' \ + test.ini > half.ini +rm -f sim.var sim.var.bak +linuxcnc -r half.ini +rm -f half.ini From 19cc1a3a6d0aadd46704c85f72a5734af23eaaa0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 10 Sep 2026 19:03:51 +0800 Subject: [PATCH 17/24] configs: the bridgemill sim shows off the tilted work plane family The old 5axisgui.ngc "drilled" a sphere with W words, W standing in for a tool length: the W axis of 5axiskins never drove a physical joint, the stroke folds into the XYZ slides, and the model's drawn quill hid that by cancelling the very motion that makes a W word work. The quill and its drawing lock go, so a W word shows the slides plunging, and 5axisgui.ngc keeps its name with comments saying what the controller cannot see. The sphere is then drilled two honest ways: g532-fused-orient-move.ngc, where G53.2 solves each plane and one fused G0 turns the head and travels to the hole, and g536-orient-then-move.ngc, where G53.6 reorients about the tip and a separate move travels in the plane. The demos and the M428 remap select the TCP kinematics with G12.1 P0, since on 5axiskins G13.1 lands on the identity. The AXIS GEOMETRY drops W: under TCP a W word never moves the tip, so W in the live plot only ever lied. --- .../axis/vismach/5axis/bridgemill/5axis.ini | 9 +++- .../vismach/5axis/bridgemill/5axisgui.ngc | 15 ++++++ .../sim/axis/vismach/5axis/bridgemill/README | 50 +++++++++++++++++-- .../bridgemill/g532-fused-orient-move.ngc | 50 +++++++++++++++++++ .../bridgemill/g536-orient-then-move.ngc | 49 ++++++++++++++++++ 5 files changed, 166 insertions(+), 7 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 38e706fac22..4c987aba743 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw) DEBUG = 0 [DISPLAY] - GEOMETRY = XYZCBW - OPEN_FILE = ./5axisgui.ngc +# GEOMETRY tells the AXIS live plot how to guess the tool position from +# the nine axis values: letters translate, ABC rotate the point. W is +# deliberately absent: the plot would add the W value as a Z offset after +# the rotations, while the kinematics spends W along the tool axis, so +# the guess never matches. + GEOMETRY = XYZCB + OPEN_FILE = ./g532-fused-orient-move.ngc INCREMENTS = 10 mm, 1 mm, .1 mm JOG_AXES = XYZC DISPLAY = axis diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc index 7b2c5a8f188..cbc165137d7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc @@ -1,3 +1,17 @@ +; 5axisgui.ngc - the historical demo program, unchanged. +; It drills a sphere pattern with W words. W was never a physical +; quill: no motor is connected to its joint, the kinematics folds +; the word into the XYZ slides, and the head sliding along the tool +; axis is what plunges the rigidly mounted tool. Watch the slides +; in vismach do the stroke. +; +; The quirk, then and now: the controller keeps W as a separate +; world coordinate, so the programmed XYZ point does not move and +; the preview shows nothing of the stroke; only the W DRO tracks it. +; Drilling the controller can see, check and preview is what the +; tilted work plane family is for (g532-fused-orient-move.ngc, +; g536-orient-then-move.ngc). + # = 60 ; sphere radius # = 5 ; safe distance # = -5 @@ -13,6 +27,7 @@ # = [90/#] g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active t#m6g43 g53 g0 x0y0z#b0c0 w0 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/README b/configs/sim/axis/vismach/5axis/bridgemill/README index 2adf5599795..4a92c6a2078 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/README +++ b/configs/sim/axis/vismach/5axis/bridgemill/README @@ -1,6 +1,46 @@ -This is a simulation of an XYZBCWY 5 axis bridge mill. +This is a simulation of an XYZBCWY 5 axis bridge mill with a +tilting head (B, C) and a W axis. -Example demo: +W is not a physical axis of the machine and never was: no motor is +connected to its joint. The kinematics folds a W word into the +XYZ joints, so the head slides along the tool axis and the +rigidly mounted tool goes with it: that is the whole trick, and it +is what vismach shows. There is no quill to draw because there is +no quill. + +The quirk: the controller keeps W as a separate world coordinate, +so a W word leaves the programmed XYZ point untouched and the +preview cannot show the stroke; the W DRO tracks it. It also +means the planner does not see the tool-axis motion, so for +drilling the controller can check and preview, use the tilted work +plane family. + +Because the W joint is virtual, a point-to-point move on it +(g53.5/g53.7 j5=) moves no motor on a real machine while the +controller believes the world moved, the opposite of a W word. +Do not drill that way. + +Demo programs: + + g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work + planes (g68.2), using g53.2 to solve each + orientation without moving (STAY), then + one fused g0 that turns the head and + travels to the hole at the same time + (TCP motion) + g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain + MOVE style): reorient about the fixed + tip, then travel in the tilted plane + 5axisgui.ngc --------------- the historical demo program, unchanged: + it drills the same pattern with W words, + the slides doing the stroke while the + preview stays blind to it + +The tool table provides tool 100 (length 100). Load it with +t100m6g43 so the vismach tool and the kinematics pivot length +(pivotsum: 250 + tool length) match. + +Example MDI: 1) $ linuxcnc 5axis.ini 2) F1 ---------- Estop off @@ -8,12 +48,12 @@ Example demo: CTRL-HOME --- home all F5 ---------- MDI tab 3) orient vismach gui as required - 4) g0w10 ; retract w + 4) g0w10 ; head slides up the tool axis, tip with it 5) g43h100 ; tool offset (100) 6) g0b45 ; tilt 45 deg wrt z 7) g0c30 ; rotate 30 deg in xy - 8) g0w-10 ; simulate drill - 9) g0w10 ; retract drill + 8) g0w-10 ; head slides back, tip plunges 10 along the tool axis + 9) g0w10 ; and back 10) etc Note: Motion for the W coordinate is incorporated diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc new file mode 100644 index 00000000000..bd193299758 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -0,0 +1,50 @@ +; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes: +; g68.2 tilts the plane onto each hole normal, g53.2 solves the head +; orientation without moving (STAY), and one fused g0 turns the head +; and travels to the hole at the same time (TCP motion). See README +; for the other demos. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.2 ; solve the orientation, stay put + g0 x0 y0 z[#+#] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc new file mode 100644 index 00000000000..2354e7e0625 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc @@ -0,0 +1,49 @@ +; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6 +; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move, +; then travel to the next hole in the tilted plane with a separate g0. +; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.6 ; reorient about the tip, a TCP move + g0 x0 y0 z[#+#] + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 From 78b357879bc85924523ee44ff0219a1763789fbe Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 15:37:46 +1000 Subject: [PATCH 18/24] motion, interp: a tool offset change under a tilt keeps the joints A module that applies the tool length itself reads it from motion, so a G43 with the head tilted moves the programmed point, not the joints; motion, task and the interpreter each did something else. Motion re-reads the point from the joints under the new tool, as after a kinematics switch, takes the external offsets off, moves the planner onto it and latches the joint hold there. The interpreter predicts the same point through the loader, from the joints the machine stands on, or for a G43.4 that switches on the same line from the joints read in the old kinematics before the switch, evaluating with the tool offset the program is under rather than the one motion has reached; the point rides along with the offset on EMC_TRAJ_SET_OFFSET, and motion aborts when its own answer differs. Under the identity, and where the module cannot be evaluated, the interpreter shifts by the offset difference as before and motion aborts only if the point moved. Task issues the offset once the queue has drained, so no queue busting is needed. tests/kins-switch applies G43.4 and a zero length in a tilted pose and after a point-to-point move: the joints hold, the point moves by the tilt term, and a move of nothing from the interpreter's point goes nowhere. --- docs/src/gcode/g-code.adoc | 13 ++++ docs/src/man/man9/kins.9.adoc | 16 +++-- docs/src/motion/switchkins.adoc | 9 ++- lib/python/rs274/interpret.py | 3 + src/emc/motion/command.c | 10 ++- src/emc/motion/control.c | 95 ++++++++++++++++++++++---- src/emc/motion/mot_priv.h | 2 + src/emc/motion/motion.h | 4 ++ src/emc/nml_intf/canon.hh | 9 +++ src/emc/nml_intf/emc.cc | 2 + src/emc/nml_intf/emc.hh | 2 +- src/emc/nml_intf/emc_nml.hh | 6 +- src/emc/rs274ngc/canonmodule.cc | 2 +- src/emc/rs274ngc/gcodemodule.cc | 21 ++++++ src/emc/rs274ngc/interp_convert.cc | 99 ++++++++++++++++++++-------- src/emc/rs274ngc/interp_workplane.cc | 92 +++++++++++++++++++++++--- src/emc/rs274ngc/rs274ngc_interp.hh | 5 ++ src/emc/sai/saicanon.cc | 10 +++ src/emc/task/emccanon.cc | 27 +++++++- src/emc/task/emctaskmain.cc | 9 ++- src/emc/task/taskintf.cc | 6 +- tests/kins-switch/README | 6 +- tests/kins-switch/test-ui.py | 68 ++++++++++++++----- 23 files changed, 436 insertions(+), 80 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index cbdf99864c7..9acf12f7964 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1641,6 +1641,19 @@ in the numbered parameters '5081-5089'. The numbered parameters '5401-5409' instead hold the loaded tool's stored offset, refreshed on tool change ('M6') and 'G10 L1'/'L10'/'L11'. +Some kinematics modules apply the tool length themselves, along the tool +axis rather than along Z (5axiskins, maxkins, the trt modules, genhexkins, +pentakins; see the <> +chapter). 'G43' does not move the machine under those either. With the +tool axis tilted at the time, the programmed coordinates of the point the +tool stands on change by more than the offset difference: the interpreter +works them out through the kinematics, so the next move starts from where +the tool tip actually is. Motion reads the point from the joints the same +way and stops the program if the two disagree. A module that can only be +evaluated in realtime leaves the interpreter with the offset difference +alone; under one of those, change the tool length with the tool axis +untilted, motion stops the program otherwise. + [NOTE] 'G43 H0' is a little special. Its behavior is different on random tool changer machines and nonrandom tool changer machines (see the diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 79a3a116e21..01cef58cc73 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -207,7 +207,9 @@ documentation for more info) *genhexkins.tool-offset*:: TCP offset from platform origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === genserkins - generalized serial kinematics @@ -254,8 +256,8 @@ replacing it. Put a given length in one column or the other, not both. Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B tilts. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. === pentakins - Pentapod Kinematics @@ -291,7 +293,9 @@ The forward kinematics iteration is controlled by HAL pins. *pentakins.tool-offset*:: TCP offset from effector origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === pumakins - kinematics for puma typed robots @@ -416,8 +420,8 @@ expected by it (XYZBCW `->` joints 0..5) Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B and C move. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. A tool length in the W column of the tool + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. A tool length in the W column of the tool table reaches the same place, once a block commands W, and adds to this one rather than replacing it. Put a given length in one column or the other, not both. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 81c5ac2ef49..db9e4c99d11 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -630,8 +630,13 @@ exports kinematicsSetTool() as well, through which motion hands it the tool offset in effect whenever that changes. A table entry flagged as the tool is overwritten with it, and the entry's pin only matters until motion has sent anything, so a config need not net -motion.tooloffset.z to the module. A kinstype registered the older -way reads its own pins and is not affected. +motion.tooloffset.z to the module. When the offset changes, motion +keeps the joints where they are and reads the point back from them +under the new offset, so a tool length change with the tool axis +tilted moves the programmed point rather than the machine; the +interpreter reads the point the same way through the non-realtime +loader, and motion stops the program if the two disagree. A kinstype +registered the older way reads its own pins and is not affected. === Module main program diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 0662adc90bc..055909ade05 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -196,5 +196,8 @@ def get_block_delete(self): def get_external_joint_positions(self): return tuple(self.s.joint_actual_position[:self.s.joints]) + def get_kinematics_type(self): + return self.s.kinematics_type + # vim:ts=8:sts=4:et: diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 30af51ae540..b669000e664 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2228,10 +2228,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); - emcmotStatus->tool_offset = emcmotCommand->tool_offset; if (kinematicsSetTool) { - kinematicsSetTool(&emcmotStatus->tool_offset); + /* the module applies the offset, so the point the joints + stand on changes with it */ + kinematicsSetTool(&emcmotCommand->tool_offset); + emcmotToolOffsetChanged(&emcmotStatus->tool_offset, + &emcmotCommand->tool_offset, + &emcmotCommand->pos, + emcmotCommand->have_point); } + emcmotStatus->tool_offset = emcmotCommand->tool_offset; break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 1e7004bb699..22cbfdaf5a4 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -194,6 +194,7 @@ static void output_to_hal(void); static void update_status(void); static void handle_kinematicsSwitch(void); +static int reanchor_pose(const double *joint_pos, EmcPose *at); /*********************************************************************** * PUBLIC FUNCTION CODE * @@ -366,8 +367,6 @@ static void handle_kinematicsSwitch(void) { return; // the kinematics in force is unchanged } - KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; - KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG double beforePose[EMCMOT_MAX_AXIS]; int anum; @@ -379,9 +378,7 @@ static void handle_kinematicsSwitch(void) { solve them is one the machine cannot run in from here: put the old one back, or the inverse would run the joints to wherever the pose we know lands in the new one */ - EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, - &tmpFFlags, &tmpIFlags)) { + if (reanchor_pose(joint_posKinsSwitch, NULL) != 0) { kinematicsSwitch(switchkins_type); reportError(_("kinematicsForward failed for kinematics type %d," " type %d is still in force"), @@ -389,7 +386,6 @@ static void handle_kinematicsSwitch(void) { SET_MOTION_ERROR_FLAG(1); // abort return; // the kinematics in force and the position are unchanged } - emcmotStatus->carte_pos_cmd = poseKinsSwitch; switchkins_type = requested_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); @@ -401,9 +397,29 @@ static void handle_kinematicsSwitch(void) { ,anum,beforePose[anum],*pcmd_p[anum],*pcmd_p[anum]-beforePose[anum]); } #endif +} //handle_kinematicsSwitch() + +/* The point re-read from the joints, for when what the joints mean has + changed without the joints moving: a kinematics switch, or a tool + offset the module applies. The pose they put the tool at is the new + commanded point, the external offsets taken off it, and the planner + is moved onto it. A forward that fails leaves the point alone. + The pose with the external offsets still on it is returned in at. */ +static int reanchor_pose(const double *joint_pos, EmcPose *at) +{ + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; + KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; + EmcPose pose = emcmotStatus->carte_pos_cmd; + + if (kinematicsForward(joint_pos, &pose, &tmpFFlags, &tmpIFlags) != 0) { + return -1; + } + emcmotStatus->carte_pos_cmd = pose; + if (at) { *at = pose; } axis_apply_ext_offsets_to_carte_pos(-1, pcmd_p); tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); -} //handle_kinematicsSwitch() + return 0; +} static void process_inputs(void) { @@ -940,17 +956,74 @@ static int joint_hold_valid = 0; static double joint_hold[EMCMOT_MAX_JOINTS]; static EmcPose joint_hold_pose; -/* whether two machine points are the same, to a hair either way */ -static int same_carte_pos(const EmcPose *a, const EmcPose *b) +/* whether two machine points are within tol of each other on every axis */ +static int carte_pos_within(const EmcPose *a, const EmcPose *b, double tol) { - const double tol = 1e-9; - 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; } +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + return carte_pos_within(a, b, 1e-9); +} + +/* A tool offset the module applies has changed under the point. The + machine stays where it is: the point is re-read from the joints under + the new offset, and the joints are held there, since the inverse of the + re-read point may answer with another joint set. The interpreter + works out the same point ahead of motion and sends it along when it + can evaluate the kinematics; where the two disagree, or where it could + not say and the point moved, the program is not let go on from a point + the interpreter does not have. Nothing to do outside coordinated mode: + the point follows the joints there anyway. */ +void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected) +{ + const double tol = 1e-4; + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + EmcPose was = emcmotStatus->carte_pos_cmd; + EmcPose now; + int joint_num; + + if (same_carte_pos(from, to) || !GET_MOTION_COORD_FLAG()) { return; } + for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { + joint_pos[joint_num] = joints[joint_num].coarse_pos; + } + if (reanchor_pose(joint_pos, &now) != 0) { + reportError(_("the kinematics cannot place the tool from the joints" + " after the tool offset change")); + SET_MOTION_ERROR_FLAG(1); + return; + } + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = joint_pos[joint_num]; + } + joint_hold_pose = now; + joint_hold_valid = 1; + + if (have_expected) { + if (!carte_pos_within(&emcmotStatus->carte_pos_cmd, expected, tol)) { + reportError(_("the tool offset change put the point at" + " %.4f %.4f %.4f, the interpreter expected" + " %.4f %.4f %.4f"), + emcmotStatus->carte_pos_cmd.tran.x, + emcmotStatus->carte_pos_cmd.tran.y, + emcmotStatus->carte_pos_cmd.tran.z, + expected->tran.x, expected->tran.y, expected->tran.z); + SET_MOTION_ERROR_FLAG(1); + } + } else if (!carte_pos_within(&was, &now, tol)) { + reportError(_("the tool offset change moved the point under a" + " kinematics the interpreter cannot evaluate;" + " change the tool offset with the machine untilted")); + SET_MOTION_ERROR_FLAG(1); + } +} + static void set_operating_mode(void) { int joint_num; diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 954de98dd84..3a0f8e5f697 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -271,6 +271,8 @@ extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num); extern void clearHomes(int joint_num); extern void emcmot_config_change(void); +extern void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index d5ce5fe936a..49cb1b00a94 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -286,6 +286,10 @@ extern "C" { double joint_target[EMCMOT_MAX_JOINTS]; int have_joint_target; double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + + /* SET_OFFSET: pos is where the interpreter expects the point to be + once the offset is on, for motion to check its own answer against */ + int have_point; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 259d43b6795..e847b17ebec 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -698,6 +698,11 @@ extern void USE_NO_SPINDLE_FORCE(); extern void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double diameter, double frontangle, double backangle, int orientation); extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset); +/* The same, with the point the interpreter expects the machine to stand + on once the offset is applied, in program coordinates: where a + kinematics applies the offset itself, motion keeps the joints and + re-reads the point from them, and compares it with this one. */ +extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point); extern void CHANGE_TOOL(); @@ -952,6 +957,10 @@ extern int GET_EXTERNAL_KINS_TYPE(); kinematics.h); -1 where it says nothing: no such type, plain kinematics, or no motion controller attached (sai, preview) */ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); +/* whether the machine's kinematics is the identity, the joints being the + world, so that no tool offset is applied by the kinematics; true where + no motion controller is attached (sai, a preview without a machine) */ +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); /* The tilted work plane the machine last executed, in the form SET_G68_FRAME takes: 1 with origin and rotation filled in, 0 when there diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 6805097bc73..58e80a70f92 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1568,6 +1568,8 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); EmcPose_update(cms, &offset); + EmcPose_update(cms, &point); + cms->update(have_point); } // cppcheck-suppress duplInheritedMember diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 269bfaf4fad..2037aba3a62 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -382,7 +382,7 @@ extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, c double vlimit_scale); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index, double angular_offset_degrees = 0.0); -extern int emcTrajSetOffset(const EmcPose& tool_offset); +extern int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 9c1787e3b67..576de74797e 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -805,7 +805,7 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_OFFSET() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_OFFSET_TYPE, sizeof(EMC_TRAJ_SET_OFFSET)), - offset{} + offset{}, point{}, have_point(0) {}; // Sub-class update() calls base-class update() @@ -814,6 +814,10 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { void update(CMS * cms); EmcPose offset; + // where the interpreter expects the machine to stand once the offset + // is on, for motion to check its own answer against; only when set + EmcPose point; + int have_point; }; class EMC_TRAJ_SET_G5X:public EMC_TRAJ_CMD_MSG { diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 66138b48bfb..dfee7b69ef0 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -253,7 +253,7 @@ BOOST_PYTHON_MODULE(emccanon) { def("USE_NO_SPINDLE_FORCE",&USE_NO_SPINDLE_FORCE); // def("USER_DEFINED_FUNCTION_ADD",&USER_DEFINED_FUNCTION_ADD); // def("USE_SPINDLE_FORCE",&USE_SPINDLE_FORCE); - def("USE_TOOL_LENGTH_OFFSET",&USE_TOOL_LENGTH_OFFSET); + def("USE_TOOL_LENGTH_OFFSET",static_cast(&USE_TOOL_LENGTH_OFFSET)); def("WAIT",&WAIT); // from interp_queue.cc diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index d81e6552989..45d679705b7 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -58,6 +58,7 @@ #include "nml_intf/interp_return.hh" #include "nml_intf/canon.hh" #include +#include // KINEMATICS_IDENTITY int _task = 0; // control preview behaviour when remapping @@ -660,6 +661,10 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) { offset.u, offset.v, offset.w})); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) { + USE_TOOL_LENGTH_OFFSET(offset); +} + void SET_FEED_REFERENCE(double /*reference*/) { } void SET_CUTTER_RADIUS_COMPENSATION(double /*radius*/) {} void START_CUTTER_RADIUS_COMPENSATION(int /*direction*/) {} @@ -962,6 +967,22 @@ CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } int GET_EXTERNAL_KINS_TYPE() { return 0; } int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { (void)ktype; return -1; } int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) { return -1; } + +// the kind of transform the machine runs, from a canon that watches the +// status buffer; one that cannot answer has no machine, and the joints +// are the world +bool GET_EXTERNAL_KINEMATICS_IDENTITY() { + if(parse_state.interp_error) return true; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_kinematics_type")) return true; + try { + return canon.attr("get_kinematics_type")().cast() == KINEMATICS_IDENTITY; + } catch(py::error_already_set &) { + return true; // the error goes with the exception + } catch(py::builtin_exception &) { + return true; + } +} void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index aec6ba979f3..9c6d0c74863 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6714,6 +6714,8 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu { int idx; EmcPose tool_offset; + double standing[EMCMOT_MAX_JOINTS]; + bool have_standing = false; ZERO_EMC_POSE(tool_offset); settings->g43_with_zero_offset = 0; @@ -6725,7 +6727,18 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // apply the offset, as if the switch line had run and drained. With // no kinematics attached there is nothing to switch to. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); - if (primary >= 0) { switch_kins_type(primary, settings); } + if (primary >= 0 && primary != settings->kins_type) { + // the switch keeps the joints and moves the point, so the point + // the offset is read from is not the one the program is at: take + // the joints, while the kinematics they are known in is in force + void *vctx; + CHP(kins_here(settings, &vctx)); + if (vctx) { + CHP(current_joints(settings, vctx, standing)); + have_standing = true; + } + switch_kins_type(primary, settings); + } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { // the offset in effect is no longer G43.4's, so G49 has no switch to undo @@ -6815,31 +6828,65 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } else { ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); } - USE_TOOL_LENGTH_OFFSET(tool_offset); - - double dx, dy, dz; - - // the tool does not move, so its program coordinates change by the - // offset difference seen from the program: the XY rotation and the - // tilted work plane taken off it - dx = settings->tool_offset.tran.x - tool_offset.tran.x; - dy = settings->tool_offset.tran.y - tool_offset.tran.y; - dz = settings->tool_offset.tran.z - tool_offset.tran.z; - - rotate(&dx, &dy, -settings->rotation_xy); - g68_unrotate(settings, &dx, &dy, &dz); - - settings->current_x += dx; - settings->current_y += dy; - settings->current_z += dz; - settings->AA_current += settings->tool_offset.a - tool_offset.a; - settings->BB_current += settings->tool_offset.b - tool_offset.b; - settings->CC_current += settings->tool_offset.c - tool_offset.c; - settings->u_current += settings->tool_offset.u - tool_offset.u; - settings->v_current += settings->tool_offset.v - tool_offset.v; - settings->w_current += settings->tool_offset.w - tool_offset.w; - - settings->tool_offset = tool_offset; + // The machine does not move, so the program coordinates of the point change. + // A kinematics that applies the offset itself moves the point by more than + // the offset difference, along the tilted tool axis: evaluate it here as + // motion will, and send the point along for motion to check against. + EmcPose point; + bool point_known = false; + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + if (point_known) { + double prog[9]; + EmcPose in_program; + + settings->tool_offset = tool_offset; + machine_pose_to_program(settings, &point, prog); + settings->current_x = prog[0]; + settings->current_y = prog[1]; + settings->current_z = prog[2]; + settings->AA_current = prog[3]; + settings->BB_current = prog[4]; + settings->CC_current = prog[5]; + settings->u_current = prog[6]; + settings->v_current = prog[7]; + settings->w_current = prog[8]; + in_program.tran.x = USER_TO_PROGRAM_LEN(point.tran.x); + in_program.tran.y = USER_TO_PROGRAM_LEN(point.tran.y); + in_program.tran.z = USER_TO_PROGRAM_LEN(point.tran.z); + in_program.a = USER_TO_PROGRAM_ANG(point.a); + in_program.b = USER_TO_PROGRAM_ANG(point.b); + in_program.c = USER_TO_PROGRAM_ANG(point.c); + in_program.u = USER_TO_PROGRAM_LEN(point.u); + in_program.v = USER_TO_PROGRAM_LEN(point.v); + in_program.w = USER_TO_PROGRAM_LEN(point.w); + USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); + } else { + double dx, dy, dz; + + USE_TOOL_LENGTH_OFFSET(tool_offset); + + // by the offset difference seen from the program: the XY rotation + // and the tilted work plane taken off it + dx = settings->tool_offset.tran.x - tool_offset.tran.x; + dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; + + rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); + + settings->current_x += dx; + settings->current_y += dy; + settings->current_z += dz; + settings->AA_current += settings->tool_offset.a - tool_offset.a; + settings->BB_current += settings->tool_offset.b - tool_offset.b; + settings->CC_current += settings->tool_offset.c - tool_offset.c; + settings->u_current += settings->tool_offset.u - tool_offset.u; + settings->v_current += settings->tool_offset.v - tool_offset.v; + settings->w_current += settings->tool_offset.w - tool_offset.w; + + settings->tool_offset = tool_offset; + } // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index a0426d228c9..b6a58493d39 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -567,11 +567,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } //---------------------------------------------------------------------- -// The kinematics. G68.3 and the orientation moves need the frames and -// the tool frame inverse of the module motion runs, evaluated here, ahead -// of motion, through the loader in kinematics_userspace/. The loader -// binds its pins to a HAL component, so the interpreter makes one, named -// by its process, the first time it is asked. +// The kinematics. G68.3, the orientation moves and a tool offset change +// need the module motion runs, evaluated here, ahead of motion, through +// the loader in kinematics_userspace/. The loader reads the module's +// pins through HAL, so the interpreter connects as a component, named by +// its process, the first time it is asked. //---------------------------------------------------------------------- #include @@ -582,12 +582,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) #define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) -// the loaded module, on the kinematics type the program is in -int Interp::kins_context(setup_pointer s, void **out) +// the module loaded, once +int Interp::kins_load(setup_pointer s) { KinematicsUserContext *ctx; - *out = NULL; if (!s->kins_ctx) { char name[HAL_NAME_LEN + 1]; int comp; @@ -607,15 +606,46 @@ int Interp::kins_context(setup_pointer s, void **out) s->kins_ctx = ctx; for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } } + return INTERP_OK; +} + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + CHP(kins_load(s)); ctx = KINS_CTX(s); CHKS((kinematicsUserIsRtOnly(ctx)), _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), _("kinematics type %d is not available outside realtime"), s->kins_type); + // with the tool offset the program is under here, not the one motion + // has reached: the interpreter runs ahead of motion + kins_set_tool(ctx, &s->tool_offset); *out = ctx; return INTERP_OK; } +// the offset the module evaluates with, given in program units like the +// interpreter keeps it +void Interp::kins_set_tool(void *vctx, const EmcPose *offset) +{ + EmcPose tool; + + tool.tran.x = PROGRAM_TO_USER_LEN(offset->tran.x); + tool.tran.y = PROGRAM_TO_USER_LEN(offset->tran.y); + tool.tran.z = PROGRAM_TO_USER_LEN(offset->tran.z); + tool.a = PROGRAM_TO_USER_ANG(offset->a); + tool.b = PROGRAM_TO_USER_ANG(offset->b); + tool.c = PROGRAM_TO_USER_ANG(offset->c); + tool.u = PROGRAM_TO_USER_LEN(offset->u); + tool.v = PROGRAM_TO_USER_LEN(offset->v); + tool.w = PROGRAM_TO_USER_LEN(offset->w); + kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); +} + void Interp::kins_release(setup_pointer s) { if (s->kins_ctx) { @@ -710,6 +740,52 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) return INTERP_OK; } +// The kinematics as far as it can be evaluated here: the context on the +// type the program is in, or NULL where there is nothing to evaluate, +// with no machine attached (sai, a preview without one) or a module that +// runs only in realtime. +int Interp::kins_here(setup_pointer s, void **out) +{ + *out = NULL; + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return INTERP_OK; } + CHP(kins_load(s)); + if (kinematicsUserIsRtOnly(KINS_CTX(s))) { return INTERP_OK; } + return kins_context(s, out); +} + +// Where the point goes when the tool offset changes: the joints stay, so it is +// read back from them under the new offset, as motion reads it. known stays +// false under the identity and where nothing can be evaluated; the caller +// then shifts by the offset difference. +int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + int flags, i; + + *known = false; + if (same_pose(offset, &s->tool_offset)) { return INTERP_OK; } + flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx || kinematicsUserIsIdentity(ctx)) { return INTERP_OK; } + if (standing) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = standing[i]; } + } else { + CHP(current_joints(s, ctx, joints)); + } + kins_set_tool(ctx, offset); + CHKS((kinematicsUserForward(ctx, joints, point) != 0), + _("the kinematics cannot place the tool from the joints after the tool offset change")); + // the machine stays on these joints + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + *known = true; + return INTERP_OK; +} + // a direction of the plane in world coordinates: the plane's rotation // then the XY rotation of the coordinate system it sits on static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index b7cd601f65a..5cf4dfdcf5f 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -388,7 +388,12 @@ public: int ptp_seconds(block_pointer block, setup_pointer settings, double x, double y, double z, double a, double b, double c, double u, double v, double w, double *seconds); + int kins_load(setup_pointer settings); int kins_context(setup_pointer settings, void **ctx); + int kins_here(setup_pointer settings, void **ctx); + int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known); + void kins_set_tool(void *ctx, const EmcPose *offset); void kins_release(setup_pointer settings); void current_machine_pose(setup_pointer settings, EmcPose *pose); void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 59c48d410fa..b6070e66c0c 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -605,6 +605,11 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) offset.tran.x, offset.tran.y, offset.tran.z, offset.a, offset.b, offset.c, offset.u, offset.v, offset.w); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) +{ + USE_TOOL_LENGTH_OFFSET(offset); +} + void CHANGE_TOOL() { PRINT("CHANGE_TOOL()\n"); @@ -859,6 +864,11 @@ extern int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) return -1; } +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return true; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 6f5580cbd39..49dac6f5172 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3039,7 +3039,7 @@ void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double EMC has no tool length offset. To implement it, we save it here, and apply it when necessary */ -void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) { auto set_offset_msg = std::make_unique(); @@ -3068,6 +3068,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) set_offset_msg->offset.v = TO_EXT_AX(AXIS_V, canon.toolOffset.v); set_offset_msg->offset.w = TO_EXT_AX(AXIS_W, canon.toolOffset.w); + set_offset_msg->have_point = (point != nullptr); + if (point) { + CANON_POSITION at(*point); + from_prog(at); + set_offset_msg->point = to_ext_pose(at); + // motion keeps the joints and stands on the point, so the next + // segment starts there and not where the old offset put it + canonUpdateEndPoint(at); + } + for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { SET_SPINDLE_SPEED(s, canon.spindle[s].speed); @@ -3076,6 +3086,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) interp_list.append(std::move(set_offset_msg)); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +{ + use_tool_length_offset(offset, nullptr); +} + +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point) +{ + use_tool_length_offset(offset, &point); +} + /* CHANGE_TOOL results from M6 */ void CHANGE_TOOL() { @@ -4075,6 +4095,11 @@ int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]) return 1; } +bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; +} + double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 8461c06baba..149b23faa39 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1992,11 +1992,14 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode, emcTrajSetSpindlesyncMsg->angular_offset_degrees); break; - case EMC_TRAJ_SET_OFFSET_TYPE: + case EMC_TRAJ_SET_OFFSET_TYPE: { // update tool offset - emcStatus->task.toolOffset = (reinterpret_cast(cmd))->offset; - retval = emcTrajSetOffset(emcStatus->task.toolOffset); + EMC_TRAJ_SET_OFFSET *msg = reinterpret_cast(cmd); + emcStatus->task.toolOffset = msg->offset; + retval = emcTrajSetOffset(emcStatus->task.toolOffset, + msg->have_point ? &msg->point : nullptr); break; + } case EMC_TRAJ_SET_ROTATION_TYPE: emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 9416079d437..c291547cf39 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1493,10 +1493,14 @@ int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajSetOffset(const EmcPose& tool_offset) +int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point) { emcmotCommand.command = EMCMOT_SET_OFFSET; emcmotCommand.tool_offset = tool_offset; + emcmotCommand.have_point = (point != nullptr); + if (point) { + emcmotCommand.pos = *point; + } return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e426b4d7554..0815a214c3c 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -8,5 +8,7 @@ the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the -kinematics with nothing netted to its pin, and that G13.1 in an -ON_ABORT_COMMAND routine does not swallow the rest of the routine. +kinematics with nothing netted to its pin and, the head being tilted, +moves the programmed point rather than the joints, with the interpreter +agreeing on where the point went, and that G13.1 in an ON_ABORT_COMMAND +routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index e62800c5cd7..00b04d34416 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -163,37 +163,75 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- the tool length reaches the kinematics with nothing netted ---------- +# ---- a tool length under a tilt keeps the joints ------------------------ # -# Motion hands the module the offset G43 puts in effect. The head is -# tilted, so the offset moves the joints, by the tilt term of the length: -# the same length along the tool axis instead of along Z. +# Motion hands the module the offset G43 puts in effect, with nothing +# netted. The head is tilted, so the length lies along the tool axis +# instead of along Z: the point the joints stand on moves by the tilt +# term of the length, and the joints stay where they are. The +# interpreter works out the same point ahead of motion, so a move to +# where it thinks the machine is goes nowhere. import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) drain() + +def pose(): + s.poll() + return list(s.position[:3]) + +def tool_length_check(what, before_joints, before_pose, after_joints, after_pose, want): + if max(abs(after_joints[j] - before_joints[j]) for j in range(JOINTS)) > 1e-6: + error("%s moved the joints by %s" % (what, + " ".join("%.4f" % (after_joints[j] - before_joints[j]) for j in range(JOINTS)))) + got = [after_pose[j] - before_pose[j] for j in (0, 1, 2)] + if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("%s moved the point by %s, not %s" % (what, + " ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) + said = [m[1].strip() for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s raised %s" % (what, said)) + # the interpreter's point: a move of nothing from it goes nowhere + mdi("G91") + stayed = mdi("G0 X0 Y0 Z0") + mdi("G90") + if max(abs(stayed[j] - after_joints[j]) for j in range(JOINTS)) > 1e-6: + error("after %s the interpreter has the point elsewhere: a move of nothing moved the joints by %s" + % (what, " ".join("%.4f" % (stayed[j] - after_joints[j]) for j in range(JOINTS)))) + mdi("G12.1 P0") mdi("G0 X10 Y10 Z-5 B-22.5 C45") mdi("G49") +errors_before = errors before = mdi("G0 X10 Y10 Z-5") +before_pose = pose() after = mdi("G43.4 H1") +pose_after = pose() L = 12.5 # tool 1 in tool.tbl b, cc = math.radians(-22.5), math.radians(45) -want = [-L * math.sin(math.pi - b) * math.cos(cc), - -L * math.sin(math.pi - b) * math.sin(cc), - -L * (1 + math.cos(math.pi - b))] -got = [after[j] - before[j] for j in (0, 1, 2)] -if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: - error("G43.4 in the tilted pose moved the joints by %s, not %s" - % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) -else: - print("G43.4 moved the joints by the tilt term of the tool length") +# the length along the tool axis, less the length along Z the offset stands for +want = [L * math.sin(math.pi - b) * math.cos(cc), + L * math.sin(math.pi - b) * math.sin(cc), + L * (1 + math.cos(math.pi - b))] +tool_length_check("G43.4 in the tilted pose", before, before_pose, after, pose_after, want) # G49 would drop to identity and hold the joints where they are; a zero # offset without a switch takes the length back out back = mdi("G43.1 Z0") -if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: - error("a zero tool length did not take the length back out of the joints") +tool_length_check("a zero tool length", after, pose_after, back, pose(), [-w for w in want]) +if errors == errors_before: + print("a tool length under a tilt moved the point, not the joints") + +# a point-to-point move ends on joints motion holds while the point +# stays; a tool length moves the point, and the joints are held on +errors_before = errors +p2p = mdi("G53.4 G0 X10 Y10 Z-5 B-22.5 C45") +p2p_pose = pose() +after = mdi("G43.1 Z%g" % L) +tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) +if errors == errors_before: + print("a tool length after a point-to-point move keeps the joints too") +mdi("G43.1 Z0") mdi("G49") # ---- a negative kinematics number is refused ----------------------------- From 82c08804345aecc66d5070db9c0b1f73c1cda540 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 13 Oct 2026 22:04:43 +1100 Subject: [PATCH 19/24] kinematics: an offset across the tool turns with the tool The X and Y of a tool offset, a tool held off the spindle axis, stayed on the work axes under every kinematics. Canon adds the whole offset along them; a module that applies the tool takes the length back off and applies it along the tool axis, but leaves X and Y where canon put them. With the head or the table turned the tip landed off the programmed point by (I - R)(x, y, 0), R the tool frame in work coordinates: on 5axiskins at B-22.5 C45 a 4, 3 offset put it 4.2 mm off. The offset is the tool's own, given against the machine with the rotaries at zero, so the shared code turns X and Y with the tool frame the module reports. kinsOpsForward() adds (I - R)(x, y, 0) to the pose the maths answers, kinsOpsInverse() takes it off the pose it hands the maths, settled on the joints the inverse finds, and kinsOpsJacobian() adds the term the turn contributes. Realtime and the loader make the same calls, so motion, the interpreter's prediction of a tool offset change and the segment cap agree. Nothing is turned without an X or Y, on a type flagged identity or machine, which leave the tool out of their maths, or for a module that reports no tool frame (scorbot-kins, genhexkins, pentakins), where X and Y stay on the work axes as before. tests/kins-params evaluates 5axiskins, the trt modules and the trsrn modules under a 4, 3 offset across the tool, the same in realtime and through the loader, and checks that each module's length lies along its own tool axis, that the tip is the point the tool hangs from less the offset turned with the tool, and the Jacobian against differences of the inverse. tests/kins-switch puts the tip on a programmed point through motion with the head tilted. Both fail without the change. Reported by Sigma1912. --- docs/src/gcode/g-code.adoc | 31 +++-- docs/src/man/man9/kins.9.adoc | 3 +- docs/src/motion/5-axis-kinematics.adoc | 2 + docs/src/motion/kinematics-conventions.adoc | 20 +++ docs/src/motion/switchkins.adoc | 6 +- src/emc/kinematics/kins_util.c | 140 ++++++++++++++++++-- tests/kins-params/check.py | 119 ++++++++++++++++- tests/kins-params/checkresult | 2 + tests/kins-params/paritycheck.c | 8 ++ tests/kins-params/test.sh | 54 +++++++- tests/kins-switch/test-ui.py | 32 +++++ 11 files changed, 387 insertions(+), 30 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 9acf12f7964..99f87cc166f 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1642,17 +1642,26 @@ in the numbered parameters '5081-5089'. The numbered parameters change ('M6') and 'G10 L1'/'L10'/'L11'. Some kinematics modules apply the tool length themselves, along the tool -axis rather than along Z (5axiskins, maxkins, the trt modules, genhexkins, -pentakins; see the <> -chapter). 'G43' does not move the machine under those either. With the -tool axis tilted at the time, the programmed coordinates of the point the -tool stands on change by more than the offset difference: the interpreter -works them out through the kinematics, so the next move starts from where -the tool tip actually is. Motion reads the point from the joints the same -way and stops the program if the two disagree. A module that can only be -evaluated in realtime leaves the interpreter with the offset difference -alone; under one of those, change the tool length with the tool axis -untilted, motion stops the program otherwise. +axis rather than along Z (5axiskins, maxkins, the trt modules, the trsrn +modules, xyzab_tdr_kins, genhexkins, pentakins; see the +<> chapter). 'G43' does +not move the machine under those either. With the tool axis tilted at the +time, the programmed coordinates of the point the tool stands on change by +more than the offset difference: the interpreter works them out through the +kinematics, so the next move starts from where the tool tip actually is. +Motion reads the point from the joints the same way and stops the program +if the two disagree. A module that can only be evaluated in realtime leaves +the interpreter with the offset difference alone; under one of those, +change the tool length with the tool axis untilted, motion stops the +program otherwise. + +Of those, the modules that also report where the tool points (5axiskins, +the trt modules, the trsrn modules) apply an X and Y of the offset along +the tool's own X and Y, as they apply the length along the tool axis. The +offset is given in the tool's axes, which are the machine's with the +rotaries at zero, so a tool held off the spindle axis keeps its offset as +the head or the table turns. Under the identity and the other modules, X +and Y apply along the work axes. [NOTE] 'G43 H0' is a little special. Its behavior is different on random diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 01cef58cc73..4b48ec336a7 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -421,7 +421,8 @@ expected by it (XYZBCW `->` joints 0..5) stays on the programmed point as B and C move. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read only until motion has sent anything. A tool offset change with B tilted - moves the programmed point, not the joints. A tool length in the W column of the tool + moves the programmed point, not the joints. An X and Y of the offset are + applied along the tool's own X and Y. A tool length in the W column of the tool table reaches the same place, once a block commands W, and adds to this one rather than replacing it. Put a given length in one column or the other, not both. diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 1c6e0fe406c..6049ec87d93 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -323,6 +323,8 @@ image::5-axis-figures/equation__38.png[align="center"] Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. +An X and Y in the offset, a tool held off the spindle axis, stay with the tool: along the machine's X and Y, however the table turns the part. + Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: [source,hal] diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index d97d7c8d01e..26e94c48474 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -587,6 +587,18 @@ code says once if the pin is left disagreeing with it, which is a config setting a tool length where the tool table should. Motion references the call weakly, so a module written before it still loads and keeps its pin. +The offset in the block is the whole of it, X, Y and Z in the tool's own axes, +which are the machine's with the rotaries at zero, and canon adds all three to +the pose along the work axes. A type's maths takes the length back off and +applies it along the tool axis, and leaves X and Y alone. The shared code turns +those with the tool: the pose the maths answers gains `(I - R)(x, y, 0)`, `R` +the tool frame in work coordinates, and the pose handed to the inverse loses +it, settled on the joints the inverse finds; the Jacobian gets the matching +term. So a tool held off the spindle axis keeps its offset in the tool frame, +as the length does along the tool axis. Nothing is turned on a type flagged +identity or machine, neither of which applies the tool, nor for a module that +reports no tool frame, where X and Y stay on the work axes. + Outside realtime a module exports `kinsDescribe()`, which hands a caller its table and the ops of each type; the caller fills a block from wherever it likes and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, @@ -641,6 +653,14 @@ Tool X:: Derive the value from the module's own rotation matrices, and write those matrices down in the module. +Tool offset:: + Apply the length, `p->tool.tran.z`, along the tool axis, and report the tip + plus the length along work Z, which is what canon added. Leave X and Y of + the offset to the shared code, which turns them with the tool frame the + module reports; that frame's third column has to be the axis the length is + applied along. A type that leaves the tool out of its maths, a robot's + flange, is flagged machine. + Orientation inverse:: A module that reports its frames gets it from the shared search and needs to do nothing. Register a closed form only where one exists, and where it does, diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index db9e4c99d11..87d3d3d0908 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -635,7 +635,11 @@ keeps the joints where they are and reads the point back from them under the new offset, so a tool length change with the tool axis tilted moves the programmed point rather than the machine; the interpreter reads the point the same way through the non-realtime -loader, and motion stops the program if the two disagree. A kinstype +loader, and motion stops the program if the two disagree. The X and +Y of the offset the shared code turns with the tool frame a kinstype +reports, so they lie along the tool's own X and Y as the length lies +along the tool axis; a kinstype flagged identity or machine, or one +without a tool frame, leaves them on the work axes. A kinstype registered the older way reads its own pins and is not affected. === Module main program diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index b66b3d9b9e7..12339ab59e3 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1670,6 +1670,90 @@ const kins_ops KINS_IDENTITY_OPS = { .identity = 1, }; +//---------------------------------------------------------------------- +// the tool offset across the tool axis +//---------------------------------------------------------------------- + +// The tool offset is given in the tool frame, and canon adds all of it to +// the pose along the work axes. A type's maths takes the length back off +// and applies it along the tool axis; X and Y it leaves, so they would +// stay on the work axes however the tool is turned. The shared code turns +// them with the tool: the pose is the module's plus (I - R)(x, y, 0), R +// the tool frame in work coordinates. Nothing to turn without an X or Y, +// on a type that is the identity or the machine frame, neither of which +// applies the tool, or for a module that reports no tool frame. +static int tool_across_applies(const kins_ops *ops, const kins_params *p) +{ + return (p->tool.tran.x != 0.0 || p->tool.tran.y != 0.0) + && !ops->identity && !ops->machine && ops->tool; +} + +// (I - R)(x, y, 0) at these joints, or -1 where the frames cannot say +static int tool_across(const kins_ops *ops, const kins_params *p, + const double *joint, PmCartesian *d) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix work, r; + const double x = p->tool.tran.x, y = p->tool.tran.y; + + if (kinsOpsToolFrame(ops, p, joint, &r, &fflags)) { return -1; } + // where nothing turns the work, the work frame is the machine's + if (kinsOpsWorkFrame(ops, p, joint, &work, &fflags) == 0 + && toolFrameInWork(&work, &r, &r)) { return -1; } + d->x = x - (r.x.x * x + r.y.x * y); + d->y = y - (r.x.y * x + r.y.y * y); + d->z = - (r.x.z * x + r.y.z * y); + return 0; +} + +static void pose_shift(EmcPose *pos, const PmCartesian *d, double sign) +{ + pos->tran.x += sign * d->x; + pos->tran.y += sign * d->y; + pos->tran.z += sign * d->z; +} + +// The Jacobian of the module's maths, at the pose that maths has the +// joints at, turned into the Jacobian of the pose with the offset across. +// The turn d follows the joints, and the joints the pose less d, so +// dq = J (dP - D dq) with D the change of d per joint, and J (I - D J) is +// the answer: D J reaches only from the rotaries into X Y Z, so it squares +// to nothing and the series stops there. +static int tool_across_jacobian(const kins_ops *ops, const kins_params *p, + const double *joint, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double q[EMCMOT_MAX_JOINTS], D[3][EMCMOT_MAX_JOINTS], DJ[3][EMCMOT_MAX_AXIS]; + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + PmCartesian dp, dm; + int i, a, b; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { q[i] = joint[i]; } + for (i = 0; i < n; i++) { + q[i] = joint[i] + KINS_JACOBIAN_STEP; + if (tool_across(ops, p, q, &dp)) { return -1; } + q[i] = joint[i] - KINS_JACOBIAN_STEP; + if (tool_across(ops, p, q, &dm)) { return -1; } + q[i] = joint[i]; + D[0][i] = (dp.x - dm.x) / (2 * KINS_JACOBIAN_STEP); + D[1][i] = (dp.y - dm.y) / (2 * KINS_JACOBIAN_STEP); + D[2][i] = (dp.z - dm.z) / (2 * KINS_JACOBIAN_STEP); + } + for (a = 0; a < 3; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { + DJ[a][b] = 0; + for (i = 0; i < n; i++) { DJ[a][b] += D[a][i] * jac[i][b]; } + } + } + for (i = 0; i < n; i++) { + double row[3] = { jac[i][0], jac[i][1], jac[i][2] }; + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { + jac[i][b] -= row[0] * DJ[0][b] + row[1] * DJ[1][b] + row[2] * DJ[2][b]; + } + } + return 0; +} + //---------------------------------------------------------------------- // asking an ops table, defaults applied //---------------------------------------------------------------------- @@ -1679,20 +1763,25 @@ int kinsOpsForward(const kins_ops *ops, const kins_params *p, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) { - int r; + PmCartesian d; + int r, across, seeded = 0; if (!ops || !ops->forward || !p || !s) { return -1; } + across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; if (ops->fwd_iterates && s->have_pose_seed) { /* no pose of our own yet: start from the caller's estimate, which stays in *pos, rather than from a never-solved seed */ - if (s->pose_seed_ok) { *pos = s->pose_seed; } + if (s->pose_seed_ok) { *pos = s->pose_seed; seeded = 1; } s->have_pose_seed = 0; } + // the caller's estimate, as the module's maths sees it + if (ops->fwd_iterates && across && !seeded) { pose_shift(pos, &d, -1); } r = ops->forward(p, s, joint, pos, fflags, iflags); if (ops->fwd_iterates && r == 0) { /* keep the result only when the solve succeeds */ s->pose_seed = *pos; s->pose_seed_ok = 1; } + if (r == 0 && across) { pose_shift(pos, &d, 1); } return r; } @@ -1701,8 +1790,33 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags) { + double seed[EMCMOT_MAX_JOINTS]; + PmCartesian used = { 0, 0, 0 }, d; + EmcPose q; + int r, pass, n; + if (!ops || !ops->inverse || !p || !s) { return -1; } - return ops->inverse(p, s, pos, joint, iflags, fflags); + if (!tool_across_applies(ops, p)) { + return ops->inverse(p, s, pos, joint, iflags, fflags); + } + // a module may read where the machine is from the joints it is + // handed, so every pass starts from the caller's + n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + memcpy(seed, joint, n * sizeof(seed[0])); + r = ops->inverse(p, s, pos, joint, iflags, fflags); + // the turn follows the joints and the joints the turn: one more pass + // settles it, the tool turning with the rotaries the pose names + for (pass = 0; r == 0 && pass < 4; pass++) { + if (tool_across(ops, p, joint, &d)) { break; } + if (fabs(d.x - used.x) < 1e-12 && fabs(d.y - used.y) < 1e-12 + && fabs(d.z - used.z) < 1e-12) { break; } + used = d; + q = *pos; + pose_shift(&q, &used, -1); + memcpy(joint, seed, n * sizeof(seed[0])); + r = ops->inverse(p, s, &q, joint, iflags, fflags); + } + return r; } int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, @@ -1736,17 +1850,27 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; KINEMATICS_FORWARD_FLAGS ffl = 0; - EmcPose q; - int j, a; + EmcPose at, q; + PmCartesian d; + int j, a, r, across; if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } - if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + // the module's maths has these joints at the pose less the turn of + // the offset across the tool + at = *pos; + across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; + if (across) { pose_shift(&at, &d, -1); } + if (ops->jacobian) { + r = ops->jacobian(p, joint, &at, jac, iflags); + if (r || !across) { return r; } + return tool_across_jacobian(ops, p, joint, jac); + } if (!ops->inverse) { return -1; } // the same differences as kinsJacobianFromInverse(), on the block form kj_zero(jac); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - q = *pos; + q = at; for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } *kj_coord(&q, a) += KINS_JACOBIAN_STEP; @@ -1759,7 +1883,7 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); } } - return 0; + return across ? tool_across_jacobian(ops, p, joint, jac) : 0; } // kinsOpsJacobian() //---------------------------------------------------------------------- diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index ca2b15df9a9..5a514e9d8a4 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -9,9 +9,10 @@ # match what paritycheck published, to rounding, or the module is not # the pure function of its parameters it claims to be. # -# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT TOOL # POSE and JNT are comma separated numbers, as given to paritycheck; -# COORDS and SPARM are a dash when the module was loaded without them. +# COORDS and SPARM are a dash when the module was loaded without them; +# TOOL is the x,y,z offset paritycheck handed the module, or a dash. import ctypes import os @@ -47,6 +48,7 @@ class EmcPose(ctypes.Structure): # "-" where it has no such pair, "no-frame" where it reports no tool frame orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" posejoint_in = sys.argv[10] if len(sys.argv) > 10 else "no-frame" +tool_in = [float(v) for v in sys.argv[11].split(",")] if len(sys.argv) > 11 and sys.argv[11] != "-" else None pose_in += [0.0] * (AXES - len(pose_in)) jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) @@ -85,6 +87,15 @@ def fail(what): fail("%s has no type %d in the block form" % (module, ktype)) sys.exit(1) +# the offset realtime was handed, as motion hands it over +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +def set_tool(xyz): + t = EmcPose() + t.x, t.y, t.z = xyz + kins.kinematicsUserSetTool(ctx, ctypes.byref(t)) +if tool_in: + set_tool(tool_in) + def pose_of(values): p = EmcPose() for n, v in zip("xyzabcuvw", values): @@ -135,8 +146,8 @@ def compare(what, ours, theirs): # the caller's tool wins over the module's pin: for a module with a tool # entry, a length of the caller's must move the inverse, and handing the -# tool back to HAL must return it to what realtime found -kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +# tool back to HAL must return it to what realtime found, which is the +# module's pin unless realtime was handed a tool as well tool_pin = None for name in ("tool-offset", "tool-offset-z"): try: @@ -144,7 +155,7 @@ def compare(what, ours, theirs): tool_pin = name except RuntimeError: pass -if r_inv == 0 and rc_inv == 0 and tool_pin: +if r_inv == 0 and rc_inv == 0 and tool_pin and not tool_in: P = pose_of(pose_in) if frompose else F T = pose_of([0.0] * AXES) T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 @@ -199,6 +210,104 @@ class Rot(ctypes.Structure): else: print("kins-params: %s type %d pose joint %s" % (module, ktype, got)) +# The offset across the tool. The offset is the tool's own, given against +# the machine with the rotaries at zero, and X and Y of it turn with the +# tool as the length does along the tool axis: the tip is the point the +# tool hangs from, the forward with no tool, less the offset turned into the +# work frame, and it has to be on the pose less the offset, which is what +# canon commands. For that the module's length has to lie along its own +# tool axis, which is checked first. +def frame_rows(fn, j): + r = Rot() + if fn(ctx, Joints(*j), ctypes.byref(r)): + return None + return [[r.x[i], r.y[i], r.z[i]] for i in range(3)] + +def tool_in_work(j): + t = frame_rows(kins.kinematicsUserToolFrame, j) + w = frame_rows(kins.kinematicsUserWorkFrame, j) + if w is None: + return t + return [[sum(w[k][a] * t[k][b] for k in range(3)) for b in range(3)] for a in range(3)] + +def forward(j, xyz): + set_tool(xyz) + p = pose_of(pose_in) + r = kins.kinematicsUserForward(ctx, Joints(*j), ctypes.byref(p)) + return p if r == 0 else None + +def expect(what, got, want): + if not close(got, want): + fail("%s: %.12g, expected %.12g" % (what, got, want)) + +def settled_inverse(P, seed): + # a module may take where the machine is from the joints it is handed, + # so run to the joints that explain themselves + q = Joints(*seed) + for _ in range(4): + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), q) != 0: + return None + return list(q) + +kins.kinematicsUserWorkFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +if tool_in and has_frame and r_fwd == 0 and r_inv == 0: + before = failures + j0 = list(qi) if frompose else list(jnt_in) + T = tool_in + xyz = lambda p: [p.x, p.y, p.z] + + # the tool's own axes are the machine's with the rotaries at zero + zero = frame_rows(kins.kinematicsUserToolFrame, [0.0] * MAX_JOINTS) + if max(abs(zero[a][b] - (a == b)) for a in range(3) for b in range(3)) > 1e-12: + fail("the tool frame at zero is not the machine's: %s" % zero) + + # the length along the tool axis the frame reports + R = tool_in_work(j0) + L = 7.0 + longer, none = forward(j0, [0.0, 0.0, L]), forward(j0, [0.0, 0.0, 0.0]) + along = [(a - b) / L for a, b in zip(xyz(longer), xyz(none))] + for a in range(3): + expect("the length along the tool axis, %s" % "xyz"[a], along[a], (a == 2) - R[a][2]) + + # the tip, through the inverse and back + C = forward(j0, T) + q = settled_inverse(C, j0) + if C is None or q is None: + fail("forward or inverse with the offset across the tool") + else: + for j in range(joints): + expect("the joints back from the pose with the offset, joint %d" % j, q[j], j0[j]) + R = tool_in_work(q) + hangs = xyz(forward(q, [0.0, 0.0, 0.0])) + set_tool(T) + for a in range(3): + tip = hangs[a] - sum(R[a][b] * T[b] for b in range(3)) + expect("the tip with the offset across the tool, %s" % "xyz"[a], tip, xyz(C)[a] - T[a]) + + # the Jacobian, against the inverse it is the derivative of; the + # loader takes it on the joints the last inverse found, which are q + J = Jac() + if kins.kinematicsUserJacobian(ctx, ctypes.byref(C), J) != 0: + fail("the Jacobian with the offset across the tool") + else: + h = 1e-4 + for a, n in enumerate("xyzabcuvw"): + ends = [] + for sign in (1, -1): + P = pose_of([getattr(C, m) for m in "xyzabcuvw"]) + setattr(P, n, getattr(P, n) + sign * h) + ends.append(settled_inverse(P, q)) + if None in ends: + fail("the inverse beside the pose with the offset across the tool") + break + for j in range(joints): + d = (ends[0][j] - ends[1][j]) / (2 * h) + if abs(d - J[j][a]) > 1e-6 * max(1.0, abs(d)): + fail("the Jacobian [%d][%s] with the offset across the tool: %.9g, differences %.9g" + % (j, n, J[j][a], d)) + if failures == before: + print("kins-params: %s type %d turns the offset across the tool with it" % (module, ktype)) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult index d3eba1a4da0..08d80381330 100755 --- a/tests/kins-params/checkresult +++ b/tests/kins-params/checkresult @@ -1,4 +1,6 @@ #!/bin/sh [ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && [ "$(grep -c 'turns the offset across the tool' "$1")" = "$(grep -c '^=== .* tool ' "$1")" ] \ + && [ "$(grep -c '^=== .* tool ' "$1")" -ge 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c index 1a3ef4664ab..97a43ced391 100644 --- a/tests/kins-params/paritycheck.c +++ b/tests/kins-params/paritycheck.c @@ -22,6 +22,8 @@ * seed (frompose=0) * jnt up to sixteen integers, the joint set (frompose=0) or the * inverse seed (frompose=1) + * tool x y z of a tool offset motion would hand the module, all + * evaluated under it; none when all three are zero */ #include #include @@ -41,6 +43,8 @@ static int pose[EMCMOT_MAX_AXIS] = { 0 }; RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); +static int tool[3] = { 0, 0, 0 }; +RTAPI_MP_ARRAY_INT(tool, 3, "tool offset x y z, as motion hands it over"); static int comp_id = -1; @@ -107,6 +111,10 @@ int rtapi_app_main(void) if (ktype > 0 && kinematicsSwitchable()) { if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } } + if (tool[0] || tool[1] || tool[2]) { + EmcPose t = { { tool[0], tool[1], tool[2] }, 0, 0, 0, 0, 0, 0 }; + if (kinematicsSetTool(&t)) { hal_exit(comp_id); return -1; } + } for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index e2693212c1b..3e56e4f4ffd 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -20,7 +20,7 @@ rm -f oot_switchkins.comp oot_userkins.comp # ONLY= in the environment runs the entries for that module alone run() { local loadrt="$1" setp="$2" parms="$3" ktype="$4" - local module coords sparm joints frompose pose jnt orient posejoint rtparms hal tok + local module coords sparm joints frompose pose jnt orient posejoint tool rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -38,6 +38,8 @@ run() { # the rotary whose sign G53.1 P names, the same three ways; where the # entry gives none, what it gives for the pair posejoint="" + # a tool offset x,y,z both sides evaluate under, as motion hands it over + tool="-" for tok in $parms; do case "$tok" in joints=*) joints=${tok#joints=} ;; @@ -46,6 +48,7 @@ run() { jnt=*) jnt=${tok#jnt=} ;; orient=*) orient=${tok#orient=} ;; posejoint=*) posejoint=${tok#posejoint=} ;; + tool=*) tool=${tok#tool=} ;; esac done [ -n "$posejoint" ] || posejoint=$orient @@ -57,10 +60,14 @@ run() { printf '%s\n' "$setp" printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" # halcmd keeps quotes, so an absent value travels as a dash - printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" "$tool" } > "$hal" - echo "=== $loadrt type ${ktype:-0}" + if [ "$tool" = "-" ]; then + echo "=== $loadrt type ${ktype:-0}" + else + echo "=== $loadrt type ${ktype:-0} tool $tool" + fi halrun -f "$hal" rm -f "$hal" } @@ -162,6 +169,45 @@ setp xyzbca_trsrn_kins.primary-angle 20 setp xyzbca_trsrn_kins.secondary-angle 35" \ "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3" 1 +# a tool offset across the tool, a tool held off the spindle axis: its X +# and Y turn with the tool frame the module reports, the same in realtime +# and outside it +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3 tool=4,3,12" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3 tool=4,3,12" 1 +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3 tool=4,3,7" +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3 tool=4,3,7" +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,4 posejoint=4 tool=4,3,50" 1 +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3 tool=4,3,50" 1 + # polar run "rosekins" "" "joints=3 jnt=10,5,30" diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 00b04d34416..ae2ff77fe0e 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -231,6 +231,38 @@ def tool_length_check(what, before_joints, before_pose, after_joints, after_pose tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) if errors == errors_before: print("a tool length after a point-to-point move keeps the joints too") + +# An offset across the tool, a tool held off the spindle axis: its X and Y +# are the tool's own and lie along the tool's X and Y under the tilt. The +# joints stay and the point moves by the offset less its turn into the tool +# frame; a move then puts the tip, the point the tool hangs from less the +# whole offset turned with the tool, on the point programmed. +errors_before = errors +sin_b, cos_b, sin_c, cos_c = math.sin(b), math.cos(b), math.sin(cc), math.cos(cc) +# 5axiskins' tool frame Rz(C) Ry(-B), its columns +tool_x = [cos_b * cos_c, cos_b * sin_c, sin_b] +tool_y = [-sin_c, cos_c, 0.0] +tool_z = [-sin_b * cos_c, -sin_b * sin_c, cos_b] +T = [4.0, 3.0, L] +turned = [T[0] * tool_x[a] + T[1] * tool_y[a] for a in range(3)] +standing_pose = pose() +across = mdi("G43.1 X4 Y3") +tool_length_check("an offset across the tool under the tilt", after, standing_pose, across, pose(), + [T[a] - turned[a] if a < 2 else -turned[a] for a in range(3)]) +target = [20.0, 5.0, -10.0] +moved = mdi("G0 X%g Y%g Z%g" % tuple(target)) +pivot = hal.get_value("5axiskins.pivot-length") +hangs = [moved[a] + pivot * ((a == 2) - tool_z[a]) for a in range(3)] +tip = [hangs[a] - turned[a] - T[2] * tool_z[a] for a in range(3)] +if max(abs(tip[a] - target[a]) for a in range(3)) > 1e-6: + error("with an offset across the tool the tip is at %s, not %s" + % (" ".join("%.4f" % v for v in tip), " ".join("%.4f" % v for v in target))) +here = pose() +back = mdi("G43.1 X0 Y0") +tool_length_check("taking the offset across the tool out", moved, here, back, pose(), + [turned[a] - T[a] if a < 2 else turned[a] for a in range(3)]) +if errors == errors_before: + print("an offset across the tool turns with the tool") mdi("G43.1 Z0") mdi("G49") From 1ab3bfe63a1131eb7a910aa17bc7a459a90a38d8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 15:58:17 +1000 Subject: [PATCH 20/24] interp: G43.5, the tool axis as a vector G43.5 is G43.4 with one thing more: while it is in effect a G0 or G1 line may give the tool axis as I J K in place of rotary words, the form of a five-axis program written for the tool rather than the machine, Fanuc's tool center point control type 2. The rotaries are solved through the module's tool frame inverse, the solver G53.1 uses, split out as orient_solve(): every orienting joint free, the pose nearest the present one, so a path of vectors runs continuous; on the pole of the primary rotary the free joint stays. The pose enters the move as program rotary words through machine_pose_to_program(), so rotary offsets are right by construction and the move interpolates like any other. The vector is read in the coordinate system the line's X Y Z are in, the tilted plane where one is active, the machine's on a G53 line. A direction is never incremental, so G91 applies to the axis words only, and I J K alone turns the tool in place. No configuration names the rotaries: the same program orients a head, a table and a robot. Refused: a vector with a rotary word, a zero vector, the identity kinematics, a vector on a G53.5 or G53.7 line. Left out: Fanuc's interpolation of the vector between lines, which needs the kinematics inside the planner. The semantics follow the G43.5 greatEndian built for xyzab_tdr_kins on the fork, including that the vector is absolute under G91; this is the generic form. tests/kins-switch covers the vector against rotary words, G91, the pole, a rotary offset and the refusals; docs in g-code.adoc. --- docs/src/gcode/g-code.adoc | 69 ++++- docs/src/gcode/overview.adoc | 2 +- docs/src/motion/kinematics-conventions.adoc | 4 +- docs/src/motion/switchkins.adoc | 7 +- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 17 +- src/emc/rs274ngc/interp_convert.cc | 22 +- src/emc/rs274ngc/interp_internal.cc | 7 +- src/emc/rs274ngc/interp_internal.hh | 2 + src/emc/rs274ngc/interp_setup.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 277 ++++++++++++-------- src/emc/rs274ngc/interp_write.cc | 9 +- src/emc/rs274ngc/rs274ngc_interp.hh | 3 + tests/kins-switch/README | 8 +- tests/kins-switch/test-ui.py | 72 +++++ 15 files changed, 372 insertions(+), 130 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 99f87cc166f..d29dd21df07 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -86,6 +86,7 @@ as the 'L number', and so on for any other letter. |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset |<> |Tool Length Offset on Primary Kinematics +|<> |Tool Length Offset with the Tool Axis as a Vector |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates @@ -1809,13 +1810,79 @@ It is an error if: kinematics, or * any of the 'G43' error conditions holds. +[[gcode:g43.5]] +== G43.5 Tool Length Offset with the Tool Axis as a Vector(((G43.5 Tool Length Offset with the Tool Axis as a Vector))) + +[source,ngc] +---- +G43.5 +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- +---- + +* 'H' - tool number (optional) +* 'I J K' - the direction the tool axis is to point along, on a 'G0' or + 'G1' line while 'G43.5' is in effect + +'G43.5' is '<>' with one thing more: while it is in +effect, a 'G0' or 'G1' line may give the direction of the tool axis as +a vector 'I J K' in place of rotary words, and the interpreter works +out where the rotary joints have to go to point the tool that way. +This is the form of a five-axis program written for the tool rather +than for the machine, the one Fanuc calls tool center point control +type 2, and it runs on any machine whose kinematics module supplies its +tool frame (see the <> chapter): the same program orients a tilting head, a +tilting table and a robot. + +The vector points from the tool tip towards the holder, in the +coordinate system the line's 'X Y Z' are in: the active coordinate +system with its rotation, the <> where +one is active, and the machine's on a '<>' line. Its +length does not matter. A word left out is zero, so 'K1' alone is the +tool vertical. The words describe a direction, so they are never +incremental: 'G91' applies to the 'X Y Z' of the line, not to 'I J K', +and 'G91.1' does not apply to them either. A line with 'I J K' and no +axis word turns the tool where it stands. + +Where more than one pose of the rotaries points the tool along the +vector, two on a five-axis machine, the interpreter takes the one +nearest where the rotaries stand inside their travel, every orienting joint free to take +part, so a path of vectors runs continuous from wherever it starts and +a rotary swings the long way round only where the short way runs out of travel. On the pole of the primary +rotary, the tool vertical on a mill, one joint no longer matters to the +direction and it stays where it is. + +The pose the interpreter finds goes into the move as ordinary rotary +words, in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +along the way; a program that needs the tool to sweep between two +directions writes the lines in between. + +'G2' and 'G3' keep 'I J K' as the arc centre; the tool direction on an +arc is given with rotary words. 'G43', 'G43.1', 'G43.2' and 'G49' end +the vector form along with the offset they replace; 'G49' cancels the +offset and undoes the kinematics switch, as after 'G43.4'. + +It is an error if: + +* 'I J K' are given together with a rotary word on the same line, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* the direction cannot be reached by the rotary joints, +* 'I J K' are given on a 'G53.5' or 'G53.7' line, or +* any of the 'G43.4' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation 'G49' also switches a switchable kinematics module back to its identity -kinematics when it cancels a 'G43.4', undoing the switch that made. It +kinematics when it cancels a 'G43.4' or 'G43.5', undoing the switch that made. It leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does after a plain 'G43', and a module that declares no identity kinematics gets the plain cancel. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 8144448f898..8a5c6952e18 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -986,7 +986,7 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 +|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G43.5, G49 |Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 26e94c48474..ac870fbd2a2 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -566,8 +566,8 @@ tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks the no-transform type, joints are axes; `primary` the working transform -`G43.4` switches to; `machine` the machine frame type `G13.1` and `G49` -cancel to and `G53.5` moves in, which a module leaves unset when its +`G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and +`G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame (see the Switchable Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 87d3d3d0908..ce9f6d52ce7 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -226,9 +226,10 @@ see Code Notes for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length offset and switches to the kinstype the module declares its working -transform, and the 'G49' that cancels it switches back to identity. -See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' -and 'G13.1', for the full description. +transform, 'G43.5' does the same and lets the lines after it give the +tool axis as a vector, and the 'G49' that cancels either switches back +to identity. See the G-code documentation for 'G43.4', 'G43.5' and +'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index c3518d7cad4..d2f52a2c2ac 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,7 +91,7 @@ const int Interp::gees[] = { /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 380 */ -1,-1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 400 */ 7,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, -/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8, 8,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 25a81f4e17b..728140ae9dc 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -311,15 +311,22 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4), - _("H word with no G43, G43.4 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_5), + _("H word with no G43, G43.4, G43.5 or G76 to use it")); } // G68.2 and G68.4 take I J K, P and Q; G68.3 only R, X Y Z; G69 nothing int plane_words = block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4; int plane_r = plane_words || block->g_modes[GM_WORK_PLANE] == G_68_3; - if (block->i_flag) { /* could still be useless if yz_plane arc */ + // under G43.5 the I J K of a G0 or G1 line are the tool axis: on the + // line that puts G43.5 in effect, and on the lines after it that do + // not change the tool length mode + int tool_vector = (block->g_modes[GM_TOOL_LENGTH_OFFSET] == G_43_5 + || (_setup.tool_vector && block->g_modes[GM_TOOL_LENGTH_OFFSET] == -1)) + && (motion == G_0 || motion == G_1); + + if (block->i_flag && !tool_vector) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && @@ -329,7 +336,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G68.4, G76, or G87 to use it")); } - if (block->j_flag) { /* could still be useless if xz_plane arc */ + if (block->j_flag && !tool_vector) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && @@ -344,7 +351,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } } - if (block->k_flag) { /* could still be useless if xy_plane arc */ + if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && !plane_words), _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 9c6d0c74863..91a217e78d1 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4220,7 +4220,8 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { if (((settings->active_g_codes[9] == G_43) || - (settings->active_g_codes[9] == G_43_4)) && ONCE(STEP_RETAIN_G43)) { + (settings->active_g_codes[9] == G_43_4) || + (settings->active_g_codes[9] == G_43_5)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); @@ -5748,13 +5749,22 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + // under G43.5 the I J K of a G0 or G1 line are the tool axis, which the + // rotaries are solved for once the line's other words are read + bool tool_vector = settings->tool_vector && (move == G_0 || move == G_1) + && (block->i_flag || block->j_flag || block->k_flag); if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { // the words name joints, by letter or by number: nothing below applies + CHKS(tool_vector, _("G43.5: a tool vector cannot go with %s, whose words name the joints"), + (block->g_modes[GM_MODAL_0] == G_53_5) ? "G53.5" : "G53.7"); CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); return INTERP_OK; } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); + if (tool_vector) { + CHP(tool_vector_ends(block, settings, &AA_end, &BB_end, &CC_end)); + } if (move == G_1) { inverse_time_rate_straight(end_x, end_y, end_z, @@ -6721,11 +6731,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); - if (g_code == G_43_4) { + if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With - // no kinematics attached there is nothing to switch to. + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); if (primary >= 0 && primary != settings->kins_type) { // the switch keeps the joints and moves the point, so the point @@ -6744,9 +6755,10 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // the offset in effect is no longer G43.4's, so G49 has no switch to undo settings->kins_by_g43_4 = false; } + settings->tool_vector = (g_code == G_43_5); if (g_code == G_49) { idx = 0; - } else if (g_code == G_43 || g_code == G_43_4) { + } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6826,7 +6838,7 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, G43.5, or G49"); } // The machine does not move, so the program coordinates of the point change. // A kinematics that applies the offset itself moves the point by more than diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 7229f324eaa..404cc2c7864 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -216,8 +216,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c if (block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_1) { block->motion_to_be = settings->motion_mode; } - } else if (!axis_flag && !polar_flag && ijk_flag && (settings->motion_mode == G_2 || settings->motion_mode == G_3)) { - // this is a block like simply "i1" which should be accepted if we're in arc mode + } else if (!axis_flag && !polar_flag && ijk_flag && + (settings->motion_mode == G_2 || settings->motion_mode == G_3 || + (settings->tool_vector && (settings->motion_mode == G_0 || settings->motion_mode == G_1)))) { + // this is a block like simply "i1" which should be accepted if we're in arc mode, + // or a tool vector alone under G43.5, which turns the tool where it stands block->motion_to_be = settings->motion_mode; } CHKS((polar_flag && block->motion_to_be == -1), _("Polar coordinates can only be used for motion")); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index cd635f539ae..34eed97347a 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -260,6 +260,7 @@ enum GCodes G_43_1 = 431, G_43_2 = 432, G_43_4 = 434, + G_43_5 = 435, G_49 = 490, G_50 = 500, G_51 = 510, @@ -834,6 +835,7 @@ struct setup bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo + bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis bool toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 26ba4049f32..dd145f94c9b 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -139,6 +139,7 @@ setup::setup() : kinsSwitch_flag(0), kins_type(0), kins_by_g43_4(false), + tool_vector(false), toolchange_flag(0), home_flag(0), input_index(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index b6a58493d39..9c1b777edd2 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -786,20 +786,32 @@ int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const doub return INTERP_OK; } -// a direction of the plane in world coordinates: the plane's rotation -// then the XY rotation of the coordinate system it sits on -static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +// a direction the program gives, in world coordinates: through the +// tilted work plane's rotation where one is active, then the XY rotation +// of the coordinate system it sits on +static void direction_in_world(setup_pointer s, const double v[3], double rotation_xy, PmCartesian *out) { - double x = s->g68_rotation[0][column]; - double y = s->g68_rotation[1][column]; - double z = s->g68_rotation[2][column]; + double x = v[0], y = v[1], z = v[2]; double t = rotation_xy * M_PI / 180.0; + if (s->g68_active) { + x = s->g68_rotation[0][0] * v[0] + s->g68_rotation[0][1] * v[1] + s->g68_rotation[0][2] * v[2]; + y = s->g68_rotation[1][0] * v[0] + s->g68_rotation[1][1] * v[1] + s->g68_rotation[1][2] * v[2]; + z = s->g68_rotation[2][0] * v[0] + s->g68_rotation[2][1] * v[1] + s->g68_rotation[2][2] * v[2]; + } out->x = x * cos(t) - y * sin(t); out->y = x * sin(t) + y * cos(t); out->z = z; } +// a direction of the plane in world coordinates +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double v[3] = { column == 0 ? 1.0 : 0.0, column == 1 ? 1.0 : 0.0, column == 2 ? 1.0 : 0.0 }; + + direction_in_world(s, v, rotation_xy, out); +} + static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) { // Rodrigues, for a unit axis @@ -886,6 +898,102 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } +// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 +// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, +// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the +// pose on #<_orient_x> and kin (Heidenhain STAY). P and Q are orient_solve()'s. +int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + int p, q, i; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot orient the tool with cutter radius compensation on")); + p = block->p_flag ? (int)round(block->p_number) : 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), + _("P word with %s must be 0, 1 or 2"), name); + q = block->q_flag ? (int)round(block->q_number) : 0; + CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), + _("Q word with %s must be 0 or 1"), name); + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHP(current_joints(s, ctx, now)); + + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + CHP(orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name)); + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &end_pose, end_prog); + + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + // An angular axis letter of a machine pose, 3 A to 8 W. static double pose_letter(const EmcPose *pose, int n) { @@ -969,55 +1077,30 @@ static int orient_fit(setup_pointer s, KinematicsUserContext *ctx, const EmcPose return kept; } -// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 -// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, -// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the -// pose on #<_orient_x> and kin (Heidenhain STAY). P picks the pose, nearest -// first or by the sign of the tilting joint; Q0 holds the joints that carry -// the work (COORD ROT), Q1 frees them (TABLE ROT). -int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +// The joints that point the tool along axis, and its x along xdir where given, +// from the joints the machine is at: every pose the module reports, on the +// nearest turn inside the travel, ranked by rotary travel. P picks by rank or by +// the sign of the tilting joint; Q0 holds the joints that carry the work +// (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name) { - void *vctx; - KinematicsUserContext *ctx; - double now[EMCMOT_MAX_JOINTS]; + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; double spin[TOOL_FRAME_MAX_SOLUTIONS]; double distance[TOOL_FRAME_MAX_SOLUTIONS]; int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; - PmCartesian axis, xdir; - EmcPose end_pose; - double end_prog[9]; unsigned int held = 0; - int p, q, n, i, j, chosen, njoints; + int n, i, j, chosen, njoints; bool reached; - const double *sol; - const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; - - CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); - CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), - _("Cannot orient the tool with cutter radius compensation on")); - p = block->p_flag ? (int)round(block->p_number) : 0; - CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), - _("P word with %s must be 0, 1 or 2"), name); - q = block->q_flag ? (int)round(block->q_number) : 0; - CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), - _("Q word with %s must be 0 or 1"), name); - CHP(kins_context(s, &vctx)); - ctx = (KinematicsUserContext *)vctx; - CHKS((kinematicsUserIsIdentity(ctx)), - _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); njoints = kinematicsUserGetNumJoints(ctx); - CHP(current_joints(s, ctx, now)); EmcPose seed = {}; current_machine_pose(s, &seed); - - plane_axis_in_world(s, 2, s->rotation_xy, &axis); - plane_axis_in_world(s, 0, s->rotation_xy, &xdir); if (q == 0) { if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } } - n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); reached = (n > 0); if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } @@ -1026,15 +1109,15 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) // leaving the plane's X to the coordinate system, or a table that // can place it pins the pose and P has nothing to choose from held = 0; - n = kinematicsUserToolFrameInverse(ctx, &axis, NULL, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, NULL, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); reached = reached || (n > 0); if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } } CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); CHKS((n == 0 && reached), - _("%s: every pose that reaches the plane's normal puts a rotary outside its travel"), name); - CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + _("%s: every pose that reaches the direction puts a rotary outside its travel"), name); + CHKS((n == 0), _("%s: the direction asked for cannot be reached by the rotary joints"), name); // nearest first, by rotary travel in joint units for (i = 0; i < n; i++) { @@ -1067,70 +1150,56 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), name, p, sign_joint, (p == 1) ? "positive" : "negative"); } - sol = solutions + chosen * njoints; - - // where that puts the machine, and what the program calls it - end_pose = (EmcPose){}; - current_machine_pose(s, &end_pose); - { - double full[EMCMOT_MAX_JOINTS]; - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } - CHKS((kinematicsUserForward(ctx, full, &end_pose) != 0), - _("%s: the kinematics cannot place the orientation it found"), name); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = full[i]; } - } - machine_pose_to_program(s, &end_pose, end_prog); - - if (code == G_53_2) { - // STAY: solve only, nothing moves. The pose goes to the named - // parameters #<_orient_x> and kin and to #5071-#5080, for the - // program to use in a move of its own making, the way - // Heidenhain's STAY fills Q120-122. The machine state does not - // change. - for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } - s->orient_valid = true; - for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } - s->parameters[5080] = 1.0; - return INTERP_OK; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + joints[i] = (i < njoints) ? solutions[chosen * njoints + i] : 0.0; } + return INTERP_OK; +} - write_canon_state_tag(block, s); - if (code == G_53_1) { - // the rotaries alone: the linear joints are where they are, since - // the solver left them at the seed, and the tool goes wherever - // that carries it - JOINT_TRAVERSE(block->line_number, sol, 1, - end_prog[0], end_prog[1], end_prog[2], - end_prog[3], end_prog[4], end_prog[5], - end_prog[6], end_prog[7], end_prog[8]); - s->current_x = end_prog[0]; - s->current_y = end_prog[1]; - s->current_z = end_prog[2]; - } else if (code == G_53_6) { - // the tool centre point stays: a Cartesian move of the rotaries - STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); +// G43.5: I J K on a G0 or G1 line are the tool axis, tip towards holder, in +// the coordinate system the line's X Y Z are in. The rotaries come from the +// tool frame inverse, every orienting joint free, the nearest pose, as program +// rotary coordinates so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, double *b, double *c) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + double v[3], prog[9]; + PmCartesian axis; + EmcPose pose; + int i; + + CHKS((block->a_flag || block->b_flag || block->c_flag), + _("G43.5: a tool vector and rotary words on one line give the orientation twice")); + v[0] = block->i_flag ? block->i_number : 0.0; + v[1] = block->j_flag ? block->j_number : 0.0; + v[2] = block->k_flag ? block->k_number : 0.0; + CHKS((vec_norm(v) < 1e-9), _("G43.5: the tool vector I J K is zero")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, now)); + if (block->g_modes[GM_MODAL_0] == G_53) { + axis.x = v[0]; + axis.y = v[1]; + axis.z = v[2]; } else { - double x = block->x_flag ? block->x_number : s->current_x; - double y = block->y_flag ? block->y_number : s->current_y; - double z = block->z_flag ? block->z_number : s->current_z; - - JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); - s->current_x = x; - s->current_y = y; - s->current_z = z; - } - s->AA_current = end_prog[3]; - s->BB_current = end_prog[4]; - s->CC_current = end_prog[5]; - if (code == G_53_1) { - s->u_current = end_prog[6]; - s->v_current = end_prog[7]; - s->w_current = end_prog[8]; + direction_in_world(s, v, s->rotation_xy, &axis); } + CHP(orient_solve(s, ctx, &axis, NULL, 0, 1, now, sol, "G43.5")); + + // where that puts the rotaries, and what the program calls it + pose = (EmcPose){}; + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, sol, &pose) != 0), + _("G43.5: the kinematics cannot place the orientation it found")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &pose, prog); + *a = prog[3]; + *b = prog[4]; + *c = prog[5]; return INTERP_OK; } diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 63dc9b73b0b..efb8c2789be 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -111,16 +111,17 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); // the kins type, not the label, is the authority: a G43 given on the - // module's primary type shows as G43.4, and the label follows the type - // motion reports after a resync. -1 is "no information" and matches - // every flag, so it is excluded before the bit test. + // module's primary type shows as G43.4, or G43.5 while I J K give the + // tool axis, and the label follows the type motion reports after a + // resync. -1 is "no information" and matches every flag, so it is + // excluded before the bit test. kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? - ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? G_43_4 : G_43) : G_49; + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? (settings->tool_vector ? G_43_5 : G_43_4) : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 5cf4dfdcf5f..f6cbaa26c0d 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -378,6 +378,9 @@ public: int work_plane_restore(setup_pointer settings); int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int orient_solve(setup_pointer settings, void *ctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name); + int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, const int flags[9], const double words[9], double *joints); diff --git a/tests/kins-switch/README b/tests/kins-switch/README index 0815a214c3c..e409e2b3cf9 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -10,5 +10,9 @@ that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the kinematics with nothing netted to its pin and, the head being tilted, moves the programmed point rather than the joints, with the interpreter -agreeing on where the point went, and that G13.1 in an ON_ABORT_COMMAND -routine does not swallow the rest of the routine. +agreeing on where the point went, that under G43.5 a tool vector I J K +lands on the joints and the point the rotary words reach, is not +incremental, keeps the free joint on the pole and is refused with a +rotary word, as a zero vector, on the identity kinematics and outside +G43.5, and that G13.1 in an ON_ABORT_COMMAND routine does not swallow +the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index ae2ff77fe0e..1b284ccdf8c 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -266,6 +266,78 @@ def tool_length_check(what, before_joints, before_pose, after_joints, after_pose mdi("G43.1 Z0") mdi("G49") +# ---- G43.5: the tool axis as a vector ------------------------------------ +# +# Under G43.5 a G0 or G1 line gives the direction of the tool axis as I J K +# and the interpreter finds the rotaries. The head's tool axis at B, C is +# (-sin B cos C, -sin B sin C, cos B), so the vector for the tilted pose +# above has to land on the joints and the point the rotary words reach. + +def refused(cmd, needle): + drain() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + 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() + +errors_before = errors +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0") +mdi("G43.5 H1") +s.poll() +if 435 not in s.gcodes: + error("G43.5 is not among the active G-codes %s" % (s.gcodes,)) +by_words = mdi("G0 X10 Y10 Z-5 B-22.5 C45") +by_words_pose = pose() +mdi("G0 X0 Y0 Z0 B0 C0") +vec = (-math.sin(b) * math.cos(cc), -math.sin(b) * math.sin(cc), math.cos(b)) +by_vector = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +if max(abs(by_vector[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("the vector put the joints at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in by_vector), " ".join("%.4f" % v for v in by_words))) +if max(abs(p - q) for p, q in zip(pose(), by_words_pose)) > 1e-5: + error("the vector put the point at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in pose()), " ".join("%.4f" % v for v in by_words_pose))) +# a direction is not incremental +mdi("G91") +held = mdi("G1 X0 I%.9f J%.9f K%.9f F1000" % vec) +mdi("G90") +if max(abs(held[j] - by_vector[j]) for j in range(JOINTS)) > 1e-6: + error("the same vector under G91 moved the joints by %s" + % " ".join("%.4f" % (held[j] - by_vector[j]) for j in range(JOINTS))) +# the pole: the tool vertical leaves C where it is, and the point stays +pole = mdi("G0 K1") +if abs(pole[3]) > 1e-6 or abs(pole[4] - 45) > 1e-3: + error("the tool vertical put B, C at %.4f, %.4f, not 0, 45" % (pole[3], pole[4])) +if max(abs(p - q) for p, q in zip(pose()[:3], by_words_pose[:3])) > 1e-5: + error("the tool vertical moved the point to %s" % " ".join("%.4f" % v for v in pose())) +# a rotary offset renames the angles, the direction is the same: the same +# joints, called something else by the program +mdi("G10 L2 P1 C30") +mdi("G0 X0 Y0 Z0 B0 C0") +offset = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +mdi("G10 L2 P1 C0") +if max(abs(offset[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("under a C offset the vector put the joints at %s, not %s" + % (" ".join("%.4f" % v for v in offset), " ".join("%.4f" % v for v in by_words))) +if errors == errors_before: + print("G43.5 turned the tool along the vector, onto the joints the rotary words reach") +refused("G0 X0 K1 B5", "twice") +refused("G0 X0 I0 J0 K0", "zero") +mdi("G13.1") +refused("G0 X0 K1", "G12.1 first") +mdi("G43.4 H1") +refused("G0 X0 K1", "K word with no") +mdi("G43.5 H1") +mdi("G49") +refused("G0 X0 K1", "K word with no") + # ---- a negative kinematics number is refused ----------------------------- drain() From bbe8b360292a36b406490bf25f58216b54950406 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 18 Sep 2026 22:50:06 +1000 Subject: [PATCH 21/24] interp: G28.5 and G30.5, the machine frame forms of G28 and G30, and MACHINE_MOVES_IN_MACHINE_FRAME G53, G28 and G30 move in the world of the kinematics in force with the offsets removed, and on a table-side kinematics that world turns with the table: G53 X0 Y0 Z0 under TCP with the table at A10 takes the tool tip to the turned origin, which the slides reach elsewhere. Under a tool centre point kinematics G53 also takes the tool length off along machine Z, where the kinematics carries it along the tool, so with the tool tilted the slides land where the tool and the head angle put them. That is what the existing switchkins configs rely on, so it stays; the G53 section says so now, and points at G53.5 for the machine frame. G28.5 and G30.5 are to G28 and G30 what G53.5 is to G53: the stored position is read in the machine frame with the orientation left out, the way G53.5 reads its words, the slides on a machine whose slides line up with its frame, and the move is a joint-space traverse there, with no offset, tool length or work plane. Axis words on the line are a machine frame waypoint, and then only those letters go to their stored values, as G28 does with its words. The stored position has to be saved on the machine frame type, where the world is that frame, or with the option below. [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME, off by default, is for a configuration that wants the machine codes to mean that frame: whenever the kinematics in force is not the machine frame type, G53, G28 and G30 run as G53.5, G28.5 and G30.5, and G28.1 and G30.1 store the machine frame position those read back. The slides then go to the words or the stored position whatever the tool, the head and the table are doing. The codes are swapped as the block is read, so the checks and the errors are the machine frame codes'. The decision is the flags the module declares for the selected type, with the machine's kinematics type as the fallback for a module that switches nothing; a plain identity type that is not the machine frame is read in the machine frame like any other, and on the machine frame type G53 stays a straight move. The letter mapping and the joint move come out of convert_ptp_joints into slide_joints and ptp_joint_move, shared by the three codes, and the check that each letter names joints of its own unit class into letters_name_joints, which the store shares. The twp-native test stores a position on the identity, checks G28.5 and G30.5 with the head tilted under TCP, the waypoint included, sets the option and checks that under TCP G53 lands where G53.5 does on one joint-space move, that G28 and G30 go back to the slides G28.1 and G30.1 stored with the head tilted, and that on the identity the codes are the world's. The ptp-machine-frame test does the same on the slanted module, where G28.5 with the head tilted lands the carriage, and checks the option under the tip and under the plain identity, where G53 Y10 holds machine X. The waypoint check wants the joint to reach its turn, and a status snapshot per task cycle misses a turn by up to 9e-3 mm at the test's deceleration, which failed on a loaded runner: the twp-native test now reads the path of every move from a sampler on the servo thread, logged by halsampler, where the turn falls inside one cycle. --- docs/src/config/ini-config.adoc | 4 + docs/src/gcode/g-code.adoc | 84 +++++++++ docs/src/gcode/overview.adoc | 2 +- docs/src/motion/switchkins.adoc | 38 ++--- src/emc/rs274ngc/interp_array.cc | 4 +- src/emc/rs274ngc/interp_check.cc | 7 + src/emc/rs274ngc/interp_convert.cc | 5 + src/emc/rs274ngc/interp_internal.cc | 2 + src/emc/rs274ngc/interp_internal.hh | 4 + src/emc/rs274ngc/interp_setup.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 243 +++++++++++++++++++++------ src/emc/rs274ngc/rs274ngc_interp.hh | 8 + src/emc/rs274ngc/rs274ngc_pre.cc | 3 + tests/ptp-machine-frame/README | 7 +- tests/ptp-machine-frame/test-ui.py | 40 +++++ tests/ptp-machine-frame/test.ini | 2 + tests/twp-native/README | 14 +- tests/twp-native/sim.hal | 16 ++ tests/twp-native/test-ui.py | 139 +++++++++++++-- tests/twp-native/test.ini | 2 + tests/twp-native/test.sh | 2 +- 21 files changed, 535 insertions(+), 92 deletions(-) diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index ed9acdbbbfe..9b9c40919a3 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -639,6 +639,10 @@ The maximum number of `USER_M_PATH` directories is defined at compile time (typ: When set M2, M30 and an abort no longer cancel a <>. A restart still does. * `DISABLE_FANUC_STYLE_SUB = 0` (Default: 0) If there is reason to disable Fanuc subroutines set it to 1. +* `MACHINE_MOVES_IN_MACHINE_FRAME = 0` (bool, Default: 0) + + When set, 'G53', 'G28' and 'G30' run as 'G53.5', 'G28.5' and 'G30.5' whenever the kinematics in force is not the machine frame type, and 'G28.1' and 'G30.1' store the machine frame position those read back: the slides go to the words or to the stored position whatever the tool, the head and the table are doing. The machine frame type is the type the module flags as such, the identity kinematics on a machine whose slides line up with its frame, and neither the tool centre point nor a raw joint identity on one whose slides do not. On the machine frame type the codes are unchanged. + Unset, these codes work in the world of the kinematics in force, which on a table-side kinematics turns with the table, and under a tool centre point kinematics with the tool tilted puts the slides where the tool and the head angle take them. + See <>. * 'G73_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) Chip breaking back-off distance in machine units * 'G83_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index d29dd21df07..a9de56b53e3 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,6 +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 |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -1072,6 +1073,14 @@ all axes will go to the <>. G28 Z2.5 (rapid to Z2.5 then to Z location specified in #5163) ---- +The 'absolute' position is the machine coordinate system of the kinematics in +force, see <>: on a kinematics that is not the identity it is +that kinematics' world, not the slides, and the stored position is a world +position too. <> is the form that moves the slides, and with +`MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the INI +file 'G28' and 'G28.1' are 'G28.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 @@ -1186,10 +1195,62 @@ if TOOL_CHANGE_AT_G30=1 is in the [EMCIO] section of the INI file. 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. + 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))) + +[source,ngc] +---- +G28.5 +G30.5 +---- + +'G28.5' and 'G30.5' are to 'G28' and 'G30' what <> is to +'G53': the stored position, parameters 5161-5169 for 'G28.5' and 5181-5189 +for 'G30.5', is read in the machine frame with the orientation left out, the +way 'G53.5' reads its words, the slides on a machine whose slides line up +with its frame, and the move is a <> there, +rapid, with no offset, tool length, rotation or work plane applied. The +values are in the machine units of the INI file, as for 'G28'. The stored +position has to be a machine frame position: store it with 'G28.1' or 'G30.1' +while the identity kinematics is in force, where the world is that frame, or +under any kinematics with `MACHINE_MOVES_IN_MACHINE_FRAME` set. + +* 'G28.5' - moves every letter the machine has to the stored position. +* 'G28.5 axes' - moves the letters named by 'axes' to the given machine + frame position first, then the same letters to their stored positions. A + letter not named holds its machine frame coordinate. + +On a mill whose head is tilted and a table-side kinematics in force, 'G28' +takes the tool tip to the stored point, where the head's tilt puts the slides +somewhere else; 'G28.5' takes the slides there, whatever the head is doing, +which is what a park or a tool change position wants. + +.G28.5 Example Line +[source,ngc] +---- +G28.5 Z0 (the Z slide to zero, then to the Z stored in #5163) +G30.5 (every slide to the position stored with G30.1) +---- + +It is an error if: + +* Cutter Compensation is turned on +* Polar coordinates are used, or incremental distance mode is in force. +* An axis letter is used that is not a joint of the kinematics, the + machine's letters name joints of the other kind, or the machine frame + kinematics cannot reach the point, see <>. +* The kinematics module cannot be evaluated by the interpreter. + [[gcode:g33]] == G33 Spindle Synchronized Motion(((G33 Spindle Synchronized Motion))) @@ -1919,6 +1980,29 @@ programmed on the same line if one is currently active. For example 'G53 G0 X0 Y0 Z0' will move the axes to the home position even if the currently selected coordinate system has offsets in effect. +The machine coordinate system is the world of the kinematics in force with the +offsets removed. With the identity kinematics, or a head-side kinematics with +the rotaries at zero, that is the slides. With a table-side kinematics, such as +'xyzac-trt-kins' in TCP or the tilted work plane types of the 'trsrn' +modules, the world turns with the table, so 'G53 X0 Y0 Z0' takes the tool tip +to the origin of that turned frame, which the slides reach at some other +position. Under a tool centre point kinematics 'G53' takes the tool length off +along machine Z, where the kinematics carries the tool along its own axis: 'G53' +puts the point a tool length above the tip at the words, the spindle nose only +while the tool is vertical, so with the tool tilted where the slides land +depends on the tool and the head angle. The move in the machine frame with the +orientation left out, the slides on a machine whose slides line up with its +frame, whatever the kinematics in force, is <>. +A configuration that wants 'G53' to mean that frame sets +`MACHINE_MOVES_IN_MACHINE_FRAME` in the `[RS274NGC]` section of the INI file, +and 'G53', 'G28' and 'G30' then run as 'G53.5', 'G28.5' and 'G30.5', and +'G28.1' and 'G30.1' store the machine frame position, whenever the kinematics +in force is not the machine frame type: the type the module flags as such (see +the <> chapter), the identity +kinematics on a machine whose slides line up with its frame, and neither the +tool centre point nor a raw joint identity on one whose slides do not. On the +machine frame type 'G53' stays a straight move in that frame. + .G53 Example [source,ngc] ---- diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 8a5c6952e18..887f3d48864 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -977,7 +977,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G28.5, G30, G30.5, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index ce9f6d52ce7..476b3c180ab 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -587,25 +587,25 @@ int switchkinsDeclare(int ktype, int flags); ---- G-code reads these declarations: 'G13.1' and 'G49' cancel to the -kinstype declared KINSTYPE_MACHINE, 'G53.5' moves in its world, and -'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever -their numbers, so a module whose -kinematics are not in the conventional order still gets working -spellings. The machine frame type is the machine with the orientation -left out, XYZ the pivot in machine coordinates and the rotary letters -the rotary joints; on a machine whose slides line up with its frame that -is the identity type, so a module that declares no machine frame -kinstype has its identity kinstype stand in, and every shipped module is -of that kind. A module whose carriage does not line up, a slanted slide -or a composite Y on a mill-turn, declares its machine frame kinstype -separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints -really are the axes, since motion and the planner skip the maths on that -flag alone. At most one kinstype may be declared identity, at most one -primary and at most one machine frame, and declaring a kinstype the -module does not provide fails the module load. A module that declares -nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but -'G13.1' and 'G43.4' are an error, since the numbers of the identity and -primary kinematics are then a guess. +kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move +in its world, and 'G43.4' switches to the kinstype declared +KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics +are not in the conventional order still gets working spellings. The +machine frame type is the machine with the orientation left out, XYZ the +pivot in machine coordinates and the rotary letters the rotary joints; +on a machine whose slides line up with its frame that is the identity +type, so a module that declares no machine frame kinstype has its +identity kinstype stand in, and every shipped module is of that kind. A +module whose carriage does not line up, a slanted slide or a composite Y +on a mill-turn, declares its machine frame kinstype separately and keeps +KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since +motion and the planner skip the maths on that flag alone. At most one +kinstype may be declared identity, at most one primary and at most one +machine frame, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +are then a guess. A module that implements kinematicsTypeFlags() itself, without switchkins.c, returns the flags of every kinstype it can switch to, 0 diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index d2f52a2c2ac..b9906756047 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -84,8 +84,8 @@ const int Interp::gees[] = { /* 220 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 240 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 260 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 280 */ 0, 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 -/* 300 */ 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 280 */ 0, 0, 0,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 +/* 300 */ 0, 0,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 320 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1, /* 340 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 728140ae9dc..6db0f3c293d 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -95,6 +95,13 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c CHKS((((block->l_number == 1 || block->l_number == 10 || block->l_number == 11) && p_int < 1)), _("P value out of range with G10 L%d"), block->l_number); } else if (mode0 == G_28) { } else if (mode0 == G_30) { + } else if (mode0 == G_28_5 || mode0 == G_30_5) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G28.5 or G30.5")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G28.5 or G30.5 in incremental distance mode")); } else if (mode0 == G_5_3) { CHKS(((mode1 != G_5_2) && (mode1 != -1)), _("Between G5.2 and G5.3 codes, only additional G5.2 codes are allowed.")); } else if (mode1 == G_5_2){ diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 91a217e78d1..534a8fa8dc1 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -3106,6 +3106,9 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) if(s->cutter_comp_side != CUTTER_COMP::OFF) { ERS(_("Cannot set reference point with cutter compensation in effect")); } + if(s->machine_moves_in_machine_frame && !kins_machine_frame(s)) { + return convert_savehome_slides(code, s); + } double x, y, z; program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); @@ -4514,6 +4517,8 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_setup(block, settings)); } else if ((code == G_28) || (code == G_30)) { CHP(convert_home(code, block, settings)); + } else if ((code == G_28_5) || (code == G_30_5)) { + CHP(convert_home_slides(code, block, settings)); } else if ((code == G_28_1) || (code == G_30_1)) { CHP(convert_savehome(code, block, settings)); } else if (code == G_28_2) { diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 404cc2c7864..971f911c0c9 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,6 +174,7 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode1 = block->g_modes[GM_MOTION]; mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) + || (mode0 == G_28_5) || (mode0 == G_30_5) || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 @@ -360,6 +361,7 @@ int Interp::parse_line(char *line, //!< array holding a line of RS274 code if(settings->skipping_o == NULL) { + machine_frame_codes(block, settings); CHP(enhance_block(block, settings)); CHP(check_items(block, settings)); int n = find_remappings(block,settings); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 34eed97347a..c2284c37460 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -243,8 +243,10 @@ enum GCodes G_28 = 280, G_28_1 = 281, G_28_2 = 282, /* G-code homing cycle (home one/all joints) */ + G_28_5 = 285, G_30 = 300, G_30_1 = 301, + G_30_5 = 305, G_33 = 330, G_33_1 = 331, G_38_2 = 382, @@ -836,6 +838,7 @@ struct setup int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis + bool machine_moves_in_machine_frame; // [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME: G53, G28, G30 run as G53.5, G28.5, G30.5 off the machine frame type bool toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried @@ -1141,4 +1144,5 @@ struct scoped_locale { // the kinematics type carrying a KINSTYPE_ flag, or -1 when the module // declares none (interp_convert.cc) int flagged_kins_type(int flag); + #endif // INTERP_INTERNAL_HH diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index dd145f94c9b..3dd796991c2 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -140,6 +140,7 @@ setup::setup() : kins_type(0), kins_by_g43_4(false), tool_vector(false), + machine_moves_in_machine_frame(false), toolchange_flag(0), home_flag(0), input_index(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 9c1b777edd2..dd4af0b9ee9 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1226,6 +1226,31 @@ int Interp::ptp_seconds(block_pointer block, setup_pointer s, return INTERP_OK; } +// Whether the kinematics in force is the machine frame type, the one G13.1 +// and G49 cancel to: the flags the module declares for the type the program +// selected, else what the machine reports (a module that switches nothing, +// or no machine attached). +bool Interp::kins_machine_frame(setup_pointer s) +{ + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0) { return (flags & KINSTYPE_MACHINE) != 0; } + return GET_EXTERNAL_KINEMATICS_IDENTITY(); +} + +// [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME: off the machine frame type G53, +// G28 and G30 are G53.5, G28.5 and G30.5, the slides at the words whatever +// the tool and the head are doing. On the machine frame type the two read +// the same point, and G53 keeps its straight line. +void Interp::machine_frame_codes(block_pointer block, setup_pointer s) +{ + int *mode0 = &block->g_modes[GM_MODAL_0]; + + if (!s->machine_moves_in_machine_frame) { return; } + if (*mode0 != G_53 && *mode0 != G_28 && *mode0 != G_30) { return; } + if (kins_machine_frame(s)) { return; } + *mode0 = (*mode0 == G_53) ? G_53_5 : (*mode0 == G_28) ? G_28_5 : G_30_5; +} + // The axis letters read as machine frame coordinates, the words the module's // machine frame type answers to: the pivot in machine coordinates, the // rotaries as joints, in program units. The joints are read in on the type @@ -1257,6 +1282,32 @@ int Interp::slide_joints(const char *name, setup_pointer s, void *vctx, const st return status; } +// Where the machine frame type is a plain identity, or the module declares +// no types, the letters name joints: refused where a letter and its joint +// are not of one unit class. +int Interp::letters_name_joints(const char *name, setup_pointer s, const struct kins_params *p, + int njoints) +{ + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + int a, j; + + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("%s: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + name, letters[a], j, turns ? "angular" : "linear", j); + } + } + return INTERP_OK; +} + // the words on the context as set: the module's machine frame type, or the // type in force with letters = joints where the module declares none int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, @@ -1268,20 +1319,7 @@ int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const str int a, j; if (undeclared || kinematicsUserIsIdentity(ctx)) { - CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); - for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); - for (j = 0; j < njoints; j++) { - int turns; - if (!(p->joints_of_axis[a] & (1 << j))) { continue; } - turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; - CHKS((turns != angular), - _("%s: on this machine %c names joint %d, which the INI file" - " declares %s, so the axis letters do not name the joints they" - " look like; give joints by number with G53.7 J%d="), - name, letters[a], j, turns ? "angular" : "linear", j); - } - } + CHP(letters_name_joints(name, s, p, njoints)); } if (!undeclared) { return machine_frame_joints(name, s, ctx, flags, words, joints); @@ -1335,6 +1373,47 @@ int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, return INTERP_OK; } +// The move to a set of joints: where that puts the tool, and what the +// program calls it, then the joint-space traverse or feed there. +int Interp::ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer s, + void *vctx, const double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double prog[9]; + int j; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} + // The two point-to-point codes below G53.4: G53.5 by axis letter in the // machine frame, the module's machine frame type, refused where that type is // a plain identity whose letters name joints of the other unit class; G53.7 @@ -1346,8 +1425,6 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po KinematicsUserContext *ctx; const kins_params *p; double joints[EMCMOT_MAX_JOINTS]; - EmcPose pose; - double prog[9]; const int flags[9] = { block->x_flag, block->y_flag, block->z_flag, block->a_flag, block->b_flag, block->c_flag, block->u_flag, block->v_flag, block->w_flag }; @@ -1373,12 +1450,8 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po given++; } CHKS((given == 0), _("G53.7 needs at least one J= joint word")); - } else { - CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); - } - // the joints of a gantry pair move together: both given, one value - if (code == G_53_7) { + // the joints of a gantry pair move together: both given, one value for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { int bits = p->joints_of_axis[a]; int first = -1; @@ -1392,36 +1465,112 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); } } + } else { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); } - // where that puts the tool, and what the program calls it - current_machine_pose(s, &pose); - CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), - _("%s: the kinematics cannot place those joints"), name); - for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } - machine_pose_to_program(s, &pose, prog); + return ptp_joint_move(name, move, block, s, ctx, joints); +} - write_canon_state_tag(block, s); - if (move == G_0) { - JOINT_TRAVERSE(block->line_number, joints, 1, - prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], - prog[6], prog[7], prog[8]); +// G28.5 and G30.5: the machine frame forms of G28 and G30, as G53.5 is of +// G53. The stored position, parameters 5161 to 5169 or 5181 to 5189, is +// read in the machine frame the way G53.5 reads its words: no offset, no +// work plane, no orientation. Axis words on the line are a machine frame +// point to pass through first, and then only those letters go to the stored +// position; with no words every letter the machine has goes. +int Interp::convert_home_slides(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + const double *parameters = s->parameters; + double joints[EMCMOT_MAX_JOINTS]; + double home[9]; + int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + const char *name = (code == G_28_5) ? "G28.5" : "G30.5"; + const int base = (code == G_28_5) ? 5161 : 5181; + int a, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + home[a] = angular ? USER_TO_PROGRAM_ANG(parameters[base + a]) + : USER_TO_PROGRAM_LEN(parameters[base + a]); + given += flags[a]; + } + + if (given) { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + CHP(ptp_joint_move(name, G_0, block, s, ctx, joints)); } else { - double seconds; - CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], - prog[6], prog[7], prog[8], &seconds)); - JOINT_FEED(block->line_number, joints, 1, - prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], - prog[6], prog[7], prog[8], seconds); + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { flags[a] = p->joints_of_axis[a] != 0; } + } + CHP(slide_joints(name, s, ctx, p, njoints, flags, home, joints)); + return ptp_joint_move(name, G_0, block, s, ctx, joints); +} + +// G28.1 and G30.1 where G28 and G30 run as G28.5 and G30.5: the position +// stored is where the slides stand in the machine frame, the point those +// read back, in the reverse of slide_joints(). +int Interp::convert_savehome_slides(int code, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + const char *name = (code == G_28_1) ? "G28.1" : "G30.1"; + const int base = (code == G_28_1) ? 5161 : 5181; + const int machine = flagged_kins_type(KINSTYPE_MACHINE); + int a, j, njoints; + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (machine >= 0) { + EmcPose pose; + int identity, placed; + CHKS((kinematicsUserSetType(ctx, machine) != 0), + _("%s: kinematics type %d is not available outside realtime"), name, machine); + identity = kinematicsUserIsIdentity(ctx); + current_machine_pose(s, &pose); + placed = (kinematicsUserForward(ctx, joints, &pose) == 0); + // back on the type in force: the caller's context + kinematicsUserSetType(ctx, s->kins_type); + if (identity) { CHP(letters_name_joints(name, s, p, njoints)); } + CHKS((!placed), _("%s: the machine frame kinematics cannot place the current joints"), name); + s->parameters[base + 0] = pose.tran.x; + s->parameters[base + 1] = pose.tran.y; + s->parameters[base + 2] = pose.tran.z; + s->parameters[base + 3] = pose.a; + s->parameters[base + 4] = pose.b; + s->parameters[base + 5] = pose.c; + s->parameters[base + 6] = pose.u; + s->parameters[base + 7] = pose.v; + s->parameters[base + 8] = pose.w; + return INTERP_OK; + } + CHP(letters_name_joints(name, s, p, njoints)); + for (a = 0; a < 9; a++) { + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { break; } + } + s->parameters[base + a] = (j < njoints) ? joints[j] : 0.0; } - s->current_x = prog[0]; - s->current_y = prog[1]; - s->current_z = prog[2]; - s->AA_current = prog[3]; - s->BB_current = prog[4]; - s->CC_current = prog[5]; - s->u_current = prog[6]; - s->v_current = prog[7]; - s->w_current = prog[8]; return INTERP_OK; } diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index f6cbaa26c0d..87a24736249 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -382,18 +382,26 @@ public: int p, int q, const double *now, double *joints, const char *name); int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int convert_home_slides(int code, block_pointer block, setup_pointer settings); + int convert_savehome_slides(int code, setup_pointer settings); + void machine_frame_codes(block_pointer block, setup_pointer settings); + int letters_name_joints(const char *name, setup_pointer settings, const struct kins_params *params, + int njoints); int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, const int flags[9], const double words[9], double *joints); int slide_words(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, int undeclared, const int flags[9], const double words[9], double *joints); int machine_frame_joints(const char *name, setup_pointer settings, void *ctx, const int flags[9], const double words[9], double *joints); + int ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer settings, + void *ctx, const double *joints); int ptp_seconds(block_pointer block, setup_pointer settings, double x, double y, double z, double a, double b, double c, double u, double v, double w, double *seconds); int kins_load(setup_pointer settings); int kins_context(setup_pointer settings, void **ctx); int kins_here(setup_pointer settings, void **ctx); + bool kins_machine_frame(setup_pointer settings); int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, EmcPose *point, bool *known); void kins_set_tool(void *ctx, const EmcPose *offset); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index d0dad43072e..333f766eab2 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1130,6 +1130,9 @@ int Interp::init() // INI file m98/m99 subprogram default setting _setup.disable_fanuc_style_sub = inifile.findBoolV("DISABLE_FANUC_STYLE_SUB", "RS274NGC", false); logDebug("init: DISABLE_FANUC_STYLE_SUB = %d", _setup.disable_fanuc_style_sub); + + // G53, G28, G30, G28.1 and G30.1 in the machine frame off the machine frame type + _setup.machine_moves_in_machine_frame = inifile.findBoolV("MACHINE_MOVES_IN_MACHINE_FRAME", "RS274NGC", false); } } diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README index c7e76a61ea1..c46b86bee68 100644 --- a/tests/ptp-machine-frame/README +++ b/tests/ptp-machine-frame/README @@ -12,5 +12,8 @@ The test checks that G13.1 and G49 select the machine frame type rather than the identity, that G53.5 puts the carriage at a machine frame point, the letters not given holding their machine coordinate and not their joint, from the machine frame type and from under the tilted tip -kinematics alike, and that a plain move after a machine frame move starts -from the right place. +kinematics alike, that G28.5 goes to a position stored on the machine +frame, through a waypoint when given one, that MACHINE_MOVES_IN_MACHINE_FRAME +runs G53, G28 and G30 as G53.5, G28.5 and G30.5 under the tip and under the +plain identity, G28.1 and G30.1 storing the carriage, and that a plain move +after a machine frame move starts from the right place. diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index 28021cc33f5..c08211639aa 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -119,6 +119,46 @@ def expect_type(what, want): expect("G53.5 X20 Y10 Z-5 under the tip", j, carriage(20, 10, -5, 30)) j = mdi("G53.5 G0 B0") expect("G53.5 B0 is the joint", j, carriage(20, 10, -5, 0)) +# and the config reads machine moves in the machine frame: under the tip G53 +# is G53.5, the carriage at the words whatever the tilt and the tool, where +# the tip's G53 would take the tool length off along machine Z +mdi("G0 B30") +j = mdi("G53 G0 X0 Y0 Z0") +expect("G53 X0 Y0 Z0 under the tip, tilted", j, carriage(0, 0, 0, 30)) +j = mdi("G53 G0 Y10") +expect("G53 Y10 under the tip holds machine X", j, carriage(0, 10, 0, 30)) +# G28.1 stores where the carriage stands and G28 and G30 go back to it the +# same way, after a plain move under the tip has taken the carriage along +j = mdi("G53 G0 X20 Y10 Z-5", "G28.1", "G30.1", "G0 X0 Y0 Z10") +j = mdi("G28") +expect("G28 under the tip, tilted", j, carriage(20, 10, -5, 30)) +b = mdi("G0 X0 Y0 Z10") +j = mdi("G30 Y0") +expect("G30 Y0, Y alone back to the store", j, + carriage(b[0] + b[1] * math.cos(SLANT), 10, b[2], 30)) +# the plain identity is not the machine frame type either: its letters name +# the joints, where the machine frame holds machine X as Y moves +mdi("G49", "G0 B0", "G12.1 P2") +j = mdi("G53 G0 X5 Y0 Z0 B0 C0") +expect("G53 to (5, 0, 0) on the plain identity", j, carriage(5, 0, 0)) +j = mdi("G53 G0 Y10") +expect("G53 Y10 on the plain identity holds X", j, carriage(5, 10, 0)) +mdi("G13.1") +drain() + +# --- G28.5 goes to a machine frame position stored on the machine frame ---- +# with no words every letter goes, B to its stored zero included; with words +# the machine passes through that machine frame point and only those +# letters go on to the stored position. Under G43.4 a program Z is the +# tool length above the carriage, the offset the interpreter carries, so the +# waypoint's Z is 10 + 50 +mdi("G49", "G53 G0 X20 Y10 Z0 B0 C0", "G28.1") +mdi("G43.4 H1", "G0 B30", "G0 X0 Y0 Z10") +j = mdi("G28.5") +expect("G28.5 with the head tilted", j, carriage(20, 10, 0, 0)) +mdi("G0 X0 Y0 Z10") +j = mdi("G28.5 X40") +expect("G28.5 X40, through X then to X only", j, carriage(20, 0, 10 + TOOL, 0)) mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") drain() diff --git a/tests/ptp-machine-frame/test.ini b/tests/ptp-machine-frame/test.ini index 20053c9a2db..0d273a3d2ba 100644 --- a/tests/ptp-machine-frame/test.ini +++ b/tests/ptp-machine-frame/test.ini @@ -8,6 +8,8 @@ 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 +# G53, G28, G30 and the stores in the machine frame off the machine frame type +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = slantkins diff --git a/tests/twp-native/README b/tests/twp-native/README index 868da2c4f14..3280aceebea 100644 --- a/tests/twp-native/README +++ b/tests/twp-native/README @@ -9,5 +9,15 @@ asked for in the plane; a move along plane X goes along plane X in the world; G68.3 reads the plane back off the oriented tool; G69 cancels. G53.6 standing near the end of C's travel reaches the plane by the nearest pose inside it, never running C past its limit. Q1 lets the table take part. The point-to-point moves are checked too: G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with -the head tilted, G53.4 G1 taking the time the straight move would in G94 and -in G93, and what they refuse. +the head tilted, G28.5 and G30.5 to a slide position stored on the identity +kinematics, through a slide waypoint when given one, G53.4 G1 taking the +time the straight move would in G94 and in G93, and what they refuse. The +config sets MACHINE_MOVES_IN_MACHINE_FRAME, so under TCP G53, G28 and G30 land +where G53.5, G28.5 and G30.5 do, G28.1 and G30.1 storing the slides, and on +the identity kinematics they are the world's. + +The path of a move is read from a sampler on the servo thread, every cycle +of the joint commands and the world position, logged by halsampler to +samples.log while the test runs: a status snapshot per task cycle misses the +turn of a move that stops and comes back. The log is left behind when a +check fails. diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal index e92c60eb526..792895c2bed 100644 --- a/tests/twp-native/sim.hal +++ b/tests/twp-native/sim.hal @@ -4,6 +4,11 @@ loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JO addf motion-command-handler servo-thread addf motion-controller servo-thread +# every servo cycle of the joints and the world position, logged for the +# test to read the path of a move, not a task-cycle snapshot of it +loadrt sampler depth=4000 cfg=fffffffff +addf sampler.0 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 @@ -11,6 +16,17 @@ 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 J0 => sampler.0.pin.0 +net J1 => sampler.0.pin.1 +net J2 => sampler.0.pin.2 +net J3 => sampler.0.pin.3 +net J4 => sampler.0.pin.4 +net J5 => sampler.0.pin.5 +net Xcmd axis.x.pos-cmd => sampler.0.pin.6 +net Ycmd axis.y.pos-cmd => sampler.0.pin.7 +net Zcmd axis.z.pos-cmd => sampler.0.pin.8 +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/twp-native/test-ui.py b/tests/twp-native/test-ui.py index b365a0c2ac5..ae5acc6b8ca 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -67,29 +67,47 @@ def mdi(*cmds): c.wait_complete(60) return settled() -# run one command and sample joints and positions on the way -# status is a task-cycle snapshot with the feedback a servo cycle behind the -# command, so two equal polls inside one task cycle do not mean the move is -# over: the last sample is taken once the move has settled +# the path of a move is read from the sampler log, every servo cycle of the +# joint commands and the world position, which halsampler writes to +# samples.log as it runs; status would give a snapshot per task cycle, which +# misses the turn of a move that stops and comes back +LOG = "samples.log" + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: # the last piece is a line still being written + v = line.split() + if len(v) != 1 + JOINTS + 3: + continue # an "overrun" line, counted from the pin at the end + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +# run one command and return the settled joints and the log of the way there: +# the log is block buffered, so it is read until it has caught up with the +# machine at rest def sampled(cmd): + n0 = len(log_samples()) c.mdi(cmd) - samples = [] - t0 = time.time() - while time.time() - t0 < 60: - s.poll() - samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) - if s.inpos and not s.queue and len(samples) > 20 and samples[-1] == samples[-2]: - break - time.sleep(0.005) c.wait_complete(60) end = settled() - s.poll() - samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) - return end, samples + deadline = time.time() + 10 + while True: + samples = log_samples() + if len(samples) > n0 and close(samples[-1][0], end, 2e-6): + break + if time.time() > deadline: + error("%s: the sampler log did not catch up with the machine" % cmd) + break + time.sleep(0.02) + return end, samples[n0:] # a point-to-point move runs every joint on a straight line in joint space, # all together: the fraction of the way each moving joint has gone is the -# same for all of them at every sample, never goes back, and reaches one +# same for all of them at every sample, never goes back, and reaches one; +# the log prints six decimals, which on a short move is 1e-5 of the way def joint_line(what, samples, start, end): moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] if not moving: @@ -99,7 +117,7 @@ def joint_line(what, samples, start, end): for n, (j, p) in enumerate(samples): fs = [(j[i] - start[i]) / (end[i] - start[i]) for i in moving] f = sum(fs) / len(fs) - if max(abs(x - f) for x in fs) > 1e-3: + if max(abs(x - f) for x in fs) > 1e-5: error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) return if f < last - 1e-6: @@ -492,6 +510,44 @@ def pose_near(pairs, b_now, c_now, limited): if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") +# --- G28.5 and G30.5 ------------------------------------------------------ +# the stored positions are saved on the identity kinematics, where the world +# is the slides: G28.1 at X10 Y20 Z-5 with the head straight, G30.1 at the +# same slides with the head at B10 +mdi("G12.1 P0", "G0 X10 Y20 Z-5 A0 B0 C0", "G28.1", "G0 B10", "G30.1", + "G0 X0 Y0 Z0 B0", "G12.1 P1") +# with the head tilted under TCP, G28.5 alone takes every slide to the +# stored position, on one joint-space move +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G28.5") +show("G28.5", after) +drain() +joint_line("G28.5", samples, before, after) +if not close(after, [10, 20, -5, 0, 0, 0], 1e-6): + error("G28.5 put the joints at %s, not at the stored slides" % (after,)) +# an axis word is a slide to pass through first, and then only that letter +# goes to its stored value: Z3 on the way, then Z-5, the rest untouched +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G30.5 Z3") +show("G30.5 Z3", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G30.5 Z3 left joint 2 at %.6f, not at the stored -5" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G30.5 Z3 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# the first move ends on Z3 and the second starts there inside the same +# servo cycle, so the turn falls between two samples: joint 2 decelerates +# into it at 700 mm/s^2, which over one 1 ms cycle is 3.5e-4 short of it, +# and it never goes past it +peak = max(j[2] for j, p in samples) +if peak > 3 + 1e-6 or peak < 3 - 1e-3: + error("G30.5 Z3 did not pass through the Z3 slide (joint 2 peaked at %.6f)" % peak) +after = mdi("G30.5") +if not close(after, [10, 20, -5, 0, 10, 0], 1e-6): + error("G30.5 put the joints at %s, not at the stored slides with B10" % (after,)) +mdi("G0 X0 Y0 Z0 A0 B0 C0") + # a point-to-point feed takes the time the straight move would: 10 mm at # F600 is one second, and F30 in G93 is two def timed(cmd): @@ -528,7 +584,8 @@ def timed(cmd): # what the point-to-point codes refuse for cmd in ("G53.4 G2 X1 I1", "G91 G53.7 G0 J0=1", "G53.7 G0 J9=1", "G53.7 G0 X1", "G53.7 G0 J1", "G53.7 G0", "G0 J0=1", "G53.7 G0 J0.5=1", "G53.7 G0 J0=1 J0=2", - "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1"): + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1", + "G91 G28.5 X1", "G30.5 G1 X1"): c.mdi(cmd) c.wait_complete(30) m = e.poll() @@ -539,6 +596,47 @@ def timed(cmd): c.mode(linuxcnc.MODE_MDI) mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") +# The config sets MACHINE_MOVES_IN_MACHINE_FRAME: under TCP G53 is G53.5, the +# slides at the words whatever the head is doing, on one joint-space move, +# where the tip's G53 would move every slide to keep the tip on machine Z +before = mdi("G12.1 P1", "G0 B30") +after, samples = sampled("G53 G0 Z-5") +show("G53 G0 Z-5 under TCP", after) +drain() +joint_line("G53 G0 Z-5", samples, before, after) +if abs(after[2] + 5) > 1e-6: + error("G53 Z-5 under TCP left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53 Z-5 under TCP moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# G28.1 and G30.1 store where the slides stand with the head tilted, and G28 +# and G30 go back to them after a plain move has taken the slides along +mdi("G53.5 G0 X10 Y20 Z-5 B-30", "G28.1", "G30.1") +here = mdi("G0 X0 Y0 Z0 B0") +after, samples = sampled("G28") +show("G28 under TCP", after) +drain() +joint_line("G28", samples, here, after) +if not close(after, [10, 20, -5, 0, -30, 0], 1e-6): + error("G28 under TCP put the joints at %s, not at the slides G28.1 stored" % (after,)) +before = mdi("G0 X0 Y0 Z0 B0") +after = mdi("G30 Z3") +if abs(after[2] + 5) > 1e-6: + error("G30 Z3 under TCP left joint 2 at %.6f, not at the stored -5" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G30 Z3 under TCP moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# on the identity kinematics the world is the slides and G53 its straight line +mdi("G0 X0 Y0 Z0 B0", "G12.1 P0") +before = mdi("G53 G0 X10 Y0 Z-5") +if not close(before, [10, 0, -5, 0, 0, 0], 1e-6): + error("G53 X10 Z-5 on the identity put the joints at %s" % (before,)) +mdi("G28.1", "G0 X0 Z0", "G28", "G12.1 P1") +if not close(settled(), [10, 0, -5, 0, 0, 0], 1e-6): + error("G28 on the identity did not go back to the G28.1 store") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + # --- a plane refuses what would move the ground under it --------------- # each refusal is an interpreter error, and the abort that follows cancels # the plane, so it is defined afresh before every one @@ -599,5 +697,10 @@ def timed(cmd): except OSError: pass +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) print("Exiting with %d errors" % errors) sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini index a3a6bcf9e1a..a8060670ed6 100644 --- a/tests/twp-native/test.ini +++ b/tests/twp-native/test.ini @@ -8,6 +8,8 @@ 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 +# G53, G28, G30 and the stores in the machine frame off the machine frame type +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzacb_trsrn diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh index 765cf14fed6..d27cc469eb5 100755 --- a/tests/twp-native/test.sh +++ b/tests/twp-native/test.sh @@ -1,4 +1,4 @@ #!/bin/bash -e # a failed run leaves the var file behind, and it carries offsets -rm -f sim.var sim.var.bak +rm -f sim.var sim.var.bak samples.log linuxcnc -r test.ini From bb221c7cf51fa1affdd916df808df7518ade2e73 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 14 Oct 2026 00:51:37 +1100 Subject: [PATCH 22/24] preview: read the machine moves on the kinematics type the machine is on With [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME set, a GUI's preview read every G53 in the machine frame whatever type the machine was on, the GUI's own first. A GUI starts its preview by restating where the machine stands, with G53 G0 to the position in the status buffer: a block per axis in AXIS, one block in the other screens. The preview could not tell which kinematics type was in force, since its canon answered type 0 and no flags for every type, so it ran on type 0 and the decision fell back on the machine's kinematics type, which for a switchable module is never the identity. The status buffer carries the type motion runs and what the module declares each type to be, as the interpreter in task reads them. The Python status now reports them as kins_type and kins_type_flags, with the KINSTYPE_ constants, and the status mixin the preview canons are built on hands them to the preview, read once a parse because the interpreter asks for the flags on every block. The preview then starts on the type the machine is on and follows the program's G12.1, G13.1, G43.4 and G49 as the interpreter in task does, so a program's G53, G28 and G30 land where running it would land them. A canon with no machine to ask answers as before. The restating codes are a different matter whenever the machine stands on another type than the machine frame, under a tool centre point type say: the position they restate is in the world of that type, and G53 is the only way to say it. They are the GUI's codes, not the program's, so the preview holds the option off until the program starts. The preview also kept the tool offset the last parse ended on, which the interpreter reads as it starts. AXIS restates the offset after the position, and under a tool centre point type with the tool tilted the restated G53 depends on the offset in force, so two previews of the same program could start apart. Each parse now starts with no offset, which the GUI's restating codes then set. The ptp-machine-frame test previews a program with a G53 through both forms of the restating codes, AXIS's with the tool offset in force, which under the tip type the kinematics evaluates in the GUI's own process, on the machine frame type, under the tip type and on the plain identity, and one that selects the machine frame type with G13.1 before its G53 under the tip, and checks that each ends where the same program with G53.5 does. A move that goes nowhere reads back the restated position, which has to be the one the machine reports, and on the plain identity the preview has to end where running the program ends. Without the type from the status buffer the run check fails, without the hold the restated position does, and without the offset reset the second preview under the tip starts elsewhere. Reported by Sigma1912. --- docs/src/config/ini-config.adoc | 1 + docs/src/config/python-interface.adoc | 7 ++ lib/python/rs274/interpret.py | 6 + src/emc/rs274ngc/gcodemodule.cc | 58 ++++++++- src/emc/rs274ngc/interp_convert.cc | 3 +- src/emc/usr_intf/axis/extensions/emcmodule.cc | 17 +++ tests/ptp-machine-frame/README | 8 +- tests/ptp-machine-frame/g13-g53.ngc | 3 + tests/ptp-machine-frame/g13-g535.ngc | 3 + tests/ptp-machine-frame/g53.ngc | 2 + tests/ptp-machine-frame/g535.ngc | 2 + tests/ptp-machine-frame/here.ngc | 2 + tests/ptp-machine-frame/test-ui.py | 118 ++++++++++++++++++ 13 files changed, 225 insertions(+), 5 deletions(-) create mode 100644 tests/ptp-machine-frame/g13-g53.ngc create mode 100644 tests/ptp-machine-frame/g13-g535.ngc create mode 100644 tests/ptp-machine-frame/g53.ngc create mode 100644 tests/ptp-machine-frame/g535.ngc create mode 100644 tests/ptp-machine-frame/here.ngc diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 9b9c40919a3..86e3e3750e1 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -642,6 +642,7 @@ The maximum number of `USER_M_PATH` directories is defined at compile time (typ: * `MACHINE_MOVES_IN_MACHINE_FRAME = 0` (bool, Default: 0) + When set, 'G53', 'G28' and 'G30' run as 'G53.5', 'G28.5' and 'G30.5' whenever the kinematics in force is not the machine frame type, and 'G28.1' and 'G30.1' store the machine frame position those read back: the slides go to the words or to the stored position whatever the tool, the head and the table are doing. The machine frame type is the type the module flags as such, the identity kinematics on a machine whose slides line up with its frame, and neither the tool centre point nor a raw joint identity on one whose slides do not. On the machine frame type the codes are unchanged. Unset, these codes work in the world of the kinematics in force, which on a table-side kinematics turns with the table, and under a tool centre point kinematics with the tool tilted puts the slides where the tool and the head angle take them. + A GUI's preview reads a program on the kinematics type the machine is on when the program is loaded, as running it would; the 'G53' the GUI puts ahead of the program to say where the machine stands stays in the world of that type. See <>. * 'G73_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) Chip breaking back-off distance in machine units diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index f55ccb764a2..c924886b1d8 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -244,6 +244,13 @@ see <> for an example. * KINEMATICS_INVERSE_ONLY * KINEMATICS_BOTH +*kins_type*:: '(returns integer)' - + the kinematics type motion runs, as 'G12.1' selects it and the motion.kins-type pin reports it; 0 for a module that switches nothing. + +*kins_type_flags*:: '(returns tuple of integers)' - + what the kinematics module declares each kinematics type to be, one entry per type: a sum of KINSTYPE_IDENTITY, KINSTYPE_PRIMARY and KINSTYPE_MACHINE, or -1 for a type the module does not provide or declares nothing about. + See the <> chapter. + *limit*:: '(returns tuple of integers)' - axis limit masks. minHardLimit=1, maxHardLimit=2, minSoftLimit=4, maxSoftLimit=8. diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 055909ade05..d7f3d17493a 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -199,5 +199,11 @@ def get_external_joint_positions(self): def get_kinematics_type(self): return self.s.kinematics_type + def get_kins_type(self): + return self.s.kins_type + + def get_kins_type_flags(self): + return self.s.kins_type_flags + # vim:ts=8:sts=4:et: diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 45d679705b7..542c207cae6 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -964,8 +964,44 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double /*tolerance*/, int / void SET_MOTION_CONTROL_MODE(double /*tolerance*/) { } void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } -int GET_EXTERNAL_KINS_TYPE() { return 0; } -int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { (void)ktype; return -1; } + +// The kinematics type the machine runs and what the module declares each +// type to be, from a canon that watches the status buffer: read once a +// parse, since the interpreter asks for the flags on every block. One that +// cannot answer has no machine, type 0 and nothing declared, which the +// interpreter takes for a standalone parse. +static int kins_type; +static std::array kins_type_flags; + +static void read_kins_types() { + py::handle canon(parse_state.callback); + std::array flags; + size_t n = 0; + + flags.fill(-1); + kins_type = 0; + kins_type_flags = flags; + if(!py::hasattr(canon, "get_kins_type_flags")) return; + try { + py::sequence seq = canon.attr("get_kins_type_flags")().cast(); + for(py::handle value : seq) { + if(n == flags.size()) break; + flags[n++] = value.cast(); + } + kins_type = canon.attr("get_kins_type")().cast(); + } catch(py::error_already_set &) { + return; // the error goes with the exception + } catch(py::builtin_exception &) { + return; + } + kins_type_flags = flags; +} + +int GET_EXTERNAL_KINS_TYPE() { return kins_type; } +int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { + if(ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) return -1; + return kins_type_flags[ktype]; +} int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) { return -1; } // the kind of transform the machine runs, from a canon that watches the @@ -998,6 +1034,8 @@ static py::object parse_file(const char *f, py::handle canon, const char *unitcode, const char *initcode, const char *interpname) { int error_line_offset = 0; + Interp *own = nullptr; + bool machine_moves = false; // How often the parse stops to report progress and let the GUI breathe. // A GUI's progress bar and its abort button both hang off this one tick - // the report moves the bar, and check_abort() is what pumps the event @@ -1087,12 +1125,27 @@ static py::object parse_file(const char *f, py::handle canon, } parse_state.pos = {}; + // the interpreter reads the offset in force as it starts, and the one + // the last parse ended on is not the machine's: the GUI restates it + parse_state.tool_offset = {}; + read_kins_types(); parse_state.pinterp->init(); parse_state.pinterp->open(f); maybe_new_line(); + // The codes ahead of the program restate the state the machine is in, + // and G53 is how a GUI says where it stands: in the world of the type + // in force, which is the type the machine is on. What + // MACHINE_MOVES_IN_MACHINE_FRAME reads in the machine frame is a + // program's G53, so it waits for the program. + own = dynamic_cast(parse_state.pinterp); + if(own) { + machine_moves = own->_setup.machine_moves_in_machine_frame; + own->_setup.machine_moves_in_machine_frame = false; + } + if(initcodes) { Py_ssize_t ncodes = PyList_Size(initcodes.ptr()); for(Py_ssize_t i=0; iexecute(); } + if(own) own->_setup.machine_moves_in_machine_frame = machine_moves; while(!parse_state.interp_error && RESULT_OK) { error_line_offset = 1; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 534a8fa8dc1..30bd30d94ff 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6657,7 +6657,8 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine // the kinematics module declares what each type is (KINSTYPE_* flags); // where the flags say nothing at all there is no kinematics attached -// (sai, preview) and the codes fall back to type 0, as before +// (sai, a preview with no machine to ask) and the codes fall back to +// type 0, as before static int kins_type_info_available() { int k; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 635726e7cd7..6cfd70e0672 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1191,6 +1191,9 @@ static PyMemberDef Stat_members[] = { { "probing", T_BOOL, O(motion.traj.probing), READONLY, NULL}, { "probe_val", T_INT, O(motion.traj.probeval), READONLY, NULL}, { "kinematics_type", T_INT, O(motion.traj.kinematics_type), READONLY, NULL}, + { "kins_type", T_INT, O(motion.traj.switchkins_type), READONLY, + "The kinematics type motion runs, as G12.1 selects it and motion.kins-type\n" + "reports it; 0 for a module that switches nothing." }, { "motion_type", T_INT, O(motion.traj.motion_type), READONLY, "The type of the currently executing motion (one of MOTION_TYPE_TRAVERSE,\n" "MOTION_TYPE_FEED, MOTION_TYPE_ARC, MOTION_TYPE_TOOLCHANGE,\n" @@ -1294,6 +1297,14 @@ static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { return res; } +static PyObject *Stat_kins_type_flags(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(SWITCHKINS_MAX_TYPES); + for (int i = 0; i < SWITCHKINS_MAX_TYPES; i++) { + PyTuple_SET_ITEM(res, i, PyLong_FromLong(s->status.motion.traj.switchkins_flags[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1600,6 +1611,8 @@ static PyGetSetDef Stat_getsetlist[] = { (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, + {(char*)"kins_type_flags", (getter)Stat_kins_type_flags, NULL, + (char*)"What the module declares each kinematics type to be, one entry per type: KINSTYPE_ flags, or -1 for a type it does not provide.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, @@ -3423,6 +3436,10 @@ PyMODINIT_FUNC PyInit_linuxcnc(void) ENUM(KINEMATICS_INVERSE_ONLY); ENUM(KINEMATICS_BOTH); + ENUM(KINSTYPE_IDENTITY); + ENUM(KINSTYPE_PRIMARY); + ENUM(KINSTYPE_MACHINE); + ENUMX(4, EMC_DEBUG_CONFIG); ENUMX(4, EMC_DEBUG_VERSIONS); ENUMX(4, EMC_DEBUG_TASK_ISSUE); diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README index c46b86bee68..08480a0f1e2 100644 --- a/tests/ptp-machine-frame/README +++ b/tests/ptp-machine-frame/README @@ -15,5 +15,9 @@ joint, from the machine frame type and from under the tilted tip kinematics alike, that G28.5 goes to a position stored on the machine frame, through a waypoint when given one, that MACHINE_MOVES_IN_MACHINE_FRAME runs G53, G28 and G30 as G53.5, G28.5 and G30.5 under the tip and under the -plain identity, G28.1 and G30.1 storing the carriage, and that a plain move -after a machine frame move starts from the right place. +plain identity, G28.1 and G30.1 storing the carriage, that a plain move +after a machine frame move starts from the right place, and that a preview +reads the type the machine is on and what each type is from the status +buffer: the G53 a GUI restates the position with ahead of the program stays +in the world of that type, and the program's G53 ends where G53.5 would, +through the switches the program makes. diff --git a/tests/ptp-machine-frame/g13-g53.ngc b/tests/ptp-machine-frame/g13-g53.ngc new file mode 100644 index 00000000000..cb6a840de63 --- /dev/null +++ b/tests/ptp-machine-frame/g13-g53.ngc @@ -0,0 +1,3 @@ +G13.1 +G53 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g13-g535.ngc b/tests/ptp-machine-frame/g13-g535.ngc new file mode 100644 index 00000000000..47f51f5d187 --- /dev/null +++ b/tests/ptp-machine-frame/g13-g535.ngc @@ -0,0 +1,3 @@ +G13.1 +G53.5 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g53.ngc b/tests/ptp-machine-frame/g53.ngc new file mode 100644 index 00000000000..b2f7efa7e05 --- /dev/null +++ b/tests/ptp-machine-frame/g53.ngc @@ -0,0 +1,2 @@ +G53 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g535.ngc b/tests/ptp-machine-frame/g535.ngc new file mode 100644 index 00000000000..6f4ee09e007 --- /dev/null +++ b/tests/ptp-machine-frame/g535.ngc @@ -0,0 +1,2 @@ +G53.5 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/here.ngc b/tests/ptp-machine-frame/here.ngc new file mode 100644 index 00000000000..836d5abe2a0 --- /dev/null +++ b/tests/ptp-machine-frame/here.ngc @@ -0,0 +1,2 @@ +G91 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index c08211639aa..ab16f9bff89 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -4,10 +4,13 @@ import linuxcnc import hal +import gcode +import preview_helpers import sys import os import time import math +from rs274.interpret import StatMixin JOINTS = 5 SLANT = math.radians(30.0) @@ -180,6 +183,121 @@ def expect_type(what, want): mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") drain() +# --- the preview reads the codes on the type the machine is on ------------- +# the status buffer says which type that is and what each type is; a GUI +# restates where the machine stands with G53 ahead of the program, in the +# world of the type in force, which the flag leaves alone, and the program +# after it reads G53 as running it would, through the switches it makes. +# AXIS hands the restating codes over as a list, the Gremlin based screens +# as a unit code and an init code. +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.end = None + + # where the last move ends, in the program's terms + def straight_traverse(self, *args): + self.end = args[-9:] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + +def preview(program, listed): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + if listed: + # as AXIS builds them: the tool, the position, the modal codes, and + # the offset in force, which under the tip the kinematics evaluates + axes = [i for i in range(9) if s.axis_mask & (1 << i)] + codes = ["G21", "G90", "T%d M6" % s.tool_in_spindle] + codes += ["G53 G0 %s%.8f" % ("XYZABCUVW"[i], s.position[i]) for i in axes] + codes += ["G%.1f" % (g * .1) for i, g in enumerate(s.gcodes) + if i not in (0, 1, 2, 12) and g != -1] + codes.append("G43.1 " + " ".join("%s%.8f" % ("XYZABCUVW"[i], s.tool_offset[i]) for i in axes)) + result, line = gcode.parse(program, canon, codes, "") + else: + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return line, gcode.strerror(result), None + return None, None, canon.end + +# The program's G53 ends where G53.5 to the same words ends, in each form, +# a list or an init code; line 0 is the codes ahead of the program +def far(a, b): + return a is None or b is None or max(abs(x - y) for x, y in zip(a, b)) > 1e-9 + +def expect_preview(what, program, same): + for listed in (True, False): + form = "list" if listed else "init code" + line, why, end = preview(program, listed) + print("%-34s %s" % ("%s, %s" % (what, form), + "line %d: %s" % (line, why) if why else "accepted")) + if why: + error("%s: the preview refused line %d" % (what, line)) + line, why, want = preview(same, listed) + if why or far(end, want): + error("%s, %s: the preview ends at %s, %s at %s" % (what, form, end, same, want)) + +# The restating G53 is the type in force's, never read in the machine frame: +# a move that goes nowhere ends on the position the machine reports. The +# list restates the tool offset after the position, which moves it. +def expect_restated(what, forms): + s.poll() + want = [v if i in (3, 4, 5) else v / 25.4 for i, v in enumerate(s.position)] + for listed in forms: + line, why, end = preview("here.ngc", listed) + if why or far(end, want): + error("%s, %s: the restated position reads %s, not %s" % + (what, "list" if listed else "init code", end, want)) + +# and running it ends there too: with no tool offset in force the restated +# position after the run is where the program's preview ended +def expect_run(what, program, *lines): + ends = [preview(program, listed)[2] for listed in (True, False)] + here = mdi() + mdi(*lines) + for listed, end in zip((True, False), ends): + there = preview("here.ngc", listed)[2] + if far(end, there): + error("%s, %s: the preview ends at %s, running it at %s" % + (what, "list" if listed else "init code", end, there)) + mdi("G53.7 G0 " + " ".join("J%d=%.9f" % (j, v) for j, v in enumerate(here))) + +mdi("G13.1") +s.poll() +flags = s.kins_type_flags +print("%-34s kins_type %d, flags %s" % ("the status buffer", s.kins_type, flags)) +if s.kins_type != kins_type(): + error("stat.kins_type %d, motion.kins-type %d" % (s.kins_type, kins_type())) +if (flags[0] & (linuxcnc.KINSTYPE_MACHINE | linuxcnc.KINSTYPE_IDENTITY) != linuxcnc.KINSTYPE_MACHINE + or not flags[1] & linuxcnc.KINSTYPE_PRIMARY + or not flags[2] & linuxcnc.KINSTYPE_IDENTITY or flags[-1] != -1): + error("stat.kins_type_flags does not say what slantkins declares") +mdi("G53 G0 X5 Y10 Z0") +expect_preview("G53 on the machine frame type", "g53.ngc", "g535.ngc") +expect_restated("on the machine frame type", (True, False)) +mdi("G43.4 H1", "G0 B30") +expect_type("G43.4 for the preview", 1) +expect_preview("G53 under the tip, tilted", "g53.ngc", "g535.ngc") +expect_preview("G13.1 then G53 under the tip", "g13-g53.ngc", "g13-g535.ngc") +expect_restated("under the tip, tilted", (False,)) +mdi("G49", "G0 B0", "G12.1 P2") +expect_type("G12.1 P2 for the preview", 2) +expect_preview("G53 on the plain identity", "g53.ngc", "g535.ngc") +expect_restated("on the plain identity", (True, False)) +expect_run("G53 on the plain identity", "g53.ngc", "G53 G0 X0") +mdi("G13.1") +drain() + for f in ("sim.var", "sim.var.bak"): try: os.unlink(f) From ddbb4402e993306c7b43fb299ba277fc4b901f91 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 18 Oct 2026 18:37:06 +1100 Subject: [PATCH 23/24] configs: the five axis sims move G53, G28 and G30 in the machine frame The bridgemill, table-dual-rotary, table-rotary-tilting and both table-rotary_spindle-rotary-nutating sims set MACHINE_MOVES_IN_MACHINE_FRAME, with a comment saying what it does, so a new user starting from one gets G53, G28 and G30 that put the slides in the same place whatever the tool and the head angle, and anyone copying the config sees the setting and where it is documented. None of their programs or remaps uses those codes under the TCP kinematics: the boat demo's G53 lines carry G49 or follow it, which cancels to the machine frame kinematics first. The G43.4 section and the switchable kinematics chapter now point at the setting too, where someone setting up a TCP machine reads. --- configs/sim/axis/vismach/5axis/bridgemill/5axis.ini | 6 ++++++ .../sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini | 6 ++++++ .../axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini | 6 ++++++ .../axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini | 6 ++++++ .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 6 ++++++ .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 6 ++++++ docs/src/gcode/g-code.adoc | 7 +++++++ docs/src/motion/switchkins.adoc | 6 +++++- 8 files changed, 48 insertions(+), 1 deletion(-) diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 4c987aba743..c66ba5e7238 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -32,6 +32,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = 5axis.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [EMCMOT] EMCMOT = motmod diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini index bd6dc79e82a..d5a083dad1d 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini @@ -22,6 +22,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap PARAMETER_FILE = xyzab-tdr.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini index 9c2bc00e33e..ae0995c189c 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini @@ -35,6 +35,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzac.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini index 45d780a251c..0311dfcba4a 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini @@ -35,6 +35,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzbc.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 0c4389ca1a9..fdbab9b291f 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -36,6 +36,12 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzacb-trsrn.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzacb_trsrn diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index 5223bc375f6..4162f8f0248 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -36,6 +36,12 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzbca-trsrn.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzbca_trsrn diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index a9de56b53e3..efc234a700e 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1865,6 +1865,13 @@ that cancels one of them does not switch either. On a machine without switchable kinematics there is nothing to switch: 'G43.4' is a plain 'G43' and 'G49' a plain cancel. +Under the working kinematics 'G53', 'G28' and 'G30' move in its world, so +with the tool tilted where they put the slides depends on the tool and the +head angle. A configuration that wants them to move the slides in the machine +frame sets `MACHINE_MOVES_IN_MACHINE_FRAME` in the `[RS274NGC]` section of the +INI file, as the five axis sims shipped with LinuxCNC do; see +<>. + It is an error if: * the kinematics module is switchable but declares no primary diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 476b3c180ab..60f5fade587 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -590,7 +590,11 @@ G-code reads these declarations: 'G13.1' and 'G49' cancel to the kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move in its world, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics -are not in the conventional order still gets working spellings. The +are not in the conventional order still gets working spellings. With +`MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the +INI file, 'G53', 'G28' and 'G30' move in the machine frame kinstype's +world too whenever another kinstype is in force; the five axis sims set +it. The machine frame type is the machine with the orientation left out, XYZ the pivot in machine coordinates and the rotary letters the rotary joints; on a machine whose slides line up with its frame that is the identity From 3bb38b667e774f83c2f22ca8906ee95006e2c9af Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 13:17:01 +1000 Subject: [PATCH 24/24] genserkins, pumakins: declare the arm kinematics the machine frame A robot's working transform reports the flange in the base frame, ABB's tool0 and KUKA's $NULLFRAME: the tool is applied on top by the interpreter, not by the maths, so the type G43.4 switches to is also the machine with the tool left out, the frame G13.1 and G49 cancel to and G53.5 moves in. pumakins and genserkins declare that one type both primary and machine. G13.1 and G49 leave a robot on its arm kinematics, where a robot controller leaves it, and the identity type, joints as axes, is reached by number; G53.5 moves the flange along a base axis and holds the rest, the orientation included, interpolated in joint space, and G28.5 and G30.5 with it. A machine frame type does not read the tool offset in the parameter block, so a module whose working transform does read it declares a type without it. The machine frame is described as the frame with the tool left out rather than with the orientation left out, since on a robot the rotary letters are the flange angles the module reports, not joints; the G13.1, G49 and G43.4 descriptions say machine frame where they said identity. tests/ptp-robot checks that G53.5 moves the flange from the arm kinematics and from the identity alike, and that G13.1, G43.4 and G49 all leave the robot on the arm kinematics, in place of the refusal it checked before. --- .../axis/vismach/puma/remap_subs/428remap.ngc | 2 +- docs/src/gcode/g-code.adoc | 82 +++++++++-------- docs/src/motion/kinematics-conventions.adoc | 31 ++++--- docs/src/motion/switchkins.adoc | 33 ++++--- include/kinematics.h | 25 ++--- include/kins_module.h | 9 +- src/emc/kinematics/genserfuncs.c | 4 + src/emc/kinematics/pumakins.c | 6 +- src/emc/rs274ngc/interp_workplane.cc | 4 +- tests/ptp-robot/README | 10 +- tests/ptp-robot/test-ui.py | 91 +++++++++++++++---- 11 files changed, 195 insertions(+), 102 deletions(-) diff --git a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc index d2d0f3c8d39..b71caa08b0d 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc @@ -3,7 +3,7 @@ o<428remap>sub # = 0 G12.1 P# ; select kinstype 0, syncs interp and motion - ; (G13.1 is identity, which is type 1 on this module) + ; (G13.1 is the machine frame, this type on a robot; identity is type 1) o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] (debug,M428: Wrong kinematics type) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index efc234a700e..a65480db020 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -952,9 +952,11 @@ G13.1 'G12.1' selects one of the kinematics provided by a switchable kinematics module; the 'P' word is the kinematics number. 'G13.1' cancels back to -identity kinematics, where joints and coordinates are the same thing. -Which number that is is declared by the module itself, so 'G13.1' means -the same kinematics whatever order a module lists its types in. +the machine frame kinematics: the identity kinematics, where joints and +coordinates are the same thing, on a machine whose slides line up with its +frame, and the arm kinematics with no tool on a robot. Which number that is +is declared by the module itself, so 'G13.1' means the same kinematics +whatever order a module lists its types in. [WARNING] Deprecation notice: selecting the kinematics by writing the @@ -970,12 +972,12 @@ planned in one kinematics and executed in another. Because of that, both codes stop any blending that was in progress, in the same way 'G4' does. The kinematics module decides what each number means, and declares -which one is identity. See the `switchkins` section of the kins(9) man -page for the modules that support switching and the order in which they +which one is the machine frame. See the `switchkins` section of the kins(9) +man page for the modules that support switching and the order in which they list their kinematics. A machine whose kinematics module is not switchable rejects the change, and 'G13.1' is an error on a switchable -module that declares no identity kinematics; such a module can still be -driven by number with 'G12.1'. +module that declares no identity or machine frame kinematics; such a module +can still be driven by number with 'G12.1'. Selecting a kinematics does not move the machine. It changes how joint positions and coordinate positions map onto each other, so the position @@ -996,14 +998,14 @@ G12.1 P# (put back whatever the caller was using) Nothing cancels the selection on its own. It survives the end of the program and an abort, so that the kinematics keeps matching what the position readout shows. End a program with 'G13.1' if it should leave the -machine in identity kinematics. +machine in the machine frame kinematics. .G12.1, G13.1 Example [source,ngc] ---- G12.1 P1 (switch to kinematics 1) G0 X0 Y0 -G13.1 (back to identity kinematics) +G13.1 (back to the machine frame kinematics) ---- It is an error if: @@ -1012,7 +1014,7 @@ It is an error if: * The 'P' word is negative. * The 'P' word names a kinematics the module does not provide. * A 'P' word is used with 'G13.1'. -* 'G13.1' is used and the module declares no identity kinematics. +* 'G13.1' is used and the module declares no identity or machine frame kinematics. [[gcode:g17-g19.1]] == G17 - G19.1 Plane Select(((G17 - G19.1 Plane Select))) @@ -1076,7 +1078,7 @@ G28 Z2.5 (rapid to Z2.5 then to Z location specified in #5163) The 'absolute' position is the machine coordinate system of the kinematics in force, see <>: on a kinematics that is not the identity it is that kinematics' world, not the slides, and the stored position is a world -position too. <> is the form that moves the slides, and with +position too. <> is the machine frame form, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the INI file 'G28' and 'G28.1' are 'G28.5' and its store whenever the kinematics in force is not the machine frame kinematics. @@ -1222,8 +1224,9 @@ with its frame, and the move is a <> there, rapid, with no offset, tool length, rotation or work plane applied. The values are in the machine units of the INI file, as for 'G28'. The stored position has to be a machine frame position: store it with 'G28.1' or 'G30.1' -while the identity kinematics is in force, where the world is that frame, or -under any kinematics with `MACHINE_MOVES_IN_MACHINE_FRAME` set. +while the machine frame kinematics is in force, what 'G13.1' selects, where +the world is that frame, or under any kinematics with +`MACHINE_MOVES_IN_MACHINE_FRAME` set. * 'G28.5' - moves every letter the machine has to the stored position. * 'G28.5 axes' - moves the letters named by 'axes' to the given machine @@ -1844,9 +1847,9 @@ declares its working transform, so the program runs with tool length compensation in the module's working kinematics. The switch happens first and the offset applies after it, as if the two had been written on consecutive lines. 'G49' is the matching cancel: it clears the -offset and switches back to identity kinematics, as long as the offset -in effect is still 'G43.4''s and no 'G12.1' or 'G13.1' has selected a -kinematics since. +offset and switches back to the machine frame kinematics, what 'G13.1' +selects, as long as the offset in effect is still 'G43.4''s and no 'G12.1' +or 'G13.1' has selected a kinematics since. The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module @@ -1949,11 +1952,13 @@ It is an error if: * 'G49' - cancels tool length compensation -'G49' also switches a switchable kinematics module back to its identity -kinematics when it cancels a 'G43.4' or 'G43.5', undoing the switch that made. It -leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does -after a plain 'G43', and a module that declares no identity kinematics -gets the plain cancel. +'G49' also switches a switchable kinematics module back to its machine +frame kinematics, what 'G13.1' selects, when it cancels a 'G43.4' or +'G43.5', undoing the switch that code made. It leaves a kinematics selected +by 'G12.1' or 'G13.1' alone, as it does after a plain 'G43', and a module +that declares no identity or machine frame kinematics gets the plain +cancel. On a robot the arm kinematics is the machine frame, so there the +cancel switches nothing. It is OK to program using the same offset already in use. It is also OK to program using no tool length offset if none is currently being @@ -2167,19 +2172,23 @@ one more layer of interpretation from the destination than the one before: without leaving its coordinate system and without the trajectory planner trying to hold the tool tip on a line the joints cannot follow at speed. * 'G53.5' takes the same axis letters as a position in the machine frame - with the orientation left out: 'X', 'Y' and 'Z' are the carriage, the - point the rotary joints do not move, in machine coordinates, and the - rotary letters are the rotary joints themselves. On a machine whose slides - line up with its frame, which every shipped module is, that is the slides: - each letter names the joints the module's identity mapping gives it, which - the `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, - and the two joints of a gantry take one value together. A module whose + with the tool left out: 'X', 'Y' and 'Z' are the point the tool hangs + from, the pivot of a head or the flange of a robot, in machine + coordinates, and the rotary letters are the orientation as the machine + frame kinematics reports it, on a mill the rotary joints themselves. On a + machine whose slides line up with its frame that is the slides: each + letter names the joints the module's identity mapping gives it, which the + `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, and + the two joints of a gantry take one value together. A module whose carriage does not line up, a slanted slide or a composite Y on a mill-turn, declares a machine frame type of its own (see the <> chapter) and 'G53.5' goes through it, so that 'Y10' is machine Y whichever slides make it, and - a letter not given holds its machine coordinate rather than a joint. - Values are in program units, so 'G20' scales them like any other axis + a letter not given holds its machine coordinate rather than a joint. On a + robot the machine frame is the arm kinematics with no tool, the flange in + the base frame, so 'Z10' puts the flange at base Z 10 with its orientation + held and 'A', 'B' and 'C' are the flange angles the module reports; the + move is still interpolated in joint space. Values are in program units, so 'G20' scales them like any other axis word. No offset, tool length, rotation or work plane applies. On a mill whose joint 2 carries the Z slide, 'G53.5 G0 Z0' puts that slide at zero whatever the head and the tool are doing, where under a tool centre point @@ -2192,10 +2201,10 @@ one more layer of interpretation from the destination than the one before: + An axis letter carries a unit class and a joint does not, so where the machine frame is the joints 'G53.5' is refused on a machine whose letters -name joints of the other kind. A serial robot answers X with its first -rotary joint, which turns in degrees, and there the whole code is refused -rather than that one letter. Which joints turn is what `[JOINT_n] TYPE` -declares. +name joints of the other kind: a module that declares no kinematics types +and answers X with a rotary joint, which turns in degrees, has the whole +code refused rather than that one letter. Which joints turn is what +`[JOINT_n] TYPE` declares. * 'G53.7' takes joint values, one word per joint, and works on every machine: 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the @@ -2203,8 +2212,9 @@ declares. the joint's own position, what `joint.n.pos-cmd` shows, in the joint's own units. Nothing is converted, not even 'G20' and 'G21'. A joint left out keeps its position, and the two joints of a gantry pair must both be given, - with one value. This is the form for a robot, and for any machine where the - letters do not name the joints they look like. + with one value. This is the form that names a joint outright, a robot's + joints by number, and the only form on a machine whose letters do not name + the joints they look like and whose module declares no machine frame. With 'G0' the speed comes from the joint limits in the INI file, `[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index ac870fbd2a2..284f46751c5 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -73,17 +73,22 @@ the zero pose or a switch would jump the reported position; a module without one keeps it too, so that `G53.5` machine frame positions and `G53` positions coincide with the rotaries at zero. -The machine frame is the frame with the orientation left out: XYZ is the -pivot, the point the rotary joints do not move, in machine coordinates, and -the rotary letters are the rotary joints as they are. On a machine whose -slides line up with its frame that is the identity type, and it is what -`G13.1` and `G49` select and `G53.5` moves in. A machine whose carriage does -not line up, a slanted slide, a composite Y made by two slides, an offset -pivot, declares a machine frame type of its own next to its identity type, -and its identity type then keeps the plain meaning, joints are axes, which a -consumer uses to skip the maths. The zero-pose rule holds for the machine -frame type as it does for the identity: the working transform and the machine -frame type agree with the rotaries at zero. +The machine frame is the frame with the tool left out: XYZ is the point the +tool hangs from, the pivot of a head or the flange of a robot, in machine +coordinates, and the rotary letters are the orientation as the machine frame +type reports it, the rotary joints on a mill, the flange angles on a robot. +It is what `G13.1` and `G49` select and `G53.5` moves in. On a machine whose +slides line up with its frame it is the identity type. A machine whose +carriage does not line up, a slanted slide, a composite Y made by two slides, +an offset pivot, declares a machine frame type of its own next to its +identity type, and its identity type then keeps the plain meaning, joints are +axes, which a consumer uses to skip the maths. A robot's working transform +reports the flange in the base frame, the tool applied on top by the +interpreter and not by the maths, so the one type is both the working +transform and the machine frame, what ABB calls `tool0` and KUKA +`$NULLFRAME`. The zero-pose rule holds for the machine frame type as it does +for the identity: the working transform and the machine frame type agree with +the rotaries at zero. Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine @@ -568,7 +573,9 @@ the last answer after a switch. A type also says what it IS: `identity` marks the no-transform type, joints are axes; `primary` the working transform `G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its -identity type is that frame (see the Switchable Kinematics chapter). A module with +identity type is that frame and sets on its working transform when that +reports the flange with no tool, as the robot modules do (see the Switchable +Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 60f5fade587..77fe0642216 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -228,7 +228,7 @@ what it is rather than by number: 'G43.4' applies the tool length offset and switches to the kinstype the module declares its working transform, 'G43.5' does the same and lets the lines after it give the tool axis as a vector, and the 'G49' that cancels either switches back -to identity. See the G-code documentation for 'G43.4', 'G43.5' and +to the machine frame kinstype, what 'G13.1' selects. See the G-code documentation for 'G43.4', 'G43.5' and 'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands @@ -574,8 +574,8 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform -. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, - the rotaries as joints +. *KINSTYPE_MACHINE* the machine frame: the tool left out, XYZ the pivot + or the flange in machine coordinates A kinstype registered with switchkinsRegisterOps() carries its flag in the ops table itself, as the 'identity', 'primary' or 'machine' field; a @@ -595,15 +595,24 @@ are not in the conventional order still gets working spellings. With INI file, 'G53', 'G28' and 'G30' move in the machine frame kinstype's world too whenever another kinstype is in force; the five axis sims set it. The -machine frame type is the machine with the orientation left out, XYZ the -pivot in machine coordinates and the rotary letters the rotary joints; -on a machine whose slides line up with its frame that is the identity -type, so a module that declares no machine frame kinstype has its -identity kinstype stand in, and every shipped module is of that kind. A -module whose carriage does not line up, a slanted slide or a composite Y -on a mill-turn, declares its machine frame kinstype separately and keeps -KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since -motion and the planner skip the maths on that flag alone. At most one +machine frame kinstype is the machine with the tool left out, XYZ the +pivot or the flange in machine coordinates and the rotary letters the +orientation as that kinstype reports it; on a machine whose slides line +up with its frame that is the identity kinstype, so a module that +declares no machine frame kinstype has its identity kinstype stand in, +and every shipped machine tool module is of that kind. A robot's +working transform reports the flange in the base frame, the tool applied +on top by the interpreter and not by the maths, so pumakins and +genserkins declare that one kinstype both primary and machine: 'G13.1' +and 'G49' leave a robot on its arm kinematics, as ABB's `tool0` and +KUKA's `$NULLFRAME` do, and its identity kinstype is reached by number. +A machine frame kinstype does not read the tool offset in the parameter +block; a module whose working transform does declares a kinstype without +it. A module whose carriage does not line up, a slanted slide or a +composite Y on a mill-turn, declares its machine frame kinstype +separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints +really are the axes, since motion and the planner skip the maths on that +flag alone. At most one kinstype may be declared identity, at most one primary and at most one machine frame, and declaring a kinstype the module does not provide fails the module load. A module that declares nothing keeps working diff --git a/include/kinematics.h b/include/kinematics.h index f19a8dba1e0..eb9f0c15241 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -120,20 +120,23 @@ extern KINEMATICS_TYPE kinematicsType(void); ** numeric-only and G13.1 refuses to guess. ** ** The machine frame type is the one whose world is the machine frame -** with the orientation left out: XYZ is the pivot, the point the rotary -** joints do not move, in machine coordinates, and the rotary letters are -** the rotary joints as they are. On a machine whose slides line up with -** its frame that is the identity type, and where a module declares no -** machine frame type its identity type stands in. A module whose -** carriage does not line up, a slanted slide or an offset pivot, declares -** its machine frame type separately and keeps identity for a type whose -** joints really are the axes, since a consumer skips the maths on that -** flag alone. +** with the tool left out: XYZ is the point the tool hangs from, the pivot +** of a head or the flange of a robot, in machine coordinates, and the +** rotary letters are the orientation as that type reports it, the rotary +** joints on a mill, the flange angles on a robot. On a machine whose +** slides line up with its frame that is the identity type, and where a +** module declares no machine frame type its identity type stands in. A +** module whose carriage does not line up, a slanted slide or an offset +** pivot, declares its machine frame type separately and keeps identity +** for a type whose joints really are the axes, since a consumer skips +** the maths on that flag alone. A robot's working transform is its +** flange in the base frame with no tool in the maths, so it is the +** machine frame type as well and carries both flags. */ #define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ #define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ -#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine - coordinates, the rotaries as joints */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the tool left out, XYZ + the pivot or flange in machine coordinates */ /* flags of a kinematics type, 0 when neither applies, or -1 for a type ** the module does not provide (and for every type on a machine with plain diff --git a/include/kins_module.h b/include/kins_module.h index 13dc8f62411..17ebfd0a576 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -330,9 +330,12 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is the module's working transform, the type G43.4 switches to. machine says - this is the machine frame type, the pivot in machine coordinates with the - rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module - that leaves it unset on every type has its identity type stand in. */ + this is the machine frame type, the tool left out, the pivot or the flange + in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a + module that leaves it unset on every type has its identity type stand in. + 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. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 75bafb520f6..8417050995b 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -713,6 +713,9 @@ static int genser_tool_frame(const kins_params *p, const double *joint, return 0; } +/* the world is the last link's frame in the base frame with no tool in the + maths, the machine frame of a robot as well as its working transform, so + the one type is both */ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, @@ -721,6 +724,7 @@ const kins_ops GENSER_OPS = { .native = &TOOL_FRAME_FLANGE, .jacobian = genser_jacobian, .primary = 1, + .machine = 1, }; /* diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 7f02f1777d6..418e68e23f0 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -419,7 +419,10 @@ static int puma_inverse(const kins_params *p, kins_scratch *s, // is the shared identity one. The maths is the ISO 9787 flange frame, so // the tool axis it produces runs holder towards tip, the opposite of the // convention; the declared half turn puts it right. No closed form -// Jacobian: the shared code differences the inverse. +// Jacobian: the shared code differences the inverse. The world is the +// flange in the base frame with no tool in the maths, D6 being geometry, +// which is the machine frame of a robot as well as its working transform, +// so the one type is both. static const kins_ops puma_ops = { .forward = puma_forward, .inverse = puma_inverse, @@ -428,6 +431,7 @@ static const kins_ops puma_ops = { .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, .primary = 1, + .machine = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index dd4af0b9ee9..d1e626df12f 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1252,8 +1252,8 @@ void Interp::machine_frame_codes(block_pointer block, setup_pointer s) } // The axis letters read as machine frame coordinates, the words the module's -// machine frame type answers to: the pivot in machine coordinates, the -// rotaries as joints, in program units. The joints are read in on the type +// machine frame type answers to: the pivot or the flange in machine +// coordinates, the orientation as that type reports it, in program units. The joints are read in on the type // in force and come out as the joints of the machine frame point, the letters // not given standing where they are. Where the machine frame type is a plain // identity the letters name joints, and a letter carries a unit class where diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README index 8fda2b1b350..72b4454251e 100644 --- a/tests/ptp-robot/README +++ b/tests/ptp-robot/README @@ -1,6 +1,8 @@ The point-to-point moves on a serial robot. -pumakins maps X, Y and Z to its first three joints, which turn rather -than slide, so G53.5 refuses the machine and says which joint gives it -away. G53.7 names joints by number and works, and G53.4 still takes a -Cartesian destination. +pumakins declares its arm kinematics the machine frame as well as the +working transform, since its world is the flange in the base frame with +no tool, so G53.5 moves the flange along a base axis and holds the rest, +from the arm kinematics and from the identity alike, and G13.1, G43.4 +and G49 all leave the robot on the arm kinematics. G53.7 names joints +by number, and G53.4 takes a Cartesian destination. diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index c013a42f2ff..375de49eaf2 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 # A serial robot has no axis letter that names the joint it looks like, so -# the letter form of the point-to-point move is refused here and the joint -# form is the one that works. +# the letter form of the point-to-point move is the machine frame, the flange +# in the base frame with no tool, and the joint form names the joints. +import hal import linuxcnc import sys import time @@ -41,19 +42,6 @@ def mdi(cmd): c.wait_complete(60) return settled() -def refused(cmd, expect): - c.mdi(cmd) - c.wait_complete(30) - m = e.poll() - if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): - error("%s was accepted" % cmd) - return - if expect not in m[1]: - error("%s said %r, which does not mention %r" % (cmd, m[1].strip(), expect)) - else: - print("refused as expected: %s" % m[1].strip()) - drain() - c.state(linuxcnc.STATE_ESTOP_RESET) c.state(linuxcnc.STATE_ON) c.wait_complete(30) @@ -90,11 +78,74 @@ def refused(cmd, expect): error("G53.7 J4=%d moved joint %d from %.6f to %.6f" % (value, j, held[j], now[j])) -# the letter form is refused whichever letter is used, because X names the -# first rotary joint here; the message says so and points at G53.7 -refused("G53.5 G0 X10", "joint 0") -refused("G53.5 G0 A10", "G53.7") -refused("G53.5 G0 Z0", "angular") +# the letter form is the machine frame: on a robot the arm kinematics with +# no tool, the flange in the base frame, so a letter moves that coordinate +# of the flange and every other one holds, the orientation included +def flange(): + s.poll() + return list(s.position[:6]) + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def check_held(what, before, after, moved): + for i, name in enumerate("XYZABC"): + want = before[i] + moved.get(name, 0.0) + off = after[i] - want + if i >= 3: + # the flange angles come back in (-180, 180] + off = (off + 180.0) % 360.0 - 180.0 + if abs(off) > 1e-3: + error("%s left %s at %.4f, not %.4f" % (what, name, after[i], want)) + +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +after_joints = mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +after = flange() +print("G53.5 G0 Z+50 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z", before, after, {"Z": 50}) +if abs(after_joints[0]) > 1e-6: + error("G53.5 Z turned the waist to %.6f" % after_joints[0]) + +before = flange() +mdi("G53.5 G0 X%.6f A%.6f" % (before[0] - 40, before[3] + 15)) +after = flange() +print("G53.5 G0 X-40 A+15 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 X A", before, after, {"X": -40, "A": 15}) + +# the same move asked from the identity kinematics lands the flange at the +# same place: the letters are the machine frame whatever type is in force, +# and G13.1 comes back to the arm kinematics, which is that frame +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +mdi("G12.1 P1") +if kins_type() != 1: + error("G12.1 P1 left kins-type %d" % kins_type()) +mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +mdi("G13.1") +if kins_type() != 0: + error("G13.1 left kins-type %d, the arm kinematics is type 0" % kins_type()) +after = flange() +print("G53.5 G0 Z+50 from the identity %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z from the identity", before, after, {"Z": 50}) + +# G43.4 and G49 leave a robot on its arm kinematics: there is no other +# frame to come back to +mdi("G43.4 H1") +if kins_type() != 0: + error("G43.4 left kins-type %d" % kins_type()) +mdi("G49") +if kins_type() != 0: + error("G49 left kins-type %d" % kins_type()) +drain() + +# a letter of the wrong unit class is not a joint any more: A is the flange +# roll, in degrees, and Z is a length, so neither is refused +mdi("G53.5 G0 A0") +drain() # and the code that takes a Cartesian target still works: the point the # robot is standing on is reachable by definition, so ask for it