From d1b713889df04a1e288fb8209e8688673f36576c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:06:27 +1000 Subject: [PATCH 1/4] interpreter: add the tilted work plane, G68.2, G68.4 and G69 A frame composed inside the offset chain, so the blocks between a definition and its cancel are programmed in the tilted plane while G54 itself is untouched: world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) O and R are the plane's origin and rotation in the coordinate system active when it was defined; rotary and UVW words do not pass through it. G68.2 takes the Fanuc forms: three angles about the axes Q names (P0 Euler, P1 fixed axes), three points over up to four blocks (P2, the origin on the first point, shifted by Q0 along the plane's own axes, R on any one block), two vectors over two blocks (P3, the X direction kept as given and the normal squared to it, refused 5 degrees or more off square, as Fanuc does), each with an R about the plane's own Z. G68.4 composes any of those onto the active plane. G69 cancels; so do init, M2 and M30. An abort cancels it and tells canon even when the read ahead had already cancelled: status carries the plane the executed canon stream last set, and an abort throws the queued G69 away, so the two disagree until canon is told. An explicit G69 tells canon either way. Canon gains SET_G68_FRAME and the frame stage in rotate_and_offset_pos() and its inverse, so positions, probe results and arcs follow the plane. Task status carries g68_offset, g68_rotation and g68_active; the python canons, AXIS and halui apply it. While a plane is active G92, G52, G10 L2/L20/L10/L11 and a coordinate system change are refused, since each defines the system the plane sits on. Each form takes only the words it reads; any other (I J K with P2, R off its block, X Y Z on the P3 Q2 block, A B C U V W anywhere) is refused rather than ignored. A P2 or P3 definition left unfinished in MDI is dropped when a program is opened. The previews restate the plane in effect on the machine as a full G68.2 block from status, on the machine's coordinate system, since the bare code in the active G-codes cannot carry it. A REMAP of G68.2, G68.4 or G69 still takes over the built-in code, so configs that bring their own, like the nutating TWP sims, keep working. Tests: tests/interp/g68-words, tests/remap/g68-override, tests/workplane-preview. Both reported by Sigma1912. --- docs/src/config/python-interface.adoc | 9 + docs/src/gcode/g-code.adoc | 125 +++++ docs/src/gcode/images/g68-2-plane.svg | 48 ++ docs/src/gcode/overview.adoc | 1 + lib/python/preview_helpers.py | 32 +- lib/python/rs274/glcanon.py | 9 + lib/python/rs274/interpret.py | 14 + src/emc/nml_intf/canon.hh | 14 + src/emc/nml_intf/emc.cc | 17 + src/emc/nml_intf/emc.hh | 1 + src/emc/nml_intf/emc_nml.hh | 23 + src/emc/nml_intf/emcops.cc | 3 + src/emc/rs274ngc/Submakefile | 1 + src/emc/rs274ngc/gcode_renderer.cc | 12 +- src/emc/rs274ngc/gcode_renderer.hh | 8 +- src/emc/rs274ngc/gcodemodule.cc | 31 +- src/emc/rs274ngc/gcodemodule.hh | 35 ++ src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 22 +- src/emc/rs274ngc/interp_convert.cc | 54 +- src/emc/rs274ngc/interp_find.cc | 40 +- src/emc/rs274ngc/interp_internal.cc | 5 + src/emc/rs274ngc/interp_internal.hh | 19 +- src/emc/rs274ngc/interp_namedparams.cc | 21 +- src/emc/rs274ngc/interp_remap.cc | 7 +- src/emc/rs274ngc/interp_setup.cc | 9 + src/emc/rs274ngc/interp_workplane.cc | 462 ++++++++++++++++++ src/emc/rs274ngc/interp_write.cc | 2 +- src/emc/rs274ngc/rs274ngc_interp.hh | 14 + src/emc/rs274ngc/rs274ngc_pre.cc | 12 + src/emc/sai/saicanon.cc | 10 + src/emc/task/emccanon.cc | 76 ++- src/emc/task/emctaskmain.cc | 11 + src/emc/usr_intf/axis/extensions/emcmodule.cc | 17 + src/emc/usr_intf/axis/scripts/axis.py | 7 + src/emc/usr_intf/halui.cc | 35 +- tests/gcode-renderer/programs.py | 21 + tests/gcode-renderer/test_transform.py | 28 ++ tests/interp/g68-frame/expected | 64 +++ tests/interp/g68-frame/g68.ngc | 61 +++ tests/interp/g68-frame/test.sh | 3 + tests/interp/g68-words/expected | 16 + tests/interp/g68-words/test.sh | 25 + tests/remap/g68-override/expected | 17 + tests/remap/g68-override/r682.ngc | 4 + tests/remap/g68-override/r684.ngc | 4 + tests/remap/g68-override/r69.ngc | 4 + tests/remap/g68-override/test.ini | 11 + tests/remap/g68-override/test.ngc | 4 + tests/remap/g68-override/test.sh | 4 + tests/workplane-preview/compose.ngc | 3 + tests/workplane-preview/expected | 6 + tests/workplane-preview/move.ngc | 2 + tests/workplane-preview/test-ui.py | 102 ++++ tests/workplane-preview/test.ini | 105 ++++ tests/workplane-preview/test.sh | 4 + 56 files changed, 1609 insertions(+), 87 deletions(-) create mode 100644 docs/src/gcode/images/g68-2-plane.svg create mode 100644 src/emc/rs274ngc/interp_workplane.cc create mode 100644 tests/interp/g68-frame/expected create mode 100644 tests/interp/g68-frame/g68.ngc create mode 100755 tests/interp/g68-frame/test.sh create mode 100644 tests/interp/g68-words/expected create mode 100755 tests/interp/g68-words/test.sh create mode 100644 tests/remap/g68-override/expected create mode 100644 tests/remap/g68-override/r682.ngc create mode 100644 tests/remap/g68-override/r684.ngc create mode 100644 tests/remap/g68-override/r69.ngc create mode 100644 tests/remap/g68-override/test.ini create mode 100644 tests/remap/g68-override/test.ngc create mode 100755 tests/remap/g68-override/test.sh create mode 100644 tests/workplane-preview/compose.ngc create mode 100644 tests/workplane-preview/expected create mode 100644 tests/workplane-preview/move.ngc create mode 100755 tests/workplane-preview/test-ui.py create mode 100644 tests/workplane-preview/test.ini create mode 100755 tests/workplane-preview/test.sh diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 25d5762445b..0fce1c760ac 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -179,6 +179,15 @@ see <> for an example. *g5x_offset*:: '(returns tuple of floats)' - offset of the currently active coordinate system. +*g68_active*:: '(returns integer)' - + a tilted work plane (G68.2) is in effect. + +*g68_offset*:: '(returns tuple of floats)' - + origin of the tilted work plane, in the coordinate system it was defined in. + +*g68_rotation*:: '(returns tuple of floats)' - + rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. + *g92_offset*:: '(returns tuple of floats)' - pose of the current g92 offset. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 8162e52c993..54c4da553f6 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -93,6 +93,7 @@ as the 'L number', and so on for any other letter. |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1990,6 +1991,130 @@ G64 P0.015 Q2 .G64 Heart 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))) + +[source,ngc] +---- +G68.2 X- Y- Z- I- J- K- (three angles, Euler) +G68.2 P1 X- Y- Z- I- J- K- (three angles about fixed axes) +G68.2 P2 Q0 X- Y- Z- (three points: a shift of the origin, then) +G68.2 P2 Q1 X- Y- Z- (a first point,) +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.4 ... (any G68.2 form, on the active plane) +G69 (cancel) +---- + +A tilted work plane is a coordinate system composed on top of the active one: +the blocks between the definition and 'G69' are programmed in the plane, with +X and Y in it and Z along its normal, while the work offset underneath, 'G54' +say, is untouched. Positions on the display, probe results and +<> all take the plane into account. It is the same offset +chain as always with one more stage, applied first: + +---- +absolute = tool offset + G5x + XY rotation applied to (G92 + origin + rotation applied to program) +---- + +.'G68.2 X50 Y30 Z40 J30': a plane tilted 30 degrees about X, on G54, on the machine coordinates +image::images/g68-2-plane.svg["G68.2 tilted work plane",align="center"] + +'X', 'Y' and 'Z' are the plane's origin ('P2' gives it by points, below) and +the plane's rotation is built from the rest of the words, both in the +coordinate system that is active when the plane is defined: the work offset +with 'G92' and the XY rotation in place, which is what the position display +shows at that moment. A rotary word does not pass through the plane, since +on a TCP kinematics the rotary coordinates are the rotary joints and a plane +does not change what a joint is. Words left out are zero. + +'P' selects how the rotation is given: + +* 'P0', or no 'P': three angles 'I', 'J', 'K' applied one after another, + each about an axis of the plane as rotated so far (Euler angles). 'Q' + names the axes with three digits, 1 for X, 2 for Y and 3 for Z, no two + adjacent alike; the default is 'Q313', Z then X then Z. +* 'P1': three angles 'I', 'J', 'K', each about an axis of the coordinate + system the plane is defined in, in the order 'Q' gives; the default is + 'Q123', X then Y then Z. +* 'P2': three points, over up to four blocks with 'Q0' to 'Q3'. The + direction from the first point to the second is the plane's +X, the third + point lies on the +Y side. The origin is the first point. The 'Q0' block, + which may be left out, moves the origin by its 'X', 'Y', 'Z' along the + plane's own axes, after 'R': 'Q0 X10' puts the origin 10 along the plane's + X from the first point, the way 'X10' in a program under the plane would. + 'R' may be given on any one of the blocks. +* 'P3': two vectors, over two blocks. 'Q1' gives the origin and the +X + direction in 'I', 'J', 'K'; 'Q2' gives +Z, the normal, in 'I', 'J', 'K'. + The X direction is kept as given. The normal need not be exactly at right + angles to it, within 5 degrees; the part of the normal along X is dropped. + +'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 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 +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.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. + +'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and +an abort: the plane is not persistent and nothing about it is written to the +parameter file. + +Defining the plane does not move anything. + +While a plane is active the codes that define the coordinate system the +plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', +'G10 L20', 'G10 L10', 'G10 L11', and a change of coordinate system +('G54' to 'G59.3'). Cancel the plane first. + +The active plane is reported in the modal G-code display as the code that +defined it. Status carries it as `g68_offset`, `g68_rotation` and +`g68_active`, next to the other offsets, for displays that want to show +plane coordinates or draw the plane. + +.G68.2 Example +[source,ngc] +---- +G54 +G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +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 +G68.4 X20 P1 I0 J0 K90 (a plane 20 along X in the old one, turned 90 about Z) +G0 X0 Y0 Z5 +G69 (back to G54 as it was) +---- + +It is an error if: + +* 'P' is not 0, 1, 2 or 3, or 'Q' with 'P0' or 'P1' is not three axis digits + with no two adjacent alike. +* A 'P2' or 'P3' definition is interrupted, or its 'Q' words come out of + order. +* The points of a 'P2' definition coincide or lie on one line, or a vector of + a 'P3' definition is zero or the two are 5 degrees or more off square. +* A word the form does not read is given: 'I', 'J' or 'K' with 'P2', 'X', + 'Y', 'Z' or 'R' on the 'P3' 'Q2' block, or 'A', 'B', 'C', 'U', 'V' or 'W' + with any form. +* 'R' is given on more than one block of a 'P2' definition. +* 'G68.4' is used with no plane active. +* Cutter compensation is on. +* Polar coordinates or a motion code are used on the same line. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/gcode/images/g68-2-plane.svg b/docs/src/gcode/images/g68-2-plane.svg new file mode 100644 index 00000000000..13ed8c6df1d --- /dev/null +++ b/docs/src/gcode/images/g68-2-plane.svg @@ -0,0 +1,48 @@ + + + + + + + + + + +X + + +Y + + +Z + +G53 + + + +X + + +Y + + +Z + +G54 + + + + +J30 + + +X + + +Y + + +Z +G68.2 + + diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 90f9a06b4ec..44f9f9dcc54 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -974,6 +974,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 |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/lib/python/preview_helpers.py b/lib/python/preview_helpers.py index 8193b0309ac..d4ac90be166 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +from math import atan2, degrees, hypot def create_unitcode_and_initcode(s, inifile): a_axis_wrapped = inifile.getbool("AXIS_A", "WRAPPED_ROTARY", fallback=False) @@ -21,7 +22,36 @@ def create_unitcode_and_initcode(s, inifile): position = "%s%.8f " % (axis, pos) initcode += position active_gcodes = s.gcodes - for i in (3,6,14,7,5,4,9,12,10,16,8,11,13,15): + # 12 is the tilted work plane, restated with its words in unitcode + for i in (3,6,14,7,5,4,9,10,16,8,11,13,15): if active_gcodes[i] > -1: initcode = initcode + 'G' + str(active_gcodes[i]/10) + ' ' + plane = workplane_code(s) + if plane: + unitcode = unitcode + ' ' + plane return unitcode, initcode + +def workplane_code(s): + """The tilted work plane in effect on the machine as one G68.2 block, + or None. The bare code in s.gcodes cannot restate it: G68.2 alone is + a plane at the origin and G68.4 alone has nothing to build on. The + words are in machine units, so the block goes where the machine's G20 + or G21 is in force; a later unit change converts the plane. The block + selects the machine's coordinate system first, since the plane sits on + it and a later block may reselect it but not change it.""" + if not getattr(s, "g68_active", 0): + return None + r = s.g68_rotation + o = s.g68_offset + # G68.2 P1 turns about X by I, then Y by J, then Z by K: Rz Ry Rx + c = hypot(r[0], r[3]) + j = atan2(-r[6], c) + if c > 1e-12: + i = atan2(r[7], r[8]) + k = atan2(r[3], r[0]) + else: + i = 0.0 + k = atan2(-r[1], r[4]) + system = next((g for g in s.gcodes[1:] if 540 <= g <= 593), 540) + return "G%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( + system // 10 if system % 10 == 0 else system / 10, o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 8d01545cb3c..6d932869375 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -1247,6 +1247,15 @@ def posstrs(self): positions[X] = _x * math.cos(t) - _y * math.sin(t) positions[Y] = _x * math.sin(t) + _y * math.cos(t) positions = [(i-j) for i, j in zip(positions, s.g92_offset)] + if s.g68_active: + # the tilted work plane sits inside G92 + r = s.g68_rotation + _x = positions[X] - s.g68_offset[X] + _y = positions[Y] - s.g68_offset[Y] + _z = positions[Z] - s.g68_offset[Z] + positions[X] = r[0]*_x + r[3]*_y + r[6]*_z + positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z + positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z else: positions = list(positions) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 8815f7f8696..3c83b5502a8 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -24,8 +24,18 @@ class Translated: g5x_offset_a = g5x_offset_b = g5x_offset_c = 0 g5x_offset_u = g5x_offset_v = g5x_offset_w = 0 rotation_xy = 0 + g68_active = 0 + g68_offset = (0.0, 0.0, 0.0) + g68_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) def rotate_and_translate(self, x,y,z,a,b,c,u,v,w): + if self.g68_active: + r = self.g68_rotation + o = self.g68_offset + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[0], + r[3]*x + r[4]*y + r[5]*z + o[1], + r[6]*x + r[7]*y + r[8]*z + o[2]) + x += self.g92_offset_x y += self.g92_offset_y z += self.g92_offset_z @@ -83,6 +93,10 @@ def set_xy_rotation(self, theta): t = math.radians(theta) self.rotation_sin = math.sin(t) self.rotation_cos = math.cos(t) + def set_g68_frame(self, x, y, z, r0, r1, r2, r3, r4, r5, r6, r7, r8, active): + self.g68_active = active + self.g68_offset = (x, y, z) + self.g68_rotation = (r0, r1, r2, r3, r4, r5, r6, r7, r8) class ArcsToSegmentsMixin: plane = 1 diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 96e558dbf23..0e6f6a0c25f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -155,6 +155,9 @@ typedef struct CanonConfig_t { rotary_unlock_for_traverse(-1), g5xOffset{}, g92Offset{}, + g68Offset{}, + g68Rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68Active(0), endPoint{}, lengthUnits(CANON_UNITS_INCHES), activePlane(CANON_PLANE::XY), @@ -178,6 +181,11 @@ typedef struct CanonConfig_t { CANON_POSITION g5xOffset; CANON_POSITION g92Offset; +/* The tilted work plane (G68.2): a frame inside the G92 stage of the chain, + in mm. Program X Y Z go through R * xyz + O before anything else. */ + double g68Offset[3]; + double g68Rotation[9]; // row major + int g68Active; /* canonEndPoint is the last programmed end point, stored in case it's needed for subsequent calculations. It's in absolute frame, mm units. @@ -250,6 +258,12 @@ extern void HOME_CYCLE(void); * [JOINT_n] INI section numbering). Maps to EMC_JOINT_HOME(joint). */ extern void HOME_CYCLE_JOINT(int joint); +/* The tilted work plane. Origin in program units and a row major rotation + matrix, both in the coordinate system active when the plane was defined; + active 0 cancels it. */ +extern void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active); + /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real numbers. The units are whatever length units are being used at the time diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 3809d6bd90c..eb113e65399 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -308,6 +308,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_ROTATION_TYPE: ((EMC_TRAJ_SET_ROTATION *) buffer)->update(cms); break; + case EMC_TRAJ_SET_G68_TYPE: + ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -531,6 +534,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_G92"; case EMC_TRAJ_SET_ROTATION_TYPE: return "EMC_TRAJ_SET_ROTATION"; + case EMC_TRAJ_SET_G68_TYPE: + return "EMC_TRAJ_SET_G68"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1418,6 +1423,9 @@ void EMC_TASK_STAT::update(CMS * cms) cms->update(g5x_index); EmcPose_update(cms, &g92_offset); cms->update(rotation_xy); + EmcPose_update(cms, &g68_offset); + cms->update(g68_rotation, 9); + cms->update(g68_active); EmcPose_update(cms, &toolOffset); cms->update(activeGCodes, ACTIVE_G_CODES); cms->update(activeMCodes, ACTIVE_M_CODES); @@ -1701,6 +1709,15 @@ void EMC_TRAJ_SET_ROTATION::update(CMS * cms) cms->update(rotation); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_G68::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &origin); + cms->update(rotation, 9); + cms->update(active); +} + /* * 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 c2f4d1b6923..6a742f5fd9b 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -111,6 +111,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_SO_ENABLE_TYPE ((NMLTYPE) 235) #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_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index d31f9a5096a..46a7eafcb75 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -897,6 +897,26 @@ class EMC_TRAJ_SET_ROTATION:public EMC_TRAJ_CMD_MSG { double rotation; }; +// the tilted work plane frame (G68.2, G68.3, G68.4, G69): origin in user +// units and a rotation matrix, both in the coordinate system that was active +// when the plane was defined +class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SET_G68() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_G68_TYPE, sizeof(EMC_TRAJ_SET_G68)), + origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose origin; + double rotation[9]; // row major + int active; +}; + class EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG() @@ -1539,6 +1559,9 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { int g5x_index; // index of active g5x system EmcPose g92_offset; // in user units, currently active double rotation_xy; + EmcPose g68_offset; // tilted work plane origin, in user units + double g68_rotation[9]; // tilted work plane rotation, row major + int g68_active; // a tilted work plane is in effect EmcPose toolOffset; // tool offset, in general pose form int activeGCodes[ACTIVE_G_CODES]; int activeMCodes[ACTIVE_M_CODES]; diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 2be2c1b6f72..708ec2a9f52 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -152,6 +152,9 @@ EMC_TASK_STAT::EMC_TASK_STAT() g5x_index(0), g92_offset{}, rotation_xy(0.0), + g68_offset{}, + g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68_active(0), toolOffset{}, activeSettings{}, programUnits(CANON_UNITS_MM), diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 95f1f71f4b9..49226eb60e3 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -15,6 +15,7 @@ LIBRS274SRCS := $(addprefix emc/rs274ngc/, \ interp_inverse.cc \ interp_read.cc \ interp_write.cc \ + interp_workplane.cc \ interp_o_word.cc \ interp_g7x.cc \ nurbs_additional_functions.cc \ diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index ffcd652b5a6..d492124cf78 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -784,7 +784,9 @@ void GCodeRenderer::publish_line() { } void GCodeRenderer::transform(const Point9 &in, Point9 &out) const { - out = in + g92_; + out = in; + frame_.apply(out); + out += g92_; if(rotation_xy_ != 0.0) { double rotx = out[P9_X] * rotation_cos_ - out[P9_Y] * rotation_sin_; out[P9_Y] = out[P9_X] * rotation_sin_ + out[P9_Y] * rotation_cos_; @@ -1211,7 +1213,7 @@ void GCodeRenderer::render_arc(int line_number, double first_end, double second_ if(suppress_ > 0) return; double radius = 0.0; arc_segments(lo_, plane_, rotation_cos_, rotation_sin_, - g5x_, g92_, first_end, second_end, + g5x_, g92_, frame_, first_end, second_end, first_axis, second_axis, rotation, axis_end_point, a, b, c, u, v, w, arcdivision_, segs_, &radius); @@ -1266,6 +1268,7 @@ static void rotate(double &x, double &y, double c, double s) { int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, + const WorkFrame &frame, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -1292,6 +1295,9 @@ int arc_segments(const Point9 &lo, int plane, o -= g5xoffset; unrotate(o[P9_X], o[P9_Y], rotation_cos, rotation_sin); o -= g92offset; + // the tilted work plane sits inside G92: off the last point on the way + // in, back on every point on the way out + frame.remove(o); double theta1 = atan2(o[Y]-cy, o[X]-cx); double theta2 = atan2(n[Y]-cy, n[X]-cx); @@ -1331,10 +1337,12 @@ int arc_segments(const Point9 &lo, int plane, p[Y] = ty + cy; p[Z] = o[Z] + d[Z] * f; for(int j = P9_A; j < P9_COUNT; j++) p[j] = o[j] + d[j] * f; + frame.apply(p); p += g92offset; rotate(p[P9_X], p[P9_Y], rotation_cos, rotation_sin); p += g5xoffset; } + frame.apply(n); n += g92offset; rotate(n[P9_X], n[P9_Y], rotation_cos, rotation_sin); n += g5xoffset; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 8451b31a8f8..02e4960f9f6 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -210,6 +210,7 @@ void renderer_canon_register(pybind11::module_ &m); int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, + const WorkFrame &frame, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -373,6 +374,10 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; + void set_g68_frame(const WorkFrame &frame) override { + if(parse_state.interp_error) return; + frame_ = frame; + } // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -485,7 +490,7 @@ private: // positions to go with them. bool read_axes(); void unrotate_xy(const Point9 &p, Point3 &out) const; - // g92 -> XY rotation -> g5x, the operations and the order + // work plane -> g92 -> XY rotation -> g5x, the operations and the order // `rs274.interpret.Translated.rotate_and_translate` applies - which is // where this came from, though that method no longer runs on a rendered // parse. Not bit-identical to it by construction: the compiler is free to @@ -508,6 +513,7 @@ private: double rotation_sin_ = 0.0; double unrot_cos_ = 1.0; // the same rotation, negated, for the double unrot_sin_ = 0.0; // rotation-removed extents + WorkFrame frame_; // the tilted work plane, inside g92 Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index b8e9012d28c..09f8675dac0 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -361,6 +361,13 @@ class CallbackCanon final : public Canon { maybe_new_line(); forward("set_xy_rotation", degrees); } + void set_g68_frame(const WorkFrame &f) override { + maybe_new_line(); + const std::array &r = f.rotation; + forward("set_g68_frame", f.origin[P9_X], f.origin[P9_Y], f.origin[P9_Z], + r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7], r[8], + (int)f.active); + } void set_plane(int plane) override { maybe_new_line(); forward("set_plane", plane); @@ -540,6 +547,15 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + WorkFrame frame; + frame.active = active != 0; + frame.origin = ensure_inch({x, y, z}); + std::copy(rotation, rotation + 9, frame.rotation.begin()); + parse_state.canon->set_g68_frame(frame); +}; + void USE_LENGTH_UNITS(CANON_UNITS u) { parse_state.metric = u == CANON_UNITS_MM; } void SELECT_PLANE(CANON_PLANE pl) { @@ -1197,10 +1213,23 @@ static py::list rs274_arc_to_segments(py::handle canon, g5xoffset[i] = attr_double(canon, G5X[i]); g92offset[i] = attr_double(canon, G92[i]); } + // The tilted work plane, from a canon that keeps one (rs274.interpret + // .Translated does); one without the attributes has no plane. + WorkFrame frame; + if(py::hasattr(canon, "g68_active") && attr_int(canon, "g68_active")) { + frame.active = true; + py::sequence origin = canon.attr("g68_offset").cast(); + py::sequence rotation = canon.attr("g68_rotation").cast(); + if(py::len(origin) != 3 || py::len(rotation) != 9) + throw py::value_error("arc_to_segments: canon.g68_offset is three " + "numbers and canon.g68_rotation nine"); + for(size_t i=0; i<3; i++) frame.origin[i] = origin[i].cast(); + for(size_t i=0; i<9; i++) frame.rotation[i] = rotation[i].cast(); + } std::vector pts; int steps = arc_segments(o, plane, rotation_cos, rotation_sin, - g5xoffset, g92offset, + g5xoffset, g92offset, frame, x1, y1, cx, cy, rot, z1, a, b, c, u, v, w, max_segments, pts); py::list segs(steps); diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index fd371446afc..dbc9c520f6e 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -91,6 +91,38 @@ inline Point9 operator*(Point9 a, double s) { return a *= s; } inline Point9 operator/(Point9 a, double s) { return a /= s; } inline Point9 operator*(double s, Point9 a) { return a *= s; } +// The tilted work plane, G68.2: a frame the program's points go through +// before anything else, so it sits inside G92 and the transform chain is +// g5x + Rz(rotation_xy) * (g92 + origin + rotation * program). The origin +// is in inches like the offsets; the rotation is row major, its rows the +// plane's X, Y and Z in the coordinates outside it. +struct WorkFrame { + bool active = false; + Point9 origin = {}; + std::array rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + + // A point of the plane to the coordinates outside it. Only xyz turn: + // the rotary and UVW axes are not in the plane. + void apply(Point9 &p) const { + if(!active) return; + const std::array &r = rotation; + double x = p[P9_X], y = p[P9_Y], z = p[P9_Z]; + p[P9_X] = r[0] * x + r[1] * y + r[2] * z + origin[P9_X]; + p[P9_Y] = r[3] * x + r[4] * y + r[5] * z + origin[P9_Y]; + p[P9_Z] = r[6] * x + r[7] * y + r[8] * z + origin[P9_Z]; + } + // The way back, by the transpose. + void remove(Point9 &p) const { + if(!active) return; + const std::array &r = rotation; + double x = p[P9_X] - origin[P9_X], y = p[P9_Y] - origin[P9_Y], + z = p[P9_Z] - origin[P9_Z]; + p[P9_X] = r[0] * x + r[3] * y + r[6] * z; + p[P9_Y] = r[1] * x + r[4] * y + r[7] * z; + p[P9_Z] = r[2] * x + r[5] * y + r[8] * z; + } +}; + // --------------------------------------------------------------------------- // The canon protocol // --------------------------------------------------------------------------- @@ -144,6 +176,9 @@ public: virtual void set_g5x_offset(int index, const Point9 &offsets) = 0; virtual void set_g92_offset(const Point9 &offsets) = 0; virtual void set_xy_rotation(double degrees) = 0; + // The tilted work plane, or its cancellation; the origin already in + // inches. + virtual void set_g68_frame(const WorkFrame &frame) = 0; virtual void set_plane(int plane) = 0; virtual void set_feed_rate(double rate) = 0; virtual void set_traverse_rate(double rate) = 0; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 6eb4e1c24f8..14dd80bf570 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -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,-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, /* 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 fc5ae57586e..a0a2fe3f069 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -295,20 +295,23 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (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)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G76, or G87 to use it")); + (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")); } 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)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G76 or G87 to use it")); + (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")); } 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)), - _("K word with no G2, G3, G6.2, G33, G33.1, G76, or G87 to use it")); + 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")); } if (block->l_number != -1) { @@ -326,6 +329,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)) && + (block->g_modes[GM_WORK_PLANE] == -1) && (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) && @@ -338,7 +342,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 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 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")); @@ -356,11 +360,12 @@ 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) && (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, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -371,6 +376,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) && (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 35e9d50027e..037cbb00bd5 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,6 +1641,8 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); + CHKS((settings->g68_active), + _("Cannot change G92 offsets while a tilted work plane (G68.2) is active")); CHKS((block->a_flag && settings->axis_wrapped[AXIS_A] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), @@ -2376,6 +2378,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change coordinate systems with cutter radius compensation on"))); + { + // the plane sits on the active system; reselecting that one is harmless + int target = (g_code < G_59_1) ? (g_code - G_54) / 10 + 1 : g_code - G_59_1 + 7; + CHKS((settings->g68_active && target != settings->origin_index), + _("Cannot change coordinate systems while a tilted work plane (G68.2) is active")); + } parameters = settings->parameters; switch (g_code) { case G_54: @@ -2972,6 +2980,7 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 { int status; + CHP(work_plane_check_sequence(block, settings)); if ((block->g_modes[GM_MODAL_0] == G_4) && ONCE(STEP_DWELL)) { status = convert_dwell(settings, block->p_number); CHP(status); @@ -3000,6 +3009,10 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 status = convert_coordinate_system(block->g_modes[GM_COORD_SYSTEM], settings); CHP(status); } + if ((block->g_modes[GM_WORK_PLANE] != -1) && ONCE(STEP_WORK_PLANE)){ + status = convert_work_plane(block->g_modes[GM_WORK_PLANE], block, settings); + CHP(status); + } if ((block->g_modes[GM_CONTROL_MODE] != -1) && ONCE(STEP_CONTROL_MODE)) { status = convert_control_mode(block->g_modes[GM_CONTROL_MODE], block->p_number, block->q_number, @@ -3052,12 +3065,8 @@ offsetless machine coordinate. void Interp::get_abs_position(setup_pointer s, double abs_pos[9]) { - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - abs_pos[0] = x + s->origin_offset_x + s->tool_offset.tran.x; - abs_pos[1] = y + s->origin_offset_y + s->tool_offset.tran.y; - abs_pos[2] = s->current_z + s->axis_offset_z + s->origin_offset_z + s->tool_offset.tran.z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, + &abs_pos[0], &abs_pos[1], &abs_pos[2]); abs_pos[3] = s->AA_current + s->AA_axis_offset + s->AA_origin_offset + s->tool_offset.a; abs_pos[4] = s->BB_current + s->BB_axis_offset + s->BB_origin_offset + s->tool_offset.b; abs_pos[5] = s->CC_current + s->CC_axis_offset + s->CC_origin_offset + s->tool_offset.c; @@ -3089,12 +3098,11 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) ERS(_("Cannot set reference point with cutter compensation in effect")); } - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); - y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); - double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); + double x, y, z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); + x = PROGRAM_TO_USER_LEN(x); + y = PROGRAM_TO_USER_LEN(y); + z = PROGRAM_TO_USER_LEN(z); double a = PROGRAM_TO_USER_AX(AXIS_A, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); double b = PROGRAM_TO_USER_AX(AXIS_B, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); double c = PROGRAM_TO_USER_AX(AXIS_C, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); @@ -3513,6 +3521,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * INCH_PER_MM); settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); + settings->g68_offset[0] = (settings->g68_offset[0] * INCH_PER_MM); + settings->g68_offset[1] = (settings->g68_offset[1] * INCH_PER_MM); + settings->g68_offset[2] = (settings->g68_offset[2] * INCH_PER_MM); scale_linear_axes(settings, INCH_PER_MM); @@ -3548,6 +3559,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * MM_PER_INCH); settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); + settings->g68_offset[0] = (settings->g68_offset[0] * MM_PER_INCH); + settings->g68_offset[1] = (settings->g68_offset[1] * MM_PER_INCH); + settings->g68_offset[2] = (settings->g68_offset[2] * MM_PER_INCH); scale_linear_axes(settings, MM_PER_INCH); @@ -4733,6 +4747,8 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { double tx, ty, tz, ta, tb, tc, tu, tv, tw; int direct = block->l_number == 1; + CHKS((settings->g68_active && !direct), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); is_near_int(&toolno, block->p_number); CHP((find_tool_index(settings, toolno, &idx))); @@ -4972,6 +4988,9 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 double c; double u, v, w; double r; + + CHKS((settings->g68_active), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); double *parameters; int p_int; @@ -5454,6 +5473,8 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 ) { /* reset stuff here */ /*1*/ + // a tilted work plane does not survive the end of the program + CHP(work_plane_cancel(settings)); if (!settings->disable_auto_g54) { rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); @@ -6720,16 +6741,21 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } USE_TOOL_LENGTH_OFFSET(tool_offset); - double dx, dy; + 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 += settings->tool_offset.tran.z - tool_offset.tran.z; + 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; diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index ed341d48049..758e0fd62dc 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -190,31 +190,14 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 #endif CHKS((block->radius_flag || block->theta_flag), _("Cannot use polar coordinates with G53")); - double cx = s->current_x + s->axis_offset_x; - double cy = s->current_y + s->axis_offset_y; - rotate(&cx, &cy, s->rotation_xy); - - if(block->x_flag) { - *px = block->x_number - s->origin_offset_x - s->tool_offset.tran.x; - } else { - *px = cx; - } - - if(block->y_flag) { - *py = block->y_number - s->origin_offset_y - s->tool_offset.tran.y; - } else { - *py = cy; - } - - rotate(px, py, -s->rotation_xy); - *px -= s->axis_offset_x; - *py -= s->axis_offset_y; - - if(block->z_flag) { - *pz = block->z_number - s->origin_offset_z - s->axis_offset_z - s->tool_offset.tran.z; - } else { - *pz = s->current_z; - } + // the words are absolute; the current point supplies the rest, + // taken to the absolute frame and back with them + double wx, wy, wz; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + if(block->x_flag) { wx = block->x_number; } + if(block->y_flag) { wy = block->y_number; } + if(block->z_flag) { wz = block->z_number; } + world_to_program_xyz(s, wx, wy, wz, px, py, pz); if(block->a_flag) { if(s->axis_wrapped[AXIS_A]) { @@ -526,12 +509,7 @@ int Interp::find_relative(double x1, //!< absolute x position double *w_2, setup_pointer settings) //!< pointer to machine settings { - *x2 = x1 - settings->origin_offset_x - settings->tool_offset.tran.x; - *y2 = y1 - settings->origin_offset_y - settings->tool_offset.tran.y; - rotate(x2, y2, -settings->rotation_xy); - *x2 -= settings->axis_offset_x; - *y2 -= settings->axis_offset_y; - *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; + world_to_program_xyz(settings, x1, y1, z1, x2, y2, z2); if(settings->axis_wrapped[AXIS_A]) { CHP(unwrap_rotary(AA_2, AA_1, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 4ec4f7aafb5..bcccebe27c2 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -175,6 +175,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92)); + // 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")); + mode_zero_covets_axes = 1; + } if (mode1 != -1) { if (mode1 == G_80) { diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 8066f0eac72..d98f2e5a155 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -274,6 +274,9 @@ enum GCodes G_59_1 = 591, G_59_2 = 592, G_59_3 = 593, + G_68_2 = 682, + G_68_4 = 684, + G_69 = 690, G_61 = 610, G_61_1 = 611, G_64 = 640, @@ -378,6 +381,7 @@ enum phases { STEP_CUTTER_COMP, STEP_TOOL_LENGTH_OFFSET, STEP_COORD_SYSTEM, + STEP_WORK_PLANE, STEP_CONTROL_MODE, STEP_DISTANCE_MODE, STEP_IJK_DISTANCE_MODE, @@ -392,7 +396,7 @@ enum phases { // Modal groups // also indices into g_modes -// unused: 9,11 +// unused: 11 enum ModalGroups { GM_MODAL_0 = 0, @@ -404,7 +408,7 @@ enum ModalGroups GM_LENGTH_UNITS = 6, GM_CUTTER_COMP = 7, GM_TOOL_LENGTH_OFFSET = 8, - // 9 unused + GM_WORK_PLANE = 9, GM_RETRACT_MODE = 10, // 11 unused GM_COORD_SYSTEM = 12, @@ -780,6 +784,17 @@ struct setup double origin_offset_y; // g5x offset y double origin_offset_z; // g5x offset z double rotation_xy; // rotation of coordinate system around Z, in degrees + // the tilted work plane (G68.2): a frame inside G92, program units, + // in the coordinate system that was active when it was defined + bool g68_active; + int g68_code; // the code that defined it, for the modal display + double g68_offset[3]; + double g68_rotation[3][3]; // row major, columns are the plane's axes + int g68_seq_code; // a three-point or two-vector definition in progress + int g68_seq_p; + 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 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 diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 6287d06dc93..b00c7206c2d 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -763,26 +763,27 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_ABS_X: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = x + _setup.origin_offset_x + _setup.tool_offset.tran.x; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[0]; } break; case NP_ABS_Y: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = y + _setup.origin_offset_y + _setup.tool_offset.tran.y; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[1]; } break; case NP_ABS_Z: // abs position - *value = _setup.current_z + _setup.axis_offset_z + - _setup.origin_offset_z + _setup.tool_offset.tran.z; + { + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[2]; + } break; case NP_ABS_A: // abs position diff --git a/src/emc/rs274ngc/interp_remap.cc b/src/emc/rs274ngc/interp_remap.cc index 286ee98363f..9f197242fcb 100644 --- a/src/emc/rs274ngc/interp_remap.cc +++ b/src/emc/rs274ngc/interp_remap.cc @@ -75,8 +75,13 @@ bool Interp::is_user_defined_m_code(block_pointer block, setup_pointer settings, return (is_m_code_remappable(m_code) && settings->m_remapped[m_code]); } +// the tilted work plane codes are built in, but configs that remap them, +// like the nutating TWP sims, keep their own bool Interp::is_g_code_remappable(int g_code) -{ return g_code > 0 && g_code < 1000 && gees[g_code] == -1; } +{ + return g_code > 0 && g_code < 1000 && + (gees[g_code] == -1 || g_code == G_68_2 || g_code == G_68_4 || g_code == G_69); +} bool Interp::is_user_defined_g_code(int g_code) { return is_g_code_remappable(g_code) && _setup.g_remapped[g_code]; } diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 7ac607b8edc..c340ff98a14 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,15 @@ setup::setup() : origin_offset_y (0.0), origin_offset_z (0.0), rotation_xy (0.0), + g68_active(false), + g68_code(0), + g68_offset{0.0, 0.0, 0.0}, + g68_rotation{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + g68_seq_code(0), + g68_seq_p(0), + g68_seq_have(0), + g68_seq_r_q(-1), + g68_seq_word{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc new file mode 100644 index 00000000000..bd9453330e3 --- /dev/null +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -0,0 +1,462 @@ +/******************************************************************** +* Description: interp_workplane.cc +* +* The tilted work plane: G68.2, G68.4 and G69, and the frame they put +* inside the offset chain. +* +* The chain, as canon applies it: +* +* world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) +* +* O and R are the plane's origin and rotation, expressed in the +* coordinate system that was active when the plane was defined: G5x +* with G92 and the XY rotation in place, which is what the operator +* sees on the display and what G68.2 X Y Z means on every control. +* Rotary and UVW words do not pass through the plane: on a TCP +* kinematics the rotary world coordinates are the rotary joints, and a +* plane does not change what a joint is. +* +* The interpreter keeps its current position in program coordinates +* and only needs the chain where it reasons about absolute coordinates +* itself (G53, G28/G30, #5021, G28.1, a G43 change). Those places +* call program_to_world_xyz() and world_to_program_xyz() from here +* rather than repeating the stages. +* +* The plane is not persistent: Interp::init(), M2/M30 and G69 clear +* it. Nothing is written to the var file. +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2026 All rights reserved. +********************************************************************/ + +#include +#include +#include "rs274ngc.hh" +#include "rs274ngc_return.hh" +#include "interp_internal.hh" +#include "rs274ngc_interp.hh" + +//---------------------------------------------------------------------- +// small matrix helpers, row major double[3][3] +//---------------------------------------------------------------------- + +static void mat_identity(double m[3][3]) +{ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { m[i][j] = (i == j) ? 1.0 : 0.0; } + } +} + +// rotation about axis 1, 2 or 3 (X, Y, Z) by an angle in degrees +static void mat_rotation(int axis, double deg, double m[3][3]) +{ + double c = cos(deg * M_PI / 180.0), s = sin(deg * M_PI / 180.0); + mat_identity(m); + switch (axis) { + case 1: m[1][1] = c; m[1][2] = -s; m[2][1] = s; m[2][2] = c; break; + case 2: m[0][0] = c; m[0][2] = s; m[2][0] = -s; m[2][2] = c; break; + default: m[0][0] = c; m[0][1] = -s; m[1][0] = s; m[1][1] = c; break; + } +} + +static void mat_mul(const double a[3][3], const double b[3][3], double out[3][3]) +{ + double r[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[i][j] = a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j]; + } + } + memcpy(out, r, sizeof(r)); +} + +static void mat_apply(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[0][1]*py + m[0][2]*pz; + *y = m[1][0]*px + m[1][1]*py + m[1][2]*pz; + *z = m[2][0]*px + m[2][1]*py + m[2][2]*pz; +} + +static void mat_apply_transposed(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[1][0]*py + m[2][0]*pz; + *y = m[0][1]*px + m[1][1]*py + m[2][1]*pz; + *z = m[0][2]*px + m[1][2]*py + m[2][2]*pz; +} + +static double vec_norm(const double v[3]) +{ + return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); +} + +static void vec_cross(const double a[3], const double b[3], double out[3]) +{ + out[0] = a[1]*b[2] - a[2]*b[1]; + out[1] = a[2]*b[0] - a[0]*b[2]; + out[2] = a[0]*b[1] - a[1]*b[0]; +} + +// a rotation whose columns are the three axes +static void mat_from_axes(const double x[3], const double y[3], const double z[3], double m[3][3]) +{ + for (int i = 0; i < 3; i++) { m[i][0] = x[i]; m[i][1] = y[i]; m[i][2] = z[i]; } +} + +//---------------------------------------------------------------------- +// the chain +//---------------------------------------------------------------------- + +// the plane stage alone: program coordinates to the system the plane was +// defined in, and back +void Interp::g68_apply(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply(s->g68_rotation, x, y, z); + *x += s->g68_offset[0]; + *y += s->g68_offset[1]; + *z += s->g68_offset[2]; +} + +void Interp::g68_remove(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + *x -= s->g68_offset[0]; + *y -= s->g68_offset[1]; + *z -= s->g68_offset[2]; + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// a displacement in the system the plane was defined in, seen from the +// program: the rotation without the origin +void Interp::g68_unrotate(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// The whole chain for X Y Z, program coordinates to the absolute (G53) +// frame: the plane, G92, the XY rotation, G5x and the tool offset. +void Interp::program_to_world_xyz(setup_pointer s, + double px, double py, double pz, + double *wx, double *wy, double *wz) +{ + double x = px, y = py, z = pz; + + g68_apply(s, &x, &y, &z); + x += s->axis_offset_x; + y += s->axis_offset_y; + z += s->axis_offset_z; + rotate(&x, &y, s->rotation_xy); + *wx = x + s->origin_offset_x + s->tool_offset.tran.x; + *wy = y + s->origin_offset_y + s->tool_offset.tran.y; + *wz = z + s->origin_offset_z + s->tool_offset.tran.z; +} + +void Interp::world_to_program_xyz(setup_pointer s, + double wx, double wy, double wz, + double *px, double *py, double *pz) +{ + double x = wx - s->origin_offset_x - s->tool_offset.tran.x; + double y = wy - s->origin_offset_y - s->tool_offset.tran.y; + double z = wz - s->origin_offset_z - s->tool_offset.tran.z; + + rotate(&x, &y, -s->rotation_xy); + x -= s->axis_offset_x; + y -= s->axis_offset_y; + z -= s->axis_offset_z; + g68_remove(s, &x, &y, &z); + *px = x; + *py = y; + *pz = z; +} + +//---------------------------------------------------------------------- +// setting and clearing the plane +//---------------------------------------------------------------------- + +// Install a plane. The tool does not move, so its program coordinates +// change: take the current point through the old chain to the absolute +// frame and back through the new one. +int Interp::work_plane_set(setup_pointer s, int code, + const double origin[3], const double rotation[3][3]) +{ + double wx, wy, wz, flat[9]; + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + + for (int i = 0; i < 3; i++) { + s->g68_offset[i] = origin[i]; + for (int j = 0; j < 3; j++) { + s->g68_rotation[i][j] = rotation[i][j]; + flat[3*i + j] = rotation[i][j]; + } + } + s->g68_active = true; + s->g68_code = code; + + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(origin[0], origin[1], origin[2], flat, 1); + return INTERP_OK; +} + +// Cancel the plane if one is in effect. Canon is told only when there was +// something to cancel, unless tell_canon_anyway: an abort throws away the +// queued cancel the read ahead sent, so status and the interpreter can +// disagree and only canon can settle it. +int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) +{ + double wx, wy, wz; + static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + s->g68_seq_code = 0; + if (!s->g68_active) { + if (tell_canon_anyway) { SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); } + return INTERP_OK; + } + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + s->g68_active = false; + s->g68_code = 0; + for (int i = 0; i < 3; i++) { s->g68_offset[i] = 0.0; } + mat_identity(s->g68_rotation); + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); + return INTERP_OK; +} + +// A block that is not part of a pending three-point or two-vector +// sequence: the sequence was left incomplete. +int Interp::work_plane_check_sequence(block_pointer block, setup_pointer s) +{ + if (s->g68_seq_code == 0) { return INTERP_OK; } + if (block->g_modes[GM_WORK_PLANE] == s->g68_seq_code) { return INTERP_OK; } + s->g68_seq_code = 0; + ERS(_("G68.2 P%d sequence is incomplete: the next block must carry the next Q"), s->g68_seq_p); +} + +//---------------------------------------------------------------------- +// the definitions +//---------------------------------------------------------------------- + +// Q names the axes of a three-angle definition, three digits from 1 to 3, +// no two adjacent alike: 313 is Z X Z, 123 is X Y Z. +static int parse_axis_order(double q, int order[3]) +{ + int n = (int)round(q); + if (fabs(q - n) > 1e-9 || n < 111 || n > 333) { return -1; } + order[0] = n / 100; + order[1] = (n / 10) % 10; + order[2] = n % 10; + for (int i = 0; i < 3; i++) { + if (order[i] < 1 || order[i] > 3) { return -1; } + } + if (order[0] == order[1] || order[1] == order[2]) { return -1; } + return 0; +} + +// The rotation of a G68.2 or G68.4 block, and whether the block completes +// a definition. The three-point and two-vector forms arrive over several +// blocks with Q; the words are kept in the setup until the last one. +int Interp::work_plane_build(block_pointer block, setup_pointer s, + double origin[3], double rotation[3][3], int *complete) +{ + int p = block->p_flag ? (int)round(block->p_number) : 0; + double r = block->r_flag ? block->r_number : 0.0; + double rz[3][3]; + + *complete = 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 3)), + _("P word with G68.2 must be 0, 1, 2 or 3")); + CHKS((block->a_flag || block->b_flag || block->c_flag || + block->u_flag || block->v_flag || block->w_flag), + _("G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z")); + + if (p == 0 || p == 1) { + // three angles. P0: each about an axis of the frame as rotated so + // far (Euler, ZXZ by default). P1: each about a fixed axis of the + // system the plane is defined in, in the order Q gives (XYZ by + // default). + int order[3]; + double angle[3], m[3][3]; + + CHKS((s->g68_seq_code != 0), _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + CHKS((parse_axis_order(block->q_flag ? block->q_number : (p == 0 ? 313.0 : 123.0), order) != 0), + _("Q word with G68.2 P%d must be three axis digits 1 to 3 with no two adjacent alike"), p); + angle[0] = block->i_flag ? block->i_number : 0.0; + angle[1] = block->j_flag ? block->j_number : 0.0; + angle[2] = block->k_flag ? block->k_number : 0.0; + + mat_identity(rotation); + for (int i = 0; i < 3; i++) { + mat_rotation(order[i], angle[i], m); + if (p == 0) { + mat_mul(rotation, m, rotation); + } else { + mat_mul(m, rotation, rotation); + } + } + 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; + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + *complete = 1; + return INTERP_OK; + } + + // the sequences; a shift of the origin in the plane's own axes, P2's + // Q0 X Y Z, is applied once the rotation is known + double shift[3] = {0.0, 0.0, 0.0}; + { + int q = block->q_flag ? (int)round(block->q_number) : -1; + int code = block->g_modes[GM_WORK_PLANE]; + int first = (p == 2) ? 0 : 1, last = (p == 2) ? 3 : 2; + int expect; + + CHKS((q < 0 || fabs(block->q_number - q) > 1e-9), _("Q word missing with G68.2 P%d"), p); + if (s->g68_seq_code == 0) { + // the first block of a sequence; a three-point definition may + // leave out Q0 and take the first point as origin + CHKS((q != first && !(p == 2 && q == 1)), + _("G68.2 P%d sequence must start with Q%d"), p, first); + s->g68_seq_code = code; + s->g68_seq_p = p; + s->g68_seq_have = 0; + s->g68_seq_r_q = -1; + } else { + CHKS((s->g68_seq_code != code || s->g68_seq_p != p), + _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + } + // a three-point definition that began at Q1 has left Q0 out + if (s->g68_seq_have & (1 << 1)) { first = 1; } + expect = -1; + for (int i = first; i <= last; i++) { + if (!(s->g68_seq_have & (1 << i))) { expect = i; break; } + } + if (!(q == expect || (p == 2 && expect == 0 && q == 1))) { + s->g68_seq_code = 0; + ERS(_("G68.2 P%d expects Q%d here"), p, expect); + } + // every block of a sequence takes only the words its form reads + if (p == 2) { + CHKS((block->i_flag || block->j_flag || block->k_flag), + _("G68.2 P2 takes no I, J or K: its points are X, Y and Z")); + // R may come on any block of the definition, but only once + if (block->r_flag) { + CHKS((s->g68_seq_r_q >= 0), _("G68.2 P2 takes R once; Q%d already gave it"), s->g68_seq_r_q); + s->g68_seq_r_q = q; + } + } else if (q == 2) { + CHKS((block->x_flag || block->y_flag || block->z_flag), + _("G68.2 P3 Q2 takes no X, Y or Z: the origin is on the Q1 block")); + CHKS((block->r_flag), _("G68.2 P3 takes R on the Q1 block only")); + } + s->g68_seq_have |= 1 << q; + s->g68_seq_word[q][0] = block->x_flag ? block->x_number : 0.0; + s->g68_seq_word[q][1] = block->y_flag ? block->y_number : 0.0; + s->g68_seq_word[q][2] = block->z_flag ? block->z_number : 0.0; + s->g68_seq_word[q][3] = block->i_flag ? block->i_number : 0.0; + s->g68_seq_word[q][4] = block->j_flag ? block->j_number : 0.0; + s->g68_seq_word[q][5] = block->k_flag ? block->k_number : 0.0; + s->g68_seq_word[q][6] = r; + if (q != last) { return INTERP_OK; } + } + + // the sequence is complete + s->g68_seq_code = 0; + if (p == 2) { + // three points: the first to the second is +X, the third lies on + // the +Y side; the origin is the first point, moved by Q0's X Y Z + // along the plane's own axes; R comes from whichever block gave it + const double *p1 = s->g68_seq_word[1], *p2 = s->g68_seq_word[2], *p3 = s->g68_seq_word[3]; + double x[3], v[3], y[3], z[3], len; + + for (int i = 0; i < 3; i++) { x[i] = p2[i] - p1[i]; v[i] = p3[i] - p1[i]; } + len = vec_norm(x); + CHKS((len < 1e-9), _("G68.2 P2: the first two points coincide")); + for (int i = 0; i < 3; i++) { x[i] /= len; } + vec_cross(x, v, z); + len = vec_norm(z); + CHKS((len < 1e-9 * fmax(1.0, vec_norm(v))), _("G68.2 P2: the three points are on one line")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + for (int i = 0; i < 3; i++) { + origin[i] = p1[i]; + shift[i] = (s->g68_seq_have & 1) ? s->g68_seq_word[0][i] : 0.0; + } + r = (s->g68_seq_r_q >= 0) ? s->g68_seq_word[s->g68_seq_r_q][6] : 0.0; + } else { + // two vectors: the origin and +X on the first block, +Z on the + // second; X is kept and Z is projected onto the plane normal to + // it, as Fanuc does, and vectors 5 degrees or more off square + // are refused + const double *q1 = s->g68_seq_word[1], *q2 = s->g68_seq_word[2]; + double x[3], y[3], z[3], len, along; + + for (int i = 0; i < 3; i++) { x[i] = q1[3 + i]; z[i] = q2[3 + i]; } + len = vec_norm(x); + CHKS((len < 1e-12), _("G68.2 P3: the X direction is a zero vector")); + for (int i = 0; i < 3; i++) { x[i] /= len; } + len = vec_norm(z); + CHKS((len < 1e-12), _("G68.2 P3: the Z direction is a zero vector")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + along = x[0]*z[0] + x[1]*z[1] + x[2]*z[2]; + CHKS((fabs(along) >= sin(5 * M_PI / 180)), + _("G68.2 P3: the X and Z directions are 5 degrees or more off square")); + for (int i = 0; i < 3; i++) { z[i] -= along * x[i]; } + len = vec_norm(z); + for (int i = 0; i < 3; i++) { z[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + for (int i = 0; i < 3; i++) { origin[i] = q1[i]; } + r = q1[6]; + } + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + mat_apply(rotation, &shift[0], &shift[1], &shift[2]); + for (int i = 0; i < 3; i++) { origin[i] += shift[i]; } + *complete = 1; + return INTERP_OK; +} + +// G68.2, G68.4 and G69 from convert_g +int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) +{ + double origin[3], rotation[3][3]; + int complete; + + if (g_code == G_69) { + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot cancel a tilted work plane with cutter radius compensation on")); + return work_plane_cancel(s, true); + } + + 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), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((g_code == G_68_4 && !s->g68_active), + _("G68.4 needs an active tilted work plane to build on")); + + CHP(work_plane_build(block, s, origin, rotation, &complete)); + if (!complete) { return INTERP_OK; } + + if (g_code == G_68_4) { + // composed onto the active plane: the new origin is a point of the + // old plane and the new rotation follows the old one + double ox = origin[0], oy = origin[1], oz = origin[2]; + + g68_apply(s, &ox, &oy, &oz); + origin[0] = ox; + origin[1] = oy; + origin[2] = oz; + mat_mul(s->g68_rotation, rotation, rotation); + } + return work_plane_set(s, g_code, origin, rotation); +} diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index fd6151a4b70..63dc9b73b0b 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -126,7 +126,7 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 settings->active_g_codes[11] = (settings->control_mode == CANON_CONTINUOUS) ? G_64 : (settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1; - settings->active_g_codes[12] = -1; + settings->active_g_codes[12] = settings->g68_active ? settings->g68_code : -1; settings->active_g_codes[13] = //I don't even know how to display the mode of an arbitrary number of spindles (andypugh 17/6/16) (settings->spindle_mode[0] == SPINDLE_MODE::CONSTANT_RPM) ? G_97 : G_96; settings->active_g_codes[14] = (settings->ijk_distance_mode == DISTANCE_MODE::ABSOLUTE) ? G_90_1 : G_91_1; diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 6054da8a41a..8460d939814 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -367,6 +367,20 @@ public: setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); int convert_kins_switch(int code, block_pointer block, setup_pointer settings); + int convert_work_plane(int g_code, block_pointer block, setup_pointer settings); + int work_plane_build(block_pointer block, setup_pointer settings, + double origin[3], double rotation[3][3], int *complete); + int work_plane_set(setup_pointer settings, int code, + const double origin[3], const double rotation[3][3]); + int work_plane_cancel(setup_pointer settings, bool tell_canon_anyway = false); + int work_plane_check_sequence(block_pointer block, setup_pointer settings); + void g68_apply(setup_pointer settings, double *x, double *y, double *z); + void g68_remove(setup_pointer settings, double *x, double *y, double *z); + void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); + void program_to_world_xyz(setup_pointer settings, double px, double py, double pz, + double *wx, double *wy, double *wz); + void world_to_program_xyz(setup_pointer settings, double wx, double wy, double wz, + double *px, double *py, double *pz); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index f594b4dc204..e0a8e5985e9 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1221,6 +1221,9 @@ int Interp::init() _setup.home_flag = false; _setup.input_flag = false; _setup.kinsSwitch_flag = false; + // the tilted work plane does not survive an abort or a program start; + // canon hears about it only if there was one + work_plane_cancel(&_setup); _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -1440,6 +1443,9 @@ int Interp::open(const char *filename) //!< string: the name of the input NC-pro CHKS((_setup.file_pointer == NULL), NCE_UNABLE_TO_OPEN_FILE, filename); Interp::nurbs_reset_global_variables(); // jf + // a three-point or two-vector plane definition left unfinished in MDI + // does not continue into a program + _setup.g68_seq_code = 0; line = _setup.linetext; for (index = -1; index == -1;) { /* skip blank lines */ @@ -2737,6 +2743,12 @@ int Interp::on_abort(int reason, const char *message) reset(); _setup.mdi_interrupt = false; + // the tilted work plane goes before the abort routine runs, so that + // routine can change coordinate systems as it likes. Canon is told + // even when the read ahead had already cancelled it, since the message + // that would have said so died with the queue. + work_plane_cancel(&_setup, true); + /* A thread's queued override restore is lost when abort clears the interpreter list, so re-assert the modal state here. */ if (_setup.speed_override[_setup.active_spindle]) { diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index d60d661d4f7..6ee6ba4d4e9 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -115,6 +115,16 @@ void SET_XY_ROTATION(double t) { void HOME_CYCLE(void) { ECHO_WITH_ARGS(""); } void HOME_CYCLE_JOINT(int joint) { ECHO_WITH_ARGS("%d", joint); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, " + "[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], %d", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 706a6e41fcd..efffbf169cf 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -191,13 +191,46 @@ static void rotate(double &x, double &y, double theta) { } +// The tilted work plane, the innermost stage of the chain: what a program +// calls X Y Z is R * xyz + O in the coordinate system that was active when +// the plane was defined. Rotary and UVW words do not pass through it. +static void g68_apply(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz + canon.g68Offset[0]; + y = r[3]*px + r[4]*py + r[5]*pz + canon.g68Offset[1]; + z = r[6]*px + r[7]*py + r[8]*pz + canon.g68Offset[2]; +} + +static void g68_remove(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x - canon.g68Offset[0]; + double py = y - canon.g68Offset[1]; + double pz = z - canon.g68Offset[2]; + x = r[0]*px + r[3]*py + r[6]*pz; + y = r[1]*px + r[4]*py + r[7]*pz; + z = r[2]*px + r[5]*py + r[8]*pz; +} + +// a direction: the rotation of the plane without its origin +static void g68_rotate(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz; + y = r[3]*px + r[4]*py + r[5]*pz; + z = r[6]*px + r[7]*py + r[8]*pz; +} + /** - * Implementation of planar rotation for a 3D vector. - * This is basically a shortcut for "rotate" when the values are stored in a - * cartesian vector. + * Rotation of a direction vector into the world frame: the tilted work + * plane first, then the planar rotation about Z. * The use of static "xy_rotation" is ugly here, but is at least consistent. */ static void to_rotated(PM_CARTESIAN &vec) { + g68_rotate(vec.x, vec.y, vec.z); rotate(vec.x,vec.y,canon.xy_rotation); } #if 0 @@ -207,6 +240,8 @@ static void from_rotated(PM_CARTESIAN &vec) { #endif static void rotate_and_offset(CANON_POSITION & pos) { + g68_apply(pos.x, pos.y, pos.z); + pos += canon.g92Offset; rotate(pos.x, pos.y, canon.xy_rotation); @@ -218,6 +253,8 @@ static void rotate_and_offset(CANON_POSITION & pos) { static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) { + g68_apply(xyz.x, xyz.y, xyz.z); + xyz += canon.g92Offset.xyz(); rotate(xyz.x, xyz.y, canon.xy_rotation); @@ -242,10 +279,14 @@ static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION& pos) { res -= canon.g92Offset; + g68_remove(res.x, res.y, res.z); + return res; } static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { + g68_apply(x, y, z); + x += canon.g92Offset.x; y += canon.g92Offset.y; z += canon.g92Offset.z; @@ -508,6 +549,26 @@ void HOME_CYCLE_JOINT(int joint) interp_list.append(std::move(msg)); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) +{ + flush_segments(); + + canon.g68Offset[0] = FROM_PROG_LEN(x); + canon.g68Offset[1] = FROM_PROG_LEN(y); + canon.g68Offset[2] = FROM_PROG_LEN(z); + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } + canon.g68Active = active; + + auto msg = std::make_unique(); + msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); + msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); + msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); + for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->active = active; + interp_list.append(std::move(msg)); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, @@ -2427,7 +2488,9 @@ void ARC_FEED(int line_number, canon_debug("line = %d\n", line_number); canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); - if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS) { + // the naive cam detector works on the world XY projection of the arc, + // which a tilted work plane takes out of the XY plane + if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS && !canon.g68Active) { double mx, my; double lx, ly, lz; double unused = 0; @@ -2661,7 +2724,7 @@ void ARC_FEED(int line_number, double j2 = FROM_EXT_LEN(emcAxisGetMaxJerk(axis2)); double j_min = MIN(j1, j2); - if(canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) { + if((canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) || canon.g68Active) { // also consider the third plane's constraint, which may get // involved since we're rotated. @@ -3442,6 +3505,9 @@ void INIT_CANON() // initialize locals to original values canon.xy_rotation = 0.0; + canon.g68Offset[0] = canon.g68Offset[1] = canon.g68Offset[2] = 0.0; + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } + canon.g68Active = 0; canon.rotary_unlock_for_traverse = -1; canon.feed_mode = 0; canon.g5xOffset.x = 0.0; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 6c1ff465b30..14ae62bc1bd 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1572,6 +1572,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: // this applies the program origin after previous motions return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; @@ -1995,6 +1996,15 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; + case EMC_TRAJ_SET_G68_TYPE: { + EMC_TRAJ_SET_G68 *g68 = reinterpret_cast(cmd); + emcStatus->task.g68_offset = g68->origin; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } + emcStatus->task.g68_active = g68->active; + retval = 0; + break; + } + case EMC_TRAJ_SET_G5X_TYPE: // struct-copy program origin emcStatus->task.g5x_offset = (reinterpret_cast(cmd))->origin; @@ -2586,6 +2596,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 0fcf4da8130..dd18ab24762 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1151,6 +1151,7 @@ static PyMemberDef Stat_members[] = { { "task_paused", T_INT, O(task.task_paused), READONLY, NULL}, { "input_timeout", T_BOOL, O(task.input_timeout), READONLY, NULL}, { "rotation_xy", T_DOUBLE, O(task.rotation_xy), READONLY, NULL}, + { "g68_active", T_INT, O(task.g68_active), READONLY, "A tilted work plane (G68.2) is in effect."}, { "ini_filename", T_STRING_INPLACE, O(task.ini_filename), READONLY, NULL}, { "delay_left", T_DOUBLE, O(task.delayLeft), READONLY, NULL}, { "queued_mdi_commands", T_INT, O(task.queuedMDIcommands), READONLY, @@ -1275,6 +1276,18 @@ static PyObject *Stat_tool_offset(pyStatChannel *s, void *) { return pose(s->status.task.toolOffset); } +static PyObject *Stat_g68_offset(pyStatChannel *s, void *) { + return pose(s->status.task.g68_offset); +} + +static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.task.g68_rotation[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1577,6 +1590,10 @@ static PyGetSetDef Stat_getsetlist[] = { {(char*)"g5x_offset", (getter)Stat_g5x_offset, NULL, NULL, NULL}, {(char*)"g5x_index", (getter)Stat_g5x_index, NULL, NULL, NULL}, {(char*)"g92_offset", (getter)Stat_g92_offset, NULL, NULL, NULL}, + {(char*)"g68_offset", (getter)Stat_g68_offset, NULL, + (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*)"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}, diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index 9365254805f..77bf513a918 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -64,6 +64,7 @@ def tkerror(self, arg): steady_seconds) from hershey import Hershey from propertywindow import properties +import preview_helpers import rs274.options import nf import locale @@ -1320,6 +1321,9 @@ def open_file_guts(f, filtered=False, addrecent=True): if not interpname: unitcode = "G%d" % (20 + (s.linear_units == 1)) initcodes.append(unitcode) + plane = preview_helpers.workplane_code(s) + if plane: + initcodes.append(plane) initcodes.append("g90") initcodes.append("t%d m6" % s.tool_in_spindle) for i in range(9): @@ -1341,6 +1345,8 @@ def open_file_guts(f, filtered=False, addrecent=True): # In particular, after issuing a non-modal G like G10, that # will appear at s.gcodes[2] which caused issue #269 if i in (0, 1, 2): continue + # 12 is the tilted work plane, restated above with its words + if i == 12: continue if g == -1: continue if g == 960: # Issue #1232 initcodes.append("G96 S%.0f" % s.settings[2]) @@ -1660,6 +1666,7 @@ def next_line(*args): pass def set_g5x_offset(*args): pass def set_g92_offset(*args): pass def set_xy_rotation(*args): pass + def set_g68_frame(*args): pass def get_external_angular_units(self): return 1.0 def get_external_length_units(self): return 1.0 def set_plane(*args): pass diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index c7fd631da99..867f6dfa9a1 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -2033,28 +2033,45 @@ static void modify_hal_pins() hal_set_bool(halui_data->joint_has_fault[joint], emcStatus->motion.joint[joint].fault); } + // the relative position: the offset chain taken off in reverse, the + // tool offset, G5x, the XY rotation, G92 and the tilted work plane + double rx = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; + double ry = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; + double rz = emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.toolOffset.tran.z; + { + double t = -emcStatus->task.rotation_xy * TO_RAD; + double x = rx * cos(t) - ry * sin(t); + double y = ry * cos(t) + rx * sin(t); + rx = x - emcStatus->task.g92_offset.tran.x; + ry = y - emcStatus->task.g92_offset.tran.y; + rz -= emcStatus->task.g92_offset.tran.z; + } + if (emcStatus->task.g68_active) { + const double *r = emcStatus->task.g68_rotation; + double x = rx - emcStatus->task.g68_offset.tran.x; + double y = ry - emcStatus->task.g68_offset.tran.y; + double z = rz - emcStatus->task.g68_offset.tran.z; + rx = r[0]*x + r[3]*y + r[6]*z; + ry = r[1]*x + r[4]*y + r[7]*z; + rz = r[2]*x + r[5]*y + r[8]*z; + } + if (axis_mask & 0x0001) { hal_set_real(halui_data->axis_pos_commanded[0], emcStatus->motion.traj.position.tran.x); hal_set_real(halui_data->axis_pos_feedback[0], emcStatus->motion.traj.actualPosition.tran.x); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - x = x * cos(-emcStatus->task.rotation_xy * TO_RAD) - y * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[0], x - emcStatus->task.g92_offset.tran.x); + hal_set_real(halui_data->axis_pos_relative[0], rx); } if (axis_mask & 0x0002) { hal_set_real(halui_data->axis_pos_commanded[1], emcStatus->motion.traj.position.tran.y); hal_set_real(halui_data->axis_pos_feedback[1], emcStatus->motion.traj.actualPosition.tran.y); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - y = y * cos(-emcStatus->task.rotation_xy * TO_RAD) + x * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[1], y - emcStatus->task.g92_offset.tran.y); + hal_set_real(halui_data->axis_pos_relative[1], ry); } if (axis_mask & 0x0004) { hal_set_real(halui_data->axis_pos_commanded[2], emcStatus->motion.traj.position.tran.z); hal_set_real(halui_data->axis_pos_feedback[2], emcStatus->motion.traj.actualPosition.tran.z); - hal_set_real(halui_data->axis_pos_relative[2], emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.g92_offset.tran.z - emcStatus->task.toolOffset.tran.z); + hal_set_real(halui_data->axis_pos_relative[2], rz); } if (axis_mask & 0x0008) { diff --git a/tests/gcode-renderer/programs.py b/tests/gcode-renderer/programs.py index 3210c284d53..68caa2f16a0 100644 --- a/tests/gcode-renderer/programs.py +++ b/tests/gcode-renderer/programs.py @@ -822,6 +822,27 @@ def rotated_xy(): """ +def tilted_work_plane(): + """A ``G68.2`` plane inside a ``G54`` offset, cut in with a line and an arc. + + The plane is turned 90 degrees about Z at (0.5, 0, 0) of G54, which is + (1, 2, 3) of the machine: a plane point (x, y) lands at machine + (1.5 - y, 2 + x, 3). The line ends at plane (1, 0), the quarter arc + runs from there to (0, 1) about the plane origin, and both stay in the + plane's Z, which is machine Z 3 throughout. + """ + return """G20 G90 G94 G17 +G10 L2 P1 X1 Y2 Z3 +G54 +G68.2 X0.5 Y0 Z0 I90 J0 K0 +G0 X0 Y0 Z0 +G1 F20 X1 Y0 +G3 X0 Y1 I-1 J0 +G69 +M2 +""" + + def blank_m2(): """A program that emits no motion at all. diff --git a/tests/gcode-renderer/test_transform.py b/tests/gcode-renderer/test_transform.py index 420d47c8634..4c01c1d27f2 100755 --- a/tests/gcode-renderer/test_transform.py +++ b/tests/gcode-renderer/test_transform.py @@ -270,5 +270,33 @@ def test_the_machine_frame_extents_are_not_the_drawn_ones(self): self.assertAlmostEqual(canon.min_extents[2], -1.5, 9) +class TiltedWorkPlane(unittest.TestCase): + """``G68.2`` turns the program's points before the offsets do.""" + + def test_the_moves_land_in_the_plane(self): + canon = parse(programs.tilted_work_plane(), "XYZ") + geometry = canon.program_geometry + drawn = geometry.positions() + kinds = geometry.kinds + # the line: plane (1, 0) is machine (1.5, 3, 3) + feed = drawn[kinds == bake.KIND_FEED] + self.assertEqual(len(feed), 1) + np.testing.assert_allclose(feed[0], (1.5, 3.0, 3.0), atol=1e-6) + # the arc: every point one unit from the plane origin, machine + # (1.5, 2, 3), at the plane's Z, ending at plane (0, 1) + arc = drawn[kinds == bake.KIND_ARC] + self.assertGreater(len(arc), 3) + radius = np.hypot(arc[:, 0] - 1.5, arc[:, 1] - 2.0) + np.testing.assert_allclose(radius, 1.0, atol=1e-6) + np.testing.assert_allclose(arc[:, 2], 3.0, atol=1e-6) + np.testing.assert_allclose(arc[-1], (0.5, 2.0, 3.0), atol=1e-6) + + def test_the_plane_is_gone_after_g69(self): + program = programs.tilted_work_plane().replace("M2", "G1 X1 Y0\nM2") + drawn = parse(program, "XYZ").program_geometry.positions() + # a plain G54 point again: (1, 0) of G54 is machine (2, 2, 3) + np.testing.assert_allclose(drawn[-1], (2.0, 2.0, 3.0), atol=1e-6) + + if __name__ == "__main__": unittest.main() diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected new file mode 100644 index 00000000000..5b51be90cb6 --- /dev/null +++ b/tests/interp/g68-frame/expected @@ -0,0 +1,64 @@ + 1 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 2 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 3 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 4 N..... SET_XY_ROTATION(0.0000) + 5 N..... SET_FEED_REFERENCE(CANON_XYZ) + 6 N..... ON_RESET() + 7 N..... COMMENT("the tilted work plane through the stand alone canon") + 8 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 9 N..... COMMENT("interpreter: setting coordinate system origin") + 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 12 N..... SET_XY_ROTATION(0.0000) + 13 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") + 14 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) + 15 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 16 N..... COMMENT("the same plane by fixed axis angles about X") + 17 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) + 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 19 N..... COMMENT("the same plane by three points, the first one its origin") + 20 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) + 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 22 N..... COMMENT("Q0 moves the origin along the plane's own axes: to 11 17 32") + 23 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) + 24 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("and after R: to 8 17 31") + 26 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) + 27 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("R on a point block turns the plane the same way: 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("the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it") + 32 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) + 33 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("R turns the plane about its own Z") + 35 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) + 36 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 37 N..... COMMENT("an arc in the plane") + 38 N..... SET_FEED_RATE(100.0000) + 39 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 40 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 41 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 42 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) + 43 N..... COMMENT("and #5021 reports them") + 44 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") + 45 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 46 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 47 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 48 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") + 49 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 50 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") + 51 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) + 52 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 53 N..... COMMENT("G69 cancels") + 54 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) + 55 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 56 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 57 N..... SET_XY_ROTATION(0.0000) + 58 N..... SET_FEED_MODE(0, 0) + 59 N..... SET_FEED_RATE(0.0000) + 60 N..... STOP_SPINDLE_TURNING(0) + 61 N..... SET_SPINDLE_MODE(0 0.0000) + 62 N..... PROGRAM_END() + 63 N..... ON_RESET() + 64 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc new file mode 100644 index 00000000000..a78fa60afd7 --- /dev/null +++ b/tests/interp/g68-frame/g68.ngc @@ -0,0 +1,61 @@ +% +(the tilted work plane through the stand alone canon) +g21 g90 +g10 l2 p2 x100 y200 z300 r0 +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 +g0 x1 y2 z3 +(the same plane by fixed axis angles about X) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +g0 x1 y2 z3 +(the same plane by three points, the first one its origin) +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +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 +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +g0 x0 y0 z0 +(and after R: to 8 17 31) +g68.2 p2 q0 x1 y2 z3 r90 +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +g0 x0 y0 z0 +(R on a point block turns the plane the same way: to 8 17 31) +g68.2 p2 q0 x1 y2 z3 +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) +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 +g0 x1 y2 z3 +(an arc in the plane) +g1 f100 x0 y0 z0 +g2 x2 y0 i1 j0 +(G53 inside the plane goes to absolute coordinates) +g53 g0 x100 y200 z300 +(and #5021 reports them) +(debug, abs #5021 #5022 #5023 prog #5420 #5421 #5422) +(a probe result comes back in plane coordinates: nothing to run here) +(a tool length change moves the program coordinates along the plane axis that is world Z) +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 +g0 x1 y2 z3 +(G69 cancels) +g69 +g0 x1 y2 z3 +m2 +% diff --git a/tests/interp/g68-frame/test.sh b/tests/interp/g68-frame/test.sh new file mode 100755 index 00000000000..c11ecd785a9 --- /dev/null +++ b/tests/interp/g68-frame/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g g68.ngc | sed 's/-0\.0000/0.0000/g' +exit "${PIPESTATUS[0]}" diff --git a/tests/interp/g68-words/expected b/tests/interp/g68-words/expected new file mode 100644 index 00000000000..a35a512b500 --- /dev/null +++ b/tests/interp/g68-words/expected @@ -0,0 +1,16 @@ +p2-ijk: G68.2 P2 takes no I, J or K: its points are X, Y and Z +p2-r-q1: ok +p2-r-q3: ok +p2-r-twice: G68.2 P2 takes R once; Q0 already gave it +p2-r-q0: ok +p2-no-q0: ok +p2-q0-late: G68.2 P2 expects Q2 here +p3-q2-xyz: G68.2 P3 Q2 takes no X, Y or Z: the origin is on the Q1 block +p3-q2-r: G68.2 P3 takes R on the Q1 block only +p3-q1-r: ok +p3-4deg: ok +p3-6deg: G68.2 P3: the X and Z directions are 5 degrees or more off square +p3-zero: G68.2 P3: the X direction is a zero vector +p1-a: G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z +g684-b: G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z +g684-none: G68.4 needs an active tilted work plane to build on diff --git a/tests/interp/g68-words/test.sh b/tests/interp/g68-words/test.sh new file mode 100755 index 00000000000..c30586f57e9 --- /dev/null +++ b/tests/interp/g68-words/test.sh @@ -0,0 +1,25 @@ +#!/bin/bash +# Each G68.2 form takes only the words it reads; any other is refused +# instead of being ignored. One case per program, the message or ok. +case_() { + printf '%s\nM2\n' "$2" > case.ngc + msg=$(rs274 -g case.ngc 2>&1 | grep -v -e '^ *[0-9]* N\.\.\.\.\.' -e '^executing' | head -1) + echo "$1: ${msg:-ok}" +} +case_ p2-ijk $'G68.2 P2 Q1 X0 Y0 Z0 I1\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q1 $'G68.2 P2 Q1 X0 Y0 Z0 R10\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q3 $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0 R10' +case_ p2-r-twice $'G68.2 P2 Q0 X0 Y0 Z0 R10\nG68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0 R5\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q0 $'G68.2 P2 Q0 X0 Y0 Z0 R10\nG68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-no-q0 $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-q0-late $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q0 X0 Y0 Z0' +case_ p3-q2-xyz $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 X1 I0 J0 K1' +case_ p3-q2-r $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I0 J0 K1 R5' +case_ p3-q1-r $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0 R5\nG68.2 P3 Q2 I0 J0 K1' +case_ p3-4deg $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I0.07 J0 K1' +case_ p3-6deg $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I-0.106 J0 K1' +case_ p3-zero $'G68.2 P3 Q1 X0 Y0 Z0 I0 J0 K0\nG68.2 P3 Q2 I0 J0 K1' +case_ p1-a $'G68.2 P1 X1 J30 A10' +case_ g684-b $'G68.2 X1 J30\nG68.4 B2 I10' +case_ g684-none $'G68.4 I10' +rm -f case.ngc diff --git a/tests/remap/g68-override/expected b/tests/remap/g68-override/expected new file mode 100644 index 00000000000..5431f62e144 --- /dev/null +++ b/tests/remap/g68-override/expected @@ -0,0 +1,17 @@ + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_REFERENCE(CANON_XYZ) + N..... ON_RESET() + N..... MESSAGE(" remapped G68.2 p=0.000000 x=1.000000 j=30.000000") + N..... MESSAGE(" remapped G68.4 i=10.000000") + N..... MESSAGE(" remapped G69") + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... STOP_SPINDLE_TURNING(0) + N..... SET_SPINDLE_MODE(0 0.0000) + N..... PROGRAM_END() + N..... ON_RESET() diff --git a/tests/remap/g68-override/r682.ngc b/tests/remap/g68-override/r682.ngc new file mode 100644 index 00000000000..d4f6794ee0f --- /dev/null +++ b/tests/remap/g68-override/r682.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G68.2 p=#

x=# j=#) +o endsub +m2 diff --git a/tests/remap/g68-override/r684.ngc b/tests/remap/g68-override/r684.ngc new file mode 100644 index 00000000000..b09d030075e --- /dev/null +++ b/tests/remap/g68-override/r684.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G68.4 i=#) +o endsub +m2 diff --git a/tests/remap/g68-override/r69.ngc b/tests/remap/g68-override/r69.ngc new file mode 100644 index 00000000000..92a9db6330c --- /dev/null +++ b/tests/remap/g68-override/r69.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G69) +o endsub +m2 diff --git a/tests/remap/g68-override/test.ini b/tests/remap/g68-override/test.ini new file mode 100644 index 00000000000..c2f5ee03093 --- /dev/null +++ b/tests/remap/g68-override/test.ini @@ -0,0 +1,11 @@ +[EMC] +DEBUG=0 +LOG_LEVEL=0 + +[RS274NGC] +SUBROUTINE_PATH = . + +# G68.2, G68.4 and G69 are built in; a REMAP of them takes over +REMAP=G68.2 modalgroup=1 argspec=pqxyzijkr ngc=r682 +REMAP=G68.4 modalgroup=1 argspec=pqxyzijkr ngc=r684 +REMAP=G69 modalgroup=1 ngc=r69 diff --git a/tests/remap/g68-override/test.ngc b/tests/remap/g68-override/test.ngc new file mode 100644 index 00000000000..32df4a338a3 --- /dev/null +++ b/tests/remap/g68-override/test.ngc @@ -0,0 +1,4 @@ +G68.2 P0 X1 J30 +G68.4 I10 +G69 +M2 diff --git a/tests/remap/g68-override/test.sh b/tests/remap/g68-override/test.sh new file mode 100755 index 00000000000..99269de600c --- /dev/null +++ b/tests/remap/g68-override/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash +export INI_FILE_NAME=test.ini +rs274 -i "$INI_FILE_NAME" -g test.ngc | awk '{$1=""; print}' +exit "${PIPESTATUS[0]}" diff --git a/tests/workplane-preview/compose.ngc b/tests/workplane-preview/compose.ngc new file mode 100644 index 00000000000..d4bd048cbab --- /dev/null +++ b/tests/workplane-preview/compose.ngc @@ -0,0 +1,3 @@ +G68.4 I5 +G1 X1 Y2 Z-0.5 F2000 +M2 diff --git a/tests/workplane-preview/expected b/tests/workplane-preview/expected new file mode 100644 index 00000000000..29ffdcc2685 --- /dev/null +++ b/tests/workplane-preview/expected @@ -0,0 +1,6 @@ +plane: same end +composed: same end +composed then G68.4: same end +gimbal: same end +no plane: preview line 1: G68.4 needs an active tilted work plane to build on; machine: G68.4 needs an active tilted work plane to build on +unfinished MDI sequence: same end diff --git a/tests/workplane-preview/move.ngc b/tests/workplane-preview/move.ngc new file mode 100644 index 00000000000..42a8f621d33 --- /dev/null +++ b/tests/workplane-preview/move.ngc @@ -0,0 +1,2 @@ +G1 X1 Y2 Z-0.5 F2000 +M2 diff --git a/tests/workplane-preview/test-ui.py b/tests/workplane-preview/test-ui.py new file mode 100755 index 00000000000..e52f1759059 --- /dev/null +++ b/tests/workplane-preview/test-ui.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# A tilted work plane set in MDI reaches the preview: the preview parse +# starts from the same plane as the machine and ends where the machine does. +import math, sys, time +import gcode, linuxcnc, linuxcnc_util, preview_helpers +from rs274.interpret import Translated + +class Canon(Translated): + def __init__(self, s): + self.s = s + self.parameter_file = "test.var" + self.end = None + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def set_g5x_offset(self, i, x, y, z, *rest): + self.g5x_offset_x, self.g5x_offset_y, self.g5x_offset_z = x, y, z + def set_g92_offset(self, x, y, z, *rest): + self.g92_offset_x, self.g92_offset_y, self.g92_offset_z = x, y, z + def set_xy_rotation(self, t): + self.rotation_xy = t + self.rotation_cos = math.cos(math.radians(t)) + self.rotation_sin = math.sin(math.radians(t)) + def set_g68_frame(self, x, y, z, *rest): + self.g68_offset = (x, y, z) + self.g68_rotation = rest[:9] + self.g68_active = rest[9] + def straight_feed(self, *a): + self.end = self.rotate_and_translate(*a)[:3] + def get_tool(self, pocket): + return (-1,) + (0,) * 13 + def get_external_length_units(self): + return self.s.linear_units + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return self.s.axis_mask + def get_block_delete(self): + return 0 + +result = open("result", "w") +def say(*a): + print(*a, file=result, flush=True) + +c = linuxcnc.command() +e = linuxcnc.error_channel() +s = linuxcnc.stat() +l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e) +inifile = linuxcnc.ini("test.ini") +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MANUAL) +c.home(-1) +l.wait_for_home(joints=[1, 1, 1, 1, 0, 0, 0, 0, 0]) + +def errors(): + msgs = [] + while True: + m = e.poll() + if not m: + return msgs + msgs.append(m[1]) + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete() + for cmd in cmds: + c.mdi(cmd) + c.wait_complete() + l.wait_for_interp_state(linuxcnc.INTERP_IDLE) + +def case(name, prog, *setup): + mdi("G69", *setup) + errors() + s.poll() + canon = Canon(s) + res, seq = gcode.parse(prog, canon, *preview_helpers.create_unitcode_and_initcode(s, inifile)) + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete() + c.program_open(prog) + c.wait_complete() + c.auto(linuxcnc.AUTO_RUN, 0) + c.wait_complete() + time.sleep(0.3) + l.wait_for_interp_state(linuxcnc.INTERP_IDLE, timeout=30) + s.poll() + machine = errors() + if res > gcode.MIN_ERROR: + say("%s: preview line %d: %s; machine: %s" % (name, seq, gcode.strerror(res).strip(), "; ".join(machine))) + return + d = max(abs(canon.end[i] - s.position[i]) for i in range(3)) + say("%s: %s%s" % (name, "same end" if d < 1e-9 else "ends %g apart" % d, + "; machine: " + "; ".join(machine) if machine else "")) + +case("plane", "move.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30") +case("composed", "move.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30", "G68.4 X2 J15") +case("composed then G68.4", "compose.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30", "G68.4 X2 J15") +case("gimbal", "move.ngc", "G20", "G68.2 P1 X0.5 Y0.2 Z0.1 I20 J90 K30") +case("no plane", "compose.ngc", "G20") +case("unfinished MDI sequence", "move.ngc", "G20", "G68.2 P2 Q1 X0 Y0 Z0") +c.state(linuxcnc.STATE_OFF) diff --git a/tests/workplane-preview/test.ini b/tests/workplane-preview/test.ini new file mode 100644 index 00000000000..51a235f0cff --- /dev/null +++ b/tests/workplane-preview/test.ini @@ -0,0 +1,105 @@ +[EMC] +VERSION = 1.1 +MACHINE = workplane_preview + DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[FILTER] + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G20 G40 G49 G54 G64P0.005 G80 G90 G94 G97 M5 M9 +PARAMETER_FILE = test.var + +[PYTHON] + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 1.0 +BASE_PERIOD = 600000 +SERVO_PERIOD = 1000000 + +[EMCIO] +TOOL_TABLE = tool.tbl +TOOL_CHANGE_POSITION = 0 0 2 + +[HAL] +HALUI = halui +HALFILE = LIB:core_sim.hal +HALFILE = LIB:simulated_home.hal +HALFILE = LIB:lathe.hal + +[TRAJ] +COORDINATES = X Y Z A +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 +DEFAULT_LINEAR_ACCEL = 500.0 +DEFAULT_ANGULAR_VELOCITY = 45.0 +MAX_LINEAR_VELOCITY = 200.0 +MAX_LINEAR_ACCEL = 250.0 +MAX_ANGULAR_VELOCITY = 90.0 + +[KINS] +KINEMATICS = trivkins coordinates=XYZA +JOINTS = 4 + +[AXIS_X] +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Y] +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Z] +MIN_LIMIT = -50.0 +MAX_LIMIT = 100.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_A] +MIN_LIMIT = -360.0 +MAX_LIMIT = 360.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 + + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -50.0 +MAX_LIMIT = 100.0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +HOME_SEQUENCE = 0 diff --git a/tests/workplane-preview/test.sh b/tests/workplane-preview/test.sh new file mode 100755 index 00000000000..0317460472d --- /dev/null +++ b/tests/workplane-preview/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +rm -f result test.var test.var.bak +linuxcnc -r test.ini +rm -f test.var test.var.bak From 24f6a4a62e835e186218fe56625b6d1cdd635ff7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 17 Sep 2026 23:13:58 +1000 Subject: [PATCH 2/4] glcanon: draw the tilted work planes a program defines Where a program's G68.2 planes sit is the hardest thing to check by reading it, so the preview draws each: a rectangle lying in the plane over the moves made under it, with a margin, the plane's X, Y and Z at the centre of the rectangle in the machine axis colours, and a cross at the plane's origin, which need not lie in the rectangle: a program drilling a sphere puts every origin at the centre and works out on the surface. The rectangle sits at the plane's Z where the program reaches it and otherwise at the nearest end of the Z range worked in, so a drilling cycle shows its holes; a plane nothing moved under gets a square a tenth of the program. A plane restated in a loop is one plane. The plane in effect on the machine, what status reports the last executed G68.2 set, is drawn over the others in its own colour, so the plane the program is in stands out; an MDI plane shows the same way. The renderer already receives every plane through set_g68_frame(); it records each as a WorkPlaneRecord, origin and axes through its transform like a move endpoint, every move under it extends the extents, and the records reach the canon with the rest of the program, as glcanon_scene.WorkPlane. WorkPlanePart draws them in the program's frame through gcode.display_points(), the GEOMETRY transform for points that are not a parse's, gated by show_workplane, colours 'workplane' and 'workplane_active'. Suggested by Sigma1912. tests/glcanon/test_workplane.py, on parsed programs; docs in the G68.2 section. --- docs/src/gcode/g-code.adoc | 20 +++ lib/python/rs274/glcanon.py | 16 ++ lib/python/rs274/glcanon_scene.py | 245 ++++++++++++++++++++++++++++- src/emc/rs274ngc/gcode_renderer.cc | 130 ++++++++++++--- src/emc/rs274ngc/gcode_renderer.hh | 46 +++++- tests/gcode-renderer/canon.py | 7 +- tests/glcanon/test.sh | 3 +- tests/glcanon/test_workplane.py | 186 ++++++++++++++++++++++ 8 files changed, 627 insertions(+), 26 deletions(-) create mode 100755 tests/glcanon/test_workplane.py diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 54c4da553f6..aaae4385c9d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2070,6 +2070,26 @@ old plane. words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. +The G-code preview draws every plane a program defines, so a program can +be checked for where its planes sit before it runs: a rectangle lying in +the plane, over the moves made under it with a margin around them, the +plane's own X, Y and Z at the centre of the rectangle in the colours of +the machine axes, and a cross at the plane's origin, which need not lie +in the rectangle: a program drilling a sphere puts every plane's origin +at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes +down to it, and otherwise at the end of the Z range the program worked +in that is nearest to it, so a drilling cycle that stays above the +origin shows its holes where they are cut; a plane nothing moved under +is drawn as a square sized from the program. A plane restated with the +same words is one plane, not one per restatement. The plane in effect +on the machine, the one the last executed 'G68.2' set, is drawn over +the others in its own colour, so as the program runs the plane it is +in stands out from the ones it has been in and will be in; a plane set +from MDI shows the same way, as a square with nothing under it. The plane +in effect is drawn no smaller than the coordinate system axes, with its +own axes as long as those, so its orientation can be checked against them +before a program runs, however small the program. + 'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and an abort: the plane is not persistent and nothing about it is written to the parameter file. diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 6d932869375..8dfdd2c4ed5 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -182,6 +182,9 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0): # fixture may hold several pieces. A canon is built per file load, so # nothing else ever clears this. self.workpieces = [] + # The tilted work planes the program defined, in order, from the + # renderer's records at the end of the parse. + self.workplanes = [] def comment(self, arg): """``(WORKPIECE,...)``, ``stop``, ``notify`` and the foam Z levels. @@ -256,6 +259,8 @@ def adopt_geometry(self, pg): # hands over, so a partial load yields the partial list it always did. self.tool_list = [tool for _lineno, tool, _points in self.program_geometry.toolchanges] + self.workplanes = [glcanon_scene.WorkPlane(*record) + for record in pg.workplanes()] # -- the program record ------------------------------------------------ @@ -543,6 +548,10 @@ class GlCanonDraw: 'limits': (1.0, 0.0, 0.0), 'workpiece': glcanon_scene.WORKPIECE_COLOR, 'workpiece_alpha': glcanon_scene.WORKPIECE_ALPHA, + 'workplane': glcanon_scene.WORKPLANE_COLOR, + 'workplane_alpha': glcanon_scene.WORKPLANE_ALPHA, + 'workplane_active': glcanon_scene.WORKPLANE_ACTIVE_COLOR, + 'workplane_active_alpha': glcanon_scene.WORKPLANE_ACTIVE_ALPHA, } def __init__(self, s=None, lp=None, g=None): self.stat = s @@ -1023,6 +1032,12 @@ def get_workpiece_opacity(self): blending no one can explain.""" return min(1.0, max(0.0, float(getattr(self, 'workpiece_opacity', 0.0)))) + def get_show_workplane(self): + """Whether the tilted work planes the program defined are drawn. + Defaulted like get_show_workpiece, and toggled the same way, by + setting self.show_workplane.""" + return getattr(self, 'show_workplane', True) + def get_workpieces(self): """The stock the loaded program declared, as rs274.glcanon_scene .Workpiece records - the declared params, the outline in machine @@ -1163,6 +1178,7 @@ def frame_context(self) -> glcanon_scene.FrameContext: show_small_origin=self.show_small_origin, show_workpiece=self.get_show_workpiece(), workpiece_opacity=self.get_workpiece_opacity(), + show_workplane=self.get_show_workplane(), program_alpha=self.get_program_alpha(), grid_size=self.get_grid_size(), highlight_line=self.get_highlight_line(), diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 72c5160ce59..8d3e999a55d 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -53,6 +53,7 @@ glDepthFunc, glDepthMask, glDisable, glEnable, glLineWidth, glUseProgram) +import gcode import glnav import linuxcnc from rs274 import glcanon_bake, glcanon_gl @@ -109,6 +110,26 @@ WORKPIECE_AMBIENT = (0.65, 0.65, 0.65) WORKPIECE_DIFFUSE = (0.35, 0.35, 0.35) +#: Colour of a tilted work plane the program defined with G68.2, drawn as a +#: rectangle lying in the plane over what the program did there, with the +#: plane's own axes at its origin in the machine-axis colours. +WORKPLANE_COLOR = (0.35, 0.75, 1.00) + +#: The rectangle sits under the path it frames, like the stock outline. +WORKPLANE_ALPHA = 0.5 + +#: The plane in effect on the machine, the one the last executed G68.2 set, +#: drawn over the others so the one the program is in can be told from the +#: ones it has been in or will be in. +WORKPLANE_ACTIVE_COLOR = (1.00, 0.80, 0.20) +WORKPLANE_ACTIVE_ALPHA = 0.9 + +#: The plane in effect is what an operator who set it from MDI looks at to +#: check the orientation before running a program, so it is drawn no smaller +#: than the coordinate system axes, an inch each way, with its own axes the +#: same length as those. +WORKPLANE_ACTIVE_SIZE = 1.0 + def minmax(*args: float) -> tuple[float, float]: return min(*args), max(*args) @@ -222,7 +243,7 @@ class FrameContext: 'view', 'width', 'height', 'show_program', 'show_rapids', 'show_extents', 'show_offsets', 'show_limits', 'show_tool', 'show_live_plot', 'show_relative', 'show_metric', 'show_small_origin', - 'show_workpiece', 'workpiece_opacity', + 'show_workpiece', 'workpiece_opacity', 'show_workplane', 'program_alpha', 'grid_size', 'highlight_line', 'enable_dro', 'cone_basesize', 'disable_cone_scaling', 'view_tool_min_dia', # callables: overridable hooks and lazily-needed values @@ -304,6 +325,7 @@ class FrameContext: #: every host) is the wireframe alone. A rendering mode, not a visibility #: flag - ``show_workpiece`` still decides whether stock is drawn at all. workpiece_opacity: float + show_workplane: bool program_alpha: bool #: Ground-grid spacing in internal units; ``0`` means "no grid", and is the #: grid part's visibility gate. @@ -2640,6 +2662,224 @@ def _triangles(*corners: tuple[Float64Points, return out +class WorkPlane: + """One tilted work plane the program defined, with G68.2, G68.3 or + G68.4: where it sits on the machine, and what the program did in it. + + A record of the renderer's: it places the plane and accumulates the + moves under it while it parses, and hands the records over with the + rest of the program. :attr:`origin` and :attr:`axes` are in absolute + machine coordinates, in the canon's units, with the g92 offset, the g5x + XY rotation and the g5x offset that were active at the definition + applied the way a move endpoint on the same line gets them. The extents + are in the plane's own coordinates, the ones the program writes under + it, accumulated from every move made while it was in effect, so a + reader can tell a plane the program only oriented to from one it cut + in; they are empty (``min > max``) while nothing moved. + + Read them off the widget's canon, as the workpieces:: + + for plane in gremlin_widget.canon.workplanes: + print(plane.lineno, plane.origin, plane.extents) + """ + + __slots__ = ('lineno', 'origin', 'axes', 'min_extents', 'max_extents') + + def __init__(self, lineno: int, origin: Sequence[float], + axes: Sequence[Sequence[float]], + min_extents: Sequence[float] = (9e99, 9e99, 9e99), + max_extents: Sequence[float] = (-9e99, -9e99, -9e99)) -> None: + #: Source line of the definition, or ``-1`` without one. + self.lineno = lineno + #: The plane's origin, machine coordinates. + self.origin = tuple(origin) + #: The plane's X, Y and Z as unit vectors in machine coordinates. + self.axes = tuple(tuple(a) for a in axes) + self.min_extents = tuple(min_extents) + self.max_extents = tuple(max_extents) + + def __repr__(self) -> str: + return "" % (self.lineno, self.origin) + + def same_as(self, origin: Sequence[float], + axes: Sequence[Sequence[float]], tol: float = 1e-9) -> bool: + """Whether a definition names this plane again.""" + for a, b in zip(self.origin, origin): + if abs(a - b) > tol: + return False + for u, v in zip(self.axes, axes): + for a, b in zip(u, v): + if abs(a - b) > tol: + return False + return True + + @property + def has_moves(self) -> bool: + return self.max_extents[X] >= self.min_extents[X] + + @property + def extents(self) -> tuple[tuple[float, ...], tuple[float, ...]]: + """``(min_xyz, max_xyz)`` of the moves made under the plane, in the + plane's coordinates.""" + return self.min_extents, self.max_extents + + def machine_point(self, x: float, y: float, z: float) -> tuple[float, float, float]: + """A point of the plane in machine coordinates.""" + o, (ax, ay, az) = self.origin, self.axes + return (o[0] + x*ax[0] + y*ay[0] + z*az[0], + o[1] + x*ax[1] + y*ay[1] + z*az[1], + o[2] + x*ax[2] + y*ay[2] + z*az[2]) + + +def active_workplane(ctx: FrameContext) -> "WorkPlane | None": + """The plane in effect on the machine, as status reports it: the one the + last executed G68.2 set, whether from the program or from MDI. + + Status carries it as the interpreter gave it, the origin in the + coordinate system the plane was defined in and in machine units, so it + goes through the g92 offset, the XY rotation and the g5x offset the way + the canon takes a definition, in the canon's units. Where it is one the + loaded program defined, that record is returned, extents and all; a + plane from MDI, or from a program no longer loaded, comes back as a + record of its own with nothing under it. + """ + s = ctx.stat + if s is None or not getattr(s, 'g68_active', 0): + return None + try: + o = ctx.to_internal_units(s.g68_offset) + r = s.g68_rotation + g92 = ctx.to_internal_units(s.g92_offset) + g5x = ctx.to_internal_units(s.g5x_offset) + t = math.radians(s.rotation_xy) + except (AttributeError, TypeError): + return None + c, sn = math.cos(t), math.sin(t) + + def through(x: float, y: float, z: float) -> tuple[float, float, float]: + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], + r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], + r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) + return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) + + origin = through(0, 0, 0) + axes = [] + for unit in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = through(*unit) + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + for plane in getattr(ctx.canon, 'workplanes', ()): + # status has been through machine units and back, so not to the bit + if plane.same_as(origin, axes, tol=1e-6): + return plane + return WorkPlane(-1, origin, axes) + + +class WorkPlanePart(Part): + """The tilted work planes the program defined, one drawing each, and + the one in effect on the machine over them in its own colour. + + A plane is drawn where the program worked in it: a rectangle lying in + the plane, over the moves made under it with a margin around them, at + the plane's own Z where the program reached that and otherwise at the + end of the Z range it worked in nearest to it, so a drilling cycle that + never comes down to the plane's origin still shows its holes; a plane + nothing moved under gets a square sized from the program. The plane's + axes stand at the centre of the rectangle in the machine-axis colours, + so the direction the program's X and Y took can be read where the work + is, and the plane's origin is marked with a cross, since the two need + not coincide: a program that drills a sphere puts every plane's origin + at the centre and works out on the surface. + + The active plane is whatever status says the last executed G68.2 set, + so it walks through the program's planes as the program runs, and an + MDI plane shows too, as a square with nothing under it. It is drawn no + smaller than the coordinate system axes, whatever the program's size, + since a plane set from MDI is checked by eye against them. + """ + + #: Inner lines each way, to read as a surface rather than an outline. + SUBDIVISIONS = 4 + + def draw(self, ctx: FrameContext) -> None: + canon = ctx.canon + planes = list(getattr(canon, 'workplanes', ())) + active = active_workplane(ctx) + if not planes and active is None: + return + # the size a plane with nothing, or only a point, under it is drawn + # at: a tenth of the program, as the extents part spaces its labels + size = max(canon.max_extents[X] - canon.min_extents[X], + canon.max_extents[Y] - canon.min_extents[Y], + canon.max_extents[Z] - canon.min_extents[Z], 2) * .1 + others = [p for p in planes if p is not active] + if others: + self.draw_planes(ctx, others, size, + ctx.colors.get('workplane', WORKPLANE_COLOR), + ctx.colors.get('workplane_alpha', WORKPLANE_ALPHA)) + if active is not None: + self.draw_planes(ctx, [active], size, + ctx.colors.get('workplane_active', WORKPLANE_ACTIVE_COLOR), + ctx.colors.get('workplane_active_alpha', WORKPLANE_ACTIVE_ALPHA), + least=WORKPLANE_ACTIVE_SIZE) + + def draw_planes(self, ctx: FrameContext, planes: Sequence[WorkPlane], + size: float, color: Color, alpha: float, + least: float = 0.0) -> None: + """``size`` is the side of the square a plane with nothing under it + gets; ``least`` a floor under every plane's half-width and axis + length, in the canon's units.""" + canon = ctx.canon + outline, inner, cross = [], [], [] + axes = {'axis_x': [], 'axis_y': [], 'axis_z': []} + n = self.SUBDIVISIONS + for plane in planes: + if plane.has_moves: + lo, hi = plane.min_extents, plane.max_extents + cx, cy = (lo[X] + hi[X]) / 2, (lo[Y] + hi[Y]) / 2 + hw = max((hi[X] - lo[X]) * .6, size / 2, least) + hh = max((hi[Y] - lo[Y]) * .6, size / 2, least) + z = min(max(lo[Z], 0.0), hi[Z]) + else: + cx = cy = z = 0.0 + hw = hh = max(size / 2, least) + x0, x1, y0, y1 = cx - hw, cx + hw, cy - hh, cy + hh + corner = [plane.machine_point(x0, y0, z), plane.machine_point(x1, y0, z), + plane.machine_point(x1, y1, z), plane.machine_point(x0, y1, z)] + for i in range(4): + outline += [corner[i], corner[(i + 1) % 4]] + for i in range(1, n): + f = i / n + inner += [plane.machine_point(x0 + (x1 - x0) * f, y0, z), + plane.machine_point(x0 + (x1 - x0) * f, y1, z), + plane.machine_point(x0, y0 + (y1 - y0) * f, z), + plane.machine_point(x1, y0 + (y1 - y0) * f, z)] + length = max(hw * .5, hh * .5, least) + centre = plane.machine_point(cx, cy, z) + axes['axis_x'] += [centre, plane.machine_point(cx + length, cy, z)] + axes['axis_y'] += [centre, plane.machine_point(cx, cy + length, z)] + axes['axis_z'] += [centre, plane.machine_point(cx, cy, z + length)] + arm = length * .4 + cross += [plane.machine_point(-arm, 0, 0), plane.machine_point(arm, 0, 0), + plane.machine_point(0, -arm, 0), plane.machine_point(0, arm, 0), + plane.machine_point(0, 0, -arm), plane.machine_point(0, 0, arm)] + def to_display(points: list[tuple[float, float, float]]) -> Any: + # machine to drawn coordinates by the program's GEOMETRY, as the + # renderer places a move; nothing is parsing, so through the + # module function rather than the canon + return np.array(gcode.display_points(canon.program_geometry, points)) + ctx.prim.draw_lines(ctx, to_display(outline), color, alpha) + ctx.prim.draw_lines(ctx, to_display(inner), color, alpha * .4) + ctx.prim.draw_lines(ctx, to_display(cross), color, alpha) + # the axes wide, as the backplot is: at one pixel in the axis colours + # they are lost against the rectangle and the program under it + set_line_width(3.0) + try: + for name, verts in axes.items(): + ctx.prim.draw_lines(ctx, to_display(verts), ctx.colors[name], alpha) + finally: + set_line_width(1.0) + + class WorkpiecePart(Part): """Stock declared by ``(WORKPIECE,...)`` comments. @@ -2734,6 +2974,7 @@ def __init__(self) -> None: self.limits_box = LimitsBoxPart() self.backplot = BackplotPart() self.workpiece = WorkpiecePart() + self.workplane = WorkPlanePart() self.tool = ToolPart() self.overlay = OverlayPart() super().__init__([ @@ -2750,6 +2991,8 @@ def __init__(self) -> None: (self.backplot, lambda ctx: ctx.show_live_plot), (self.workpiece, lambda ctx: ctx.show_workpiece and ctx.canon is not None), + (self.workplane, lambda ctx: ctx.show_workplane + and ctx.canon is not None), (self.tool, lambda ctx: ctx.show_tool), (self.overlay, lambda ctx: ctx.enable_dro), ]) diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index d492124cf78..c79d3e90661 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -58,6 +58,42 @@ void GCodeRenderer::set_spindle_speed(double rpm) { / (2 * M_PI) * rpm; } +// The plane's origin and axes on the machine, through the transform the way +// a move endpoint on this line goes, so the drawn plane and the moves in it +// agree. Recorded here rather than at hand over because the offsets in +// force at the definition are what place it, and they move on. +void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { + if(parse_state.interp_error) return; + frame_ = frame; + workplane_ = -1; + if(!frame.active || !data_) return; + Point9 at; + transform({}, at); + Point3 origin = {at[P9_X], at[P9_Y], at[P9_Z]}; + std::array axes; + for(int i = 0; i < P3_COUNT; i++) { + Point9 unit = {}; + unit[i] = 1.0; + transform(unit, at); + for(int j = 0; j < P3_COUNT; j++) axes[i][j] = at[j] - origin[j]; + } + std::vector &planes = data_->workplanes; + if(!planes.empty() && planes.back().same_as(origin, axes)) { + workplane_ = (long)planes.size() - 1; + return; + } + WorkPlaneRecord record; + record.lineno = parse_state.current_line(); + record.origin = origin; + record.axes = axes; + for(int i = 0; i < P3_COUNT; i++) { + record.extents[BOX_MIN][i] = 9e99; + record.extents[BOX_MAX][i] = -9e99; + } + planes.push_back(record); + workplane_ = (long)planes.size() - 1; +} + void GCodeRenderer::arc_feed(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c, @@ -289,6 +325,18 @@ void preview_geometry_register(py::module_ &m) { points_tuple(r.pts, d.nplanes))); return out; }, "(lineno, tool number, points per plane) per tool change") + .def("workplanes", [](const PreviewData &d) { + py::list out; + for(const WorkPlaneRecord &r : d.workplanes) + out.append(py::make_tuple(r.lineno, triple(r.origin), + py::make_tuple(triple(r.axes[0]), triple(r.axes[1]), + triple(r.axes[2])), + triple(r.extents[BOX_MIN]), + triple(r.extents[BOX_MAX]))); + return out; + }, "(lineno, origin, (x, y, z) axes, min, max) per tilted work " + "plane; origin and axes in the machine frame, the extents " + "of the moves made under it in the plane's own coordinates") .def("tool_offsets", [](const PreviewData &d) { py::list out; for(const ToolOffsetRecord &r : d.tool_offsets) @@ -456,28 +504,43 @@ static py::tuple point_rows(const std::vector &xyz) { return out; } -static py::tuple renderer_transform(py::handle self, py::handle points) { - if(!parse_state.in_parse || !parse_state.canon) - throw py::value_error("transform: no parse in progress"); - // The parse transforms *its* canon's points. A canon left over from an - // earlier parse would otherwise read this one's offsets and be believed. - if(self.ptr() != parse_state.callback) - throw py::value_error("transform: not the canon of the parse in " - "flight"); +// The GEOMETRY-string transform (the C vertex9), for one point through one +// compiled plane. +static void plane_point(const std::vector &ops, + const Point9 &pts9, Point3 &out) { + out = {}; + for(const GeomOp &op : ops) op.apply(pts9, out); +} + +// Rows of 3 or 9 numbers as 9-DOF points, the short ones zero-filled. +static std::vector read_points(const char *what, py::handle points) { std::vector in; for(py::handle row : points) { Point9 p = {}; size_t n = 0; for(py::handle value : py::reinterpret_borrow(row)) { if(n == P9_COUNT) - throw py::value_error("transform: a point takes 3 or 9 " - "numbers"); + throw py::value_error(std::string(what) + + ": a point takes 3 or 9 numbers"); p[n++] = value.cast(); } if(n != P3_COUNT && n != P9_COUNT) - throw py::value_error("transform: a point takes 3 or 9 numbers"); + throw py::value_error(std::string(what) + + ": a point takes 3 or 9 numbers"); in.push_back(p); } + return in; +} + +static py::tuple renderer_transform(py::handle self, py::handle points) { + if(!parse_state.in_parse || !parse_state.canon) + throw py::value_error("transform: no parse in progress"); + // The parse transforms *its* canon's points. A canon left over from an + // earlier parse would otherwise read this one's offsets and be believed. + if(self.ptr() != parse_state.callback) + throw py::value_error("transform: not the canon of the parse in " + "flight"); + std::vector in = read_points("transform", points); std::vector machine(in.size() * P3_COUNT); std::vector display(in.size() * P3_COUNT); if(!parse_state.canon->transform_points(in.data(), in.size(), @@ -486,6 +549,35 @@ static py::tuple renderer_transform(py::handle self, py::handle points) { return py::make_tuple(point_rows(machine), point_rows(display)); } +// The GEOMETRY transform of a canon's first drawn plane, for points that +// are not a parse's: a work plane an operator set from MDI is drawn in +// the same frame the program is, and there is no parse in flight to ask. +static py::tuple display_points(py::handle program_geometry, py::handle points) { + py::object ro = program_geometry.attr("ro"); + long mask = ro.attr("axis_mask").cast(); + bool respect = PyObject_IsTrue(ro.attr("respect_offsets").ptr()); + double rox = 0.0, roy = 0.0, roz = 0.0; + if(respect) { + rox = ro.attr("x").cast(); + roy = ro.attr("y").cast(); + roz = ro.attr("z").cast(); + } + py::sequence names = program_geometry.attr("planes").cast(); + if(py::len(names) < 1) + throw py::value_error("display_points: no drawn plane"); + std::vector ops = GeomOp::compile(names[0].cast(), + mask, rox, roy, roz); + std::vector in = read_points("display_points", points); + std::vector display(in.size() * P3_COUNT); + for(size_t i = 0; i < in.size(); i++) { + Point3 drawn; + plane_point(ops, in[i], drawn); + for(int c = 0; c < P3_COUNT; c++) + display[i * P3_COUNT + c] = drawn[c]; + } + return point_rows(display); +} + // `gcode.RendererCanon` and its time-estimating subclass. These hold a // reference of their own: the module attribute is deletable, and a type freed // under us would leave every later PyObject_TypeCheck reading freed memory. @@ -575,6 +667,13 @@ void renderer_canon_register(py::module_ &m) { "TimeEstimateCanon", py::tuple(), tns); m.attr("TimeEstimateCanon") = timed; time_estimate_canon_type = (PyTypeObject *)timed.release().ptr(); + m.def("display_points", &display_points, + py::arg("program_geometry"), py::arg("points"), + "Machine points to display coordinates through a canon's " + "program_geometry: the GEOMETRY string of its first drawn plane " + "and its rotation offsets, as the renderer applies them. Points " + "are 3 or 9 numbers each. For geometry that is not a program's, " + "outside any parse."); renderer_canon_type = (PyTypeObject *)cls.release().ptr(); } @@ -807,14 +906,6 @@ void GCodeRenderer::untransform(const Point9 &in, Point9 &out) const { out -= g92_; } -// The GEOMETRY-string transform (the C vertex9), for one point through one -// compiled plane. -static void plane_point(const std::vector &ops, - const Point9 &pts9, Point3 &out) { - out = {}; - for(const GeomOp &op : ops) op.apply(pts9, out); -} - // Points through the live transform, by the two steps a move endpoint takes: // the g92 -> XY rotation -> g5x chain, and then the GEOMETRY string of the // plane drawn first. Nothing is sampled, linearised or rebuilt, so a point @@ -925,6 +1016,7 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, unsigned char cat) { data_->moves ++; accumulate_extents(p1, p2); + reach(p2); double dx = p2[P9_X] - p1[P9_X], dy = p2[P9_Y] - p1[P9_Y], dz = p2[P9_Z] - p1[P9_Z]; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 02e4960f9f6..e62171777fe 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -134,6 +134,40 @@ struct ToolOffsetRecord { Point9 offsets; // xo..wo, as the canon was given them }; +// A tilted work plane the program defined, G68.2, G68.3 or G68.4: where it +// sits on the machine, and what the program did in it. The origin and the +// axes are in the machine frame, through the offsets in force at the +// definition the way a move endpoint on the same line gets them; the +// extents are in the plane's own coordinates, over every move made while +// it was in effect, empty (min > max) while nothing moved. A definition +// that names the plane in effect again is the same record, so a program +// that restates its plane in a loop has one plane, not one per pass. +struct WorkPlaneRecord { + int lineno; + Point3 origin; + std::array axes; // the plane's X, Y and Z, unit + Box3 extents; + + bool same_as(const Point3 &o, const std::array &a) const { + for(int i = 0; i < P3_COUNT; i++) { + if(fabs(origin[i] - o[i]) > 1e-9) return false; + for(int j = 0; j < P3_COUNT; j++) + if(fabs(axes[i][j] - a[i][j]) > 1e-9) return false; + } + return true; + } + // Take in a machine point the program reached under the plane. + void extend(const Point9 &p) { + Point3 d = {p[P9_X] - origin[P3_X], p[P9_Y] - origin[P3_Y], + p[P9_Z] - origin[P3_Z]}; + for(int i = 0; i < P3_COUNT; i++) { + double v = d[0] * axes[i][0] + d[1] * axes[i][1] + d[2] * axes[i][2]; + if(v < extents[BOX_MIN][i]) extents[BOX_MIN][i] = v; + if(v > extents[BOX_MAX][i]) extents[BOX_MAX][i] = v; + } + } +}; + struct PreviewData { PreviewData() {}; ~PreviewData(); @@ -186,6 +220,7 @@ struct PreviewData { std::vector dwells; std::vector toolchanges; std::vector tool_offsets; + std::vector workplanes; // Record an offset at the current row; drop records governing no row. void set_tool_offset(const Point9 &offsets); void drop_trailing_tool_offset(); @@ -374,10 +409,7 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; - void set_g68_frame(const WorkFrame &frame) override { - if(parse_state.interp_error) return; - frame_ = frame; - } + void set_g68_frame(const WorkFrame &frame) override; // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -472,6 +504,11 @@ private: // One move into the geometry: extents, length, then its vertices. void fill(int line_number, const Point9 &p1, const Point9 &p2, unsigned char cat); + // The plane in effect takes in the end of every move made under it. + void reach(const Point9 &p) { + if(workplane_ >= 0 && data_) + data_->workplanes[(size_t)workplane_].extend(p); + } // One record vertex at `at`, writing its per-plane position to `points`. void mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points); @@ -514,6 +551,7 @@ private: double unrot_cos_ = 1.0; // the same rotation, negated, for the double unrot_sin_ = 0.0; // rotation-removed extents WorkFrame frame_; // the tilted work plane, inside g92 + long workplane_ = -1; // its record, or none in effect Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/tests/gcode-renderer/canon.py b/tests/gcode-renderer/canon.py index 41c6dc88e2f..8e20be946fe 100644 --- a/tests/gcode-renderer/canon.py +++ b/tests/gcode-renderer/canon.py @@ -221,7 +221,8 @@ class FakePreview: def __init__(self, planes, lines, kinds, tools=None, moves=None, rapid_length=0.0, cutting_length=0.0, tool_numbers=None, dwells=(), toolchanges=(), dwell_time=0.0, extents=None, - axes="", axis_positions=None, tool_offsets=()): + axes="", axis_positions=None, tool_offsets=(), + workplanes=()): self._planes = [np.ascontiguousarray(p, dtype=np.float32) for p in planes] lines = np.asarray(lines, dtype=np.uint32) @@ -241,6 +242,7 @@ def __init__(self, planes, lines, kinds, tools=None, moves=None, self._dwells = list(dwells) self._toolchanges = list(toolchanges) self._tool_offsets = list(tool_offsets) + self._workplanes = list(workplanes) #: The machine's letters come back from every parse; the positions are #: (N, 0) unless asked for, which is what C hands over. self.axes = axes @@ -270,6 +272,9 @@ def axis_positions(self): def tool_offsets(self): return self._tool_offsets + def workplanes(self): + return list(self._workplanes) + def extents(self): return self._extents diff --git a/tests/glcanon/test.sh b/tests/glcanon/test.sh index 0268ec90d1f..cf53bc3c761 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -15,8 +15,9 @@ set -e if python3 -c 'import OpenGL' 2>/dev/null; then ./test_workpiece.py >&2 + ./test_workplane.py >&2 else - echo "skip: test_workpiece.py needs PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py new file mode 100755 index 00000000000..5f40f76f6a3 --- /dev/null +++ b/tests/glcanon/test_workplane.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +"""The tilted work planes the preview draws: what the renderer records from +G68.2 and the moves under it, and what the part draws from that. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: nothing below calls into OpenGL. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import rs274.glcanon as glcanon +from rs274 import glcanon_bake, glcanon_scene +from test_workpiece import run + + +def program(*lines, offset="G10 L2 P1 X0 Y0 Z0"): + """A G20 program under G54: the plane definitions and moves given.""" + return "\n".join(("G20 G90 G94 G17", offset, "G54", "F10") + lines + + ("M2", "")) + + +#: A plane at (0, 40, 20) of G54, tilted 30 degrees about X; G68.2 takes +#: Euler angles about Z, X and Z, so the middle one alone tilts about X. +TILTED = "G68.2 X0 Y40 Z20 I0 J30 K0" + + +class WorkPlaneRecordTest(unittest.TestCase): + def test_where_the_plane_sits(self): + canon = run(program(TILTED, offset="G10 L2 P1 X100 Y0 Z0")) + plane, = canon.workplanes + # the origin goes through the g5x offset like a move endpoint + self.assertAlmostEqual(plane.origin[0], 100) + self.assertAlmostEqual(plane.origin[1], 40) + self.assertAlmostEqual(plane.origin[2], 20) + # X is untouched by a tilt about X; Z leans back towards -Y + x, y, z = plane.axes + self.assertAlmostEqual(x[0], 1) + self.assertAlmostEqual(z[1], -math.sin(math.radians(30))) + self.assertAlmostEqual(z[2], math.cos(math.radians(30))) + self.assertFalse(plane.has_moves) + self.assertEqual(plane.lineno, 5) + + def test_moves_extend_the_plane_in_its_own_coordinates(self): + canon = run(program(TILTED, "G1 X-30 Y-20 Z5", "G1 X30 Y20 Z-4")) + plane, = canon.workplanes + self.assertTrue(plane.has_moves) + lo, hi = plane.extents + for got, want in zip(lo + hi, (-30, -20, -4, 30, 20, 5)): + self.assertAlmostEqual(got, want) + # and a point of the plane comes back where the move went + p = plane.machine_point(30, 20, -4) + for a, b in zip(p, canon.program_geometry.positions()[-1]): + self.assertAlmostEqual(a, b, 5) + + def test_cancel_stops_the_recording(self): + canon = run(program("G68.2 X0 Y0 Z0 I0 J0 K0", "G1 X1 Y1 Z0", "G69", + "G1 X50 Y50 Z50")) + plane, = canon.workplanes + self.assertAlmostEqual(plane.max_extents[0], 1) + + def test_restating_the_plane_is_one_plane(self): + lines = [] + for i in range(3): + lines += ["G68.2 X0 Y0 Z0 I0 J30 K0", "G1 X%d Y0 Z0" % i] + lines.append("G68.2 X0 Y0 Z0 I0 J45 K0") + canon = run(program(*lines)) + self.assertEqual(len(canon.workplanes), 2) + self.assertAlmostEqual(canon.workplanes[0].max_extents[0], 2) + + def test_an_arc_under_the_plane_stays_in_it(self): + # a quarter circle in the tilted plane: every drawn point is in the + # plane, one unit from its origin + canon = run(program(TILTED, "G1 X1 Y0 Z0", "G3 X0 Y1 I-1 J0")) + plane, = canon.workplanes + o, (ax, ay, az) = plane.origin, plane.axes + geometry = canon.program_geometry + arc = geometry.positions()[geometry.kinds == glcanon_bake.KIND_ARC] + self.assertGreater(len(arc), 3) + for p in arc: + d = [p[i] - o[i] for i in range(3)] + self.assertAlmostEqual(sum(d[i] * az[i] for i in range(3)), 0, 5) + self.assertAlmostEqual(math.sqrt(sum(v * v for v in d)), 1, 5) + lo, hi = plane.extents + self.assertAlmostEqual(hi[0], 1, 5) + self.assertAlmostEqual(hi[1], 1, 5) + + +class StatStub: + """The plane in effect as status reports it, in machine units (mm).""" + + def __init__(self, active=0, origin=(0, 0, 0), tilt_about_x=0.0): + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + self.g68_active = active + self.g68_offset = tuple(origin) + (0,) * 6 + self.g68_rotation = (1, 0, 0, 0, c, -s, 0, s, c) + self.g92_offset = (0,) * 9 + self.g5x_offset = (0,) * 9 + self.rotation_xy = 0.0 + + +class CtxStub: + """What WorkPlanePart is allowed to read, and a record of what it drew.""" + + class Prim: + def __init__(self): + self.calls = [] + self.points = [] + + def draw_lines(self, ctx, points, color, alpha=1.0): + self.calls.append((len(points), tuple(color), alpha)) + self.points.append(points) + + def __init__(self, canon, stat=None): + self.canon = canon + self.stat = stat + self.colors = glcanon.GlCanonDraw.colors + self.prim = self.Prim() + + @staticmethod + def to_internal_units(pos): + return [v / 25.4 for v in pos[:3]] + list(pos[3:]) + + +def drawn(*lines, stat=None, **kw): + """Parse, then draw the planes through the stub, and hand back the stub.""" + canon = run(program(*lines, **kw)) + canon.calc_extents() + ctx = CtxStub(canon, stat) + glcanon_scene.WorkPlanePart().draw(ctx) + return ctx + + +class WorkPlanePartTest(unittest.TestCase): + def test_draws_the_outline_the_grid_and_the_axes(self): + ctx = drawn(TILTED, "G1 X-30 Y-20 Z5", "G1 X30 Y20 Z-4") + n = glcanon_scene.WorkPlanePart.SUBDIVISIONS + # four edges, the inner lines each way, the origin cross, then one + # line per axis + self.assertEqual([c[0] for c in ctx.prim.calls], [8, 4 * (n - 1), 6, 2, 2, 2]) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual([c[1] for c in ctx.prim.calls[3:]], + [tuple(glcanon.GlCanonDraw.colors[k]) for k in ('axis_x', 'axis_y', 'axis_z')]) + + def test_draws_nothing_without_planes(self): + for canon in (None, object(), run(program("G1 X1"))): + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(ctx.prim.calls, []) + + def test_the_active_plane_is_the_program_record(self): + # the canon counts in inches, status in this machine's mm + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + canon = run(program(TILTED, "G1 X-30 Y-20 Z5", + "G68.2 X0 Y0 Z0 I0 J0 K0", "G1 X1 Y1 Z1")) + canon.calc_extents() + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + glcanon_scene.WorkPlanePart().draw(ctx) + # the other plane in the plane colour, then the active one in its own + colors = [c[1] for c in ctx.prim.calls] + self.assertEqual(colors[0], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual(colors[6], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + self.assertEqual(len(ctx.prim.calls), 12) + + def test_an_mdi_plane_is_drawn_on_its_own(self): + canon = run(program("G1 X1")) + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=1, origin=(10, 0, 0))) + plane = glcanon_scene.active_workplane(ctx) + self.assertEqual(plane.lineno, -1) + self.assertAlmostEqual(plane.origin[0], 10 / 25.4) + self.assertFalse(plane.has_moves) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + + def test_no_plane_in_effect(self): + ctx = drawn("G68.2 X0 Y0 Z0 I0 J0 K0", stat=StatStub(active=0)) + self.assertIsNone(glcanon_scene.active_workplane(ctx)) + self.assertEqual(len(ctx.prim.calls), 6) + + +if __name__ == '__main__': + unittest.main() From b3597c79ea6efe68fdf436303ebcb45be5675699 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 30 Sep 2026 10:50:48 +1000 Subject: [PATCH 3/4] nutating sim remap: build the G68.2 planes the way Fanuc and the native code do The sim's remap and the native G68.2 built different planes from the same program in four places; the remap's G68.2 and G68.4 now follow the native code, which follows the Fanuc 0i-F operator manual (B-64604EN-2, 5.7.1): - R turned the plane about the work Z; it now turns it about the plane's own Z, in every form. - The three-point form (P2) took Q0's X Y Z as the origin in the work coordinates. The origin is now the first point, and Q0, which may be left out, shifts it along the plane's own axes. R may be given on any one block; R on two blocks is refused, as natively, since the manual does not say which would win. - The two-vector form (P3) read the X vector as a point and subtracted the origin from it, and it took Q0 and Q1 where Fanuc takes Q1 and Q2. It now takes Q1 and Q2, reads I J K as a direction, keeps X as given, squares the normal to it and refuses vectors 5 degrees or more off square, or a zero vector. --- .../python/remap.py | 216 +++++++++++------- 1 file changed, 128 insertions(+), 88 deletions(-) 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 index 05fc53261a1..0dfc2aeaca1 100755 --- 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 @@ -914,8 +914,9 @@ def g682(self, **words): # 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(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] @@ -961,8 +962,9 @@ def g682(self, **words): # 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(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] @@ -981,39 +983,38 @@ def g682(self, **words): # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} + 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 four calls to fill all required parameters - if q == 0: # define new origin and rotation + # 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 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' + 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[3] = 'done' + 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[4] = 'done' + 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[5] = 'done' + twp_flag[4] = 'done' else: # reset the twp parameters reset_twp_params() @@ -1024,11 +1025,23 @@ def g682(self, **words): 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, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] + [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'] @@ -1064,9 +1077,12 @@ def g682(self, **words): 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_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] @@ -1077,16 +1093,14 @@ def g682(self, **words): 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 - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) 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 = {'q0':[], 'q1':[]} + twp_build_params = {'q1':[], 'q2':[]} log.debug(' twp_build_params: %s', twp_build_params) - if q == 0: # define new origin of the twp + 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 @@ -1096,13 +1110,13 @@ def g682(self, **words): 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['q0'] = [x,y,z,i,j,k,r] + twp_build_params['q1'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp + 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['q1'] = [i1,j1,k1] + twp_build_params['q2'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters @@ -1116,30 +1130,37 @@ def g682(self, **words): # 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['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] + 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)) - # build unit vector defining x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) + # 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) - # the two vectors must be orthogonal - if orth > 0.001: + if abs(orth) >= sin(radians(5)): reset_twp_params() - msg = ("G68.2 P3: Vectors are not orthogonal.") + 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) @@ -1156,8 +1177,9 @@ def g682(self, **words): 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_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)) @@ -1280,8 +1302,9 @@ def g684(self, **words): # 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(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] @@ -1327,8 +1350,9 @@ def g684(self, **words): # 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(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] @@ -1347,39 +1371,38 @@ def g684(self, **words): # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} + 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 four calls to fill all required parameters - if q == 0: # define new origin and rotation + # 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 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' + 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[3] = 'done' + 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[4] = 'done' + 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[5] = 'done' + twp_flag[4] = 'done' else: # reset the twp parameters reset_twp_params() @@ -1390,11 +1413,23 @@ def g684(self, **words): 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, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] + [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'] @@ -1430,9 +1465,12 @@ def g684(self, **words): 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_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] @@ -1443,14 +1481,12 @@ def g684(self, **words): 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 - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) 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 = {'q0':[], 'q1':[]} - if q == 0: # define new origin and first vector (direction of x in the twp) + 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 @@ -1460,13 +1496,13 @@ def g684(self, **words): 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['q0'] = [x,y,z,i,j,k,r] + twp_build_params['q1'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp + 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['q1'] = [i1,j1,k1] + twp_build_params['q2'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters @@ -1480,34 +1516,37 @@ def g684(self, **words): # 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['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] + 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)) - # build unit vector defining x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) + # 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) - # the two vectors must be orthogonal - if orth != 0: - # reset the twp parameters + if abs(orth) >= sin(radians(5)): reset_twp_params() - ## reset the parameter values - #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - #twp_build_params = {'q0':[], 'q1':[]} - msg = ("G68.4 P3: Vectors are not orthogonal.") + 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) @@ -1524,8 +1563,9 @@ def g684(self, **words): 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 - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_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)) From 39cd0c85c4175fd5e606781583074cc8bfcf17bf Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 1 Oct 2026 17:54:41 +1000 Subject: [PATCH 4/4] nutating sim remap: build the G68.2 planes the way Fanuc and the native code do The sim's remap and the native G68.2 built different planes from the same program in four places, and the remap refused one program the native code accepts; the remap's G68.2 and G68.4 now follow the native code, which follows the Fanuc 0i-F operator manual (B-64604EN-2, 5.7.1): - R turned the plane about the work Z; it now turns it about the plane's own Z, in every form. - The three-point form (P2) took Q0's X Y Z as the origin in the work coordinates. The origin is now the first point, and Q0, which may be left out, shifts it along the plane's own axes. R may be given on any one block; R on two blocks is refused, as natively, since the manual does not say which would win. - The two-vector form (P3) read the X vector as a point and subtracted the origin from it, and it took Q0 and Q1 where Fanuc takes Q1 and Q2. It now takes Q1 and Q2, reads I J K as a direction, keeps X as given, squares the normal to it and refuses vectors 5 degrees or more off square, or a zero vector. - G68.4 refused to build on a plane that was defined but not yet activated with G53.n. Like the native code, it now needs only a defined plane. --- .../table-rotary_spindle-rotary-nutating/python/remap.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index 0dfc2aeaca1..7431b8ad635 100755 --- 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 @@ -1237,10 +1237,10 @@ def g684(self, **words): ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' yield INTERP_EXECUTE_FINISH - if not hal.get_value(twp_is_active): # ie there is currently no TWP defined + 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 active to increment from. Run G68.2 or G68.3 first.") + 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