diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 38e706fac22..c66ba5e7238 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw) DEBUG = 0 [DISPLAY] - GEOMETRY = XYZCBW - OPEN_FILE = ./5axisgui.ngc +# GEOMETRY tells the AXIS live plot how to guess the tool position from +# the nine axis values: letters translate, ABC rotate the point. W is +# deliberately absent: the plot would add the W value as a Z offset after +# the rotations, while the kinematics spends W along the tool axis, so +# the guess never matches. + GEOMETRY = XYZCB + OPEN_FILE = ./g532-fused-orient-move.ngc INCREMENTS = 10 mm, 1 mm, .1 mm JOG_AXES = XYZC DISPLAY = axis @@ -27,6 +32,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = 5axis.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [EMCMOT] EMCMOT = motmod diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc index 7b2c5a8f188..cbc165137d7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc @@ -1,3 +1,17 @@ +; 5axisgui.ngc - the historical demo program, unchanged. +; It drills a sphere pattern with W words. W was never a physical +; quill: no motor is connected to its joint, the kinematics folds +; the word into the XYZ slides, and the head sliding along the tool +; axis is what plunges the rigidly mounted tool. Watch the slides +; in vismach do the stroke. +; +; The quirk, then and now: the controller keeps W as a separate +; world coordinate, so the programmed XYZ point does not move and +; the preview shows nothing of the stroke; only the W DRO tracks it. +; Drilling the controller can see, check and preview is what the +; tilted work plane family is for (g532-fused-orient-move.ngc, +; g536-orient-then-move.ngc). + # = 60 ; sphere radius # = 5 ; safe distance # = -5 @@ -13,6 +27,7 @@ # = [90/#] g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active t#m6g43 g53 g0 x0y0z#b0c0 w0 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/README b/configs/sim/axis/vismach/5axis/bridgemill/README index 2adf5599795..4a92c6a2078 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/README +++ b/configs/sim/axis/vismach/5axis/bridgemill/README @@ -1,6 +1,46 @@ -This is a simulation of an XYZBCWY 5 axis bridge mill. +This is a simulation of an XYZBCWY 5 axis bridge mill with a +tilting head (B, C) and a W axis. -Example demo: +W is not a physical axis of the machine and never was: no motor is +connected to its joint. The kinematics folds a W word into the +XYZ joints, so the head slides along the tool axis and the +rigidly mounted tool goes with it: that is the whole trick, and it +is what vismach shows. There is no quill to draw because there is +no quill. + +The quirk: the controller keeps W as a separate world coordinate, +so a W word leaves the programmed XYZ point untouched and the +preview cannot show the stroke; the W DRO tracks it. It also +means the planner does not see the tool-axis motion, so for +drilling the controller can check and preview, use the tilted work +plane family. + +Because the W joint is virtual, a point-to-point move on it +(g53.5/g53.7 j5=) moves no motor on a real machine while the +controller believes the world moved, the opposite of a W word. +Do not drill that way. + +Demo programs: + + g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work + planes (g68.2), using g53.2 to solve each + orientation without moving (STAY), then + one fused g0 that turns the head and + travels to the hole at the same time + (TCP motion) + g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain + MOVE style): reorient about the fixed + tip, then travel in the tilted plane + 5axisgui.ngc --------------- the historical demo program, unchanged: + it drills the same pattern with W words, + the slides doing the stroke while the + preview stays blind to it + +The tool table provides tool 100 (length 100). Load it with +t100m6g43 so the vismach tool and the kinematics pivot length +(pivotsum: 250 + tool length) match. + +Example MDI: 1) $ linuxcnc 5axis.ini 2) F1 ---------- Estop off @@ -8,12 +48,12 @@ Example demo: CTRL-HOME --- home all F5 ---------- MDI tab 3) orient vismach gui as required - 4) g0w10 ; retract w + 4) g0w10 ; head slides up the tool axis, tip with it 5) g43h100 ; tool offset (100) 6) g0b45 ; tilt 45 deg wrt z 7) g0c30 ; rotate 30 deg in xy - 8) g0w-10 ; simulate drill - 9) g0w10 ; retract drill + 8) g0w-10 ; head slides back, tip plunges 10 along the tool axis + 9) g0w10 ; and back 10) etc Note: Motion for the W coordinate is incorporated diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc new file mode 100644 index 00000000000..bd193299758 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -0,0 +1,50 @@ +; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes: +; g68.2 tilts the plane onto each hole normal, g53.2 solves the head +; orientation without moving (STAY), and one fused g0 turns the head +; and travels to the hole at the same time (TCP motion). See README +; for the other demos. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.2 ; solve the orientation, stay put + g0 x0 y0 z[#+#] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc new file mode 100644 index 00000000000..2354e7e0625 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc @@ -0,0 +1,49 @@ +; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6 +; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move, +; then travel to the next hole in the tilted plane with a separate g0. +; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.6 ; reorient about the tip, a TCP move + g0 x0 y0 z[#+#] + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini index bd6dc79e82a..d5a083dad1d 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini @@ -22,6 +22,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap PARAMETER_FILE = xyzab-tdr.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini index 9c2bc00e33e..ae0995c189c 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini @@ -35,6 +35,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzac.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini index 45d780a251c..0311dfcba4a 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini @@ -35,6 +35,12 @@ SUBROUTINE_PATH = ./remap_subs REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap PARAMETER_FILE = xyzbc.var +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README index fea7c5d4882..ba7fafdbe6d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README @@ -1,27 +1,20 @@ This is a simulation configuration for a 6 axis machine with one table rotary and two spindle rotary joints -This simulation also includes a python remap of Gcodes for tilted workplane (TWP) functionality. -Both the kinematic and the twp remap support nutation of the secondary rotary joints (ie A or B ) form 0 to 90°. -Hence this also works for the 'usual' orthogonal spindle rotary-tilt type machines by setting the nutation angle to 90°. +This simulation uses the interpreter's tilted work plane codes, documented in the G-code section of the manual: -Implemented TWP functionality: -G68.2 : defines twp using euler-angles, pitch-roll-yaw, 2-vectors, 3 points, optionally with offset in XYZ and rotation in XY -G68.3 : defines twp from current tool orientation, optionally with offset in XYZ and rotation in XY -G68.4 : same as G68.2 but as an incremental definition from an active TWP plane -G69 : cancels the current twp (resets all parameters, moves to G54 and sets Identity kinematics) -G53.1 (P) : spindle orientation without tcp, switches to G59 and activates tool kinematics -G53.3 (P XYZ) : same as G53.1 but with simultaneous move the the XYZ coords on the twp plane -G53.6 (P) : same as G53.1 but spindle orientation with tcp +G68.2 : defines the plane by three angles, three points or two vectors, with an origin in XYZ and a turn R about the plane's Z +G68.3 : defines the plane from the current tool direction, with an origin in XYZ and a turn R +G68.4 : any G68.2 form, composed onto the active plane +G69 : cancels the plane +G53.1 (P Q) : orients the tool to the plane, rotaries only, the linear joints stay where they are +G53.3 (P Q XYZ) : orients the tool and moves to XYZ in the plane, interpolated in joint space +G53.6 (P Q) : orients the tool with the tool centre point held -- Spindle is C primary, A secondary or B secondary as defined in the [TWP] section of the ini file -- All G53.x commands will respect axis limits as set in the ini file for the respective primary and secondary spindle joints. -- The P word sets the orientation strategy: 0(default)=shortest distance, - 1=positive rotation only, - 2=negative rotation only - (this applies to the primary rotary, the secondary moves the shortest distance) +The orientation codes need the TCP kinematics (G12.1 P1). P picks the solution, nearest to the present rotary position first. Q0 holds the table and lets the head do it, Q1 lets the table take part as well. + +The kinematic supports nutation of the secondary rotary joint (A or B) from 0 to 90 degrees, so this also covers the usual orthogonal spindle rotary-tilt machines by setting the nutation angle to 90 degrees. + +The python maths this configuration used to carry as a remap lives on in tests/kins-twp, where the kinematics module is checked against it. For more: https://forum.linuxcnc.org/show-your-stuff/49103-kinematic-model-for-a-5axis-mill-with-universal-nutating-head?start=0#271334 - -Full Documentation can be found at: -https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/table-rotary_spindle-rotary-nutating/Documentation diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc index 31feddcff12..4dc18624cf2 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -13,5 +14,6 @@ o100 REPEAT[100] g0 y50 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc index 37b6f85032d..e1d5bb5d34b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc @@ -1,4 +1,5 @@ g69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g68.2 q121 i0 j5 @@ -20,4 +21,5 @@ o100 REPEAT[1000] o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc index f01324916b5..0a2e4731539 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -7,7 +8,7 @@ o100 REPEAT[100] g68.4 q131 i-35 j-35 k0 ;g53.3 p0 x50y50z150 g53.6 - x50y50z150 + g0 x50y50z150 g0 z100 g0 x-50 g0 y-50 @@ -16,5 +17,6 @@ o100 REPEAT[100] g0 x0y0z120 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc index 7586f2f1d3c..bc2f46c6b6b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc @@ -1,4 +1,6 @@ g69 +g13.1 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g0 x0y0z100 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc index c7e263ce601..c1f2bbf2abe 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc @@ -5,5 +5,4 @@ osub g0 x50 g0 y50 g0 x0y0z120 - g52 x0y0z0 oendsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py deleted file mode 100755 index 7431b8ad635..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ /dev/null @@ -1,1614 +0,0 @@ -# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' -# G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 -# -# Copyright ()c) 2025 David Mueller -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -''' -The remap does the following: - -- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). -- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). -- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). -- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. -- Sets the kinematic modes -- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. -- Used MDI commands to: - - Move the rotary joints to the calculated positions - - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates -- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode -''' - - -import sys -import traceback -import numpy as np -from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs -from interpreter import * -import emccanon -from util import lineno, call_pydevd -import hal - -# logging -import logging -# this name will be printed first on each log message -log = logging.getLogger('remap.py; TWP') -# we have to setup a handler to be able to set the log level for this module -handler = logging.StreamHandler() -formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') -handler.setFormatter(formatter) -log.addHandler(handler) - -# set up parsing of the inifile -import os -import linuxcnc -# get the path for the ini file used to start this config -inifile = os.environ.get("INI_FILE_NAME") - -# adding the remap_funcs folder to the system path. The machine specific -# functions live beside the ini file, which is the working directory, and the -# parent is searched too so a config may keep them one level up and share them -# between variants. -cwd = os.getcwd() -parent = os.path.abspath(os.path.join(cwd, os.pardir)) -sys.path.insert(0, parent) -sys.path.insert(0, cwd) -from remap_funcs_twp import * - -# instantiate the LinuxCNC ini-parser -config = linuxcnc.ini(inifile) - -# debug setting -try: - debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) - if debug_setting > 4: debug_setting = 4 - if debug_setting < 0: debug_setting = 0 -except Exception as error: - debug_setting = 1 - log.warning("Unable to parse debug setting given in INI. Setting it to 1.") -debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) -log.setLevel(debug_levels[debug_setting]) - -## ROTARY JOINT LETTERS -# primary rotary joint (independent of the secondary joint) -joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# secondary rotary joint (dependent on the primary joint) -joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() - -if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): - log.error("Unable to parse joint letters given in INI [TWP].") -elif joint_letter_primary == joint_letter_secondary: - log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") -else: - # get the MIN/MAX limits of the respective rotary joint letters - category = 'AXIS_' + joint_letter_primary - primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', - joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) - category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', - joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) - - -## CONNECTIONS TO THE HELPER COMPONENT -twp_comp = 'twp-helper-comp.' -twp_is_defined = twp_comp + 'twp-is-defined' -twp_is_active = twp_comp + 'twp-is-active' - -# Which rotary joint should be prioritized when calculating optimal joint rotation angles -try: - optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) -except Exception as error: - log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") - optimization_priority = 1 - -# raise InterpreterException if execute() or read() fail -throw_exceptions = 1 - -## VALUE INITIALIZATION -# we start with the identity matrix (ie the twp is equal to the world coordinates) -twp_matrix = np.asmatrix(np.identity(4)) - -# some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for G53.n -# [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] -# note that we use string since boolean True == 1, which gives wrong results if we want -# to count the elements that are True because it is counted as integer '1' -# eg: twp_flag = [0, 1, 'empty'] -twp_flag = [] -# we need a place to store the twp-build-parameters if the mode needs more than one call -twp_build_params = {} -# container to store the current work offset during twp operations -current_work_offset_number = 1 -saved_work_offset = [0,0,0] -# orientation mode refers to the strategy used to choose from the different rotary angles for a given -# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being -# the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) -orient_mode = 0 - - - -# define the basic rotation matrices -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) - - - -def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians - # returns the rotation matrices for given order and angles - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - debug_msg = (f' Euler order {order} requested with angles: ' - f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') - log.debug(debug_msg) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - #log.debug(' Returning euler rotation as matrix: \n %s', matrix) - return matrix - - -def calc_joint_angles(z_vector_req, x_vector_req): - # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector - # returns an empty list if no valid position could be found - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(' z_vector_requested: %s', z_vector_req) - log.debug(' x_vector_requested: %s', x_vector_req) - # set the tolerance value - epsilon = 0.0001 - # create np.array so we can easily calculate differences and check elements - z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) - # calculate joint values using kinematic specific formula - try: - (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) - except Exception as error: - log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) - - # remove any duplicate values from the results - theta_1_calcd = tuple(set(theta_1_calcd)) - theta_2_calcd = tuple(set(theta_2_calcd)) - log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) - log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) - if theta_1_calcd == None or theta_2_calcd == None: - return [] - angle_pairs_list = [] - # create a list of paired combinations of returned angles (theta_1 , theta_2) - for i in range(len(theta_1_calcd)): - for j in range(len(theta_2_calcd)): - angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) - angle_pairs_list = list(set(angle_pairs_list)) - # iterate through the list and check if a particular pair actually produces the requested z-vector orientation - joint_angles_list = [] - for i in range(len(angle_pairs_list)): - debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' - f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') - log.debug(debug_msg) - # we start with an identity matrix (ie oriented to world) - matrix_in = np.asmatrix(np.identity(4)) - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - try: - matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) - except Exception as error: - log.error('kins_calc_transformation_matrix, %s', error) - # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column - z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) - # calculate the difference of the respective elements - z_vector_diff = z_vector_req - z_vector_would_be - log.debug(' z_vector_diff: %s', z_vector_diff) - # and check if all elements are within [-epsilon,epsilon] - match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) - log.debug(' Is the z-vector-vector close enough ? %s', match_z) - if match_z: - joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) - for (theta_1, theta_2) in joint_angles_list: - log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') - return joint_angles_list # returns radians - - -def calc_shortest_distance(pos, trgt, mode): - pos = degrees(pos) - trgt = degrees(trgt) - # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation we may need to return the long distance instead - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - dist_short = (trgt - pos + 180) % 360 - 180 - # calculate short and long distance - if dist_short >= 0: # ie dist_long should be negative - dist_long = -(360 - dist_short) - else: - dist_long = 360 + dist_short - log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') - if mode == 1: # positive rotation only, ie we want a positive distance - if dist_short >= 0: # ie we want this one - dist = dist_short - else: # ie we need to go the other way - dist = dist_long - elif mode == 2: # negative rotation only ie we want a positive distance - if dist_short > 0: # ie we need to go the other way - dist = dist_long - else: # ie we want this one - dist = dist_short - else: # mode = 0 ie we want the shortest distance either way - dist = dist_short - log.debug(f'Returning distance: {dist:.4f}°') - return radians(dist) - - -def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians - # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] - # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance - # from the given position to that target angle - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') - # calculate the shortest distance from position to target for the strategy given by - # the operator (ie shortest (= default), positive rotation only, negative rotation only ) - dist = calc_shortest_distance(pos, trgt, mode) - # check that the result is within the rotary axis limits defined in the ini file - if dist >= 0: # shortest way is in the positive direction - if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer way in the other direction - log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') - if mode == 0: - dist = dist - 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - else: # shortest way is in the negative direction - if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if negative limits would be exceeded we need to go the longer way int the other direction - log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') - if mode == 0: - dist = dist + 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - if theta is not None: - log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') - # we also attach the distance for this particular move and mode - return theta, dist # returns radians - - -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians - # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves - # in (min_limit, max_linit) from the current joint positions using the orient_mode set by - # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit - global orient_mode - # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians - # we want to return a list of angles that are optimized for the orient_mode and the - # rotary axes limits as set in the ini file - target_dist_list= [] - for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # For the priortized joint we apply the orient mode requested by the operator - # the other we optimize for shortest move - if optimization_priority == 2: - primary_strategy = 0 - secondary_strategy = orient_mode - else: - primary_strategy = orient_mode - secondary_strategy = 0 - # primary joint - prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, - primary_max_limit, primary_min_limit, - primary_strategy) - # secondary joint - sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, - secondary_max_limit, secondary_min_limit, - secondary_strategy) - # if a solution has been found for this particular pair then we add it to the list - if not (prim_move == None) and not (sec_move == None): - target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: - debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' - f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') - log.debug(debug_msg) - return target_dist_list # returns radians - - -def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): - # find the optimal joint move from current to target positions in the list - # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only - # For orient_mode=(1,2): If no move can be found within joint limits we return None - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global orient_mode - # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that - # will result in correct tool orientation, stay within the rotary axis limits and respect the - # orient_mode if set by the operator - valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) - if len(valid_joint_moves_and_distances) < 1: - log.error(f' No valid joint moves found.') - return (None, None) - # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the prioritized joint - (theta_1, theta_2) = (None, None) - joint = optimization_priority - 1 - dist = 10 # some large initial value - for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - if theta_1 is not None: - debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' - f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') - log.debug(debug_msg) - return theta_1, theta_2 # returns radians - - -def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians - # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested - # orientation after rotation - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # tolerance setting for check if x-vector-vector needs to be rotated at all - epsilon = 0.00000001 - log.info(" x-vector-requested: %s", x_vector_req) - debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' - f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') - log.debug(debug_msg) - # run matrix_in through the kinematic transformation in the requested direction - # using the given joint angles and zero virtual-rotation - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the x-vector for the given machine joint rotations is found directly in the first column - x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(" X-vector after machine rotation would be: %s", x_vector_is) - # we calculate the angular difference between the two vectors so we can add a virtual rotation - # around z-vector or work-z to match the requested x orientation after machine rotation - # just to be sure we normalize the two vectors - x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) - x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) - # check if the x-vector is already in the required orientation (ie parallel) - log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) - if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: - log.info(" X-vector already oriented, setting virtual-rotation = 0") - # if we are already parallel then we don't need to add a virtual rotation - virtual_rot = 0 - else: - # we can use the cross product to determine the direction we need to rotate - cross = np.cross(x_vector_req, x_vector_is) - log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) - virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) - log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') - # To find out which quadrant we need the angle to be in we create a list of them all - virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] - log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) - # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector - for virtual_rot in virtual_rot_list: - log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') - zeta = 0.0001 - # run the identity matrix through the kinematic transformation in the requested direction - # using the given joint angles and virtual-rotation angle in the list - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the oriented x-vector is found directly in the first column - x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(' x_vector_would_be: %s', x_vector_would_be) - # calculate the difference of the respective elements - x_vector_diff = x_vector_req - x_vector_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) - log.debug(' Is the X-vector close enough ? %s', match) - if match: - # if we have a match we leave the loop and use this angle - break - log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') - return virtual_rot # returns radians - - -def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians - # transforms a 4x4 input matrix using the current transformation matrix - # (forward or inverse) using the kinematic model of the machine - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global kins_virtual_rotation - # read current spindle rotary angles (radians) - theta_1, theta_2 = get_current_rotary_positions(self) - # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector - log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") - # run matrix_in through the kinematic transformation in the requested direction - # using the current joint angles and virtual-rotation as requested - try: - twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_twp_matrix_from_joint_position, %s', error) - return twp_matrix - - -def gui_update_twp(): - # The tilted-work-plane is created in identity mode and must NOT be updated after a switch - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, saved_work_offset - # twp origin as vector (in world coords) from current work-offset to the origin of the twp - try: - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - # publish the twp offset coordinates in world coordinates (ie identity) - [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) - # this is used to translate the rotated twp to the correct position - # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created in the origin, - # then rotated according to the rotary joint position and then translated. - # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then - # translated by the offset values of the identity mode. - try: - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - - -# NOTE: Due to easier abort handling we currently restrict the use of twp to G54 -# as LinuxCNC seems to revert to G54 as the default system -def get_current_work_offset(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # get which offset is active (g54=1 .. g59.3=9) - active_offset = int(self.params[5220]) - current_work_offset_number = active_offset - # set the relevant parameter numbers that hold the active offset values - # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) - work_offset_x = (active_offset-1)*20 + 5221 - work_offset_y = work_offset_x + 1 - work_offset_z = work_offset_x + 2 - co_x = self.params[work_offset_x] - co_y = self.params[work_offset_y] - co_z = self.params[work_offset_z] - current_work_offset = (co_x, co_y, co_z) - return [current_work_offset_number, current_work_offset] - - -def get_current_rotary_positions(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global joint_letter_primary, joint_letter_secondary - if joint_letter_primary == 'A': - theta_1 = radians(self.AA_current) - elif joint_letter_primary == 'B': - theta_1 = radians(self.BB_current) - elif joint_letter_primary == 'C': - theta_1 = radians(self.CC_current) - log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') - # read current spindle rotary angles and convert to radians - if joint_letter_secondary == 'A': - theta_2 = radians(self.AA_current) - elif joint_letter_secondary == 'B': - theta_2 = radians(self.BB_current) - elif joint_letter_secondary == 'C': - theta_2 = radians(self.CC_current) - log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') - return theta_1, theta_2 - - -def reset_twp_params(): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global virtual_rot, twp_matrix, twp_flag, twp_build_params - virtual_rot = 0 - # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_virtual_rot(0)! - twp_flag = [] - twp_build_params = {} - log.info(" Resetting TWP-matrix") - twp_matrix = np.asmatrix(np.identity(4)) - - -def g53n_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) - # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the - # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from - # an ngc reamp that contains a quebuster before calling this code. - # IMPORTANT: - # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called - # (ie do it in the ngc remap mentioned above!) - global saved_work_offset, twp_matrix, twp_flag, virtual_rot - global joint_letter_primary, joint_letter_secondary, twp_error_status - global orient_mode - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - if not hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: No TWP defined." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - elif hal.get_value(twp_is_active): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: TWP already active" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # Check if any words have been passed with the respective G53.n command - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - x = c.i_number if c.i_flag else None - y = c.j_number if c.j_flag else None - z = c.k_number if c.k_flag else None - log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) - - if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params() - msg = "G53.n : unrecognised P-Word found." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - orient_mode = p - z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation - try: - # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians - except Exception as error: - log.error('calc_joint_angles, %s', error) - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - if possible_prim_sec_angle_pairs == []: - # reset the twp parameters - log.error('G53.n: No possible primary/secondary angle pairs found.') - reset_twp_params() - msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - try: - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians - except Exception as error: - log.error('G53.n: Calculation of optimal joint move failed, %s', error) - if theta_1 == None or theta_2 == None: - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # get the particular conditions to be met for the kinematic at hand - try: - (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, - z_vector_requested, - twp_matrix) - except Exception as error: - log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) - # calculate the virtual-rotation needed - virtual_rot = calc_virtual_rotation(theta_1, - theta_2, - x_vector_requested, - z_vector_requested, - matrix_in, - direction) # returns radians - log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") - - # mark twp-flag as active - twp_flag = [0, 'active'] - gui_update_twp() - - # set the virtual-rotation value in the kinematic component - debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' - f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') - log.debug(debug_msg) - try: - kins_set_values(theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: kins_set_values failed, %s', error) - - # calculate the work offset in transformed-coordinatess - log.debug(" G53.n: Saved work offset: %s", saved_work_offset) - twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - try: - new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) - debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' - f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') - log.debug(debug_msg) - # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - - log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") - if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool and the requested work plane - self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # switch to the dedicated TWP work offsets - self.execute("G59", lineno()) - # activate TWP kinematics - self.execute("G12.1 P2") - if (x,y,z) != (None,None,None): - log.debug(' G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % - (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # set twp-state to 'active' (2) - self.execute("M68 E2 Q2") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# Cancel an active TWP definition and reset the parameters to zero -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc that contains a quebuster before calling this code -def g69_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_flag, saved_work_offset_number, saved_work_offset - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - log.info('G69 called') - # reset the twp parameters - reset_twp_params() - gui_update_twp() - # set twp-state to 'undefined' (0) - self.execute("M68 E2 Q0") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation -def g683(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg =("G68.3 ERROR: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - # get which offset is active (g54=1 .. g59.3=9) - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.3 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - c = self.blocks[self.remap_level] - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of x-vector around the origin - r = radians(c.r_number) if c.r_flag else 0 - - twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) # radians - # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand - try: - virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) - except Exception as error: - log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) - log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) - # then we need to calculate the transformation matrix of the current orientation with the including the - # calculated virtual-rotation. - # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the - # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle - # passed in the R word of the G68.3 command - start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) - # the required transformation direction may depend on the kinematic at hand - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) - log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) - # put the requested origin into the twp_matrix - (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) - # update the build state of the twp call - twp_flag[2] = 'done' - log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset = offsets - saved_work_offset_number = n - log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g682(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): # ie TWP has already been defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: TWP already defined.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset_number = n - saved_work_offset = offsets - log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional - twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs three calls, plus an optional Q0 first - if q == 0 and 'done' in twp_flag[2:]: - q = -1 # Q0 after the points, refused below - if q == 0: # shift of the origin from point 1, along the plane's axes - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - twp_build_params['q0'] = [x,y,z] - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[2] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[3] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[4] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # R may come on any one block of the definition - if c.r_flag: - if 'r_q' in twp_build_params: - msg = ("G68.2 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) - reset_twp_params() - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - twp_build_params['r_q'] = q - twp_build_params['r'] = radians(c.r_number) - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z] = twp_build_params['q0'][0:3] - r = twp_build_params.get('r', 0) - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.2 P2: Point 1: %s",p1) - log.debug(" G68.2 P2: Point 2: %s",p2) - log.debug(" G68.2 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.2 P2 (v2): %s",v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - # the origin is point 1, moved by Q0's x y z along the plane's own axes - twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - log.info(' first call') - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q1':[], 'q2':[]} - log.debug(' twp_build_params: %s', twp_build_params) - if q == 1: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 2: # define second vector (the normal vector of the twp) - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q2'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] - r = twp_build_params['q1'][6] - (i, j, k) = twp_build_params['q1'][3:6] - (i1, j1, k1) = twp_build_params['q2'] - log.debug("(x, y, z): %s", (x, y, z)) - log.debug("(i, j, k): %s", (i, j, k)) - log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # X is kept as given and Z is projected onto the plane normal - # to it, as Fanuc does; vectors 5 degrees or more off square, - # or a zero vector, are refused - twp_vect_x = np.array([i, j, k], dtype=float) - twp_vect_z = np.array([i1, j1, k1], dtype=float) - len_x = np.linalg.norm(twp_vect_x) - len_z = np.linalg.norm(twp_vect_z) - orth = 1.0 - if len_x > 1e-12 and len_z > 1e-12: - twp_vect_x = twp_vect_x / len_x - twp_vect_z = twp_vect_z / len_z - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - if abs(orth) >= sin(radians(5)): - reset_twp_params() - msg = ("G68.2 P3: The vectors are zero or 5 degrees or more off square.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - twp_vect_z = twp_vect_z - orth * twp_vect_x - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.2: twp_flag: %s", twp_flag) - log.debug(" G68.2: calls required: %s", twp_flag.count('done')) - log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) - log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.2: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.2: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.2: twp vector-z: %s", twp_vect_z) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g684(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if not hal.get_value(twp_is_defined): # ie there is currently no TWP defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No TWP defined to increment from. Run G68.2 or G68.3 first.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - n = get_current_work_offset(self)[0] - # Must be in one of the dedicated offset systems for TWP - if False: #n < 6: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # store the current TWP to - twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional - twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs three calls, plus an optional Q0 first - if q == 0 and 'done' in twp_flag[2:]: - q = -1 # Q0 after the points, refused below - if q == 0: # shift of the origin from point 1, along the plane's axes - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - twp_build_params['q0'] = [x,y,z] - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[2] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[3] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[4] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # R may come on any one block of the definition - if c.r_flag: - if 'r_q' in twp_build_params: - msg = ("G68.4 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) - reset_twp_params() - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - twp_build_params['r_q'] = q - twp_build_params['r'] = radians(c.r_number) - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z] = twp_build_params['q0'][0:3] - r = twp_build_params.get('r', 0) - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.4 P2: Point 1: %s",p1) - log.debug(" G68.4 P2: Point 2: %s",p2) - log.debug(" G68.4 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.4 P2: (v2) %s", v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - # the origin is point 1, moved by Q0's x y z along the plane's own axes - twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q1':[], 'q2':[]} - if q == 1: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 2: # define second vector (the normal vector of the twp) - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q2'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] - r = twp_build_params['q1'][6] - (i, j, k) = twp_build_params['q1'][3:6] - (i1, j1, k1) = twp_build_params['q2'] - log.debug("(x, y, z) %s", (x, y, z)) - log.debug("(i, j, k) %s", (i, j, k)) - log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # X is kept as given and Z is projected onto the plane normal - # to it, as Fanuc does; vectors 5 degrees or more off square, - # or a zero vector, are refused - twp_vect_x = np.array([i, j, k], dtype=float) - twp_vect_z = np.array([i1, j1, k1], dtype=float) - len_x = np.linalg.norm(twp_vect_x) - len_z = np.linalg.norm(twp_vect_z) - orth = 1.0 - if len_x > 1e-12 and len_z > 1e-12: - twp_vect_x = twp_vect_x / len_x - twp_vect_z = twp_vect_z / len_z - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - if abs(orth) >= sin(radians(5)): - reset_twp_params() - msg = ("G68.4 P3: The vectors are zero or 5 degrees or more off square.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - twp_vect_z = twp_vect_z - orth * twp_vect_x - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication, - # R turns about the plane's own Z - twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.4: twp_flag: %s", twp_flag) - log.debug(" G68.4: calls required: %s", twp_flag.count('done')) - log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) - log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info(" G68.4: calculating new twp_matrix...") - twp_matrix_new = twp_matrix_current * twp_matrix - log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.4: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.4: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.4: twp vector-z: %s", twp_vect_z) - log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) - twp_matrix = twp_matrix_new - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py deleted file mode 100755 index c7ce432a045..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py +++ /dev/null @@ -1,20 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2012, 2013 Dewey Garrett , -# Michael Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import remap - diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py index 44748312c4e..ee6994e7196 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py @@ -1,87 +1,47 @@ #!/usr/bin/env python3 +# Publishes the tilted work plane for the vismach model: the origin and the +# X and Z directions in the coordinate system the plane was defined in, +# read from status where the interpreter keeps them (G68.2, G68.3, G68.4, +# G69), and the active work offset the model translates the plane by. import hal import linuxcnc +import time h = hal.component("twp-helper-comp") -# this pin reflects the machine.analog pin used for the -# twp-status -h.newpin("twp-status", hal.Type.REAL, hal.Dir.IN) -# these pins are created here from 'twp-status'' -h.newpin("twp-is-redefined", hal.Type.BOOL, hal.Dir.OUT) +h.newpin("twp-status", hal.Type.REAL, hal.Dir.OUT) # 0 undefined, 1 defined h.newpin("twp-is-defined", hal.Type.BOOL, hal.Dir.OUT) h.newpin("twp-is-active", hal.Type.BOOL, hal.Dir.OUT) -# twp origin vector -h.newpin("twp-ox-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz", hal.Type.REAL, hal.Dir.OUT) -# twp x-orientation vector -h.newpin("twp-xx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xz", hal.Type.REAL, hal.Dir.OUT) -# twp z-orientation vector -h.newpin("twp-zx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zz", hal.Type.REAL, hal.Dir.OUT) -# twp origin vector in machine coordinate system -h.newpin("twp-ox-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz-world", hal.Type.REAL, hal.Dir.OUT) +for name in ("twp-ox", "twp-oy", "twp-oz", + "twp-xx", "twp-xy", "twp-xz", + "twp-zx", "twp-zy", "twp-zz", + "twp-ox-world", "twp-oy-world", "twp-oz-world"): + h.newpin(name, hal.Type.REAL, hal.Dir.OUT) h.ready() -# create a connection to the status channel s = linuxcnc.stat() try: while 1: - # publish twp-status - if h['twp-status'] == 1: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 0 - elif h['twp-status'] == 2: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 1 - else: - h['twp-is-defined'] = 0 - h['twp-is-active'] = 0 - - # passthrough the twp arguments - h['twp-ox'] = h['twp-ox-in'] - h['twp-oy'] = h['twp-oy-in'] - h['twp-oz'] = h['twp-oz-in'] - h['twp-xx'] = h['twp-xx-in'] - h['twp-xy'] = h['twp-xy-in'] - h['twp-xz'] = h['twp-xz-in'] - h['twp-zx'] = h['twp-zx-in'] - h['twp-zy'] = h['twp-zy-in'] - h['twp-zz'] = h['twp-zz-in'] - - # we only want to expose offsets when twp is not defined - if not h['twp-is-defined']: - s.poll() # get current values - g5x_offset = s.g5x_offset - h['twp-ox-world'] = g5x_offset[0] - h['twp-oy-world'] = g5x_offset[1] - h['twp-oz-world'] = g5x_offset[2] - else : # use the values from the remap - h['twp-ox-world'] = h['twp-ox-world-in'] - h['twp-oy-world'] = h['twp-oy-world-in'] - h['twp-oz-world'] = h['twp-oz-world-in'] + s.poll() + active = 1 if s.g68_active else 0 + h['twp-status'] = active + h['twp-is-defined'] = active + h['twp-is-active'] = active + + o = s.g68_offset + r = s.g68_rotation + h['twp-ox'], h['twp-oy'], h['twp-oz'] = o[0], o[1], o[2] + # columns of the rotation: the plane's X and Z + h['twp-xx'], h['twp-xy'], h['twp-xz'] = r[0], r[3], r[6] + h['twp-zx'], h['twp-zy'], h['twp-zz'] = r[2], r[5], r[8] + + g5x = s.g5x_offset + h['twp-ox-world'], h['twp-oy-world'], h['twp-oz-world'] = g5x[0], g5x[1], g5x[2] + time.sleep(0.05) except KeyboardInterrupt: raise SystemExit diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py deleted file mode 100755 index 59b012058ad..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py +++ /dev/null @@ -1,67 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2013 Dewey Garrett , Michael -# Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import inspect -import emccanon - -# O-word procedure to trap into the Pydevd debugger -# start debug server in Eclipse, then -# call as 'O call' from MDI - -# example setup for debugging embedded Python code -# see http://pydev.org/manual_adv_remote_debugger.html -# if this points to a valid directory, - -def call_pydevd(): - """ trap into the pydevd debugger""" - - import os,sys - - pydevdir= '/home/mah/.eclipse/org.eclipse.platform_3.5.0_155965261/plugins/org.python.pydev.debug_2.0.0.2011040403/pysrc/' - - # the 'emctask' module is present only in the milltask instance, otherwise both the UI and - # milltask would try to connect to the debug server. - - if os.path.isdir(pydevdir) and 'emctask' in sys.builtin_module_names: - sys.path.append(pydevdir) - sys.path.insert(0,pydevdir) - try: - import pydevd - emccanon.MESSAGE("pydevd imported, connecting to Eclipse debug server...") - pydevd.settrace() - except: - emccanon.MESSAGE("no pydevd module found") - pass - - - -def lineno(): - """ return line number in the current Python script """ - return inspect.currentframe().f_back.f_lineno - -def error_stack(self): - """ print the Interpreters error stack (function names) """ - print("error stack level=%d" % (self.stack_index)) - for s in self.stack(): - print("--'%s'" % (s)) - -def callstack(self): - """ print the O-Word call stack """ - for i in range(self.call_level): - c = self.sub_context[i] - print("%d: pos=%d seq=%d filename=%s sub=%s" % (i,c.position, c.sequence_number,c.filename,c.subname)) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc deleted file mode 100755 index 4b2fd293c61..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.1 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

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

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

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

) -o105 else - #

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

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

]] -o100 else - #

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

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc deleted file mode 100755 index 9efd1b7db29..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ /dev/null @@ -1,11 +0,0 @@ -; this is the wrapper remap to cancel TWP - -osub -M66 L0 E0 ; force sync, stop read ahead -M469 ; call the python G69_core code -G13.1 ; back to identity kins -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G54 ; switch to G54 -M66 L0 E0 ; force sync, stop read ahead -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc deleted file mode 100755 index 1cbf3d41db7..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ /dev/null @@ -1,15 +0,0 @@ -;This is a workaround for a bug that leads to stat.gcodes and parameter[5250] -;to get out of sync after some program aborts -;in [RS274NGC] section of the ini add: ON_ABORT_COMMAND = o call -;save this to a path specified in SUBROUTINE_PATH = -;NOTE: we cannot run remapped codes here only custom Mcodes (ie M100..M199) - -o sub -;(msg, on_abort START) -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G13.1 ; back to identity kins -G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events -G54 ; switch to G54 -(msg, on_abort END) -o endsub -M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 2be585e6ef8..fdbab9b291f 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -29,46 +29,24 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 ON_ABORT_COMMAND = o call -#ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzacb-trsrn.var - -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzacb_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = B - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl @@ -76,11 +54,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal #HALCMD = loadusr ../python/feed_zero.py - -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-y xyzacb_trsrn_kins.y-rot-axis @@ -93,8 +66,6 @@ HALCMD = net :offset-y xyzacb_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -150,7 +121,6 @@ HALCMD = net twp-status xyzacb-trsrn-gui.twp HALCMD = net twp-is-defined xyzacb-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzacb-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -220,7 +190,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -231,7 +200,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -252,7 +220,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - #table rotary [JOINT_3] TYPE = ANGULAR diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d3032855aee..4162f8f0248 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -28,56 +28,30 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 -#ON_ABORT_COMMAND = o call -ON_ABORT_COMMAND = o call +ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzbca-trsrn.var - -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 +# Off the machine frame kinematics (under TCP), G53, G28 and G30 move the +# slides in the machine frame, as G53.5, G28.5 and G30.5 do, wherever the tool, +# the head and the table point, and G28.1 and G30.1 store that position. Unset, +# they move in the world of the kinematics in force. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzbca_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = A - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-x xyzbca_trsrn_kins.x-rot-axis @@ -90,8 +64,6 @@ HALCMD = net :offset-y xyzbca_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -147,7 +119,6 @@ HALCMD = net twp-status xyzbca-trsrn-gui.twp HALCMD = net twp-is-defined xyzbca-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzbca-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -217,7 +188,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -228,7 +198,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -249,7 +218,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - # spindle secondary joint [JOINT_3] TYPE = ANGULAR diff --git a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc index d2d0f3c8d39..b71caa08b0d 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc @@ -3,7 +3,7 @@ o<428remap>sub # = 0 G12.1 P# ; select kinstype 0, syncs interp and motion - ; (G13.1 is identity, which is type 1 on this module) + ; (G13.1 is the machine frame, this type on a robot; identity is type 1) o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] (debug,M428: Wrong kinematics type) diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index ed9acdbbbfe..86e3e3750e1 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -639,6 +639,11 @@ The maximum number of `USER_M_PATH` directories is defined at compile time (typ: When set M2, M30 and an abort no longer cancel a <>. A restart still does. * `DISABLE_FANUC_STYLE_SUB = 0` (Default: 0) If there is reason to disable Fanuc subroutines set it to 1. +* `MACHINE_MOVES_IN_MACHINE_FRAME = 0` (bool, Default: 0) + + When set, 'G53', 'G28' and 'G30' run as 'G53.5', 'G28.5' and 'G30.5' whenever the kinematics in force is not the machine frame type, and 'G28.1' and 'G30.1' store the machine frame position those read back: the slides go to the words or to the stored position whatever the tool, the head and the table are doing. The machine frame type is the type the module flags as such, the identity kinematics on a machine whose slides line up with its frame, and neither the tool centre point nor a raw joint identity on one whose slides do not. On the machine frame type the codes are unchanged. + Unset, these codes work in the world of the kinematics in force, which on a table-side kinematics turns with the table, and under a tool centre point kinematics with the tool tilted puts the slides where the tool and the head angle take them. + A GUI's preview reads a program on the kinematics type the machine is on when the program is loaded, as running it would; the 'G53' the GUI puts ahead of the program to say where the machine stands stays in the world of that type. + See <>. * 'G73_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) Chip breaking back-off distance in machine units * 'G83_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index f55ccb764a2..c924886b1d8 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -244,6 +244,13 @@ see <> for an example. * KINEMATICS_INVERSE_ONLY * KINEMATICS_BOTH +*kins_type*:: '(returns integer)' - + the kinematics type motion runs, as 'G12.1' selects it and the motion.kins-type pin reports it; 0 for a module that switches nothing. + +*kins_type_flags*:: '(returns tuple of integers)' - + what the kinematics module declares each kinematics type to be, one entry per type: a sum of KINSTYPE_IDENTITY, KINSTYPE_PRIMARY and KINSTYPE_MACHINE, or -1 for a type the module does not provide or declares nothing about. + See the <> chapter. + *limit*:: '(returns tuple of integers)' - axis limit masks. minHardLimit=1, maxHardLimit=2, minSoftLimit=4, maxSoftLimit=8. diff --git a/docs/src/gcode/coordinates.adoc b/docs/src/gcode/coordinates.adoc index f35a7cdd324..ffe321c982e 100644 --- a/docs/src/gcode/coordinates.adoc +++ b/docs/src/gcode/coordinates.adoc @@ -53,6 +53,11 @@ to clear the work area and access the workpiece in the vise. G53 is a non modal command. It must be used in every block where a move in machine coordinate system is desired. +On a machine with rotary axes the machine coordinates are those of the +kinematics in force, which can turn with the table or follow the tool, so +G53 does not always put the slides at the position given; G53.5 does, see +<>. + == Coordinate Systems [[fig:coordinate-systems-example]] diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 46386681cce..a65480db020 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,6 +74,7 @@ as the 'L number', and so on for any other letter. |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position +|<> |Go to Predefined Slide Position |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -86,14 +87,17 @@ as the 'L number', and so on for any other letter. |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset |<> |Tool Length Offset on Primary Kinematics +|<> |Tool Length Offset with the Tool Axis as a Vector |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates +|<> |Orient the Tool to the Tilted Work Plane +|<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance -|<> |Tilted Work Plane +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -948,9 +952,11 @@ G13.1 'G12.1' selects one of the kinematics provided by a switchable kinematics module; the 'P' word is the kinematics number. 'G13.1' cancels back to -identity kinematics, where joints and coordinates are the same thing. -Which number that is is declared by the module itself, so 'G13.1' means -the same kinematics whatever order a module lists its types in. +the machine frame kinematics: the identity kinematics, where joints and +coordinates are the same thing, on a machine whose slides line up with its +frame, and the arm kinematics with no tool on a robot. Which number that is +is declared by the module itself, so 'G13.1' means the same kinematics +whatever order a module lists its types in. [WARNING] Deprecation notice: selecting the kinematics by writing the @@ -966,12 +972,12 @@ planned in one kinematics and executed in another. Because of that, both codes stop any blending that was in progress, in the same way 'G4' does. The kinematics module decides what each number means, and declares -which one is identity. See the `switchkins` section of the kins(9) man -page for the modules that support switching and the order in which they +which one is the machine frame. See the `switchkins` section of the kins(9) +man page for the modules that support switching and the order in which they list their kinematics. A machine whose kinematics module is not switchable rejects the change, and 'G13.1' is an error on a switchable -module that declares no identity kinematics; such a module can still be -driven by number with 'G12.1'. +module that declares no identity or machine frame kinematics; such a module +can still be driven by number with 'G12.1'. Selecting a kinematics does not move the machine. It changes how joint positions and coordinate positions map onto each other, so the position @@ -992,14 +998,14 @@ G12.1 P# (put back whatever the caller was using) Nothing cancels the selection on its own. It survives the end of the program and an abort, so that the kinematics keeps matching what the position readout shows. End a program with 'G13.1' if it should leave the -machine in identity kinematics. +machine in the machine frame kinematics. .G12.1, G13.1 Example [source,ngc] ---- G12.1 P1 (switch to kinematics 1) G0 X0 Y0 -G13.1 (back to identity kinematics) +G13.1 (back to the machine frame kinematics) ---- It is an error if: @@ -1008,7 +1014,7 @@ It is an error if: * The 'P' word is negative. * The 'P' word names a kinematics the module does not provide. * A 'P' word is used with 'G13.1'. -* 'G13.1' is used and the module declares no identity kinematics. +* 'G13.1' is used and the module declares no identity or machine frame kinematics. [[gcode:g17-g19.1]] == G17 - G19.1 Plane Select(((G17 - G19.1 Plane Select))) @@ -1069,6 +1075,14 @@ all axes will go to the <>. G28 Z2.5 (rapid to Z2.5 then to Z location specified in #5163) ---- +The 'absolute' position is the machine coordinate system of the kinematics in +force, see <>: on a kinematics that is not the identity it is +that kinematics' world, not the slides, and the stored position is a world +position too. <> is the machine frame form, and with +`MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the INI +file 'G28' and 'G28.1' are 'G28.5' and its store whenever the kinematics in +force is not the machine frame kinematics. + It is an error if : * Cutter Compensation is turned on @@ -1183,10 +1197,63 @@ if TOOL_CHANGE_AT_G30=1 is in the [EMCIO] section of the INI file. G30 Z2.5 (rapid to Z2.5 then to the Z location specified in #5183) ---- +The 'absolute' position is the machine coordinate system of the kinematics in +force, see <>; <> is the form that moves +the slides, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and 'G30.1' are +'G30.5' and its store whenever the kinematics in force is not the machine frame +kinematics. + It is an error if : * Cutter Compensation is turned on +[[gcode:g28.5]] +== G28.5, G30.5 Go to Predefined Slide Position(((G28.5 Go to Predefined Slide Position))) + +[source,ngc] +---- +G28.5 +G30.5 +---- + +'G28.5' and 'G30.5' are to 'G28' and 'G30' what <> is to +'G53': the stored position, parameters 5161-5169 for 'G28.5' and 5181-5189 +for 'G30.5', is read in the machine frame with the orientation left out, the +way 'G53.5' reads its words, the slides on a machine whose slides line up +with its frame, and the move is a <> there, +rapid, with no offset, tool length, rotation or work plane applied. The +values are in the machine units of the INI file, as for 'G28'. The stored +position has to be a machine frame position: store it with 'G28.1' or 'G30.1' +while the machine frame kinematics is in force, what 'G13.1' selects, where +the world is that frame, or under any kinematics with +`MACHINE_MOVES_IN_MACHINE_FRAME` set. + +* 'G28.5' - moves every letter the machine has to the stored position. +* 'G28.5 axes' - moves the letters named by 'axes' to the given machine + frame position first, then the same letters to their stored positions. A + letter not named holds its machine frame coordinate. + +On a mill whose head is tilted and a table-side kinematics in force, 'G28' +takes the tool tip to the stored point, where the head's tilt puts the slides +somewhere else; 'G28.5' takes the slides there, whatever the head is doing, +which is what a park or a tool change position wants. + +.G28.5 Example Line +[source,ngc] +---- +G28.5 Z0 (the Z slide to zero, then to the Z stored in #5163) +G30.5 (every slide to the position stored with G30.1) +---- + +It is an error if: + +* Cutter Compensation is turned on +* Polar coordinates are used, or incremental distance mode is in force. +* An axis letter is used that is not a joint of the kinematics, the + machine's letters name joints of the other kind, or the machine frame + kinematics cannot reach the point, see <>. +* The kinematics module cannot be evaluated by the interpreter. + [[gcode:g33]] == G33 Spindle Synchronized Motion(((G33 Spindle Synchronized Motion))) @@ -1639,6 +1706,28 @@ in the numbered parameters '5081-5089'. The numbered parameters '5401-5409' instead hold the loaded tool's stored offset, refreshed on tool change ('M6') and 'G10 L1'/'L10'/'L11'. +Some kinematics modules apply the tool length themselves, along the tool +axis rather than along Z (5axiskins, maxkins, the trt modules, the trsrn +modules, xyzab_tdr_kins, genhexkins, pentakins; see the +<> chapter). 'G43' does +not move the machine under those either. With the tool axis tilted at the +time, the programmed coordinates of the point the tool stands on change by +more than the offset difference: the interpreter works them out through the +kinematics, so the next move starts from where the tool tip actually is. +Motion reads the point from the joints the same way and stops the program +if the two disagree. A module that can only be evaluated in realtime leaves +the interpreter with the offset difference alone; under one of those, +change the tool length with the tool axis untilted, motion stops the +program otherwise. + +Of those, the modules that also report where the tool points (5axiskins, +the trt modules, the trsrn modules) apply an X and Y of the offset along +the tool's own X and Y, as they apply the length along the tool axis. The +offset is given in the tool's axes, which are the machine's with the +rotaries at zero, so a tool held off the spindle axis keeps its offset as +the head or the table turns. Under the identity and the other modules, X +and Y apply along the work axes. + [NOTE] 'G43 H0' is a little special. Its behavior is different on random tool changer machines and nonrandom tool changer machines (see the @@ -1758,9 +1847,9 @@ declares its working transform, so the program runs with tool length compensation in the module's working kinematics. The switch happens first and the offset applies after it, as if the two had been written on consecutive lines. 'G49' is the matching cancel: it clears the -offset and switches back to identity kinematics, as long as the offset -in effect is still 'G43.4''s and no 'G12.1' or 'G13.1' has selected a -kinematics since. +offset and switches back to the machine frame kinematics, what 'G13.1' +selects, as long as the offset in effect is still 'G43.4''s and no 'G12.1' +or 'G13.1' has selected a kinematics since. The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module @@ -1779,22 +1868,97 @@ that cancels one of them does not switch either. On a machine without switchable kinematics there is nothing to switch: 'G43.4' is a plain 'G43' and 'G49' a plain cancel. +Under the working kinematics 'G53', 'G28' and 'G30' move in its world, so +with the tool tilted where they put the slides depends on the tool and the +head angle. A configuration that wants them to move the slides in the machine +frame sets `MACHINE_MOVES_IN_MACHINE_FRAME` in the `[RS274NGC]` section of the +INI file, as the five axis sims shipped with LinuxCNC do; see +<>. + It is an error if: * the kinematics module is switchable but declares no primary kinematics, or * any of the 'G43' error conditions holds. +[[gcode:g43.5]] +== G43.5 Tool Length Offset with the Tool Axis as a Vector(((G43.5 Tool Length Offset with the Tool Axis as a Vector))) + +[source,ngc] +---- +G43.5 +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- +---- + +* 'H' - tool number (optional) +* 'I J K' - the direction the tool axis is to point along, on a 'G0' or + 'G1' line while 'G43.5' is in effect + +'G43.5' is '<>' with one thing more: while it is in +effect, a 'G0' or 'G1' line may give the direction of the tool axis as +a vector 'I J K' in place of rotary words, and the interpreter works +out where the rotary joints have to go to point the tool that way. +This is the form of a five-axis program written for the tool rather +than for the machine, the one Fanuc calls tool center point control +type 2, and it runs on any machine whose kinematics module supplies its +tool frame (see the <> chapter): the same program orients a tilting head, a +tilting table and a robot. + +The vector points from the tool tip towards the holder, in the +coordinate system the line's 'X Y Z' are in: the active coordinate +system with its rotation, the <> where +one is active, and the machine's on a '<>' line. Its +length does not matter. A word left out is zero, so 'K1' alone is the +tool vertical. The words describe a direction, so they are never +incremental: 'G91' applies to the 'X Y Z' of the line, not to 'I J K', +and 'G91.1' does not apply to them either. A line with 'I J K' and no +axis word turns the tool where it stands. + +Where more than one pose of the rotaries points the tool along the +vector, two on a five-axis machine, the interpreter takes the one +nearest where the rotaries stand inside their travel, every orienting joint free to take +part, so a path of vectors runs continuous from wherever it starts and +a rotary swings the long way round only where the short way runs out of travel. On the pole of the primary +rotary, the tool vertical on a mill, one joint no longer matters to the +direction and it stays where it is. + +The pose the interpreter finds goes into the move as ordinary rotary +words, in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +along the way; a program that needs the tool to sweep between two +directions writes the lines in between. + +'G2' and 'G3' keep 'I J K' as the arc centre; the tool direction on an +arc is given with rotary words. 'G43', 'G43.1', 'G43.2' and 'G49' end +the vector form along with the offset they replace; 'G49' cancels the +offset and undoes the kinematics switch, as after 'G43.4'. + +It is an error if: + +* 'I J K' are given together with a rotary word on the same line, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* the direction cannot be reached by the rotary joints, +* 'I J K' are given on a 'G53.5' or 'G53.7' line, or +* any of the 'G43.4' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation -'G49' also switches a switchable kinematics module back to its identity -kinematics when it cancels a 'G43.4', undoing the switch that made. It -leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does -after a plain 'G43', and a module that declares no identity kinematics -gets the plain cancel. +'G49' also switches a switchable kinematics module back to its machine +frame kinematics, what 'G13.1' selects, when it cancels a 'G43.4' or +'G43.5', undoing the switch that code made. It leaves a kinematics selected +by 'G12.1' or 'G13.1' alone, as it does after a plain 'G43', and a module +that declares no identity or machine frame kinematics gets the plain +cancel. On a robot the arm kinematics is the machine frame, so there the +cancel switches nothing. It is OK to program using the same offset already in use. It is also OK to program using no tool length offset if none is currently being @@ -1828,6 +1992,29 @@ programmed on the same line if one is currently active. For example 'G53 G0 X0 Y0 Z0' will move the axes to the home position even if the currently selected coordinate system has offsets in effect. +The machine coordinate system is the world of the kinematics in force with the +offsets removed. With the identity kinematics, or a head-side kinematics with +the rotaries at zero, that is the slides. With a table-side kinematics, such as +'xyzac-trt-kins' in TCP or the tilted work plane types of the 'trsrn' +modules, the world turns with the table, so 'G53 X0 Y0 Z0' takes the tool tip +to the origin of that turned frame, which the slides reach at some other +position. Under a tool centre point kinematics 'G53' takes the tool length off +along machine Z, where the kinematics carries the tool along its own axis: 'G53' +puts the point a tool length above the tip at the words, the spindle nose only +while the tool is vertical, so with the tool tilted where the slides land +depends on the tool and the head angle. The move in the machine frame with the +orientation left out, the slides on a machine whose slides line up with its +frame, whatever the kinematics in force, is <>. +A configuration that wants 'G53' to mean that frame sets +`MACHINE_MOVES_IN_MACHINE_FRAME` in the `[RS274NGC]` section of the INI file, +and 'G53', 'G28' and 'G30' then run as 'G53.5', 'G28.5' and 'G30.5', and +'G28.1' and 'G30.1' store the machine frame position, whenever the kinematics +in force is not the machine frame type: the type the module flags as such (see +the <> chapter), the identity +kinematics on a machine whose slides line up with its frame, and neither the +tool centre point nor a raw joint identity on one whose slides do not. On the +machine frame type 'G53' stays a straight move in that frame. + .G53 Example [source,ngc] ---- @@ -1842,6 +2029,253 @@ It is an error if: * G53 is used without G0 or G1 being active, * or G53 is used while cutter compensation is on. +[[gcode:g53.1]] +== G53.1, G53.2, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) + +[source,ngc] +---- +G53.1 +G53.2 +G53.3 X- Y- Z- +G53.6 +---- + +All four solve where the rotary joints have to go so that the tool axis is +normal to the active <>, the plane's Z. They +differ in what happens then: + +* 'G53.1' moves the rotaries alone. The linear joints stay where they are, + and the tool tip swings to wherever that carries it. It is a + point-to-point move, like <>. +* 'G53.6' keeps the tool centre point where it is. It is a Cartesian move of + the rotary words, so the kinematics compensates the linear joints all along. +* 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the + plane, in one point-to-point move. A word left out keeps the present value. +* 'G53.2' moves nothing. It only publishes the solved pose on the named + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', + '#<_orient_b>' and '#<_orient_c>', and on the numbered parameters + '#5071' to '#5079' ('X Y Z A B C U V W'), in program units in the plane, + and sets '#<_orient_valid>' and '#5080' to 1. The program can then reach + the pose with a move of its own making, for instance a single 'G0' that + names 'X Y Z' and the rotary words together, so the turn and the travel + are one Cartesian move under the TCP kinematics. This is what Heidenhain + calls `STAY`. Reading '#<_orient_x>' and kin before the first 'G53.2' is + an error; '#<_orient_valid>' and '#5080' say whether they hold a pose. + +The kinematics module answers where the rotaries have to go, with its tool +frame inverse (see the kinematics conventions chapter), so no configuration +carries the formula. That answer has more than one solution on most +machines, two on a five-axis one, and often a choice of which joints to use. + +'P' picks which of them. Without 'P', or with 'P0', the machine takes the +one nearest where its rotaries are standing, which is the shortest move and +depends on where that is. 'P1' and 'P2' name the pose instead, so a program +reaches the same one wherever it starts from: the two poses tilt opposite +ways, and they differ in the sign of the first rotary met going from the +tool to the work. On a machine with two head rotaries that is the one whose +axis the other carries; with two table rotaries, the one that carries the +other, A on a table that turns C in an A trunnion; with one of each, the +head rotary. 'P1' is the pose with that rotary positive and 'P2' the pose +with it negative. The interpreter works out which rotary that is by asking +the kinematics module, so nothing is configured for it. This is the choice +Heidenhain writes as `SEQ+` and `SEQ-` on its master axis, with one +difference: a control offering `SEQ` falls back to the nearer pose when both +lie the same side of home, where 'P1' and 'P2' refuse and say so. + +The two poses become one where that rotary stands at zero, the tool along +the other rotary's axis, straight up on a vertical mill, and there every +form gives the same answer. + +Every form stays inside the travel `[JOINT_n] MIN_LIMIT` and `MAX_LIMIT` give the rotary joints, and inside the `[AXIS_n]` travel of the rotary axes they drive, unless the axis is a wrapped or modulo rotary. A rotary reaches its angle by the turn nearest where it stands, and where that turn is outside its travel, by the nearest turn inside it, so it goes the long way round rather than run out of travel. A pose no turn brings inside the travel is not taken: without 'P' the next nearest is, and with 'P1' or 'P2' the code refuses. Under 'Q0' the table is freed before a pose is refused, as when no pose reaches the direction with it held. + +'P' names a pose only on a machine that orients the tool with two rotaries. +On a machine that is not of this shape, a robot among them, only the nearest +form is available. + +'Q' says whether the joints that carry the work, the table, take part. + +* 'Q0', the default, holds them and lets the head do the work. On a machine + whose head has only two rotaries the plane's X is then not something the + joints can place, and it is left to the coordinate system, which the plane + already carries; nothing else is done with it. This is what Heidenhain + calls `COORD ROT`. If nothing is reachable with the table held, the move + is tried again with every joint free, the plane's X still left to the + coordinate system; on a machine whose rotaries are all in the table, that + is how its two poses are reached. +* 'Q1' frees the table from the start. With the plane's X asked for as well, + a machine with the joints for it turns the table so that the plane's X is + reached by the machine, Heidenhain's `TABLE ROT`. + +Which joints carry the work is read off the kinematics module's work frame, +so 'Q' means the same thing on every module and needs no INI entry. + +The orientation is evaluated by the interpreter, ahead of the moves before +it, on the kinematics type the program is in. It needs a kinematics type +whose frames describe the machine: on a switchable module that is its TCP +kinematics, selected with <>, and not the identity +kinematics the module starts in. + +.G53.1 Example +[source,ngc] +---- +G12.1 P1 (the TCP kinematics) +G68.2 X50 Y50 Z0 I30 J20 K0 (a plane) +G53.1 (rotaries to its normal, the head does it) +G0 X0 Y0 Z10 (rapid to a point in the plane, above its origin) +G1 Z-2 F200 (down, along the plane's normal) +G69 +---- + +It is an error if: + +* No tilted work plane is active. +* The kinematics type in force is the identity kinematics, the module cannot + be evaluated by the interpreter, or it reports no frames, so it cannot say + where its joints point the tool. +* The plane's normal cannot be reached by the rotary joints. +* 'P' is anything but 0, 1 or 2, no reachable pose has the rotary that + names it the way 'P' asks for, or the machine has no pair of poses a + tilting joint tells apart. +* 'Q' is anything but 0 or 1. +* Axis words are used with 'G53.1', 'G53.2' or 'G53.6', or words other than + 'X', 'Y' and 'Z' with 'G53.3'. +* Cutter compensation is on. + +[[gcode:g53.4]] +== G53.4, G53.5, G53.7 Point-to-Point Move(((G53.4 Point-to-Point Move))) + +[source,ngc] +---- +G53.4 G0 +G53.4 G1 F- +G53.5 G0 +G53.5 G1 F- +G53.7 G0 J= ... +G53.7 G1 F- J= ... +---- + +A point-to-point move defines its two ends and leaves the path between them +to the joints. The inverse kinematics runs once, at the destination, and every +joint then travels from where it is to where it must be, all together, the +slowest setting the pace. The tool does not follow a straight line; on a +machine with rotary joints it swings. The joints are the motion controller's, +<>, the ones the kinematics module maps the axes +to; what HAL connects behind each of them is not part of the interpolation. + +All three are non-modal, like 'G53': they apply to the one block they are +written in, which must have 'G0' or 'G1' in force or on the line. Each strips +one more layer of interpretation from the destination than the one before: + +* 'G53.4' takes a destination in program coordinates, through the offsets and + the tilted work plane like any other move. This is the move a program uses + to cross a kinematic singularity, or to turn a rotary head right round, + without leaving its coordinate system and without the trajectory planner + trying to hold the tool tip on a line the joints cannot follow at speed. +* 'G53.5' takes the same axis letters as a position in the machine frame + with the tool left out: 'X', 'Y' and 'Z' are the point the tool hangs + from, the pivot of a head or the flange of a robot, in machine + coordinates, and the rotary letters are the orientation as the machine + frame kinematics reports it, on a mill the rotary joints themselves. On a + machine whose slides line up with its frame that is the slides: each + letter names the joints the module's identity mapping gives it, which the + `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, and + the two joints of a gantry take one value together. A module whose + carriage does not line up, a slanted slide or a composite Y on a mill-turn, + declares a machine frame type of its own (see the + <> chapter) and 'G53.5' + goes through it, so that 'Y10' is machine Y whichever slides make it, and + a letter not given holds its machine coordinate rather than a joint. On a + robot the machine frame is the arm kinematics with no tool, the flange in + the base frame, so 'Z10' puts the flange at base Z 10 with its orientation + held and 'A', 'B' and 'C' are the flange angles the module reports; the + move is still interpolated in joint space. Values are in program units, so 'G20' scales them like any other axis + word. No offset, tool length, rotation or work plane applies. On a mill + whose joint 2 carries the Z slide, 'G53.5 G0 Z0' puts that slide at zero + whatever the head and the tool are doing, where under a tool centre point + kinematics 'G53 G0 Z0' puts the point a tool length above the tip at + machine Z zero, which moves with the head angle and the tool. + This is the move for parking, tool change and home positions, + where the slides matter and the part does not. It is not a joint jog: the + machine stays in world mode and the position on the screen stays true + through the move. ++ +An axis letter carries a unit class and a joint does not, so where the +machine frame is the joints 'G53.5' is refused on a machine whose letters +name joints of the other kind: a module that declares no kinematics types +and answers X with a rotary joint, which turns in degrees, has the whole +code refused rather than that one letter. Which joints turn is what +`[JOINT_n] TYPE` declares. + +* 'G53.7' takes joint values, one word per joint, and works on every machine: + 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the + `[JOINT_n]` section and the `joint.n` HAL pins, and the value after '=' is + the joint's own position, what `joint.n.pos-cmd` shows, in the joint's own + units. Nothing is converted, not even 'G20' and 'G21'. A joint left out + keeps its position, and the two joints of a gantry pair must both be given, + with one value. This is the form that names a joint outright, a robot's + joints by number, and the only form on a machine whose letters do not name + the joints they look like and whose module declares no machine frame. + +With 'G0' the speed comes from the joint limits in the INI file, +`[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint +exceeds its own; the rapid override applies. With 'G1' the move takes the time +the straight move to the same destination would take at the programmed feed: +under G94 the F word applies to the distance between the two ends by the same +rule as a straight move, XYZ if any of them move, else UVW, else the rotary +words; under G93 the move takes 1/F minutes exactly, which is the form to use +when the ends coincide or only the joints move. The feed override applies, and +the joint limits still cap it. Nothing blends into or out of a point-to-point +move: the move before it comes to a stop and so does the move itself. + +'G53.4' is also the move <> and <> make. + +.G53.4 Example +[source,ngc] +---- +G0 X0 Y0 Z100 A0 C0 +G53.4 G0 A90 C180 (swing the rotaries round, the tip goes where the joints take it) +G0 X0 Y0 Z100 (and back on a straight line) +---- + +.G53.5 Example +[source,ngc] +---- +G53.5 G0 Z0 (the Z slide to its zero, whatever the head's tilt) +G53.5 G0 X0 Y0 B0 C0 (park X, Y and both head joints; the Z slide stays) +G93 G53.5 G1 F0.5 C180 (turn the C joint to 180 in two minutes) +G94 +---- + +.G53.7 Example +[source,ngc] +---- +G53.7 G0 J2=0 (the Z slide, joint 2, to its zero) +G53.7 G0 J0=0 J1=0 J4=0 J5=0 (park those four joints; joint 2 stays) +G93 G53.7 G1 F0.5 J5=180 (turn joint 5 to 180 in two minutes) +G94 +---- + +It is an error if: + +* Neither 'G0' nor 'G1' is in force. +* An axis letter is without a real value, or one is used that is not + configured. +* With 'G53.5', no axis word is given, a letter is used that is not a joint of + the kinematics, the machine's letters name joints of the other kind, or the + machine frame kinematics cannot reach the point. +* With 'G53.7', an axis word or a plain 'J' word is used, no joint word is + given, a joint number is not a whole number from 0 to 15 or is beyond the + joints of the kinematics, or one joint of a gantry pair is given without the + other or with a different value. +* With 'G53.5' or 'G53.7', polar coordinates are used, incremental distance + mode is in force, or the kinematics module cannot be evaluated by the + interpreter. +* A joint word, 'J=', is used without 'G53.7'. +* With 'G1', the feed is zero, or under G94 the two ends coincide, or feed per + revolution (G95) is in force. +* Cutter compensation is on. +* An external offset is applied when the move reaches motion. + [[gcode:g54-g59.3]] == G54-G59.3 Select Coordinate System(((G54-G59.3 Select Coordinate System))) @@ -2017,7 +2451,7 @@ G64 P0.015 Q2 image::images/G64_Heart_Q2.png["G64 Heart",align="center"] [[gcode:g68.2]] -== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) +== G68.2, G68.3, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) [source,ngc] ---- @@ -2029,6 +2463,7 @@ G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side; R on one block) G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.3 X- Y- Z- (the plane from the tool direction) G68.4 ... (any G68.2 form, on the active plane) G69 (cancel) ---- @@ -2078,12 +2513,17 @@ does not change what a joint is. Words left out are zero. 'R' turns the plane about its own Z after everything else, in degrees. -The forms follow the Fanuc tilted working plane commands, with a few -extensions: 'R' on 'P0', 'P1' and 'P3' (Fanuc gives it with 'P2' only), -'Q' on 'P0' (Fanuc's Euler angles are always Z, X, Z), 'P1' orders that -repeat an axis, such as 'Q121', besides the six that name each axis once, -and all three angles zero, which is the plane of the coordinate system -underneath rather than an error, as on a Fanuc with parameter ATW set. +The forms are numbered as Fanuc numbers them, and the two angle forms are the +conventions Heidenhain writes as `PLANE EULER` and `PLANE SPATIAL`. Fanuc has +a fifth form, projection angles, that is not implemented. See the +<> chapter. + +Beyond the Fanuc commands, the forms accept 'R' on 'P0', 'P1' and 'P3' +(Fanuc gives it with 'P2' only), 'Q' on 'P0' (Fanuc's Euler angles are +always Z, X, Z), 'P1' orders that repeat an axis, such as 'Q121', besides +the six that name each axis once, and all three angles zero, which is the +plane of the coordinate system underneath rather than an error, as on a +Fanuc with parameter ATW set. The blocks of a 'P2' or 'P3' definition have to follow one another; any other block in between is an error. A definition left unfinished in MDI is @@ -2091,6 +2531,14 @@ dropped when a program is opened. A definition with a plane already active replaces it, with the words in the coordinate system underneath, not in the old plane. +'G68.3' takes the plane from the tool: Z is the tool axis as the rotary +joints have it at that moment, X is the default tool X of the kinematics +conventions, tool X turned about the tool axis by the smaller angle that +makes it parallel to the machine XY plane, and machine X when the tool is +vertical; 'R' turns the plane from there. It asks the kinematics module for +the tool direction, so it needs a kinematics type whose frames describe the +machine, as <> does. + 'G68.4' takes any 'G68.2' form and composes it onto the active plane: the words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. @@ -2133,7 +2581,8 @@ does not set one itself. With the setting on: Nothing about the plane is written to the parameter file, so a restart always cancels it. -Defining the plane does not move anything. +Defining the plane does not move anything. To bring the tool normal to it +use <>. While a plane is active the codes that change the coordinate system the plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G10 L2', @@ -2150,7 +2599,7 @@ its own axes, as Fanuc's local coordinate system does in a tilted working plane: 'G52 X10' moves the program's zero 10 along the plane's X. It is not written to the 'G92' parameters; 'G52 X0 Y0 Z0' takes it out, and 'G69' drops it. A 'G52' given before the plane stays in 'G92', under the plane. A -new plane, by 'G68.2' or 'G68.4', is refused while a 'G52' in the plane is +new plane, by 'G68.2', 'G68.3' or 'G68.4', is refused while a 'G52' in the plane is in effect, as on Fanuc. The active plane is reported in the modal G-code display as the code that @@ -2163,6 +2612,7 @@ plane coordinates or draw the plane. ---- G54 G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G53.1 (tool normal to it) G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) G1 Z-3 F150 (a hole 3 deep, straight into the plane) G0 Z5 @@ -2184,7 +2634,11 @@ It is an error if: with any form. * 'R' is given on more than one block of a 'P2' definition. * 'G68.4' is used with no plane active. -* 'G68.2' or 'G68.4' is used while a 'G52' given in the plane is in effect. +* 'G68.2', 'G68.3' or 'G68.4' is used while a 'G52' given in the plane is in effect. +* 'G68.3' is used where the kinematics cannot be evaluated, is the identity + kinematics, or reports no frames. +* 'G68.3' is given 'I', 'J', 'K', 'P' or 'Q', or 'G69' any of those or 'R': + a word the code does not use is an error, not ignored. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 3aeb7ba083e..887f3d48864 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -122,7 +122,7 @@ The table includes N and O for completeness, even though, as defined above, line |G | General function (See table <>) |H | Tool length offset index |I | X offset for arcs and G87 canned cycles -|J | Y offset for arcs and G87 canned cycles +|J | Y offset for arcs and G87 canned cycles; as J=, the position of joint n for <> .2+|K | Z offset for arcs and G87 canned cycles. <| Spindle-Motion Ratio for G33 synchronized movements. |L | generic parameter word for G10, M66 and others @@ -305,6 +305,11 @@ example '##2' means the value of the parameter whose index is the which the G38 took place. Volatile. * '5070' - <> probe result: 1 if success, 0 if probe failed to close. Used with G38.3 and G38.5. Volatile. +* '5071-5079' - Pose last solved by <> for X, Y, Z, A, + B, C, U, V & W, in program units in the tilted work plane. Same values + as `#<_orient_x>` and kin. Read-only, volatile. +* '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. + Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, Z, A, B, C, U, V & W, in the current program units. Set by `G43`/`G43.1`/`G43.2`, and 0 when `G49` is in effect. These report the @@ -508,6 +513,13 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last + solved, in program units in the tilted work plane. '#<_orient_valid>' is + 1 once a 'G53.2' has run; reading the others before that is an error. The + same values are on the numbered parameters '#5071' to '#5080'. See + <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] @@ -965,7 +977,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G28.5, G30, G30.5, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 @@ -974,8 +986,8 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 -|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 +|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G43.5, G49 +|Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 79a3a116e21..4b48ec336a7 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -207,7 +207,9 @@ documentation for more info) *genhexkins.tool-offset*:: TCP offset from platform origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === genserkins - generalized serial kinematics @@ -254,8 +256,8 @@ replacing it. Put a given length in one column or the other, not both. Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B tilts. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. === pentakins - Pentapod Kinematics @@ -291,7 +293,9 @@ The forward kinematics iteration is controlled by HAL pins. *pentakins.tool-offset*:: TCP offset from effector origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === pumakins - kinematics for puma typed robots @@ -416,8 +420,9 @@ expected by it (XYZBCW `->` joints 0..5) Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B and C move. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. A tool length in the W column of the tool + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. An X and Y of the offset are + applied along the tool's own X and Y. A tool length in the W column of the tool table reaches the same place, once a block commands W, and adds to this one rather than replacing it. Put a given length in one column or the other, not both. diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 1c6e0fe406c..6049ec87d93 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -323,6 +323,8 @@ image::5-axis-figures/equation__38.png[align="center"] Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. +An X and Y in the offset, a tool held off the spindle axis, stay with the tool: along the machine's X and Y, however the table turns the part. + Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: [source,hal] diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 02446cf425a..284f46751c5 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -63,6 +63,33 @@ against, and the two halves of this chapter use different pairs deliberately. Positions are measured in the work frame, which is what makes a program independent of how the table is set. +With every rotary joint at zero, the work frame position is what the linear +joints read. The shipped head and table modules fold the pivot lengths and +the offsets in, so that the tool tip and the slides agree at the zero pose +and the pivot shows only as the compensation, `L(1 - cos B)` on a head, +once a rotary leaves zero. A module with a switchable identity type keeps +this of necessity, since the identity and the geometric type must agree at +the zero pose or a switch would jump the reported position; a module without +one keeps it too, so that `G53.5` machine frame positions and `G53` positions +coincide with the rotaries at zero. + +The machine frame is the frame with the tool left out: XYZ is the point the +tool hangs from, the pivot of a head or the flange of a robot, in machine +coordinates, and the rotary letters are the orientation as the machine frame +type reports it, the rotary joints on a mill, the flange angles on a robot. +It is what `G13.1` and `G49` select and `G53.5` moves in. On a machine whose +slides line up with its frame it is the identity type. A machine whose +carriage does not line up, a slanted slide, a composite Y made by two slides, +an offset pivot, declares a machine frame type of its own next to its +identity type, and its identity type then keeps the plain meaning, joints are +axes, which a consumer uses to skip the maths. A robot's working transform +reports the flange in the base frame, the tool applied on top by the +interpreter and not by the maths, so the one type is both the working +transform and the machine frame, what ABB calls `tool0` and KUKA +`$NULLFRAME`. The zero-pose rule holds for the machine frame type as it does +for the identity: the working transform and the machine frame type agree with +the rotaries at zero. + Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine coordinates. A consumer that wants the tool in workpiece coordinates composes @@ -274,6 +301,15 @@ are tilted work plane handling, tool length compensation along a tilted axis, previews and simulation models, and probing routines that have to say which way the stylus faces. +The frames are optional. A module that leaves them out answers neither, and a +caller that needs them, a tilted work plane first of all, refuses under it. +Of the in-tree modules whose joints tilt the tool or the work, all report both +frames except scorbot-kins, genhexkins and pentakins. scorbot-kins controls the +wrist centre and passes the wrist joints through as A and B, so it has no tool +frame to report. genhexkins and pentakins solve the forward iteratively, so the +platform's orientation needs the seed the forward keeps, which the frame calls +do not take. + The joint values that reach a requested orientation, through `kinematicsToolFrameInverse()`, which is the inverse of the pair above and the question a tilted work plane asks when it has to orient the machine. @@ -329,6 +365,13 @@ its X points, and `G53.1` moves the rotaries to align the tool axis. Neither refuses a program for naming both directions on a five-axis machine, and neither should this. +In tree the G-code side of that is `G68.2`, which defines the plane, and +`G53.1`, `G53.2`, `G53.3` and `G53.6`, which ask this inverse where the +rotaries go, with the plane's normal and its X as the request. Their `Q` word is the +`held` mask: `Q0` holds the joints the work frame survey finds and takes the +reported turn as the coordinate rotation it is, `Q1` holds nothing. See the +G-code chapter. + Some requests still do not pin the machine down. A five-axis machine asked to point the tool along the axis its primary rotary turns about can hold any primary angle; a machine with three orientation joints asked only for a tool @@ -527,8 +570,12 @@ the optional work and tool frames with the native rotation that relates the tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks -the no-transform type `G13.1` cancels to, `primary` the working transform -`G43.4` switches to (see the Switchable Kinematics chapter). A module with +the no-transform type, joints are axes; `primary` the working transform +`G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and +`G49` cancel to and `G53.5` moves in, which a module leaves unset when its +identity type is that frame and sets on its working transform when that +reports the flange with no tool, as the robot modules do (see the Switchable +Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. @@ -547,6 +594,18 @@ code says once if the pin is left disagreeing with it, which is a config setting a tool length where the tool table should. Motion references the call weakly, so a module written before it still loads and keeps its pin. +The offset in the block is the whole of it, X, Y and Z in the tool's own axes, +which are the machine's with the rotaries at zero, and canon adds all three to +the pose along the work axes. A type's maths takes the length back off and +applies it along the tool axis, and leaves X and Y alone. The shared code turns +those with the tool: the pose the maths answers gains `(I - R)(x, y, 0)`, `R` +the tool frame in work coordinates, and the pose handed to the inverse loses +it, settled on the joints the inverse finds; the Jacobian gets the matching +term. So a tool held off the spindle axis keeps its offset in the tool frame, +as the length does along the tool axis. Nothing is turned on a type flagged +identity or machine, neither of which applies the tool, nor for a module that +reports no tool frame, where X and Y stay on the work axes. + Outside realtime a module exports `kinsDescribe()`, which hands a caller its table and the ops of each type; the caller fills a block from wherever it likes and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, @@ -581,6 +640,11 @@ Frames:: separately, each against the machine frame, so that a consumer placing both bodies can. +Zero pose:: + With every rotary joint at zero the work frame position equals the linear + joints. Pivots and offsets live in the module and show only as compensation + once a rotary moves. + Signs:: Positive A, B and C are counterclockwise about work X, Y and Z viewed from the positive end, describing the motion of the tool relative to the @@ -596,6 +660,14 @@ Tool X:: Derive the value from the module's own rotation matrices, and write those matrices down in the module. +Tool offset:: + Apply the length, `p->tool.tran.z`, along the tool axis, and report the tip + plus the length along work Z, which is what canon added. Leave X and Y of + the offset to the shared code, which turns them with the tool frame the + module reports; that frame's third column has to be the axis the length is + applied along. A type that leaves the tool out of its maths, a robot's + flange, is flagged machine. + Orientation inverse:: A module that reports its frames gets it from the shared search and needs to do nothing. Register a closed form only where one exists, and where it does, @@ -633,6 +705,34 @@ Mount orientation is not this:: determinant, rather than as three angles whose ordering convention is written down nowhere. +== Relation to Other Controls + +The tilted work plane borrows its shape from the controls that had one first, +so a program or a post moving either way reads the same. + +The two angle forms are the two those controls define. `G68.2 P0`, three +angles each about an axis of the plane as rotated so far, is what Heidenhain +calls `PLANE EULER`: a precession about Z, a nutation about the X the +precession has already turned, and a rotation about the tilted Z. `G68.2 P1`, +three angles about the axes of the system underneath, is `PLANE SPATIAL`, +whose three angles are each about a non-tilted axis. The numbering of the +forms follows Fanuc's `G68.2`, where P0 is Euler angles, P1 roll, pitch and +yaw, P2 three points and P3 two vectors. Fanuc has a fifth form, projection +angles, that is not implemented. + +Selecting one of the two poses that reach a plane, `P1` and `P2` on +<>, is Heidenhain's `SEQ+` and `SEQ-`, and takes the same +reference: the sign of the rotary measured from its home position. Two +differences are worth knowing. A control offering `SEQ` falls back to the +nearer pose when both lie the same side of home, where these refuse and say +so. And `SEQ` is offered on a machine whose rotaries all carry the work, +keyed to the tilting one, where `P` here names a pose only when a rotary +turns the tool, so on a table-table machine only the nearest form is +available. + +Whether the joints carrying the work take part, `Q0` and `Q1`, is +Heidenhain's `COORD ROT` and `TABLE ROT`. + == References * <>, for the axis nomenclature diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 32b87d51a4a..c5c88f77c38 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -48,6 +48,28 @@ typically refer to the usual Cartesian coordinates. The A B C axes refer to rotational coordinates about the X Y Z axes respectively. The U V W axes refer to additional coordinates that are commonly made colinear to the X Y Z axes respectively. +[[sec:joint-space]] +=== Joint Space + +The joints the motion controller works with are the ones the kinematics +module defines: the numbers `kinematicsInverse()` produces and +`kinematicsForward()` reads, numbered as the `[JOINT_n]` sections of the INI +file and the `joint.N` HAL pins. Their zero is where homing put it and their +limits are the `[JOINT_n]` limits. _Joint space_ is the set of these +positions taken together, one coordinate per joint, as opposed to the +Cartesian coordinates of the axes. + +What sits behind a joint is a HAL matter. On most machines +`joint.N.motor-pos-cmd` drives one motor, so a joint and an actuator are the +same thing, but a joint can also drive two motors, or one motor through a +ratio, or feed a linkage computed in HAL, and the motion controller does not +know the difference. A point-to-point move, <>, runs each +of the controller's joints on a straight line from its start to its end +position at a rate within that joint's limits; the actuators behind them +follow through whatever HAL puts in between. With switchable kinematics the +joints do not change when the kinematics type does, only the mapping between +them and the axes. + == Trivial Kinematics The simplest machines are those in which which each joint is placed diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index edb93924ae2..77fe0642216 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -226,9 +226,10 @@ see Code Notes for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length offset and switches to the kinstype the module declares its working -transform, and the 'G49' that cancels it switches back to identity. -See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' -and 'G13.1', for the full description. +transform, 'G43.5' does the same and lets the lines after it give the +tool axis as a vector, and the 'G49' that cancels either switches back +to the machine frame kinstype, what 'G13.1' selects. See the G-code documentation for 'G43.4', 'G43.5' and +'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands @@ -573,8 +574,8 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform -. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, - the rotaries as joints +. *KINSTYPE_MACHINE* the machine frame: the tool left out, XYZ the pivot + or the flange in machine coordinates A kinstype registered with switchkinsRegisterOps() carries its flag in the ops table itself, as the 'identity', 'primary' or 'machine' field; a @@ -586,24 +587,38 @@ int switchkinsDeclare(int ktype, int flags); ---- G-code reads these declarations: 'G13.1' and 'G49' cancel to the -kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype -declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose -kinematics are not in the conventional order still gets working -spellings. The machine frame type is the machine with the orientation -left out, XYZ the pivot in machine coordinates and the rotary letters -the rotary joints; on a machine whose slides line up with its frame that -is the identity type, so a module that declares no machine frame -kinstype has its identity kinstype stand in, and every shipped module is -of that kind. A module whose carriage does not line up, a slanted slide -or a composite Y on a mill-turn, declares its machine frame kinstype +kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move +in its world, and 'G43.4' switches to the kinstype declared +KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics +are not in the conventional order still gets working spellings. With +`MACHINE_MOVES_IN_MACHINE_FRAME` set in the `[RS274NGC]` section of the +INI file, 'G53', 'G28' and 'G30' move in the machine frame kinstype's +world too whenever another kinstype is in force; the five axis sims set +it. The +machine frame kinstype is the machine with the tool left out, XYZ the +pivot or the flange in machine coordinates and the rotary letters the +orientation as that kinstype reports it; on a machine whose slides line +up with its frame that is the identity kinstype, so a module that +declares no machine frame kinstype has its identity kinstype stand in, +and every shipped machine tool module is of that kind. A robot's +working transform reports the flange in the base frame, the tool applied +on top by the interpreter and not by the maths, so pumakins and +genserkins declare that one kinstype both primary and machine: 'G13.1' +and 'G49' leave a robot on its arm kinematics, as ABB's `tool0` and +KUKA's `$NULLFRAME` do, and its identity kinstype is reached by number. +A machine frame kinstype does not read the tool offset in the parameter +block; a module whose working transform does declares a kinstype without +it. A module whose carriage does not line up, a slanted slide or a +composite Y on a mill-turn, declares its machine frame kinstype separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since motion and the planner skip the maths on that -flag alone. At most one kinstype may be declared identity, at most one -primary and at most one machine frame, and declaring a kinstype the -module does not provide fails the module load. A module that declares -nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but -'G13.1' and 'G43.4' are an error, since the numbers of the identity and -primary kinematics are then a guess. +flag alone. At most one +kinstype may be declared identity, at most one primary and at most one +machine frame, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +are then a guess. A module that implements kinematicsTypeFlags() itself, without switchkins.c, returns the flags of every kinstype it can switch to, 0 @@ -629,8 +644,17 @@ exports kinematicsSetTool() as well, through which motion hands it the tool offset in effect whenever that changes. A table entry flagged as the tool is overwritten with it, and the entry's pin only matters until motion has sent anything, so a config need not net -motion.tooloffset.z to the module. A kinstype registered the older -way reads its own pins and is not affected. +motion.tooloffset.z to the module. When the offset changes, motion +keeps the joints where they are and reads the point back from them +under the new offset, so a tool length change with the tool axis +tilted moves the programmed point rather than the machine; the +interpreter reads the point the same way through the non-realtime +loader, and motion stops the program if the two disagree. The X and +Y of the offset the shared code turns with the tool frame a kinstype +reports, so they lie along the tool's own X and Y as the length lies +along the tool axis; a kinstype flagged identity or machine, or one +without a tool frame, leaves them on the work axes. A kinstype +registered the older way reads its own pins and is not affected. === Module main program diff --git a/include/kinematics.h b/include/kinematics.h index f19a8dba1e0..eb9f0c15241 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -120,20 +120,23 @@ extern KINEMATICS_TYPE kinematicsType(void); ** numeric-only and G13.1 refuses to guess. ** ** The machine frame type is the one whose world is the machine frame -** with the orientation left out: XYZ is the pivot, the point the rotary -** joints do not move, in machine coordinates, and the rotary letters are -** the rotary joints as they are. On a machine whose slides line up with -** its frame that is the identity type, and where a module declares no -** machine frame type its identity type stands in. A module whose -** carriage does not line up, a slanted slide or an offset pivot, declares -** its machine frame type separately and keeps identity for a type whose -** joints really are the axes, since a consumer skips the maths on that -** flag alone. +** with the tool left out: XYZ is the point the tool hangs from, the pivot +** of a head or the flange of a robot, in machine coordinates, and the +** rotary letters are the orientation as that type reports it, the rotary +** joints on a mill, the flange angles on a robot. On a machine whose +** slides line up with its frame that is the identity type, and where a +** module declares no machine frame type its identity type stands in. A +** module whose carriage does not line up, a slanted slide or an offset +** pivot, declares its machine frame type separately and keeps identity +** for a type whose joints really are the axes, since a consumer skips +** the maths on that flag alone. A robot's working transform is its +** flange in the base frame with no tool in the maths, so it is the +** machine frame type as well and carries both flags. */ #define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ #define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ -#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine - coordinates, the rotaries as joints */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the tool left out, XYZ + the pivot or flange in machine coordinates */ /* flags of a kinematics type, 0 when neither applies, or -1 for a type ** the module does not provide (and for every type on a machine with plain diff --git a/include/kins_module.h b/include/kins_module.h index 13dc8f62411..17ebfd0a576 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -330,9 +330,12 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is the module's working transform, the type G43.4 switches to. machine says - this is the machine frame type, the pivot in machine coordinates with the - rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module - that leaves it unset on every type has its identity type stand in. */ + this is the machine frame type, the tool left out, the pivot or the flange + in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a + module that leaves it unset on every type has its identity type stand in. + A machine frame type does not read the tool offset in the parameter block, + so a working transform that leaves the tool out anyway, a robot's flange, + carries primary and machine both. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 3c83b5502a8..d7f3d17493a 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -193,5 +193,17 @@ def get_axis_mask(self): def get_block_delete(self): return self.s.block_delete + def get_external_joint_positions(self): + return tuple(self.s.joint_actual_position[:self.s.joints]) + + def get_kinematics_type(self): + return self.s.kinematics_type + + def get_kins_type(self): + return self.s.kins_type + + def get_kins_type_flags(self): + return self.s.kins_type_flags + # vim:ts=8:sts=4:et: diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index c43156490c9..8417050995b 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -631,6 +631,27 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); + /* The Jacobian holds only near the estimate, and a far pose asks + for a step of many radians: the arm lands somewhere unrelated + and converges by way of whole turns its limits refuse. Cap the + step, keeping its direction, and let the iteration walk there. */ + { + double worst = 0.0; + + for (link = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_ANGLE == linkout[link].quantity + && fabs(dj[link]) > worst) { + worst = fabs(dj[link]); + } + } + if (worst > GENSER_MAX_ANGLE_STEP) { + double scale = GENSER_MAX_ANGLE_STEP / worst; + for (link = 0; link < genser->link_num; link++) { + dj[link] *= scale; + } + } + } + //pass through 678 as uvw if (p->max_joints > 6) joints[6] = world->u; if (p->max_joints > 7) joints[7] = world->v; @@ -668,11 +689,42 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, return GO_RESULT_ERROR; } +// The arm carries the tool and nothing turns the work. The tool frame is the +// flange frame the DH chain ends in, z along the last joint axis and out of +// the flange as on pumakins, which the forward reports as roll, pitch and yaw +// in A B C; the half turn to the convention is the native rotation. +static int genser_tool_frame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose world; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double ca, sa, cb, sb, cc, sc; + + memset(&world, 0, sizeof(world)); + if (genser_forward(p, NULL, joint, &world, fflags, &iflags) != 0) { return -1; } + ca = cos(world.a * PM_PI / 180); sa = sin(world.a * PM_PI / 180); + cb = cos(world.b * PM_PI / 180); sb = sin(world.b * PM_PI / 180); + cc = cos(world.c * PM_PI / 180); sc = sin(world.c * PM_PI / 180); + // Rz(C) Ry(B) Rx(A), by columns + rot->x.x = cc * cb; rot->y.x = cc * sb * sa - sc * ca; rot->z.x = cc * sb * ca + sc * sa; + rot->x.y = sc * cb; rot->y.y = sc * sb * sa + cc * ca; rot->z.y = sc * sb * ca - cc * sa; + rot->x.z = -sb; rot->y.z = cb * sa; rot->z.z = cb * ca; + return 0; +} + +/* the world is the last link's frame in the base frame with no tool in the + maths, the machine frame of a robot as well as its working transform, so + the one type is both */ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, + .work = kinsIdentityFrame, + .tool = genser_tool_frame, + .native = &TOOL_FRAME_FLANGE, .jacobian = genser_jacobian, .primary = 1, + .machine = 1, }; /* diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c50cddde3e3..52f369a958e 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -45,6 +45,8 @@ #define GENSER_MAX_JOINTS 6 #define GENSER_DEFAULT_MAX_ITERATIONS 100 +/* the most a rotary joint moves in one pass of the inverse, in radians */ +#define GENSER_MAX_ANGLE_STEP 0.2 #define PI_2 GO_PI_2 diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index b66b3d9b9e7..12339ab59e3 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1670,6 +1670,90 @@ const kins_ops KINS_IDENTITY_OPS = { .identity = 1, }; +//---------------------------------------------------------------------- +// the tool offset across the tool axis +//---------------------------------------------------------------------- + +// The tool offset is given in the tool frame, and canon adds all of it to +// the pose along the work axes. A type's maths takes the length back off +// and applies it along the tool axis; X and Y it leaves, so they would +// stay on the work axes however the tool is turned. The shared code turns +// them with the tool: the pose is the module's plus (I - R)(x, y, 0), R +// the tool frame in work coordinates. Nothing to turn without an X or Y, +// on a type that is the identity or the machine frame, neither of which +// applies the tool, or for a module that reports no tool frame. +static int tool_across_applies(const kins_ops *ops, const kins_params *p) +{ + return (p->tool.tran.x != 0.0 || p->tool.tran.y != 0.0) + && !ops->identity && !ops->machine && ops->tool; +} + +// (I - R)(x, y, 0) at these joints, or -1 where the frames cannot say +static int tool_across(const kins_ops *ops, const kins_params *p, + const double *joint, PmCartesian *d) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix work, r; + const double x = p->tool.tran.x, y = p->tool.tran.y; + + if (kinsOpsToolFrame(ops, p, joint, &r, &fflags)) { return -1; } + // where nothing turns the work, the work frame is the machine's + if (kinsOpsWorkFrame(ops, p, joint, &work, &fflags) == 0 + && toolFrameInWork(&work, &r, &r)) { return -1; } + d->x = x - (r.x.x * x + r.y.x * y); + d->y = y - (r.x.y * x + r.y.y * y); + d->z = - (r.x.z * x + r.y.z * y); + return 0; +} + +static void pose_shift(EmcPose *pos, const PmCartesian *d, double sign) +{ + pos->tran.x += sign * d->x; + pos->tran.y += sign * d->y; + pos->tran.z += sign * d->z; +} + +// The Jacobian of the module's maths, at the pose that maths has the +// joints at, turned into the Jacobian of the pose with the offset across. +// The turn d follows the joints, and the joints the pose less d, so +// dq = J (dP - D dq) with D the change of d per joint, and J (I - D J) is +// the answer: D J reaches only from the rotaries into X Y Z, so it squares +// to nothing and the series stops there. +static int tool_across_jacobian(const kins_ops *ops, const kins_params *p, + const double *joint, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double q[EMCMOT_MAX_JOINTS], D[3][EMCMOT_MAX_JOINTS], DJ[3][EMCMOT_MAX_AXIS]; + const int n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + PmCartesian dp, dm; + int i, a, b; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { q[i] = joint[i]; } + for (i = 0; i < n; i++) { + q[i] = joint[i] + KINS_JACOBIAN_STEP; + if (tool_across(ops, p, q, &dp)) { return -1; } + q[i] = joint[i] - KINS_JACOBIAN_STEP; + if (tool_across(ops, p, q, &dm)) { return -1; } + q[i] = joint[i]; + D[0][i] = (dp.x - dm.x) / (2 * KINS_JACOBIAN_STEP); + D[1][i] = (dp.y - dm.y) / (2 * KINS_JACOBIAN_STEP); + D[2][i] = (dp.z - dm.z) / (2 * KINS_JACOBIAN_STEP); + } + for (a = 0; a < 3; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { + DJ[a][b] = 0; + for (i = 0; i < n; i++) { DJ[a][b] += D[a][i] * jac[i][b]; } + } + } + for (i = 0; i < n; i++) { + double row[3] = { jac[i][0], jac[i][1], jac[i][2] }; + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { + jac[i][b] -= row[0] * DJ[0][b] + row[1] * DJ[1][b] + row[2] * DJ[2][b]; + } + } + return 0; +} + //---------------------------------------------------------------------- // asking an ops table, defaults applied //---------------------------------------------------------------------- @@ -1679,20 +1763,25 @@ int kinsOpsForward(const kins_ops *ops, const kins_params *p, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) { - int r; + PmCartesian d; + int r, across, seeded = 0; if (!ops || !ops->forward || !p || !s) { return -1; } + across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; if (ops->fwd_iterates && s->have_pose_seed) { /* no pose of our own yet: start from the caller's estimate, which stays in *pos, rather than from a never-solved seed */ - if (s->pose_seed_ok) { *pos = s->pose_seed; } + if (s->pose_seed_ok) { *pos = s->pose_seed; seeded = 1; } s->have_pose_seed = 0; } + // the caller's estimate, as the module's maths sees it + if (ops->fwd_iterates && across && !seeded) { pose_shift(pos, &d, -1); } r = ops->forward(p, s, joint, pos, fflags, iflags); if (ops->fwd_iterates && r == 0) { /* keep the result only when the solve succeeds */ s->pose_seed = *pos; s->pose_seed_ok = 1; } + if (r == 0 && across) { pose_shift(pos, &d, 1); } return r; } @@ -1701,8 +1790,33 @@ int kinsOpsInverse(const kins_ops *ops, const kins_params *p, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags) { + double seed[EMCMOT_MAX_JOINTS]; + PmCartesian used = { 0, 0, 0 }, d; + EmcPose q; + int r, pass, n; + if (!ops || !ops->inverse || !p || !s) { return -1; } - return ops->inverse(p, s, pos, joint, iflags, fflags); + if (!tool_across_applies(ops, p)) { + return ops->inverse(p, s, pos, joint, iflags, fflags); + } + // a module may read where the machine is from the joints it is + // handed, so every pass starts from the caller's + n = p->max_joints < EMCMOT_MAX_JOINTS ? p->max_joints : EMCMOT_MAX_JOINTS; + memcpy(seed, joint, n * sizeof(seed[0])); + r = ops->inverse(p, s, pos, joint, iflags, fflags); + // the turn follows the joints and the joints the turn: one more pass + // settles it, the tool turning with the rotaries the pose names + for (pass = 0; r == 0 && pass < 4; pass++) { + if (tool_across(ops, p, joint, &d)) { break; } + if (fabs(d.x - used.x) < 1e-12 && fabs(d.y - used.y) < 1e-12 + && fabs(d.z - used.z) < 1e-12) { break; } + used = d; + q = *pos; + pose_shift(&q, &used, -1); + memcpy(joint, seed, n * sizeof(seed[0])); + r = ops->inverse(p, s, &q, joint, iflags, fflags); + } + return r; } int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, @@ -1736,17 +1850,27 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; KINEMATICS_FORWARD_FLAGS ffl = 0; - EmcPose q; - int j, a; + EmcPose at, q; + PmCartesian d; + int j, a, r, across; if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } - if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + // the module's maths has these joints at the pose less the turn of + // the offset across the tool + at = *pos; + across = tool_across_applies(ops, p) && tool_across(ops, p, joint, &d) == 0; + if (across) { pose_shift(&at, &d, -1); } + if (ops->jacobian) { + r = ops->jacobian(p, joint, &at, jac, iflags); + if (r || !across) { return r; } + return tool_across_jacobian(ops, p, joint, jac); + } if (!ops->inverse) { return -1; } // the same differences as kinsJacobianFromInverse(), on the block form kj_zero(jac); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { - q = *pos; + q = at; for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } *kj_coord(&q, a) += KINS_JACOBIAN_STEP; @@ -1759,7 +1883,7 @@ int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); } } - return 0; + return across ? tool_across_jacobian(ops, p, joint, jac) : 0; } // kinsOpsJacobian() //---------------------------------------------------------------------- diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index a32981d12cf..1f2e6e596aa 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -173,9 +173,46 @@ static int max_jacobian(const kins_params *p, const double *joints, return 0; } +// The C table turns the work by +C into the pose, so the work frame is Rz(-C). +// The head tilts the tool about machine y by B, the sense set by the pin: the +// W joint runs the tip out along (-con*sin(B), 0, -cos(B)), so the tool axis, +// tip towards holder, is the third column of Ry(con*B). +static int max_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)fflags; + const double sc = sin(d2r(joints[5])), cc = cos(d2r(joints[5])); + + rot->x.x = cc; rot->y.x = sc; rot->z.x = 0; + rot->x.y = -sc; rot->y.y = cc; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; + + return 0; +} + +static int max_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double con = CON(p); + const double sb = sin(d2r(joints[4])), cb = cos(d2r(joints[4])); + + rot->x.x = cb; rot->y.x = 0; rot->z.x = con * sb; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = -con * sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} + static const kins_ops max_ops = { .forward = max_forward, .inverse = max_inverse, + .work = max_work_frame, + .tool = max_tool_frame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = max_jacobian, }; diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 7f02f1777d6..418e68e23f0 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -419,7 +419,10 @@ static int puma_inverse(const kins_params *p, kins_scratch *s, // is the shared identity one. The maths is the ISO 9787 flange frame, so // the tool axis it produces runs holder towards tip, the opposite of the // convention; the declared half turn puts it right. No closed form -// Jacobian: the shared code differences the inverse. +// Jacobian: the shared code differences the inverse. The world is the +// flange in the base frame with no tool in the maths, D6 being geometry, +// which is the machine frame of a robot as well as its working transform, +// so the one type is both. static const kins_ops puma_ops = { .forward = puma_forward, .inverse = puma_inverse, @@ -428,6 +431,7 @@ static const kins_ops puma_ops = { .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, .primary = 1, + .machine = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index fd9760e59ac..b669000e664 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -120,6 +120,50 @@ void emcmotApplyPendingPlannerType(void) } /* ===== END PLANNER_SWITCH_DEFER ==================================================== */ +/* the inverse once, for an endpoint, run to a fixed point: some modules + read the joints they are handed (a nutating head takes its rotary angles + from them), so one pass from a stale seed answers for the wrong angles, + and running again from its own answer settles it */ +static int inverse_settled(EmcPose *pos, double *joints, + KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int pass, j; + + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { prev[j] = joints[j]; } + if (kinematicsInverse(pos, joints, iflags, fflags) != 0) { return -1; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { worst = fmax(worst, fabs(joints[j] - prev[j])); } + if (worst < 1e-9) { break; } + } + return 0; +} + +/* Where the queue leaves the joints, which is the seed an iterative + inverse wants: the reading runs ahead of the machine, so the joints to + hand it are not the ones the machine is standing in. A joint + interpolated segment knows its own end; the rest take the answer the + last endpoint checked came out with. Returns 0, and the joints the + machine stands in, when there is nothing queued to ask. */ +static double planned_joints[EMCMOT_MAX_JOINTS]; +static int planned_joints_ok = 0; + +static int queue_end_joints(double *joints_out) +{ + int j; + + if (tpGetQueueEndJoints(&emcmotInternal->coord_tp, joints_out)) { return 1; } + if (planned_joints_ok && tpQueueDepth(&emcmotInternal->coord_tp) > 0) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { joints_out[j] = planned_joints[j]; } + return 1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + joints_out[j] = (j < ALL_JOINTS) ? joints[j].pos_cmd : 0.0; + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -268,17 +312,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) /* Now, check that the endpoint puts the joints within their limits too */ - /* fill in all joints with 0 */ - for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { - joint = &joints[joint_num]; - joint_pos[joint_num] = joint->pos_cmd; - } + /* start the inverse from where the queue leaves the joints */ + queue_end_joints(joint_pos); /* now fill in with real values, for joints that are used */ - if (kinematicsInverse(&pos, joint_pos, &iflags, &fflags) != 0) + if (inverse_settled(&pos, joint_pos, &iflags, &fflags) != 0) { reportError(_("%s move on line %d fails kinematicsInverse"), move_type, id); + planned_joints_ok = 0; return 0; } @@ -309,6 +351,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) move_type, id, joint_num, joint->min_pos_limit); } } + + /* an endpoint on its way to the queue is where the next one starts + from; a refused one leaves the queue as it was */ + if (in_range) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + planned_joints[joint_num] = joint_pos[joint_num]; + } + planned_joints_ok = 1; + } return in_range; } @@ -1122,6 +1173,161 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } break; + case EMCMOT_SET_JOINT_LINE: { + /* a move interpolated in joint space to a Cartesian endpoint: the + inverse runs once here, at the endpoint, and the planner takes + the joints from there; or the endpoint is given as joints and + the forward says where that is */ + double start[EMCMOT_MAX_JOINTS], target[EMCMOT_MAX_JOINTS]; + EmcPose end = emcmotCommand->pos; + double length = 0.0, vmax = 0.0, amax = 0.0, jmax = 0.0; + int moving = 0, jerk_limited = 0, bad = 0, axis_num; + + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_LINE"); + if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { + reportError(_("need to be enabled, in coord mode for joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; + SET_MOTION_ERROR_FLAG(1); + break; + } + if (!limits_ok()) { + reportError(_("can't do joint interpolated move with limits exceeded")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + /* an external offset is applied to the world position on the way + to the inverse every cycle, which a joint interpolated segment + does not go through */ + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + if (axis_get_ext_offset_curr_pos(axis_num) != 0.0) { bad = 1; } + } + if (bad) { + reportError(_("can't do joint interpolated move on line %d with an external offset applied"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* where the queue ends in joint space */ + if (!queue_end_joints(start)) { + EmcPose goal; + tpGetGoalPos(&emcmotInternal->coord_tp, &goal); + if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { target[joint_num] = start[joint_num]; } + if (emcmotCommand->have_joint_target) { + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + target[joint_num] = emcmotCommand->joint_target[joint_num]; + } + if (kinematicsForward(target, &end, &fflags, &iflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsForward"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } else { + if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + if (inverse_settled(&end, target, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + /* the endpoint must be inside the joint limits, and every joint + that moves needs limits to move within; the segment length is + the joint space distance and each joint's limits are scaled + onto it so that the slowest joint sets the pace */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + double d = target[joint_num] - start[joint_num]; + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint)) { continue; } + if (!isfinite(target[joint_num])) { + reportError(_("joint interpolated move on line %d gave non-finite joint location on joint %d"), + emcmotCommand->id, joint_num); + bad = 1; + } else if (target[joint_num] > joint->max_pos_limit || target[joint_num] < joint->min_pos_limit) { + reportError(_("joint interpolated move on line %d would exceed joint %d's limit"), + emcmotCommand->id, joint_num); + bad = 1; + } + length += d * d; + } + length = sqrt(length); + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS && !bad; joint_num++) { + double d = fabs(target[joint_num] - start[joint_num]); + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint) || d < TP_POS_EPSILON) { continue; } + if (joint->vel_limit <= 0.0 || joint->acc_limit <= 0.0) { + reportError(_("joint interpolated move on line %d: joint %d has no velocity or acceleration limit"), + emcmotCommand->id, joint_num); + bad = 1; + break; + } + if (!moving || joint->vel_limit * length / d < vmax) { vmax = joint->vel_limit * length / d; } + if (!moving || joint->acc_limit * length / d < amax) { amax = joint->acc_limit * length / d; } + if (joint->jerk_limit > 0.0) { + if (!jerk_limited || joint->jerk_limit * length / d < jmax) { jmax = joint->jerk_limit * length / d; } + jerk_limited = 1; + } + moving = 1; + } + if (bad) { + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* a feed asks for a time; the joint limits still cap it */ + double vreq = vmax; + if (emcmotCommand->joint_seconds > 0.0 && length / emcmotCommand->joint_seconds < vmax) { + vreq = length / emcmotCommand->joint_seconds; + } + tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id); + int res_addjoint = tpAddJointLine(&emcmotInternal->coord_tp, + start, target, NO_OF_KINS_JOINTS, end, + emcmotCommand->motion_type, + vreq, vmax, amax, jmax, + emcmotStatus->enables_new, + emcmotCommand->tag); + if (res_addjoint < 0) { + reportError(_("can't add joint interpolated move at line %d, error code %d"), + emcmotCommand->id, res_addjoint); + emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } else if (res_addjoint == 0) { + SET_MOTION_ERROR_FLAG(0); + rehomeAll = 1; + } + break; + } + case EMCMOT_SET_CIRCLE: /* emcmotInternal->coord_tp up a circular move */ /* requires coordinated mode, enable on, not on limits */ @@ -2022,10 +2228,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); - emcmotStatus->tool_offset = emcmotCommand->tool_offset; if (kinematicsSetTool) { - kinematicsSetTool(&emcmotStatus->tool_offset); + /* the module applies the offset, so the point the joints + stand on changes with it */ + kinematicsSetTool(&emcmotCommand->tool_offset); + emcmotToolOffsetChanged(&emcmotStatus->tool_offset, + &emcmotCommand->tool_offset, + &emcmotCommand->pos, + emcmotCommand->have_point); } + emcmotStatus->tool_offset = emcmotCommand->tool_offset; break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 7c9a35c237b..22cbfdaf5a4 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -194,6 +194,7 @@ static void output_to_hal(void); static void update_status(void); static void handle_kinematicsSwitch(void); +static int reanchor_pose(const double *joint_pos, EmcPose *at); /*********************************************************************** * PUBLIC FUNCTION CODE * @@ -366,8 +367,6 @@ static void handle_kinematicsSwitch(void) { return; // the kinematics in force is unchanged } - KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; - KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG double beforePose[EMCMOT_MAX_AXIS]; int anum; @@ -379,9 +378,7 @@ static void handle_kinematicsSwitch(void) { solve them is one the machine cannot run in from here: put the old one back, or the inverse would run the joints to wherever the pose we know lands in the new one */ - EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, - &tmpFFlags, &tmpIFlags)) { + if (reanchor_pose(joint_posKinsSwitch, NULL) != 0) { kinematicsSwitch(switchkins_type); reportError(_("kinematicsForward failed for kinematics type %d," " type %d is still in force"), @@ -389,7 +386,6 @@ static void handle_kinematicsSwitch(void) { SET_MOTION_ERROR_FLAG(1); // abort return; // the kinematics in force and the position are unchanged } - emcmotStatus->carte_pos_cmd = poseKinsSwitch; switchkins_type = requested_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); @@ -401,9 +397,29 @@ static void handle_kinematicsSwitch(void) { ,anum,beforePose[anum],*pcmd_p[anum],*pcmd_p[anum]-beforePose[anum]); } #endif +} //handle_kinematicsSwitch() + +/* The point re-read from the joints, for when what the joints mean has + changed without the joints moving: a kinematics switch, or a tool + offset the module applies. The pose they put the tool at is the new + commanded point, the external offsets taken off it, and the planner + is moved onto it. A forward that fails leaves the point alone. + The pose with the external offsets still on it is returned in at. */ +static int reanchor_pose(const double *joint_pos, EmcPose *at) +{ + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; + KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; + EmcPose pose = emcmotStatus->carte_pos_cmd; + + if (kinematicsForward(joint_pos, &pose, &tmpFFlags, &tmpIFlags) != 0) { + return -1; + } + emcmotStatus->carte_pos_cmd = pose; + if (at) { *at = pose; } axis_apply_ext_offsets_to_carte_pos(-1, pcmd_p); tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); -} //handle_kinematicsSwitch() + return 0; +} static void process_inputs(void) { @@ -932,6 +948,82 @@ static void check_for_faults(void) } } +/* The joints a joint interpolated segment ended on, held while the + planner stays at that point: a module's inverse answers with its own + joint set, which a robot wrist reaches with the forearm turned half a + revolution from the one asked for. The hold ends when the point moves. */ +static int joint_hold_valid = 0; +static double joint_hold[EMCMOT_MAX_JOINTS]; +static EmcPose joint_hold_pose; + +/* whether two machine points are within tol of each other on every axis */ +static int carte_pos_within(const EmcPose *a, const EmcPose *b, double tol) +{ + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + return carte_pos_within(a, b, 1e-9); +} + +/* A tool offset the module applies has changed under the point. The + machine stays where it is: the point is re-read from the joints under + the new offset, and the joints are held there, since the inverse of the + re-read point may answer with another joint set. The interpreter + works out the same point ahead of motion and sends it along when it + can evaluate the kinematics; where the two disagree, or where it could + not say and the point moved, the program is not let go on from a point + the interpreter does not have. Nothing to do outside coordinated mode: + the point follows the joints there anyway. */ +void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected) +{ + const double tol = 1e-4; + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + EmcPose was = emcmotStatus->carte_pos_cmd; + EmcPose now; + int joint_num; + + if (same_carte_pos(from, to) || !GET_MOTION_COORD_FLAG()) { return; } + for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { + joint_pos[joint_num] = joints[joint_num].coarse_pos; + } + if (reanchor_pose(joint_pos, &now) != 0) { + reportError(_("the kinematics cannot place the tool from the joints" + " after the tool offset change")); + SET_MOTION_ERROR_FLAG(1); + return; + } + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = joint_pos[joint_num]; + } + joint_hold_pose = now; + joint_hold_valid = 1; + + if (have_expected) { + if (!carte_pos_within(&emcmotStatus->carte_pos_cmd, expected, tol)) { + reportError(_("the tool offset change put the point at" + " %.4f %.4f %.4f, the interpreter expected" + " %.4f %.4f %.4f"), + emcmotStatus->carte_pos_cmd.tran.x, + emcmotStatus->carte_pos_cmd.tran.y, + emcmotStatus->carte_pos_cmd.tran.z, + expected->tran.x, expected->tran.y, expected->tran.z); + SET_MOTION_ERROR_FLAG(1); + } + } else if (!carte_pos_within(&was, &now, tol)) { + reportError(_("the tool offset change moved the point under a" + " kinematics the interpreter cannot evaluate;" + " change the tool offset with the machine untilted")); + SET_MOTION_ERROR_FLAG(1); + } +} + static void set_operating_mode(void) { int joint_num; @@ -1430,18 +1522,73 @@ static void get_pos_cmds(long period) emcmotStatus->syncOverrunSpindle = 0; SET_MOTION_ERROR_FLAG(1); } + + if (tpGetJointPos(&emcmotInternal->coord_tp, positions) > 0) { + /* a joint interpolated segment: the planner hands out the + joints and the forward kinematics says where the tool is, + for status and for the display; nothing is inverted, and + the planner's own position is the chord between the ends. + The joints are commanded either way; a forward that fails, + as an iterating one can at a singularity, leaves the last + solved position reported rather than an unsolved one */ + EmcPose pose = emcmotStatus->carte_pos_cmd; + if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { + emcmotStatus->carte_pos_cmd = pose; + emcmotStatus->carte_pos_cmd_ok = 1; + } else { + emcmotStatus->carte_pos_cmd_ok = 0; + } + result = 0; + } else { + /* a joint interpolated segment that ended this cycle is gone + from the queue: its end joints seed the inverse, since the + modules that read their rotary angles from the seed would + otherwise get last cycle's */ + int joint_end_fresh = tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); - if (axis_update_coord_with_bound(pcmd_p, servo_period)) { + if (tpJointSegmentsQueued(&emcmotInternal->coord_tp)) { + /* an external offset cannot ride on a joint interpolated + segment: its joints were solved without one, and the + queue refused the segment while one was applied. A + request that arrives while one is queued waits here, + unplanned, and ramps in at its own limits once the last + joint segment is done, instead of landing as a step at + the segment's ends */ + } else if (axis_update_coord_with_bound(pcmd_p, servo_period)) { ext_offset_coord_limit = 1; } else { ext_offset_coord_limit = 0; } - /* OUTPUT KINEMATICS - convert to joints in local array */ - result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, - &iflags, &fflags); + /* OUTPUT KINEMATICS - convert to joints in local array, or + hold the joints a joint interpolated segment ended on while + the planner stays at the point they put the machine on */ + if (joint_end_fresh) { + EmcPose at = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 0; + if (kinematicsForward(positions, &at, &fflags, &iflags) == 0 + && same_carte_pos(&at, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = positions[joint_num]; + } + joint_hold_pose = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 1; + } + } + if (joint_hold_valid + && same_carte_pos(&joint_hold_pose, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + positions[joint_num] = joint_hold[joint_num]; + } + result = 0; + } else { + joint_hold_valid = 0; + result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, + &iflags, &fflags); + } + } if(result == 0) { /* copy to joint structures and spline them up */ diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 954de98dd84..3a0f8e5f697 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -271,6 +271,8 @@ extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num); extern void clearHomes(int joint_num); extern void emcmot_config_change(void); +extern void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1a48aa02f57..49cb1b00a94 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -117,6 +117,7 @@ extern "C" { EMCMOT_SET_LINE, /* queue up a linear move */ EMCMOT_SET_CIRCLE, /* queue up a circular move */ + EMCMOT_SET_JOINT_LINE, /* queue up a joint interpolated move */ EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */ EMCMOT_PROBE, /* go to pos, stop if probe trips, record trip pos */ @@ -278,6 +279,17 @@ extern "C" { struct state_tag_t tag; int switchkins_type; /* switchkins type requested by G12.1 */ + + /* a joint interpolated move: either pos is the endpoint and the joints + come from the inverse, or these are the joints and pos comes from + the forward */ + double joint_target[EMCMOT_MAX_JOINTS]; + int have_joint_target; + double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + + /* SET_OFFSET: pos is where the interpreter expects the point to be + once the offset is on, for motion to check its own answer against */ + int have_point; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 9a0a4de911c..e847b17ebec 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -298,6 +298,24 @@ extern void STRAIGHT_TRAVERSE(int lineno, double x, double y, double z, double a, double b, double c, double u, double v, double w); + +/* A traverse interpolated in joint space. The endpoint x..w is in program + coordinates like STRAIGHT_TRAVERSE's; motion runs the inverse once there + and interpolates the joints to it. With have_joints the joints given + (machine units, one per joint) are the endpoint instead and x..w say + where the interpreter believes that is. Nothing blends into or out of + it. */ +extern void JOINT_TRAVERSE(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); +/* The same move at feed: it is to take 'seconds' seconds at the programmed + feed, the feed override applies, and the joint limits still cap it. */ +extern void JOINT_FEED(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds); /* Move at traverse rate so that at any time during the move, all axes @@ -680,6 +698,11 @@ extern void USE_NO_SPINDLE_FORCE(); extern void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double diameter, double frontangle, double backangle, int orientation); extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset); +/* The same, with the point the interpreter expects the machine to stand + on once the offset is applied, in program coordinates: where a + kinematics applies the offset itself, motion keeps the joints and + re-reads the point from them, and compares it with this one. */ +extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point); extern void CHANGE_TOOL(); @@ -934,6 +957,10 @@ extern int GET_EXTERNAL_KINS_TYPE(); kinematics.h); -1 where it says nothing: no such type, plain kinematics, or no motion controller attached (sai, preview) */ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); +/* whether the machine's kinematics is the identity, the joints being the + world, so that no tool offset is applied by the kinematics; true where + no motion controller is attached (sai, a preview without a machine) */ +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); /* The tilted work plane the machine last executed, in the form SET_G68_FRAME takes: 1 with origin and rotation filled in, 0 when there @@ -1006,6 +1033,12 @@ extern double GET_EXTERNAL_POSITION_V(); // returns the current w-axis position extern double GET_EXTERNAL_POSITION_W(); +// Copies up to max of the joint positions the machine stands in and +// returns how many were written. A point does not name one joint set, so +// an iterative inverse needs somewhere to start. Zero when the caller +// has no machine to ask. +extern int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max); + // Returns the position of the specified axis at the last probe trip, // in the current work coordinate system. diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index dee2d5699fc..58e80a70f92 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -295,6 +295,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_G68_TYPE: ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -510,6 +513,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_ROTATION"; case EMC_TRAJ_SET_G68_TYPE: return "EMC_TRAJ_SET_G68"; + case EMC_TRAJ_JOINT_MOVE_TYPE: + return "EMC_TRAJ_JOINT_MOVE"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1563,6 +1568,8 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); EmcPose_update(cms, &offset); + EmcPose_update(cms, &point); + cms->update(have_point); } // cppcheck-suppress duplInheritedMember @@ -1660,6 +1667,16 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) cms->update(active); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index d251e08abc6..2037aba3a62 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -109,6 +109,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) #define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) +#define EMC_TRAJ_JOINT_MOVE_TYPE ((NMLTYPE) 240) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -375,12 +376,13 @@ extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale, int indexer_jnum); +extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index, double angular_offset_degrees = 0.0); -extern int emcTrajSetOffset(const EmcPose& tool_offset); +extern int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index f71030156bb..576de74797e 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -805,7 +805,7 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_OFFSET() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_OFFSET_TYPE, sizeof(EMC_TRAJ_SET_OFFSET)), - offset{} + offset{}, point{}, have_point(0) {}; // Sub-class update() calls base-class update() @@ -814,6 +814,10 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { void update(CMS * cms); EmcPose offset; + // where the interpreter expects the machine to stand once the offset + // is on, for motion to check its own answer against; only when set + EmcPose point; + int have_point; }; class EMC_TRAJ_SET_G5X:public EMC_TRAJ_CMD_MSG { @@ -935,6 +939,27 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { unsigned char probe_type; }; +// a move interpolated in joint space: to the world endpoint, whose joints +// motion finds with the inverse; or to the joints given, whose world +// position motion finds with the forward +class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_JOINT_MOVE() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), + end{}, joints{}, have_joints(0), seconds(0.0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose end; + double joints[EMCMOT_MAX_JOINTS]; + int have_joints; + double seconds; /* 0 for a rapid, else the time it is to take */ +}; + class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_RIGID_TAP() diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 49226eb60e3..801542ad8f5 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -42,7 +42,7 @@ TARGETS += ../lib/librs274.so ../lib/librs274.so.0 ../lib/librs274.so.0: $(patsubst %.cc,objects/%.o,$(LIBRS274SRCS)) \ ../lib/liblinuxcncini.so ../lib/libpyplugin.so ../lib/liblinuxcnchal.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking $(notdir $@) @mkdir -p ../lib @rm -f $@ diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 66138b48bfb..dfee7b69ef0 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -253,7 +253,7 @@ BOOST_PYTHON_MODULE(emccanon) { def("USE_NO_SPINDLE_FORCE",&USE_NO_SPINDLE_FORCE); // def("USER_DEFINED_FUNCTION_ADD",&USER_DEFINED_FUNCTION_ADD); // def("USE_SPINDLE_FORCE",&USE_SPINDLE_FORCE); - def("USE_TOOL_LENGTH_OFFSET",&USE_TOOL_LENGTH_OFFSET); + def("USE_TOOL_LENGTH_OFFSET",static_cast(&USE_TOOL_LENGTH_OFFSET)); def("WAIT",&WAIT); // from interp_queue.cc diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f725a708dff..542c207cae6 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -58,6 +58,7 @@ #include "nml_intf/interp_return.hh" #include "nml_intf/canon.hh" #include +#include // KINEMATICS_IDENTITY int _task = 0; // control preview behaviour when remapping @@ -526,6 +527,28 @@ void STRAIGHT_FEED(int line_number, ensure_inch({x, y, z, a, b, c, u, v, w})); } +// the preview draws a joint interpolated move as the traverse between its +// ends: the path between them depends on the kinematics, which the +// preview does not have +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)joints; + (void)have_joints; + STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, double seconds) { + (void)joints; + (void)have_joints; + (void)seconds; + STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, @@ -638,6 +661,10 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) { offset.u, offset.v, offset.w})); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) { + USE_TOOL_LENGTH_OFFSET(offset); +} + void SET_FEED_REFERENCE(double /*reference*/) { } void SET_CUTTER_RADIUS_COMPENSATION(double /*radius*/) {} void START_CUTTER_RADIUS_COMPENSATION(int /*direction*/) {} @@ -753,6 +780,30 @@ double GET_EXTERNAL_POSITION_C() { return parse_state.pos[P9_C]; } double GET_EXTERNAL_POSITION_U() { return parse_state.pos[P9_U]; } double GET_EXTERNAL_POSITION_V() { return parse_state.pos[P9_V]; } double GET_EXTERNAL_POSITION_W() { return parse_state.pos[P9_W]; } + +// Where the machine's joints stand. A point does not name one joint set, so +// an iterative inverse has to start somewhere: a canon that watches the +// status buffer says where. One that cannot, or that answers something that +// is not a sequence of numbers, answers nothing, and the interpreter works +// from the point alone. +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) { + int n = 0; + if(parse_state.interp_error) return 0; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_external_joint_positions")) return 0; + try { + py::sequence seq = canon.attr("get_external_joint_positions")().cast(); + for(py::handle value : seq) { + if(n == max) break; + joints[n++] = value.cast(); + } + } catch(py::error_already_set &) { + return 0; // the error goes with the exception + } catch(py::builtin_exception &) { + return 0; + } + return n; +} void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) @@ -913,9 +964,61 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double /*tolerance*/, int / void SET_MOTION_CONTROL_MODE(double /*tolerance*/) { } void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } -int GET_EXTERNAL_KINS_TYPE() { return 0; } -int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { (void)ktype; return -1; } + +// The kinematics type the machine runs and what the module declares each +// type to be, from a canon that watches the status buffer: read once a +// parse, since the interpreter asks for the flags on every block. One that +// cannot answer has no machine, type 0 and nothing declared, which the +// interpreter takes for a standalone parse. +static int kins_type; +static std::array kins_type_flags; + +static void read_kins_types() { + py::handle canon(parse_state.callback); + std::array flags; + size_t n = 0; + + flags.fill(-1); + kins_type = 0; + kins_type_flags = flags; + if(!py::hasattr(canon, "get_kins_type_flags")) return; + try { + py::sequence seq = canon.attr("get_kins_type_flags")().cast(); + for(py::handle value : seq) { + if(n == flags.size()) break; + flags[n++] = value.cast(); + } + kins_type = canon.attr("get_kins_type")().cast(); + } catch(py::error_already_set &) { + return; // the error goes with the exception + } catch(py::builtin_exception &) { + return; + } + kins_type_flags = flags; +} + +int GET_EXTERNAL_KINS_TYPE() { return kins_type; } +int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { + if(ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) return -1; + return kins_type_flags[ktype]; +} int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) { return -1; } + +// the kind of transform the machine runs, from a canon that watches the +// status buffer; one that cannot answer has no machine, and the joints +// are the world +bool GET_EXTERNAL_KINEMATICS_IDENTITY() { + if(parse_state.interp_error) return true; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_kinematics_type")) return true; + try { + return canon.attr("get_kinematics_type")().cast() == KINEMATICS_IDENTITY; + } catch(py::error_already_set &) { + return true; // the error goes with the exception + } catch(py::builtin_exception &) { + return true; + } +} void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) @@ -931,6 +1034,8 @@ static py::object parse_file(const char *f, py::handle canon, const char *unitcode, const char *initcode, const char *interpname) { int error_line_offset = 0; + Interp *own = nullptr; + bool machine_moves = false; // How often the parse stops to report progress and let the GUI breathe. // A GUI's progress bar and its abort button both hang off this one tick - // the report moves the bar, and check_abort() is what pumps the event @@ -1020,12 +1125,27 @@ static py::object parse_file(const char *f, py::handle canon, } parse_state.pos = {}; + // the interpreter reads the offset in force as it starts, and the one + // the last parse ended on is not the machine's: the GUI restates it + parse_state.tool_offset = {}; + read_kins_types(); parse_state.pinterp->init(); parse_state.pinterp->open(f); maybe_new_line(); + // The codes ahead of the program restate the state the machine is in, + // and G53 is how a GUI says where it stands: in the world of the type + // in force, which is the type the machine is on. What + // MACHINE_MOVES_IN_MACHINE_FRAME reads in the machine frame is a + // program's G53, so it waits for the program. + own = dynamic_cast(parse_state.pinterp); + if(own) { + machine_moves = own->_setup.machine_moves_in_machine_frame; + own->_setup.machine_moves_in_machine_frame = false; + } + if(initcodes) { Py_ssize_t ncodes = PyList_Size(initcodes.ptr()); for(Py_ssize_t i=0; iexecute(); } + if(own) own->_setup.machine_moves_in_machine_frame = machine_moves; while(!parse_state.interp_error && RESULT_OK) { error_line_offset = 1; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 14dd80bf570..b9906756047 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -84,19 +84,19 @@ const int Interp::gees[] = { /* 220 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 240 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 260 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 280 */ 0, 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 -/* 300 */ 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 280 */ 0, 0, 0,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 +/* 300 */ 0, 0,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 320 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1, /* 340 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 380 */ -1,-1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 400 */ 7,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, -/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8, 8,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0, 0, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9, 9, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, @@ -229,6 +229,7 @@ const int Interp::required_parameters[] = { const int Interp::readonly_parameters[] = { 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y ... W, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index b2c8bfc80b8..6db0f3c293d 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -95,6 +95,13 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c CHKS((((block->l_number == 1 || block->l_number == 10 || block->l_number == 11) && p_int < 1)), _("P value out of range with G10 L%d"), block->l_number); } else if (mode0 == G_28) { } else if (mode0 == G_30) { + } else if (mode0 == G_28_5 || mode0 == G_30_5) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G28.5 or G30.5")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G28.5 or G30.5 in incremental distance mode")); } else if (mode0 == G_5_3) { CHKS(((mode1 != G_5_2) && (mode1 != -1)), _("Between G5.2 and G5.3 codes, only additional G5.2 codes are allowed.")); } else if (mode1 == G_5_2){ @@ -110,6 +117,30 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { + } else if (mode0 == G_53_1 || mode0 == G_53_2 || mode0 == G_53_6) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Cannot use axis words with G53.1, G53.2 or G53.6")); + } else if (mode0 == G_53_3) { + CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Only X, Y and Z words can be used with G53.3")); + } else if (mode0 == G_53_4 || mode0 == G_53_5 || mode0 == G_53_7) { + CHKS(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)), + _("G53.4, G53.5 and G53.7 need G0 or G1")); + if (mode0 == G_53_5 || mode0 == G_53_7) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G53.5 or G53.7")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G53.5 or G53.7 in incremental distance mode")); + } + if (mode0 == G_53_7) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("G53.7 takes joint words, J=, not axis words; G53.5 takes the axis words")); + CHKS((block->j_flag), _("Cannot use a J word with G53.7; a joint is J=")); + } } else if (mode0 == G_12_1){ // kins-switch CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); @@ -287,32 +318,50 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4), - _("H word with no G43, G43.4 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_5), + _("H word with no G43, G43.4, G43.5 or G76 to use it")); } - if (block->i_flag) { /* could still be useless if yz_plane arc */ + // G68.2 and G68.4 take I J K, P and Q; G68.3 only R, X Y Z; G69 nothing + int plane_words = block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4; + int plane_r = plane_words || block->g_modes[GM_WORK_PLANE] == G_68_3; + + // under G43.5 the I J K of a G0 or G1 line are the tool axis: on the + // line that puts G43.5 in effect, and on the lines after it that do + // not change the tool length mode + int tool_vector = (block->g_modes[GM_TOOL_LENGTH_OFFSET] == G_43_5 + || (_setup.tool_vector && block->g_modes[GM_TOOL_LENGTH_OFFSET] == -1)) + && (motion == G_0 || motion == G_1); + + if (block->i_flag && !tool_vector) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G68.4, G76, or G87 to use it")); } - if (block->j_flag) { /* could still be useless if xz_plane arc */ + if (block->j_flag && !tool_vector) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); + !plane_words), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G68.4, G76 or G87 to use it")); + } + + for (int n = 0; n < EMCMOT_MAX_JOINTS; n++) { + if (block->joint_flag[n]) { + CHKS((block->g_modes[GM_MODAL_0] != G_53_7), _("J%d= word with no G53.7 to use it"), n); + break; + } } - if (block->k_flag) { /* could still be useless if xy_plane arc */ + if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -330,7 +379,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -343,7 +393,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.2 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -361,12 +411,13 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (motion != G_6) && (motion != G_6_2) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.2, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -377,7 +428,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_r && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 379b21eaba6..30bd30d94ff 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -3106,6 +3106,9 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) if(s->cutter_comp_side != CUTTER_COMP::OFF) { ERS(_("Cannot set reference point with cutter compensation in effect")); } + if(s->machine_moves_in_machine_frame && !kins_machine_frame(s)) { + return convert_savehome_slides(code, s); + } double x, y, z; program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); @@ -4220,7 +4223,8 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { if (((settings->active_g_codes[9] == G_43) || - (settings->active_g_codes[9] == G_43_4)) && ONCE(STEP_RETAIN_G43)) { + (settings->active_g_codes[9] == G_43_4) || + (settings->active_g_codes[9] == G_43_5)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); @@ -4513,6 +4517,8 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_setup(block, settings)); } else if ((code == G_28) || (code == G_30)) { CHP(convert_home(code, block, settings)); + } else if ((code == G_28_5) || (code == G_30_5)) { + CHP(convert_home_slides(code, block, settings)); } else if ((code == G_28_1) || (code == G_30_1)) { CHP(convert_savehome(code, block, settings)); } else if (code == G_28_2) { @@ -4521,7 +4527,11 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_axis_offsets(code, block, settings)); } else if ((code == G_5_3)||(code == G_6_3)) { // jjf CHP(convert_nurbs(code, block, settings)); - } else if ((code == G_4) || (code == G_53)); // handled elsewhere + } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) + || (code == G_53_7)); // handled elsewhere + else if ((code == G_53_1) || (code == G_53_2) || (code == G_53_3) || (code == G_53_6)) { + CHP(convert_orient_tool(code, block, settings)); + } else if ((code == G_12_1) || (code == G_13_1)) { // The flag makes the interpreter wait for motion to drain, so that no // motion is planned across a change of kinematics. Reading runs far @@ -5744,8 +5754,22 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + // under G43.5 the I J K of a G0 or G1 line are the tool axis, which the + // rotaries are solved for once the line's other words are read + bool tool_vector = settings->tool_vector && (move == G_0 || move == G_1) + && (block->i_flag || block->j_flag || block->k_flag); + if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { + // the words name joints, by letter or by number: nothing below applies + CHKS(tool_vector, _("G43.5: a tool vector cannot go with %s, whose words name the joints"), + (block->g_modes[GM_MODAL_0] == G_53_5) ? "G53.5" : "G53.7"); + CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); + return INTERP_OK; + } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); + if (tool_vector) { + CHP(tool_vector_ends(block, settings, &AA_end, &BB_end, &CC_end)); + } if (move == G_1) { inverse_time_rate_straight(end_x, end_y, end_z, @@ -5757,7 +5781,28 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 // Create a state tag and dump it to canon write_canon_state_tag(block, settings); - if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ + if (block->g_modes[GM_MODAL_0] == G_53_4) { + // point-to-point: the endpoint is this Cartesian point, the path to it + // is whatever the joints make of it + CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use G53.4 with cutter radius compensation on")); + tag_straight(block,end_x, end_y); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end); + } else { + double seconds; + CHP(ptp_seconds(block, settings, end_x, end_y, end_z, + AA_end, BB_end, CC_end, u_end, v_end, w_end, &seconds)); + JOINT_FEED(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end, seconds); + } + settings->current_x = end_x; + settings->current_y = end_y; + settings->current_z = end_z; + } else if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ (settings->cutter_comp_radius > 0.0)) { /* radius always is >= 0 */ CHKS((block->g_modes[GM_MODAL_0] == G_53), @@ -6612,7 +6657,8 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine // the kinematics module declares what each type is (KINSTYPE_* flags); // where the flags say nothing at all there is no kinematics attached -// (sai, preview) and the codes fall back to type 0, as before +// (sai, a preview with no machine to ask) and the codes fall back to +// type 0, as before static int kins_type_info_available() { int k; @@ -6626,7 +6672,7 @@ static int kins_type_info_available() // the type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test -static int flagged_kins_type(int flag) +int flagged_kins_type(int flag) { int k, f; @@ -6684,26 +6730,41 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu { int idx; EmcPose tool_offset; + double standing[EMCMOT_MAX_JOINTS]; + bool have_standing = false; ZERO_EMC_POSE(tool_offset); settings->g43_with_zero_offset = 0; CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); - if (g_code == G_43_4) { + if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With - // no kinematics attached there is nothing to switch to. + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); - if (primary >= 0) { switch_kins_type(primary, settings); } + if (primary >= 0 && primary != settings->kins_type) { + // the switch keeps the joints and moves the point, so the point + // the offset is read from is not the one the program is at: take + // the joints, while the kinematics they are known in is in force + void *vctx; + CHP(kins_here(settings, &vctx)); + if (vctx) { + CHP(current_joints(settings, vctx, standing)); + have_standing = true; + } + switch_kins_type(primary, settings); + } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { // the offset in effect is no longer G43.4's, so G49 has no switch to undo settings->kins_by_g43_4 = false; } + settings->tool_vector = (g_code == G_43_5); if (g_code == G_49) { idx = 0; - } else if (g_code == G_43 || g_code == G_43_4) { + } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6783,33 +6844,67 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, G43.5, or G49"); + } + // The machine does not move, so the program coordinates of the point change. + // A kinematics that applies the offset itself moves the point by more than + // the offset difference, along the tilted tool axis: evaluate it here as + // motion will, and send the point along for motion to check against. + EmcPose point; + bool point_known = false; + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + if (point_known) { + double prog[9]; + EmcPose in_program; + + settings->tool_offset = tool_offset; + machine_pose_to_program(settings, &point, prog); + settings->current_x = prog[0]; + settings->current_y = prog[1]; + settings->current_z = prog[2]; + settings->AA_current = prog[3]; + settings->BB_current = prog[4]; + settings->CC_current = prog[5]; + settings->u_current = prog[6]; + settings->v_current = prog[7]; + settings->w_current = prog[8]; + in_program.tran.x = USER_TO_PROGRAM_LEN(point.tran.x); + in_program.tran.y = USER_TO_PROGRAM_LEN(point.tran.y); + in_program.tran.z = USER_TO_PROGRAM_LEN(point.tran.z); + in_program.a = USER_TO_PROGRAM_ANG(point.a); + in_program.b = USER_TO_PROGRAM_ANG(point.b); + in_program.c = USER_TO_PROGRAM_ANG(point.c); + in_program.u = USER_TO_PROGRAM_LEN(point.u); + in_program.v = USER_TO_PROGRAM_LEN(point.v); + in_program.w = USER_TO_PROGRAM_LEN(point.w); + USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); + } else { + double dx, dy, dz; + + USE_TOOL_LENGTH_OFFSET(tool_offset); + + // by the offset difference seen from the program: the XY rotation + // and the tilted work plane taken off it + dx = settings->tool_offset.tran.x - tool_offset.tran.x; + dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; + + rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); + + settings->current_x += dx; + settings->current_y += dy; + settings->current_z += dz; + settings->AA_current += settings->tool_offset.a - tool_offset.a; + settings->BB_current += settings->tool_offset.b - tool_offset.b; + settings->CC_current += settings->tool_offset.c - tool_offset.c; + settings->u_current += settings->tool_offset.u - tool_offset.u; + settings->v_current += settings->tool_offset.v - tool_offset.v; + settings->w_current += settings->tool_offset.w - tool_offset.w; + + settings->tool_offset = tool_offset; } - USE_TOOL_LENGTH_OFFSET(tool_offset); - - double dx, dy, dz; - - // the tool does not move, so its program coordinates change by the - // offset difference seen from the program: the XY rotation and the - // tilted work plane taken off it - dx = settings->tool_offset.tran.x - tool_offset.tran.x; - dy = settings->tool_offset.tran.y - tool_offset.tran.y; - dz = settings->tool_offset.tran.z - tool_offset.tran.z; - - rotate(&dx, &dy, -settings->rotation_xy); - g68_unrotate(settings, &dx, &dy, &dz); - - settings->current_x += dx; - settings->current_y += dy; - settings->current_z += dz; - settings->AA_current += settings->tool_offset.a - tool_offset.a; - settings->BB_current += settings->tool_offset.b - tool_offset.b; - settings->CC_current += settings->tool_offset.c - tool_offset.c; - settings->u_current += settings->tool_offset.u - tool_offset.u; - settings->v_current += settings->tool_offset.v - tool_offset.v; - settings->w_current += settings->tool_offset.w - tool_offset.w; - - settings->tool_offset = tool_offset; // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index bcccebe27c2..971f911c0c9 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,9 +174,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode1 = block->g_modes[GM_MOTION]; mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) - || (mode0 == G_52) || (mode0 == G_92)); + || (mode0 == G_28_5) || (mode0 == G_30_5) + || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way - if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 + || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); mode_zero_covets_axes = 1; } @@ -215,8 +217,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c if (block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_1) { block->motion_to_be = settings->motion_mode; } - } else if (!axis_flag && !polar_flag && ijk_flag && (settings->motion_mode == G_2 || settings->motion_mode == G_3)) { - // this is a block like simply "i1" which should be accepted if we're in arc mode + } else if (!axis_flag && !polar_flag && ijk_flag && + (settings->motion_mode == G_2 || settings->motion_mode == G_3 || + (settings->tool_vector && (settings->motion_mode == G_0 || settings->motion_mode == G_1)))) { + // this is a block like simply "i1" which should be accepted if we're in arc mode, + // or a tool vector alone under G43.5, which turns the tool where it stands block->motion_to_be = settings->motion_mode; } CHKS((polar_flag && block->motion_to_be == -1), _("Polar coordinates can only be used for motion")); @@ -286,6 +291,10 @@ int Interp::init_block(block_pointer block) //!< pointer to a block to be i block->h_number = -1; block->i_flag = false; block->j_flag = false; + for (n = 0; n < EMCMOT_MAX_JOINTS; n++) { + block->joint_flag[n] = false; + block->joint_value[n] = 0.0; + } block->k_flag = false; block->l_number = -1; block->l_flag = false; @@ -352,6 +361,7 @@ int Interp::parse_line(char *line, //!< array holding a line of RS274 code if(settings->skipping_o == NULL) { + machine_frame_codes(block, settings); CHP(enhance_block(block, settings)); CHP(check_items(block, settings)); int n = find_remappings(block,settings); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index ebe9d0d0d49..c2284c37460 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -243,8 +243,10 @@ enum GCodes G_28 = 280, G_28_1 = 281, G_28_2 = 282, /* G-code homing cycle (home one/all joints) */ + G_28_5 = 285, G_30 = 300, G_30_1 = 301, + G_30_5 = 305, G_33 = 330, G_33_1 = 331, G_38_2 = 382, @@ -260,11 +262,19 @@ enum GCodes G_43_1 = 431, G_43_2 = 432, G_43_4 = 434, + G_43_5 = 435, G_49 = 490, G_50 = 500, G_51 = 510, G_52 = 520, G_53 = 530, + G_53_1 = 531, + G_53_2 = 532, + G_53_3 = 533, + G_53_4 = 534, + G_53_5 = 535, + G_53_6 = 536, + G_53_7 = 537, G_54 = 540, G_55 = 550, G_56 = 560, @@ -275,6 +285,7 @@ enum GCodes G_59_2 = 592, G_59_3 = 593, G_68_2 = 682, + G_68_3 = 683, G_68_4 = 684, G_69 = 690, G_61 = 610, @@ -514,6 +525,9 @@ struct block_struct bool dollar_flag{}; + bool joint_flag[EMCMOT_MAX_JOINTS]{}; // J= words, for G53.5 + double joint_value[EMCMOT_MAX_JOINTS]{}; + double radius{}; double theta{}; int radius_flag{}; @@ -796,6 +810,19 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r + // the pose G53.2 last solved, in program words, for #<_orient_a> and kin + bool orient_valid; + double orient_pose[6]; // x y z a b c + // the kinematics, for G68.3 and the orientation moves: loaded on first + // use through the non-realtime loader, on a HAL component of our own + void *kins_ctx; // KinematicsUserContext + int kins_comp_id; + char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it + int kins_joints; // [KINS] JOINTS + int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_joint_min[EMCMOT_MAX_JOINTS]; // [JOINT_n] MIN_LIMIT + double kins_joint_max[EMCMOT_MAX_JOINTS]; // [JOINT_n] MAX_LIMIT + double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer @@ -810,6 +837,8 @@ struct setup bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo + bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis + bool machine_moves_in_machine_frame; // [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME: G53, G28, G30 run as G53.5, G28.5, G30.5 off the machine frame type bool toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried @@ -878,6 +907,8 @@ struct setup AxisKinds axis_kinds; // [AXIS_] TYPE, [TRAJ] FEED_AXES int axis_wrapped[9]; // by AxisIndex; angular axes only int axis_rotary_modulo[9]; // angular axes only + double axis_min[9]; // [AXIS_n] MIN_LIMIT of a bounded angular axis, else -1e99 + double axis_max[9]; // [AXIS_n] MAX_LIMIT of a bounded angular axis, else 1e99 int rotary_modulo_literal; // M26 = shortest path (default), M27 = literal absolute int axis_indexer_jnum[9]; // -1 where the axis has no locking indexer @@ -1109,4 +1140,9 @@ struct scoped_locale { }; #define FORCE_LC_NUMERIC_C scoped_locale force_lc_numeric_c(LC_NUMERIC, "C") + +// the kinematics type carrying a KINSTYPE_ flag, or -1 when the module +// declares none (interp_convert.cc) +int flagged_kins_type(int flag); + #endif // INTERP_INTERNAL_HH diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 6efa911a465..01574b1fef5 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,13 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_ORIENT_VALID, + NP_ORIENT_X, + NP_ORIENT_Y, + NP_ORIENT_Z, + NP_ORIENT_A, + NP_ORIENT_B, + NP_ORIENT_C, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -546,6 +553,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose + *value = _setup.orient_valid; + break; + + case NP_ORIENT_X: // _orient_x and kin: the pose G53.2 last solved + case NP_ORIENT_Y: + case NP_ORIENT_Z: + case NP_ORIENT_A: + case NP_ORIENT_B: + case NP_ORIENT_C: + if (!_setup.orient_valid) { + ERS(_("no G53.2 has solved an orientation yet")); + } + *value = _setup.orient_pose[cmd - NP_ORIENT_X]; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -923,6 +946,15 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the pose G53.2 last solved: 1.0 once one has been, and its words + init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); + init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); + init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); + init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); + init_readonly_param("_orient_a", NP_ORIENT_A, PA_USE_LOOKUP); + init_readonly_param("_orient_b", NP_ORIENT_B, PA_USE_LOOKUP); + init_readonly_param("_orient_c", NP_ORIENT_C, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_read.cc b/src/emc/rs274ngc/interp_read.cc index 6e58d1635dc..d0a34a21b8a 100644 --- a/src/emc/rs274ngc/interp_read.cc +++ b/src/emc/rs274ngc/interp_read.cc @@ -888,11 +888,15 @@ Returned Value: int NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED 2. A j_coordinate has already been inserted in the block. NCE_MULTIPLE_J_WORDS_ON_ONE_LINE + 3. The value is followed by '=' and is not a joint number, or that joint + already has a value in the block. Side effects: counter is reset. The j_flag in the block is turned on. A j_coordinate setting is inserted in the block. + For the J= form, joint_flag[n] is turned on and joint_value[n] + is set instead; j_flag is left alone. Called by: read_one_item @@ -918,8 +922,20 @@ int Interp::read_j(char *line, //!< string: line of RS274 code being processed CHKS((line[*counter] != 'j'), NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED); *counter = (*counter + 1); - CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); CHP(read_real_value(line, counter, &value, parameters)); + if (line[*counter] == '=') { + // J=: a joint value for G53.5, n is the joint number + int n = (int)value; + CHKS((value != (double)n || n < 0 || n >= EMCMOT_MAX_JOINTS), + _("The joint number in a J= word must be a whole number from 0 to %d"), EMCMOT_MAX_JOINTS - 1); + CHKS((block->joint_flag[n]), _("Multiple J%d= words on one line"), n); + *counter = (*counter + 1); + CHP(read_real_value(line, counter, &value, parameters)); + block->joint_flag[n] = true; + block->joint_value[n] = value; + return INTERP_OK; + } + CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); block->j_flag = true; block->j_number = value; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index bd513053de2..3dd796991c2 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -114,6 +114,16 @@ setup::setup() : g68_seq_have(0), g68_seq_r_q(-1), g68_seq_word{}, + orient_valid(false), + orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + kins_ctx(nullptr), + kins_comp_id(0), + kins_module{}, + kins_joints(0), + kins_angular_joints(0), + kins_joint_min{}, + kins_joint_max{}, + kins_seed{}, parameters{0}, parameter_occurrence(0), @@ -129,6 +139,8 @@ setup::setup() : kinsSwitch_flag(0), kins_type(0), kins_by_g43_4(false), + tool_vector(false), + machine_moves_in_machine_frame(false), toolchange_flag(0), home_flag(0), input_index(0), @@ -194,6 +206,8 @@ setup::setup() : axis_kinds(axisKindsDefault()), axis_wrapped{}, axis_rotary_modulo{}, + axis_min{}, + axis_max{}, rotary_modulo_literal(0), axis_indexer_jnum{-1, -1, -1, -1, -1, -1, -1, -1, -1}, @@ -219,6 +233,8 @@ setup::setup() : init_once(CANON_STOPPED) { std::fill(parameters, parameters + interp_param_global::RS274NGC_MAX_PARAMETERS, 0); + std::fill(axis_min, axis_min + 9, -1e99); + std::fill(axis_max, axis_max + 9, 1e99); } setup::~setup() { diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 6290218c72a..d1e626df12f 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -535,6 +535,10 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) _("Cannot cancel a tilted work plane with cutter radius compensation on")); return work_plane_cancel(s, true); } + if (g_code == G_68_3) { + CHKS((s->g68_seq_code != 0), _("G68.3 cannot interrupt a G68.2 sequence")); + return convert_work_plane_from_tool(block, s); + } CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -561,3 +565,1012 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } return work_plane_set(s, g_code, origin, rotation); } + +//---------------------------------------------------------------------- +// The kinematics. G68.3, the orientation moves and a tool offset change +// need the module motion runs, evaluated here, ahead of motion, through +// the loader in kinematics_userspace/. The loader reads the module's +// pins through HAL, so the interpreter connects as a component, named by +// its process, the first time it is asked. +//---------------------------------------------------------------------- + +#include +#include +#include "units.h" +#include +#include + +#define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) + +// the module loaded, once +int Interp::kins_load(setup_pointer s) +{ + KinematicsUserContext *ctx; + + if (!s->kins_ctx) { + char name[HAL_NAME_LEN + 1]; + int comp; + + CHKS((!s->kins_module[0]), + _("the INI file names no [KINS] KINEMATICS, so the kinematics cannot be evaluated here")); + CHKS((s->kins_joints < 1), + _("the INI file gives no [KINS] JOINTS, so the kinematics cannot be evaluated here")); + snprintf(name, sizeof(name), "interp.%d", (int)getpid()); + comp = hal_init(name); + CHKS((comp < 0), + _("cannot connect to HAL to evaluate the kinematics (is realtime running?)")); + s->kins_comp_id = comp; + ctx = kinematicsUserInitString(s->kins_module, s->kins_joints, comp, name); + hal_ready(comp); + CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); + s->kins_ctx = ctx; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + } + return INTERP_OK; +} + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + CHP(kins_load(s)); + ctx = KINS_CTX(s); + CHKS((kinematicsUserIsRtOnly(ctx)), + _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); + CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), + _("kinematics type %d is not available outside realtime"), s->kins_type); + // with the tool offset the program is under here, not the one motion + // has reached: the interpreter runs ahead of motion + kins_set_tool(ctx, &s->tool_offset); + *out = ctx; + return INTERP_OK; +} + +// the offset the module evaluates with, given in program units like the +// interpreter keeps it +void Interp::kins_set_tool(void *vctx, const EmcPose *offset) +{ + EmcPose tool; + + tool.tran.x = PROGRAM_TO_USER_LEN(offset->tran.x); + tool.tran.y = PROGRAM_TO_USER_LEN(offset->tran.y); + tool.tran.z = PROGRAM_TO_USER_LEN(offset->tran.z); + tool.a = PROGRAM_TO_USER_ANG(offset->a); + tool.b = PROGRAM_TO_USER_ANG(offset->b); + tool.c = PROGRAM_TO_USER_ANG(offset->c); + tool.u = PROGRAM_TO_USER_LEN(offset->u); + tool.v = PROGRAM_TO_USER_LEN(offset->v); + tool.w = PROGRAM_TO_USER_LEN(offset->w); + kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); +} + +void Interp::kins_release(setup_pointer s) +{ + if (s->kins_ctx) { + kinematicsUserFree(KINS_CTX(s)); + s->kins_ctx = NULL; + } + if (s->kins_comp_id > 0) { + hal_exit(s->kins_comp_id); + s->kins_comp_id = 0; + } +} + +// the current point as the machine sees it: the absolute frame, in the +// machine's units, which is what the kinematics works in +void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) +{ + double abs_pos[9]; + + get_abs_position(s, abs_pos); + pose->tran.x = PROGRAM_TO_USER_LEN(abs_pos[0]); + pose->tran.y = PROGRAM_TO_USER_LEN(abs_pos[1]); + pose->tran.z = PROGRAM_TO_USER_LEN(abs_pos[2]); + pose->a = PROGRAM_TO_USER_ANG(abs_pos[3]); + pose->b = PROGRAM_TO_USER_ANG(abs_pos[4]); + pose->c = PROGRAM_TO_USER_ANG(abs_pos[5]); + pose->u = PROGRAM_TO_USER_LEN(abs_pos[6]); + pose->v = PROGRAM_TO_USER_LEN(abs_pos[7]); + pose->w = PROGRAM_TO_USER_LEN(abs_pos[8]); +} + +// and back: a machine pose as program coordinates, through the chain +void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, double prog[9]) +{ + world_to_program_xyz(s, + USER_TO_PROGRAM_LEN(pose->tran.x), + USER_TO_PROGRAM_LEN(pose->tran.y), + USER_TO_PROGRAM_LEN(pose->tran.z), + &prog[0], &prog[1], &prog[2]); + prog[3] = USER_TO_PROGRAM_ANG(pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_ANG(pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_ANG(pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_LEN(pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_LEN(pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; +} + +// whether two machine points are the same, to a hair either way +static bool same_pose(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints the machine is at, as far as the interpreter can know ahead of +// motion: the seed while it still explains the current point, since a point +// does not name one joint set, else the joints the machine stands in +int Interp::current_joints(setup_pointer s, void *vctx, double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double standing[EMCMOT_MAX_JOINTS]; + EmcPose pose, seeded; + int pass, i, n; + + current_machine_pose(s, &pose); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + return INTERP_OK; + } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + for (i = 0; i < n; i++) { joints[i] = standing[i]; } + if (n > 0) { + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("the kinematics cannot invert the current position")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; +} + +// The kinematics as far as it can be evaluated here: the context on the +// type the program is in, or NULL where there is nothing to evaluate, +// with no machine attached (sai, a preview without one) or a module that +// runs only in realtime. +int Interp::kins_here(setup_pointer s, void **out) +{ + *out = NULL; + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return INTERP_OK; } + CHP(kins_load(s)); + if (kinematicsUserIsRtOnly(KINS_CTX(s))) { return INTERP_OK; } + return kins_context(s, out); +} + +// Where the point goes when the tool offset changes: the joints stay, so it is +// read back from them under the new offset, as motion reads it. known stays +// false under the identity and where nothing can be evaluated; the caller +// then shifts by the offset difference. +int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + int flags, i; + + *known = false; + if (same_pose(offset, &s->tool_offset)) { return INTERP_OK; } + flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx || kinematicsUserIsIdentity(ctx)) { return INTERP_OK; } + if (standing) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = standing[i]; } + } else { + CHP(current_joints(s, ctx, joints)); + } + kins_set_tool(ctx, offset); + CHKS((kinematicsUserForward(ctx, joints, point) != 0), + _("the kinematics cannot place the tool from the joints after the tool offset change")); + // the machine stays on these joints + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + *known = true; + return INTERP_OK; +} + +// a direction the program gives, in world coordinates: through the +// tilted work plane's rotation where one is active, then the XY rotation +// of the coordinate system it sits on +static void direction_in_world(setup_pointer s, const double v[3], double rotation_xy, PmCartesian *out) +{ + double x = v[0], y = v[1], z = v[2]; + double t = rotation_xy * M_PI / 180.0; + + if (s->g68_active) { + x = s->g68_rotation[0][0] * v[0] + s->g68_rotation[0][1] * v[1] + s->g68_rotation[0][2] * v[2]; + y = s->g68_rotation[1][0] * v[0] + s->g68_rotation[1][1] * v[1] + s->g68_rotation[1][2] * v[2]; + z = s->g68_rotation[2][0] * v[0] + s->g68_rotation[2][1] * v[1] + s->g68_rotation[2][2] * v[2]; + } + out->x = x * cos(t) - y * sin(t); + out->y = x * sin(t) + y * cos(t); + out->z = z; +} + +// a direction of the plane in world coordinates +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double v[3] = { column == 0 ? 1.0 : 0.0, column == 1 ? 1.0 : 0.0, column == 2 ? 1.0 : 0.0 }; + + direction_in_world(s, v, rotation_xy, out); +} + +static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) +{ + // Rodrigues, for a unit axis + PmCartesian c; + double d = axis->x*v->x + axis->y*v->y + axis->z*v->z; + + pmCartCartCross(axis, v, &c); + v->x = v->x*cos(rad) + c.x*sin(rad) + axis->x*d*(1 - cos(rad)); + v->y = v->y*cos(rad) + c.y*sin(rad) + axis->y*d*(1 - cos(rad)); + v->z = v->z*cos(rad) + c.z*sin(rad) + axis->z*d*(1 - cos(rad)); +} + +// G68.3: the plane from the tool. Z is the tool axis as the joints have it, +// X the default tool X of the conventions chapter, R turns the plane from +// there; X Y Z are the origin, as G68.2's +int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + PmRotationMatrix work, tool, in_work; + PmCartesian zt, xt, yt, zm, xm, x, y; + double origin[3], rotation[3][3], r, t, along; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((s->g68_local[0] != 0.0 || s->g68_local[1] != 0.0 || s->g68_local[2] != 0.0), + _("Cancel the G52 in the tilted work plane (G52 X0 Y0 Z0) before G68.3")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, joints)); + CHKS((kinematicsUserWorkFrame(ctx, joints, &work) != 0 + || kinematicsUserToolFrame(ctx, joints, &tool) != 0), + _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); + toolFrameInWork(&work, &tool, &in_work); + zt = in_work.z; + xt = in_work.x; + yt = in_work.y; + // machine Z and X seen from the work: the rows of the work frame + zm.x = work.x.z; zm.y = work.y.z; zm.z = work.z.z; + xm.x = work.x.x; xm.y = work.y.x; xm.z = work.z.x; + + pmCartCartCross(&zt, &zm, &x); + if (sqrt(x.x*x.x + x.y*x.y + x.z*x.z) < 1e-9) { + // vertical: tool x is machine x, less whatever of it lies along + // the tool axis, which is rounding + along = xm.x*zt.x + xm.y*zt.y + xm.z*zt.z; + x.x = xm.x - along*zt.x; x.y = xm.y - along*zt.y; x.z = xm.z - along*zt.z; + pmCartUnitEq(&x); + } else { + // the turn about the tool axis that takes tool x into the + // machine XY plane: (cos t x + sin t y) . zm = 0, the root nearer + // to no turn at all + double xz = xt.x*zm.x + xt.y*zm.y + xt.z*zm.z; + double yz = yt.x*zm.x + yt.y*zm.y + yt.z*zm.z; + + t = atan2(-xz, yz); + if (t > M_PI / 2) { t -= M_PI; } + if (t < -M_PI / 2) { t += M_PI; } + x = xt; + rotate_about(&zt, t, &x); + } + r = block->r_flag ? block->r_number : 0.0; + rotate_about(&zt, r * M_PI / 180.0, &x); + pmCartCartCross(&zt, &x, &y); + + // from world directions to the system the plane is defined in: the + // XY rotation comes off + { + PmCartesian cols[3] = { x, y, zt }; + double c = cos(-s->rotation_xy * M_PI / 180.0), sn = sin(-s->rotation_xy * M_PI / 180.0); + + for (int j = 0; j < 3; j++) { + rotation[0][j] = cols[j].x * c - cols[j].y * sn; + rotation[1][j] = cols[j].x * sn + cols[j].y * c; + rotation[2][j] = cols[j].z; + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + return work_plane_set(s, G_68_3, origin, rotation); +} + +// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 +// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, +// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the +// pose on #<_orient_x> and kin (Heidenhain STAY). P and Q are orient_solve()'s. +int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + int p, q, i; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot orient the tool with cutter radius compensation on")); + p = block->p_flag ? (int)round(block->p_number) : 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), + _("P word with %s must be 0, 1 or 2"), name); + q = block->q_flag ? (int)round(block->q_number) : 0; + CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), + _("Q word with %s must be 0 or 1"), name); + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHP(current_joints(s, ctx, now)); + + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + CHP(orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name)); + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &end_pose, end_prog); + + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + +// An angular axis letter of a machine pose, 3 A to 8 W. +static double pose_letter(const EmcPose *pose, int n) +{ + const double v[6] = {pose->a, pose->b, pose->c, pose->u, pose->v, pose->w}; + return v[n - 3]; +} + +// The solver reports each answer in (-180, 180], but the machine stands +// somewhere in turn space: every angular joint of each pose goes onto the +// turn nearest where it stands, or the nearest pose is not the nearest move +// and a free rotary swings the long way round. Of the turns, only those +// inside the joint's [JOINT_n] travel count, and inside the [AXIS_n] travel +// of the letter a whole turn of the joint turns with it, so a rotary that +// would run out of travel the short way goes the long way, and a pose no +// turn brings inside is dropped, as is one the forward kinematics puts an +// angular letter outside its travel at. seed is where the forward starts. +// Returns how many poses are left, packed at the front. +static int orient_fit(setup_pointer s, KinematicsUserContext *ctx, const EmcPose *seed, + double *solutions, int n, int njoints, const double *now) +{ + int i, j, l, kept = 0; + + for (i = 0; i < n; i++) { + double *pose = &solutions[i*njoints]; + double q[EMCMOT_MAX_JOINTS] = {}; + EmcPose base = *seed, turned; + bool inside = true, placed; + + for (j = 0; j < njoints; j++) { q[j] = pose[j]; } + placed = (kinematicsUserForward(ctx, q, &base) == 0); + for (j = 0; j < njoints; j++) { + if (!(s->kins_angular_joints & (1 << j))) { continue; } + double turns = floor((now[j] - pose[j]) / 360.0 + 0.5); + double first = -1e99, last = 1e99; + if (j < s->kins_joints) { + first = ceil((s->kins_joint_min[j] - pose[j]) / 360.0 - 1e-9); + last = floor((s->kins_joint_max[j] - pose[j]) / 360.0 + 1e-9); + } + // the letter a whole turn of the joint carries a whole turn + // along, if any, and the turns its own travel leaves + turned = base; + q[j] += 360.0; + if (placed && kinematicsUserForward(ctx, q, &turned) == 0) { + for (l = 3; l < 9; l++) { + double step = pose_letter(&turned, l) - pose_letter(&base, l); + if (fabs(fabs(step) - 360.0) > 1e-6) { continue; } + double at = pose_letter(&base, l), lo, hi; + if (step > 0) { + lo = ceil((s->axis_min[l] - at) / 360.0 - 1e-9); + hi = floor((s->axis_max[l] - at) / 360.0 + 1e-9); + } else { + lo = ceil((at - s->axis_max[l]) / 360.0 - 1e-9); + hi = floor((at - s->axis_min[l]) / 360.0 + 1e-9); + } + first = fmax(first, lo); + last = fmin(last, hi); + break; + } + } + q[j] = pose[j]; + if (first > last) { + inside = false; + break; + } + turns = fmin(fmax(turns, first), last); + pose[j] += 360.0 * turns; + q[j] = pose[j]; + } + if (inside && placed && kinematicsUserForward(ctx, q, &base) == 0) { + for (l = 3; l < 9; l++) { + double at = pose_letter(&base, l); + if (at < s->axis_min[l] - 1e-9 || at > s->axis_max[l] + 1e-9) { inside = false; } + } + } + if (!inside) { continue; } + if (kept != i) { + for (j = 0; j < njoints; j++) { solutions[kept*njoints + j] = pose[j]; } + } + kept++; + } + return kept; +} + +// The joints that point the tool along axis, and its x along xdir where given, +// from the joints the machine is at: every pose the module reports, on the +// nearest turn inside the travel, ranked by rotary travel. P picks by rank or by +// the sign of the tilting joint; Q0 holds the joints that carry the work +// (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + double distance[TOOL_FRAME_MAX_SOLUTIONS]; + int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + unsigned int held = 0; + int n, i, j, chosen, njoints; + bool reached; + + njoints = kinematicsUserGetNumJoints(ctx); + EmcPose seed = {}; + current_machine_pose(s, &seed); + if (q == 0) { + if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } + } + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + reached = (n > 0); + if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } + if (n == 0 && held) { + // nothing reachable with the work held still: let it move, still + // leaving the plane's X to the coordinate system, or a table that + // can place it pins the pose and P has nothing to choose from + held = 0; + n = kinematicsUserToolFrameInverse(ctx, axis, NULL, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + reached = reached || (n > 0); + if (n > 0) { n = orient_fit(s, ctx, &seed, solutions, n, njoints, now); } + } + CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0 && reached), + _("%s: every pose that reaches the direction puts a rotary outside its travel"), name); + CHKS((n == 0), _("%s: the direction asked for cannot be reached by the rotary joints"), name); + + // nearest first, by rotary travel in joint units + for (i = 0; i < n; i++) { + distance[i] = 0.0; + for (j = 0; j < njoints; j++) { distance[i] += fabs(solutions[i*njoints + j] - now[j]); } + order[i] = i; + } + for (i = 1; i < n; i++) { + int k = order[i]; + for (j = i; j > 0 && distance[order[j-1]] > distance[k]; j--) { order[j] = order[j-1]; } + order[j] = k; + } + if (p == 0) { + chosen = order[0]; + } else { + // P names the pose rather than its rank, so that the same program + // reaches the same pose from wherever the machine is standing + int sign_joint; + CHKS((kinematicsUserPoseJoint(ctx, now, &sign_joint) != 0), + _("%s P%d: the poses of this machine cannot be told apart by a tilting" + " joint, so leave P out and take the nearest"), name, p); + chosen = -1; + for (i = 0; i < n; i++) { + double value = solutions[order[i]*njoints + sign_joint]; + if ((p == 1 && value > 1e-9) || (p == 2 && value < -1e-9)) { + chosen = order[i]; + break; + } + } + CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), + name, p, sign_joint, (p == 1) ? "positive" : "negative"); + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + joints[i] = (i < njoints) ? solutions[chosen * njoints + i] : 0.0; + } + return INTERP_OK; +} + +// G43.5: I J K on a G0 or G1 line are the tool axis, tip towards holder, in +// the coordinate system the line's X Y Z are in. The rotaries come from the +// tool frame inverse, every orienting joint free, the nearest pose, as program +// rotary coordinates so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, double *b, double *c) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + double v[3], prog[9]; + PmCartesian axis; + EmcPose pose; + int i; + + CHKS((block->a_flag || block->b_flag || block->c_flag), + _("G43.5: a tool vector and rotary words on one line give the orientation twice")); + v[0] = block->i_flag ? block->i_number : 0.0; + v[1] = block->j_flag ? block->j_number : 0.0; + v[2] = block->k_flag ? block->k_number : 0.0; + CHKS((vec_norm(v) < 1e-9), _("G43.5: the tool vector I J K is zero")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, now)); + if (block->g_modes[GM_MODAL_0] == G_53) { + axis.x = v[0]; + axis.y = v[1]; + axis.z = v[2]; + } else { + direction_in_world(s, v, s->rotation_xy, &axis); + } + CHP(orient_solve(s, ctx, &axis, NULL, 0, 1, now, sol, "G43.5")); + + // where that puts the rotaries, and what the program calls it + pose = (EmcPose){}; + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, sol, &pose) != 0), + _("G43.5: the kinematics cannot place the orientation it found")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &pose, prog); + *a = prog[3]; + *b = prog[4]; + *c = prog[5]; + return INTERP_OK; +} + +// how long a point-to-point feed is to take: the time the same straight +// move would take at the programmed feed +int Interp::ptp_seconds(block_pointer block, setup_pointer s, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds) +{ + if (s->feed_mode == FEED_MODE::INVERSE_TIME) { + CHKS((block->f_number <= 0.0), _("F must be positive with G93")); + *seconds = 60.0 / block->f_number; + } else if (s->feed_mode == FEED_MODE::UNITS_PER_MINUTE) { + double length = find_straight_length(x, y, z, a, b, c, u, v, w, + s->current_x, s->current_y, s->current_z, + s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current); + CHKS((length <= 0.0), + _("a point-to-point feed with no displacement has nothing to apply F to in G94 mode; use G93 or G0")); + *seconds = 60.0 * length / s->feed_rate; + } else { + ERS(_("Cannot use feed per revolution with a point-to-point move")); + } + return INTERP_OK; +} + +// Whether the kinematics in force is the machine frame type, the one G13.1 +// and G49 cancel to: the flags the module declares for the type the program +// selected, else what the machine reports (a module that switches nothing, +// or no machine attached). +bool Interp::kins_machine_frame(setup_pointer s) +{ + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0) { return (flags & KINSTYPE_MACHINE) != 0; } + return GET_EXTERNAL_KINEMATICS_IDENTITY(); +} + +// [RS274NGC] MACHINE_MOVES_IN_MACHINE_FRAME: off the machine frame type G53, +// G28 and G30 are G53.5, G28.5 and G30.5, the slides at the words whatever +// the tool and the head are doing. On the machine frame type the two read +// the same point, and G53 keeps its straight line. +void Interp::machine_frame_codes(block_pointer block, setup_pointer s) +{ + int *mode0 = &block->g_modes[GM_MODAL_0]; + + if (!s->machine_moves_in_machine_frame) { return; } + if (*mode0 != G_53 && *mode0 != G_28 && *mode0 != G_30) { return; } + if (kins_machine_frame(s)) { return; } + *mode0 = (*mode0 == G_53) ? G_53_5 : (*mode0 == G_28) ? G_28_5 : G_30_5; +} + +// The axis letters read as machine frame coordinates, the words the module's +// machine frame type answers to: the pivot or the flange in machine +// coordinates, the orientation as that type reports it, in program units. The joints are read in on the type +// in force and come out as the joints of the machine frame point, the letters +// not given standing where they are. Where the machine frame type is a plain +// identity the letters name joints, and a letter carries a unit class where +// a joint does not: on a serial robot the first joint answers to X and turns +// in degrees, and the whole machine is refused rather than that one letter, +// since a mapping that lies about X is telling nothing useful about A. A +// module that declares no types at all is read as letters = joints through +// its identity mapping, gantry pairs sharing the value. +int Interp::slide_joints(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + const int machine = flagged_kins_type(KINSTYPE_MACHINE); + int a, given = 0, status; + + for (a = 0; a < 9; a++) { given += flags[a] ? 1 : 0; } + CHKS((given == 0), _("%s needs at least one axis word"), name); + if (machine < 0) { + return slide_words(name, s, ctx, p, njoints, 1, flags, words, joints); + } + CHKS((kinematicsUserSetType(ctx, machine) != 0), + _("%s: kinematics type %d is not available outside realtime"), name, machine); + status = slide_words(name, s, ctx, p, njoints, 0, flags, words, joints); + // back on the type in force whatever happened: the caller's context + kinematicsUserSetType(ctx, s->kins_type); + return status; +} + +// Where the machine frame type is a plain identity, or the module declares +// no types, the letters name joints: refused where a letter and its joint +// are not of one unit class. +int Interp::letters_name_joints(const char *name, setup_pointer s, const struct kins_params *p, + int njoints) +{ + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + int a, j; + + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("%s: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + name, letters[a], j, turns ? "angular" : "linear", j); + } + } + return INTERP_OK; +} + +// the words on the context as set: the module's machine frame type, or the +// type in force with letters = joints where the module declares none +int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, int undeclared, const int flags[9], const double words[9], + double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + int a, j; + + if (undeclared || kinematicsUserIsIdentity(ctx)) { + CHP(letters_name_joints(name, s, p, njoints)); + } + if (!undeclared) { + return machine_frame_joints(name, s, ctx, flags, words, joints); + } + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + double value; + if (!flags[a]) { continue; } + CHKS((p->joints_of_axis[a] == 0), + _("%s: %c is not a joint of this kinematics"), name, letters[a]); + value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } + } + } + return INTERP_OK; +} + +// The joints of a machine frame point, on the machine frame type the +// context is set to: where the joints handed in stand in that frame, the +// letters given replacing their coordinate, and the inverse iterated to a +// fixed point as current_joints() does, since an iterative inverse takes +// its seed from the joints handed in. +int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, + const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double *coord[9]; + int a, i, pass; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the machine frame kinematics cannot place the current joints"), name); + coord[0] = &pose.tran.x; coord[1] = &pose.tran.y; coord[2] = &pose.tran.z; + coord[3] = &pose.a; coord[4] = &pose.b; coord[5] = &pose.c; + coord[6] = &pose.u; coord[7] = &pose.v; coord[8] = &pose.w; + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + if (!flags[a]) { continue; } + *coord[a] = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("%s: the machine frame kinematics cannot reach that point"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + return INTERP_OK; +} + +// The move to a set of joints: where that puts the tool, and what the +// program calls it, then the joint-space traverse or feed there. +int Interp::ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer s, + void *vctx, const double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double prog[9]; + int j; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} + +// The two point-to-point codes below G53.4: G53.5 by axis letter in the +// machine frame, the module's machine frame type, refused where that type is +// a plain identity whose letters name joints of the other unit class; G53.7 +// by J= in the joint's own units. A letter or joint left out keeps +// its position. +int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + const int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + const char *name = (code == G_53_5) ? "G53.5" : "G53.7"; + int a, j, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (code == G_53_7) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + if (!block->joint_flag[j]) { continue; } + CHKS((j >= njoints), _("G53.7: this kinematics has no joint %d"), j); + joints[j] = block->joint_value[j]; + given++; + } + CHKS((given == 0), _("G53.7 needs at least one J= joint word")); + + // the joints of a gantry pair move together: both given, one value + for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + int first = -1; + if (!(bits & (bits - 1))) { continue; } + for (j = 0; j < njoints; j++) { + if (!(bits & (1 << j))) { continue; } + if (first < 0) { first = j; continue; } + CHKS((block->joint_flag[j] != block->joint_flag[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give both"), first, j); + CHKS((block->joint_flag[j] && block->joint_value[j] != block->joint_value[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); + } + } + } else { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + } + + return ptp_joint_move(name, move, block, s, ctx, joints); +} + +// G28.5 and G30.5: the machine frame forms of G28 and G30, as G53.5 is of +// G53. The stored position, parameters 5161 to 5169 or 5181 to 5189, is +// read in the machine frame the way G53.5 reads its words: no offset, no +// work plane, no orientation. Axis words on the line are a machine frame +// point to pass through first, and then only those letters go to the stored +// position; with no words every letter the machine has goes. +int Interp::convert_home_slides(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + const double *parameters = s->parameters; + double joints[EMCMOT_MAX_JOINTS]; + double home[9]; + int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + const char *name = (code == G_28_5) ? "G28.5" : "G30.5"; + const int base = (code == G_28_5) ? 5161 : 5181; + int a, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + home[a] = angular ? USER_TO_PROGRAM_ANG(parameters[base + a]) + : USER_TO_PROGRAM_LEN(parameters[base + a]); + given += flags[a]; + } + + if (given) { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + CHP(ptp_joint_move(name, G_0, block, s, ctx, joints)); + } else { + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { flags[a] = p->joints_of_axis[a] != 0; } + } + CHP(slide_joints(name, s, ctx, p, njoints, flags, home, joints)); + return ptp_joint_move(name, G_0, block, s, ctx, joints); +} + +// G28.1 and G30.1 where G28 and G30 run as G28.5 and G30.5: the position +// stored is where the slides stand in the machine frame, the point those +// read back, in the reverse of slide_joints(). +int Interp::convert_savehome_slides(int code, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + const char *name = (code == G_28_1) ? "G28.1" : "G30.1"; + const int base = (code == G_28_1) ? 5161 : 5181; + const int machine = flagged_kins_type(KINSTYPE_MACHINE); + int a, j, njoints; + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (machine >= 0) { + EmcPose pose; + int identity, placed; + CHKS((kinematicsUserSetType(ctx, machine) != 0), + _("%s: kinematics type %d is not available outside realtime"), name, machine); + identity = kinematicsUserIsIdentity(ctx); + current_machine_pose(s, &pose); + placed = (kinematicsUserForward(ctx, joints, &pose) == 0); + // back on the type in force: the caller's context + kinematicsUserSetType(ctx, s->kins_type); + if (identity) { CHP(letters_name_joints(name, s, p, njoints)); } + CHKS((!placed), _("%s: the machine frame kinematics cannot place the current joints"), name); + s->parameters[base + 0] = pose.tran.x; + s->parameters[base + 1] = pose.tran.y; + s->parameters[base + 2] = pose.tran.z; + s->parameters[base + 3] = pose.a; + s->parameters[base + 4] = pose.b; + s->parameters[base + 5] = pose.c; + s->parameters[base + 6] = pose.u; + s->parameters[base + 7] = pose.v; + s->parameters[base + 8] = pose.w; + return INTERP_OK; + } + CHP(letters_name_joints(name, s, p, njoints)); + for (a = 0; a < 9; a++) { + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { break; } + } + s->parameters[base + a] = (j < njoints) ? joints[j] : 0.0; + } + return INTERP_OK; +} diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 63dc9b73b0b..efb8c2789be 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -111,16 +111,17 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); // the kins type, not the label, is the authority: a G43 given on the - // module's primary type shows as G43.4, and the label follows the type - // motion reports after a resync. -1 is "no information" and matches - // every flag, so it is excluded before the bit test. + // module's primary type shows as G43.4, or G43.5 while I J K give the + // tool axis, and the label follows the type motion reports after a + // resync. -1 is "no information" and matches every flag, so it is + // excluded before the bit test. kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? - ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? G_43_4 : G_43) : G_49; + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? (settings->tool_vector ? G_43_5 : G_43_4) : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 5e4dce0d250..87a24736249 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -376,6 +376,39 @@ public: int work_plane_check_sequence(block_pointer block, setup_pointer settings); int work_plane_local(block_pointer block, setup_pointer settings); int work_plane_restore(setup_pointer settings); + int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); + int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int orient_solve(setup_pointer settings, void *ctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name); + int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); + int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int convert_home_slides(int code, block_pointer block, setup_pointer settings); + int convert_savehome_slides(int code, setup_pointer settings); + void machine_frame_codes(block_pointer block, setup_pointer settings); + int letters_name_joints(const char *name, setup_pointer settings, const struct kins_params *params, + int njoints); + int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, const int flags[9], const double words[9], double *joints); + int slide_words(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, int undeclared, const int flags[9], const double words[9], double *joints); + int machine_frame_joints(const char *name, setup_pointer settings, void *ctx, + const int flags[9], const double words[9], double *joints); + int ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer settings, + void *ctx, const double *joints); + int ptp_seconds(block_pointer block, setup_pointer settings, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds); + int kins_load(setup_pointer settings); + int kins_context(setup_pointer settings, void **ctx); + int kins_here(setup_pointer settings, void **ctx); + bool kins_machine_frame(setup_pointer settings); + int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known); + void kins_set_tool(void *ctx, const EmcPose *offset); + void kins_release(setup_pointer settings); + void current_machine_pose(setup_pointer settings, EmcPose *pose); + void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); + int current_joints(setup_pointer settings, void *ctx, double *joints); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index f420af7b336..333f766eab2 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -178,6 +178,7 @@ InterpBase *makeInterp() } Interp::~Interp() { + kins_release(&_setup); if(log_file) { if(log_file != stderr) fclose(log_file); @@ -919,10 +920,35 @@ int Interp::init() if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", section)) { _setup.axis_indexer_jnum[n] = *inival; } + // the travel a rotary is turned within by the orientation + // codes; a wrapped or modulo rotary has none + if (!_setup.axis_wrapped[n] && !_setup.axis_rotary_modulo[n]) { + _setup.axis_min[n] = inifile.findRealV("MIN_LIMIT", section, -1e99); + _setup.axis_max[n] = inifile.findRealV("MAX_LIMIT", section, 1e99); + } } _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); + // the kinematics, for the codes that ask it something + if (auto kins = inifile.findString("KINEMATICS", "KINS")) { + snprintf(_setup.kins_module, sizeof(_setup.kins_module), "%s", kins->c_str()); + } + _setup.kins_joints = inifile.findIntV("JOINTS", "KINS", 0); + // which joints turn rather than slide, so that an axis letter is + // refused where it would name a joint of the other kind, and the + // travel of each, so that a rotary is taken the way that stays in it + _setup.kins_angular_joints = 0; + for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { + char section[16]; + snprintf(section, sizeof(section), "JOINT_%d", jno); + if (auto type = inifile.findString("TYPE", section)) { + if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } + } + _setup.kins_joint_min[jno] = inifile.findRealV("MIN_LIMIT", section, -1e99); + _setup.kins_joint_max[jno] = inifile.findRealV("MAX_LIMIT", section, 1e99); + } + _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); _setup.naivecam_tolerance_default = inifile.findRealV("G64_DEFAULT_NAIVETOLERANCE", "RS274NGC", 0.0); @@ -1104,6 +1130,9 @@ int Interp::init() // INI file m98/m99 subprogram default setting _setup.disable_fanuc_style_sub = inifile.findBoolV("DISABLE_FANUC_STYLE_SUB", "RS274NGC", false); logDebug("init: DISABLE_FANUC_STYLE_SUB = %d", _setup.disable_fanuc_style_sub); + + // G53, G28, G30, G28.1 and G30.1 in the machine frame off the machine frame type + _setup.machine_moves_in_machine_frame = inifile.findBoolV("MACHINE_MOVES_IN_MACHINE_FRAME", "RS274NGC", false); } } diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 11ea4dfae7e..b6070e66c0c 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -222,6 +222,49 @@ void SET_TRAVERSE_RATE(double rate) _sai._traverse_rate = rate; } +void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + +void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/, + double seconds) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c, seconds); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c, seconds); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void STRAIGHT_TRAVERSE( int /*line_number*/, double x, double y, double z , double a /*AA*/ @@ -562,6 +605,11 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) offset.tran.x, offset.tran.y, offset.tran.z, offset.a, offset.b, offset.c, offset.u, offset.v, offset.w); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) +{ + USE_TOOL_LENGTH_OFFSET(offset); +} + void CHANGE_TOOL() { PRINT("CHANGE_TOOL()\n"); @@ -816,6 +864,11 @@ extern int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) return -1; } +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return true; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); @@ -895,6 +948,11 @@ double GET_EXTERNAL_POSITION_W() return 0.; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) +{ + return 0; +} + double GET_EXTERNAL_PROBE_POSITION_U() { return 0.; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 5ad736c59e8..49dac6f5172 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1146,6 +1146,47 @@ void generate_fast_move(double x, double y, double z, canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } +static void joint_move(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + auto msg = std::make_unique(); + + flush_segments(); + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = have_joints ? 1 : 0; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { + msg->joints[i] = (have_joints && joints) ? joints[i] : 0.0; + } + msg->seconds = seconds; + interp_list.set_line_number(line_number); + tag_and_send(std::move(msg), _tag); + + canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); +} + +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, 0.0); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, seconds); +} + void generate_move(double vel,double x, double y, double z, double a, double b, double c, double u, double v, double w) @@ -2998,7 +3039,7 @@ void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double EMC has no tool length offset. To implement it, we save it here, and apply it when necessary */ -void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) { auto set_offset_msg = std::make_unique(); @@ -3027,6 +3068,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) set_offset_msg->offset.v = TO_EXT_AX(AXIS_V, canon.toolOffset.v); set_offset_msg->offset.w = TO_EXT_AX(AXIS_W, canon.toolOffset.w); + set_offset_msg->have_point = (point != nullptr); + if (point) { + CANON_POSITION at(*point); + from_prog(at); + set_offset_msg->point = to_ext_pose(at); + // motion keeps the joints and stands on the point, so the next + // segment starts there and not where the old offset put it + canonUpdateEndPoint(at); + } + for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { SET_SPINDLE_SPEED(s, canon.spindle[s].speed); @@ -3035,6 +3086,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) interp_list.append(std::move(set_offset_msg)); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +{ + use_tool_length_offset(offset, nullptr); +} + +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point) +{ + use_tool_length_offset(offset, &point); +} + /* CHANGE_TOOL results from M6 */ void CHANGE_TOOL() { @@ -3918,6 +3979,17 @@ double GET_EXTERNAL_POSITION_W(void) return position.w; } +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) +{ + int n = emcStatus->motion.traj.joints; + + if (n > max) { n = max; } + for (int i = 0; i < n; i++) { + joints[i] = emcStatus->motion.joint[i].output; + } + return n; +} + double GET_EXTERNAL_PROBE_POSITION_X(void) { CANON_POSITION position; @@ -4023,6 +4095,11 @@ int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]) return 1; } +bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; +} + double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 049c640f1af..149b23faa39 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -527,6 +527,9 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) case EMC_TRAJ_LINEAR_MOVE_TYPE: break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + break; + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: break; @@ -1564,6 +1567,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) break; case EMC_TRAJ_LINEAR_MOVE_TYPE: + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: case EMC_TRAJ_SET_ACCELERATION_TYPE: @@ -1931,6 +1935,13 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajLinearMoveMsg->indexer_jnum); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: { + EMC_TRAJ_JOINT_MOVE *jm = reinterpret_cast(cmd); + emcTrajUpdateTag(jm->tag); + retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds); + break; + } + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: emcTrajUpdateTag((reinterpret_cast(cmd))->tag); emcTrajCircularMoveMsg = reinterpret_cast(cmd); @@ -1981,11 +1992,14 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode, emcTrajSetSpindlesyncMsg->angular_offset_degrees); break; - case EMC_TRAJ_SET_OFFSET_TYPE: + case EMC_TRAJ_SET_OFFSET_TYPE: { // update tool offset - emcStatus->task.toolOffset = (reinterpret_cast(cmd))->offset; - retval = emcTrajSetOffset(emcStatus->task.toolOffset); + EMC_TRAJ_SET_OFFSET *msg = reinterpret_cast(cmd); + emcStatus->task.toolOffset = msg->offset; + retval = emcTrajSetOffset(emcStatus->task.toolOffset, + msg->have_point ? &msg->point : nullptr); break; + } case EMC_TRAJ_SET_ROTATION_TYPE: emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; @@ -2578,6 +2592,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_SYSTEM_CMD; break; + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_LINEAR_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 2a429b0f66b..c291547cf39 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1476,10 +1476,31 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } -int emcTrajSetOffset(const EmcPose& tool_offset) +int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds) +{ + int i; + + emcmotCommand.command = EMCMOT_SET_JOINT_LINE; + emcmotCommand.pos = end; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.motion_type = seconds > 0.0 ? EMC_MOTION_TYPE_FEED : EMC_MOTION_TYPE_TRAVERSE; + emcmotCommand.joint_seconds = seconds; + emcmotCommand.have_joint_target = have_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + emcmotCommand.joint_target[i] = (have_joints && joints) ? joints[i] : 0.0; + } + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + +int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point) { emcmotCommand.command = EMCMOT_SET_OFFSET; emcmotCommand.tool_offset = tool_offset; + emcmotCommand.have_point = (point != nullptr); + if (point) { + emcmotCommand.pos = *point; + } return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 25953a8e01c..72976edb34b 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -200,6 +200,7 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou tcCircleStartAccelUnitVector(tc,out); break; case TC_SPHERICAL: + case TC_JOINT: return -1; default: return -1; @@ -207,6 +208,24 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * The world direction of a joint interpolated segment, end minus start, + * for the status fields that want a direction. The path between them is + * not straight, so this is the chord, and there is none when only the + * rotaries move. + */ +static int tcJointChordUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian d; + double mag; + + pmCartCartSub(&tc->coords.joint.world_end.tran, &tc->coords.joint.world_start.tran, &d); + pmCartMag(&d, &mag); + if (mag < TP_POS_EPSILON) { return -1; } + pmCartScalMult(&d, 1.0 / mag, out); + return 0; +} + /** * Get the acceleration direction unit vector for blend velocity calculations. * This calculates the direction of acceleration at the end of a segment. @@ -325,6 +344,8 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -348,6 +369,8 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -403,6 +426,8 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -526,6 +551,30 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_JOINT: { + // the ends are exact; between them this is the chord, a proxy + // for the status fields, and the servo thread reports the + // real position from the forward kinematics + const PmJointLine *jl = &tc->coords.joint; + double f = (tc->target > 0.0) ? progress / tc->target : 0.0; + EmcPose d; + + emcPoseSub(&jl->world_end, &jl->world_start, &d); + pos->tran.x = jl->world_start.tran.x + f * d.tran.x; + pos->tran.y = jl->world_start.tran.y + f * d.tran.y; + pos->tran.z = jl->world_start.tran.z + f * d.tran.z; + pos->a = jl->world_start.a + f * d.a; + pos->b = jl->world_start.b + f * d.b; + pos->c = jl->world_start.c + f * d.c; + pos->u = jl->world_start.u + f * d.u; + pos->v = jl->world_start.v + f * d.v; + pos->w = jl->world_start.w + f * d.w; + if (of_point == TC_GET_ENDPOINT) { *pos = jl->world_end; } + return TP_ERR_OK; + } + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetPosReal!\n", tc->motion_type); + return TP_ERR_FAIL; } if (res_fit == TP_ERR_OK) { @@ -536,6 +585,26 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) } +/** + * The joints of a joint interpolated segment at its progress. + * Returns the joint count, or 0 for any other segment. + */ +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) +{ + const PmJointLine *jl; + double f; + int i; + + if (!tc || tc->motion_type != TC_JOINT) { return 0; } + jl = &tc->coords.joint; + f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; + if (f > 1.0) { f = 1.0; } + for (i = 0; i < jl->num_joints; i++) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } + return jl->num_joints; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. @@ -621,7 +690,7 @@ int tcIsBlending(TC_STRUCT * const tc) { //FIXME Disabling blends for rigid tap cycle until changes can be verified. int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; + tc->motion_type != TC_RIGIDTAP && tc->motion_type != TC_JOINT; //Latch up the blending_next status here, so that even if the prev conditions //aren't necessarily true we still blend to completion once the blend diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index dcafd088e31..d6072f824c2 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -37,6 +37,7 @@ int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints); int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index c7406ecf801..53e11266d31 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -33,7 +33,8 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_JOINT = 5 } tc_motion_type_t; typedef enum { @@ -117,6 +118,19 @@ typedef struct { RIGIDTAP_STATE state; } PmRigidTap; +/* A segment interpolated in joint space: every joint runs from start to + * end together, the longest one setting the pace. The world poses at the + * two ends are what the segments around it see; the position along the way + * is not a line in world space and the servo thread reports it from the + * forward kinematics. */ +typedef struct { + double start[EMCMOT_MAX_JOINTS]; + double end[EMCMOT_MAX_JOINTS]; + int num_joints; + EmcPose world_start; + EmcPose world_end; +} PmJointLine; + typedef struct { double cycle_time; //Position stuff @@ -161,11 +175,13 @@ typedef struct { PmCircle9 circle; PmRigidTap rigidtap; Arc9 arc; + PmJointLine joint; } coords; int motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or - // TC_RIGIDTAP (coords.rigidtap) + // TC_RIGIDTAP (coords.rigidtap) or + // TC_JOINT (coords.joint) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? int term_cond; // gcode requests continuous feed at the end of diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 2b4024c9ab1..8302a2793fb 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -158,6 +158,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; + case TC_JOINT: + return true; default: tp_debug_print("Unknown motion type!\n"); return false; @@ -439,12 +441,23 @@ STATIC void tpReleaseQueuedPlanners(TP_STRUCT * const tp) * intended to put the motion queue in the state it would be if all queued * motions finished at the current position. */ +/* What the planner knows in joint space belongs to the queue: with the + queue reset, the queue end is no longer at those joints, no segment is + left to hand its end joints out, and no joint segment is waiting. */ +STATIC void tpForgetJoints(TP_STRUCT * const tp) +{ + tp->queue_end_joints_valid = 0; + tp->joint_end_valid = 0; + tp->joint_segments_queued = 0; +} + int tpClear(TP_STRUCT * const tp) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + tpForgetJoints(tp); // Clear out status ID's tp->nextId = 0; tp->execId = 0; @@ -1651,6 +1664,8 @@ int tpAddRigidTap(TP_STRUCT * const tp, acc, ini_maxjerk); + tp->queue_end_joints_valid = 0; + // Setup rigid tap geometry pmRigidTapInit(&tc.coords.rigidtap, &tp->goalPos, @@ -2073,6 +2088,11 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp_debug_print(" queue empty\n"); return NO_BLEND; } + if (prev_tc->motion_type == TC_JOINT) { + // nothing blends with a joint interpolated segment + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + return NO_BLEND; + } if (prev_tc->progress > prev_tc->target / 2.0) { tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); return NO_BLEND; @@ -2111,6 +2131,80 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { return blend_used; } +/** + * Add a joint interpolated segment to the tc queue. + * + * The joints run from start to end together over a "length" that is the + * joint space distance between them. vel and acc are already the tightest + * per-joint limits scaled onto that length, so no joint exceeds its own. + * Nothing blends into or out of it: the segment before it is made to stop + * and so is this one, since the path between the two world poses is not a + * line and the next segment has to start from rest at world_end. + */ +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag) +{ + TC_STRUCT tc = {0}; + PmJointLine *jl = &tc.coords.joint; + TC_STRUCT *prev_tc; + double length = 0.0; + int i; + + if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return TP_ERR_MISSING_INPUT; + } + if (tp->aborting) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); + return TP_ERR_FAIL; + } + + tcInit(&tc, TC_JOINT, canon_motion_type, tp->cycleTime, enables, 0); + tc.tag = tag; + tpSetupSyncedIO(tp, &tc); + tcSetupState(&tc, tp); + // a joint move has no path to synchronise to a spindle along + tc.synchronized = TC_SYNC_NONE; + tc.uu_per_rev = 0.0; + tcSetupMotion(&tc, vel, ini_maxvel, acc, ini_maxjerk); + + jl->num_joints = num_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + jl->start[i] = (i < num_joints) ? start[i] : 0.0; + jl->end[i] = (i < num_joints) ? end[i] : 0.0; + length += (jl->end[i] - jl->start[i]) * (jl->end[i] - jl->start[i]); + } + jl->world_start = tp->goalPos; + jl->world_end = world_end; + + tc.target = pmSqrt(length); + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + // it runs in joint units, so the max velocity slider does not cap it + tc.vlimit_scale = 0.0; + tc.indexer_jnum = -1; + tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + + prev_tc = tcqLast(&tp->queue); + if (prev_tc) { + tcSetTermCond(prev_tc, &tc, TC_TERM_COND_STOP); + tcFinalizeLength(prev_tc); + } + + int retval = tpAddSegmentToQueue(tp, &tc, true); + if (retval == TP_ERR_OK) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->queue_end_joints[i] = jl->end[i]; } + tp->queue_end_joints_valid = 1; + tp->joint_segments_queued++; + } + tpRunOptimization(tp); + return retval; +} + //TODO final setup steps as separate functions // /** @@ -2153,6 +2247,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, ini_maxjerk); tc.vlimit_scale = vlimit_scale; // Setup line geometry + tp->queue_end_joints_valid = 0; pmLine9Init(&tc.coords.line, &tp->goalPos, &end); @@ -2225,6 +2320,7 @@ int tpAddCircle(TP_STRUCT * const tp, tp->cycleTime, enables, atspeed); + tp->queue_end_joints_valid = 0; tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); @@ -3320,6 +3416,7 @@ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3375,6 +3472,16 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, return TP_ERR_FAIL; } + // a joint interpolated segment leaves its end joints behind for the + // servo thread: it asks after the segment is gone, and would otherwise + // invert the end position with the previous cycle's joints as seed + if (tc->motion_type == TC_JOINT) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = tc->coords.joint.end[i]; } + tp->joint_end_valid = 1; + if (tp->joint_segments_queued > 0) { tp->joint_segments_queued--; } + } + //Clear status flags associated since segment is done //TODO stuff into helper function? tc->active = 0; @@ -3421,6 +3528,7 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3518,7 +3626,8 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_MISSING_INPUT; } - if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_JOINT + || tc->synchronized != TC_SYNC_NONE)) { //Can't activate a segment with synced motion in reverse return TP_ERR_REVERSE_EMPTY; } @@ -4476,6 +4585,72 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) return TP_ERR_OK; } +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos) +{ + if (0 == tp) { + ZERO_EMC_POSE((*pos)); + return TP_ERR_FAIL; + } + *pos = tp->goalPos; + return TP_ERR_OK; +} + +/** + * The joints the active segment commands, when it is a joint interpolated + * one: the servo thread takes these instead of inverting the position. + * Returns the joint count, or 0 when the active segment is any other kind. + */ +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints) +{ + TC_STRUCT const *tc; + + if (!tp || !joints) { return 0; } + tc = tcqItem((TC_QUEUE_STRUCT *)&tp->queue, 0); + if (!tc || !tc->active) { return 0; } + return tcGetJointPos(tc, joints); +} + +/** + * The end joints of a joint interpolated segment that completed this + * cycle, once: the seed for the servo thread's inverse of the position the + * planner is now at, which is that segment's end and whatever a following + * segment added in the rest of the cycle. Returns 1 and fills the joints, + * or 0. + */ +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->joint_end_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->joint_end[i]; } + tp->joint_end_valid = 0; + return 1; +} + +/** + * How many joint interpolated segments the queue holds, active one + * included. An external offset cannot ride on one, so its planning waits + * while any is queued. + */ +int tpJointSegmentsQueued(TP_STRUCT const * const tp) +{ + return tp ? tp->joint_segments_queued : 0; +} + +/** + * Where the queue ends in joint space, if the last segment queued was a + * joint interpolated one. Returns 1 and fills the joints, or 0 when the + * answer is the inverse of the goal position. + */ +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->queue_end_joints_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->queue_end_joints[i]; } + return 1; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { @@ -4568,6 +4743,12 @@ EXPORT_SYMBOL(tpAbort); EXPORT_SYMBOL(tpActiveDepth); EXPORT_SYMBOL(tpAddCircle); EXPORT_SYMBOL(tpAddLine); +EXPORT_SYMBOL(tpAddJointLine); +EXPORT_SYMBOL(tpGetGoalPos); +EXPORT_SYMBOL(tpGetJointPos); +EXPORT_SYMBOL(tpTakeJointEnd); +EXPORT_SYMBOL(tpJointSegmentsQueued); +EXPORT_SYMBOL(tpGetQueueEndJoints); EXPORT_SYMBOL(tpAddRigidTap); EXPORT_SYMBOL(tpClear); EXPORT_SYMBOL(tpCreate); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 5aed4015707..f11955e0442 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -65,6 +65,15 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, double vlimit_scale, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag); +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints); +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints); +int tpJointSegmentsQueued(TP_STRUCT const * const tp); +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 610f629fbcb..05e95316a53 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -117,6 +117,17 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + /* where the queue ends in joint space, known when the last segment + queued was a joint interpolated one; a world segment after it makes + the answer the inverse of goalPos again */ + double queue_end_joints[EMCMOT_MAX_JOINTS]; + int queue_end_joints_valid; + /* the end joints of a joint interpolated segment that completed this + cycle, for the servo thread to seed its inverse with: the segment is + gone from the queue by the time it asks */ + double joint_end[EMCMOT_MAX_JOINTS]; + int joint_end_valid; + int joint_segments_queued; /* joint interpolated segments in the queue */ int queueSize; double cycleTime; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 635726e7cd7..6cfd70e0672 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1191,6 +1191,9 @@ static PyMemberDef Stat_members[] = { { "probing", T_BOOL, O(motion.traj.probing), READONLY, NULL}, { "probe_val", T_INT, O(motion.traj.probeval), READONLY, NULL}, { "kinematics_type", T_INT, O(motion.traj.kinematics_type), READONLY, NULL}, + { "kins_type", T_INT, O(motion.traj.switchkins_type), READONLY, + "The kinematics type motion runs, as G12.1 selects it and motion.kins-type\n" + "reports it; 0 for a module that switches nothing." }, { "motion_type", T_INT, O(motion.traj.motion_type), READONLY, "The type of the currently executing motion (one of MOTION_TYPE_TRAVERSE,\n" "MOTION_TYPE_FEED, MOTION_TYPE_ARC, MOTION_TYPE_TOOLCHANGE,\n" @@ -1294,6 +1297,14 @@ static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { return res; } +static PyObject *Stat_kins_type_flags(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(SWITCHKINS_MAX_TYPES); + for (int i = 0; i < SWITCHKINS_MAX_TYPES; i++) { + PyTuple_SET_ITEM(res, i, PyLong_FromLong(s->status.motion.traj.switchkins_flags[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1600,6 +1611,8 @@ static PyGetSetDef Stat_getsetlist[] = { (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, + {(char*)"kins_type_flags", (getter)Stat_kins_type_flags, NULL, + (char*)"What the module declares each kinematics type to be, one entry per type: KINSTYPE_ flags, or -1 for a type it does not provide.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, @@ -3423,6 +3436,10 @@ PyMODINIT_FUNC PyInit_linuxcnc(void) ENUM(KINEMATICS_INVERSE_ONLY); ENUM(KINEMATICS_BOTH); + ENUM(KINSTYPE_IDENTITY); + ENUM(KINSTYPE_PRIMARY); + ENUM(KINSTYPE_MACHINE); + ENUMX(4, EMC_DEBUG_CONFIG); ENUMX(4, EMC_DEBUG_VERSIONS); ENUMX(4, EMC_DEBUG_TASK_ISSUE); diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 0444c68e1d1..853de4a8ea2 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -196,9 +196,33 @@ static int tdr_jacobian(const kins_params *p, const double *j, return 0; } // tdr_jacobian() +// both rotaries carry the work, so the tool frame is the shared identity one; +// the forward's coefficients of the X, Y and Z joints are the rotation from +// machine into work, so the work frame is their transpose +static int tdr_work_frame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)fflags; + double sa = sin(j[3]*TO_RAD); + double ca = cos(j[3]*TO_RAD); + double sb = sin(j[4]*TO_RAD); + double cb = cos(j[4]*TO_RAD); + + rot->x.x = cb; rot->y.x = sa*sb; rot->z.x = -ca*sb; + rot->x.y = 0; rot->y.y = ca; rot->z.y = sa; + rot->x.z = sb; rot->y.z = -cb*sa; rot->z.z = ca*cb; + + return 0; +} // tdr_work_frame() + static const kins_ops tdr_ops = { .forward = tdr_forward, .inverse = tdr_inverse, + .work = tdr_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = tdr_jacobian, .primary = 1, }; diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected index dc9016af4d2..0d844086ec2 100644 --- a/tests/interp/g68-frame/expected +++ b/tests/interp/g68-frame/expected @@ -11,76 +11,81 @@ 11 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) 12 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 13 N..... SET_XY_ROTATION(0.0000) - 14 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") - 15 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 16 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 17 N..... COMMENT("the same plane by fixed axis angles about X") - 18 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 19 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 20 N..... COMMENT("the same plane by three points, the first one its origin") - 21 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 22 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 23 N..... COMMENT("Q0 moves the origin along the plane's own axes: to 11 17 32") - 24 N..... SET_G68_FRAME(11.0000, 17.0000, 32.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 25 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 26 N..... COMMENT("and after R: to 8 17 31") - 27 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 28 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 29 N..... COMMENT("R on a point block turns the plane the same way: to 8 17 31") - 30 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 31 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 32 N..... COMMENT("the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it") - 33 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 34 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 35 N..... COMMENT("R turns the plane about its own Z") - 36 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 37 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 38 N..... COMMENT("an arc in the plane") - 39 N..... SET_FEED_RATE(100.0000) - 40 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 41 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) - 42 N..... COMMENT("G53 inside the plane goes to absolute coordinates") - 43 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) - 44 N..... COMMENT("and #5021 reports them") - 45 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") - 46 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") - 47 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") - 48 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 49 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") - 50 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 51 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") - 52 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) - 53 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 54 N..... COMMENT("G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1") - 55 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 56 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 57 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 58 N..... MESSAGE(" prog 0.000000 2.000000 1.000000") - 59 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 60 N..... COMMENT("G52 X0 Y0 Z0 takes it out again") - 61 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 62 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 63 N..... COMMENT("a change of coordinate system takes the plane along, turned by its XY rotation; the tool stays put") - 64 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 65 N..... SET_G5X_OFFSET(3, 50.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 66 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 67 N..... SET_XY_ROTATION(90.0000) - 68 N..... MESSAGE(" abs 110.000000 220.000000 330.000000") - 69 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 70 N..... MESSAGE(" at the plane origin on G56: abs 30.000000 10.000000 30.000000, expect 30 10 30") - 71 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) - 72 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 73 N..... SET_XY_ROTATION(0.0000) - 74 N..... COMMENT("G69 drops a G52 given in the plane") - 75 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 76 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) - 77 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 78 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 79 N..... SET_XY_ROTATION(0.0000) - 80 N..... SET_FEED_MODE(0, 0) - 81 N..... SET_FEED_RATE(0.0000) - 82 N..... STOP_SPINDLE_TURNING(0) - 83 N..... SET_SPINDLE_MODE(0 0.0000) - 84 N..... PROGRAM_END() - 85 N..... ON_RESET() - 86 N..... ON_RESET() + 14 N..... COMMENT("one plane, four ways. Three distinct angles, none of them right, so that") + 15 N..... COMMENT("the order the rotations compose in shows in the answer") + 16 N..... COMMENT("P0 turns about the frame as it goes, ZXZ by default") + 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 19 N..... COMMENT("the same plane about the fixed axes of the system it sits in, XYZ") + 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 22 N..... COMMENT("the same plane by three points: the first is the origin, the second on +X, the third on the +Y side") + 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("Q0 moves the origin along the plane's own axes: to 11 17 32") + 26 N..... SET_G68_FRAME(11.0000, 17.0000, 32.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 27 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("and after R: to 8 17 31") + 29 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 30 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 31 N..... COMMENT("R on a point block turns the plane the same way: to 8 17 31") + 32 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 33 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("the same plane by two vectors") + 35 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 36 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 37 N..... COMMENT("two vectors: X is kept and Z, 3 degrees off square, is squared to it") + 38 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 39 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 40 N..... COMMENT("R turns the plane about its own Z") + 41 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.7134, -0.6459, 0.2717, 0.6561, 0.4797, -0.5826, 0.2460, 0.5939, 0.7660], 1) + 42 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 43 N..... COMMENT("an arc in the plane") + 44 N..... SET_FEED_RATE(100.0000) + 45 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 46 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 47 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 48 N..... STRAIGHT_TRAVERSE(-27.6365, -20.9503, -14.0466, 0.0000, 0.0000, 0.0000) + 49 N..... COMMENT("and #5021 reports them") + 50 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -27.636497 -20.950346 -14.046603") + 51 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 52 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 53 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 54 N..... MESSAGE(" prog -29.358386 -25.107354 -19.408914") + 55 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 56 N..... COMMENT("G68.4 composes onto the plane, in its own axes") + 57 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.6477, -0.5920, 0.4796, 0.4862, -0.1635, -0.8584, 0.5865, 0.7892, 0.1820], 1) + 58 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 59 N..... COMMENT("G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1") + 60 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 61 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 62 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 63 N..... MESSAGE(" prog 0.000000 2.000000 1.000000") + 64 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 65 N..... COMMENT("G52 X0 Y0 Z0 takes it out again") + 66 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 67 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 68 N..... COMMENT("a change of coordinate system takes the plane along, turned by its XY rotation; the tool stays put") + 69 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 70 N..... SET_G5X_OFFSET(3, 50.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 71 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 72 N..... SET_XY_ROTATION(90.0000) + 73 N..... MESSAGE(" abs 110.000000 220.000000 330.000000") + 74 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 75 N..... MESSAGE(" at the plane origin on G56: abs 30.000000 10.000000 30.000000, expect 30 10 30") + 76 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 77 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 78 N..... SET_XY_ROTATION(0.0000) + 79 N..... COMMENT("G69 drops a G52 given in the plane") + 80 N..... SET_G68_FRAME(11.0000, 18.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 81 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) + 82 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 83 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 84 N..... SET_XY_ROTATION(0.0000) + 85 N..... SET_FEED_MODE(0, 0) + 86 N..... SET_FEED_RATE(0.0000) + 87 N..... STOP_SPINDLE_TURNING(0) + 88 N..... SET_SPINDLE_MODE(0 0.0000) + 89 N..... PROGRAM_END() + 90 N..... ON_RESET() + 91 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc index 08afdee8683..673da944992 100644 --- a/tests/interp/g68-frame/g68.ngc +++ b/tests/interp/g68-frame/g68.ngc @@ -4,16 +4,18 @@ g21 g90 g10 l2 p2 x100 y200 z300 r0 g10 l2 p3 x50 y0 z0 r90 g55 -(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) -g68.2 x10 y20 z30 i0 j90 k0 +(one plane, four ways. Three distinct angles, none of them right, so that) +(the order the rotations compose in shows in the answer) +(P0 turns about the frame as it goes, ZXZ by default) +g68.2 x10 y20 z30 i25 j40 k-15 g0 x1 y2 z3 -(the same plane by fixed axis angles about X) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +(the same plane about the fixed axes of the system it sits in, XYZ) +g68.2 p1 q123 x10 y20 z30 i39.025043525 j9.576578516 k13.400523974 g0 x1 y2 z3 -(the same plane by three points, the first one its origin) +(the same plane by three points: the first is the origin, the second on +X, the third on the +Y side) g68.2 p2 q1 x10 y20 z30 -g68.2 p2 q2 x15 y20 z30 -g68.2 p2 q3 x10 y20 z35 +g68.2 p2 q2 x14.796086535 y21.142635331 z29.168171623 +g68.2 p2 q3 x9.765570001 y22.339990858 z31.862655459 g0 x1 y2 z3 (Q0 moves the origin along the plane's own axes: to 11 17 32) g68.2 p2 q0 x1 y2 z3 @@ -33,12 +35,16 @@ g68.2 p2 q1 x10 y20 z30 g68.2 p2 q2 x15 y20 z30 g68.2 p2 q3 x10 y20 z35 r90 g0 x0 y0 z0 -(the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it) +(the same plane by two vectors) +g68.2 p3 q1 x10 y20 z30 i0.959217579 j0.228526484 k-0.166364909 +g68.2 p3 q2 i0.271653782 j-0.582563416 k0.766044443 +g0 x1 y2 z3 +(two vectors: X is kept and Z, 3 degrees off square, is squared to it) g68.2 p3 q1 x10 y20 z30 i1 j0 k0 g68.2 p3 q2 i0.05 j-1 k0 g0 x1 y2 z3 (R turns the plane about its own Z) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g68.2 x10 y20 z30 i25 j40 k-15 r37.5 g0 x1 y2 z3 (an arc in the plane) g1 f100 x0 y0 z0 @@ -52,8 +58,8 @@ g53 g0 x100 y200 z300 g43.1 z7 (debug, prog #5420 #5421 #5422) g49 -(G68.4 composes: a further 90 about the plane's X) -g68.4 p1 q123 i90 j0 k0 +(G68.4 composes onto the plane, in its own axes) +g68.4 p1 q123 i35 j-20 k10 g0 x1 y2 z3 (G52 in the plane shifts it along its own axes and the tool stays put: prog 0 2 1) g68.2 x10 y20 z30 i0 j90 k0 diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 48c09ee0d98..e92522018b1 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 6 ] \ +[ "$(grep -c 'frames agree' "$1")" = 10 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index ed6acf27a08..8ca5ce37ca0 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -55,6 +55,9 @@ RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 fo static int quill = -1; RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); +static int abc_flange = 0; +RTAPI_MP_INT(abc_flange, "1 when the forward reports the flange orientation as roll, pitch and yaw in A B C"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -211,14 +214,38 @@ static void check(const double *j, int own_kinematics) if (quill >= 0) { /* the joint runs the tool out along its own axis, away from the - holder, so the tip moves along the tool axis reversed: the one - tie between the reported frame and the forward transform on a - machine that turns nothing but the tool */ + holder, so the tip moves along the tool axis reversed, seen from + the work: the one tie between the reported tool frame and the + forward transform on a machine whose head turns the tool */ response(j, quill, &d); - expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + expect(close3(&d, -dot(&work.x, &tool.z), -dot(&work.y, &tool.z), + -dot(&work.z, &tool.z)), "the quill runs out along the tool axis", j); } + if (abc_flange) { + /* an arm's forward reports the flange frame of ISO 9787, z out of + the interface towards the work, as roll, pitch and yaw in A B C; + the tool frame is that frame turned a half turn about its x, so + turned back it has to be the rotation A B C describe: + Rz(C) Ry(B) Rx(A) */ + EmcPose w; + KINEMATICS_INVERSE_FLAGS inf = 0; + double ca, sa, cb, sb, cc, sc; + + memset(&w, 0, sizeof(w)); + kinematicsForward(j, &w, &ff, &inf); + ca = cos(w.a * TO_RAD); sa = sin(w.a * TO_RAD); + cb = cos(w.b * TO_RAD); sb = sin(w.b * TO_RAD); + cc = cos(w.c * TO_RAD); sc = sin(w.c * TO_RAD); + expect(close3(&tool.x, cc * cb, sc * cb, -sb), + "tool x is the flange x A B C give", j); + expect(close3(&tool.y, -(cc * sb * sa - sc * ca), -(sc * sb * sa + cc * ca), -(cb * sa)), + "tool y is the flange y A B C give, reversed", j); + expect(close3(&tool.z, -(cc * sb * ca + sc * sa), -(sc * sb * ca - cc * sa), -(cb * ca)), + "tool axis is the flange z A B C give, reversed", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 109e69821dc..3a18f726919 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,8 +49,29 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4 fixed_spindle=1 ktype=1" + +# a head that tilts and a table that turns, the tilt's sense set by a pin: +# the W joint ties the tool frame to the forward either way +run "maxkins" "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 quill=8" +run "maxkins" "setp maxkins.pivot-length 100 +setp maxkins.conventional-directions 1" \ + "joints=9 r1=4 r2=5 quill=8" + run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" run "pumakins" "setp pumakins.A2 300" \ - "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" + "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0 abc_flange=1" + +# the same arm described by its DH chain +run "genserkins" "" \ + "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0 abc_flange=1" diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index ca2b15df9a9..5a514e9d8a4 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -9,9 +9,10 @@ # match what paritycheck published, to rounding, or the module is not # the pure function of its parameters it claims to be. # -# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT TOOL # POSE and JNT are comma separated numbers, as given to paritycheck; -# COORDS and SPARM are a dash when the module was loaded without them. +# COORDS and SPARM are a dash when the module was loaded without them; +# TOOL is the x,y,z offset paritycheck handed the module, or a dash. import ctypes import os @@ -47,6 +48,7 @@ class EmcPose(ctypes.Structure): # "-" where it has no such pair, "no-frame" where it reports no tool frame orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" posejoint_in = sys.argv[10] if len(sys.argv) > 10 else "no-frame" +tool_in = [float(v) for v in sys.argv[11].split(",")] if len(sys.argv) > 11 and sys.argv[11] != "-" else None pose_in += [0.0] * (AXES - len(pose_in)) jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) @@ -85,6 +87,15 @@ def fail(what): fail("%s has no type %d in the block form" % (module, ktype)) sys.exit(1) +# the offset realtime was handed, as motion hands it over +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +def set_tool(xyz): + t = EmcPose() + t.x, t.y, t.z = xyz + kins.kinematicsUserSetTool(ctx, ctypes.byref(t)) +if tool_in: + set_tool(tool_in) + def pose_of(values): p = EmcPose() for n, v in zip("xyzabcuvw", values): @@ -135,8 +146,8 @@ def compare(what, ours, theirs): # the caller's tool wins over the module's pin: for a module with a tool # entry, a length of the caller's must move the inverse, and handing the -# tool back to HAL must return it to what realtime found -kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +# tool back to HAL must return it to what realtime found, which is the +# module's pin unless realtime was handed a tool as well tool_pin = None for name in ("tool-offset", "tool-offset-z"): try: @@ -144,7 +155,7 @@ def compare(what, ours, theirs): tool_pin = name except RuntimeError: pass -if r_inv == 0 and rc_inv == 0 and tool_pin: +if r_inv == 0 and rc_inv == 0 and tool_pin and not tool_in: P = pose_of(pose_in) if frompose else F T = pose_of([0.0] * AXES) T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 @@ -199,6 +210,104 @@ class Rot(ctypes.Structure): else: print("kins-params: %s type %d pose joint %s" % (module, ktype, got)) +# The offset across the tool. The offset is the tool's own, given against +# the machine with the rotaries at zero, and X and Y of it turn with the +# tool as the length does along the tool axis: the tip is the point the +# tool hangs from, the forward with no tool, less the offset turned into the +# work frame, and it has to be on the pose less the offset, which is what +# canon commands. For that the module's length has to lie along its own +# tool axis, which is checked first. +def frame_rows(fn, j): + r = Rot() + if fn(ctx, Joints(*j), ctypes.byref(r)): + return None + return [[r.x[i], r.y[i], r.z[i]] for i in range(3)] + +def tool_in_work(j): + t = frame_rows(kins.kinematicsUserToolFrame, j) + w = frame_rows(kins.kinematicsUserWorkFrame, j) + if w is None: + return t + return [[sum(w[k][a] * t[k][b] for k in range(3)) for b in range(3)] for a in range(3)] + +def forward(j, xyz): + set_tool(xyz) + p = pose_of(pose_in) + r = kins.kinematicsUserForward(ctx, Joints(*j), ctypes.byref(p)) + return p if r == 0 else None + +def expect(what, got, want): + if not close(got, want): + fail("%s: %.12g, expected %.12g" % (what, got, want)) + +def settled_inverse(P, seed): + # a module may take where the machine is from the joints it is handed, + # so run to the joints that explain themselves + q = Joints(*seed) + for _ in range(4): + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), q) != 0: + return None + return list(q) + +kins.kinematicsUserWorkFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +if tool_in and has_frame and r_fwd == 0 and r_inv == 0: + before = failures + j0 = list(qi) if frompose else list(jnt_in) + T = tool_in + xyz = lambda p: [p.x, p.y, p.z] + + # the tool's own axes are the machine's with the rotaries at zero + zero = frame_rows(kins.kinematicsUserToolFrame, [0.0] * MAX_JOINTS) + if max(abs(zero[a][b] - (a == b)) for a in range(3) for b in range(3)) > 1e-12: + fail("the tool frame at zero is not the machine's: %s" % zero) + + # the length along the tool axis the frame reports + R = tool_in_work(j0) + L = 7.0 + longer, none = forward(j0, [0.0, 0.0, L]), forward(j0, [0.0, 0.0, 0.0]) + along = [(a - b) / L for a, b in zip(xyz(longer), xyz(none))] + for a in range(3): + expect("the length along the tool axis, %s" % "xyz"[a], along[a], (a == 2) - R[a][2]) + + # the tip, through the inverse and back + C = forward(j0, T) + q = settled_inverse(C, j0) + if C is None or q is None: + fail("forward or inverse with the offset across the tool") + else: + for j in range(joints): + expect("the joints back from the pose with the offset, joint %d" % j, q[j], j0[j]) + R = tool_in_work(q) + hangs = xyz(forward(q, [0.0, 0.0, 0.0])) + set_tool(T) + for a in range(3): + tip = hangs[a] - sum(R[a][b] * T[b] for b in range(3)) + expect("the tip with the offset across the tool, %s" % "xyz"[a], tip, xyz(C)[a] - T[a]) + + # the Jacobian, against the inverse it is the derivative of; the + # loader takes it on the joints the last inverse found, which are q + J = Jac() + if kins.kinematicsUserJacobian(ctx, ctypes.byref(C), J) != 0: + fail("the Jacobian with the offset across the tool") + else: + h = 1e-4 + for a, n in enumerate("xyzabcuvw"): + ends = [] + for sign in (1, -1): + P = pose_of([getattr(C, m) for m in "xyzabcuvw"]) + setattr(P, n, getattr(P, n) + sign * h) + ends.append(settled_inverse(P, q)) + if None in ends: + fail("the inverse beside the pose with the offset across the tool") + break + for j in range(joints): + d = (ends[0][j] - ends[1][j]) / (2 * h) + if abs(d - J[j][a]) > 1e-6 * max(1.0, abs(d)): + fail("the Jacobian [%d][%s] with the offset across the tool: %.9g, differences %.9g" + % (j, n, J[j][a], d)) + if failures == before: + print("kins-params: %s type %d turns the offset across the tool with it" % (module, ktype)) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult index d3eba1a4da0..08d80381330 100755 --- a/tests/kins-params/checkresult +++ b/tests/kins-params/checkresult @@ -1,4 +1,6 @@ #!/bin/sh [ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && [ "$(grep -c 'turns the offset across the tool' "$1")" = "$(grep -c '^=== .* tool ' "$1")" ] \ + && [ "$(grep -c '^=== .* tool ' "$1")" -ge 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c index 1a3ef4664ab..97a43ced391 100644 --- a/tests/kins-params/paritycheck.c +++ b/tests/kins-params/paritycheck.c @@ -22,6 +22,8 @@ * seed (frompose=0) * jnt up to sixteen integers, the joint set (frompose=0) or the * inverse seed (frompose=1) + * tool x y z of a tool offset motion would hand the module, all + * evaluated under it; none when all three are zero */ #include #include @@ -41,6 +43,8 @@ static int pose[EMCMOT_MAX_AXIS] = { 0 }; RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); +static int tool[3] = { 0, 0, 0 }; +RTAPI_MP_ARRAY_INT(tool, 3, "tool offset x y z, as motion hands it over"); static int comp_id = -1; @@ -107,6 +111,10 @@ int rtapi_app_main(void) if (ktype > 0 && kinematicsSwitchable()) { if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } } + if (tool[0] || tool[1] || tool[2]) { + EmcPose t = { { tool[0], tool[1], tool[2] }, 0, 0, 0, 0, 0, 0 }; + if (kinematicsSetTool(&t)) { hal_exit(comp_id); return -1; } + } for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 1e623690fa4..3e56e4f4ffd 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -20,7 +20,7 @@ rm -f oot_switchkins.comp oot_userkins.comp # ONLY= in the environment runs the entries for that module alone run() { local loadrt="$1" setp="$2" parms="$3" ktype="$4" - local module coords sparm joints frompose pose jnt orient posejoint rtparms hal tok + local module coords sparm joints frompose pose jnt orient posejoint tool rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -38,6 +38,8 @@ run() { # the rotary whose sign G53.1 P names, the same three ways; where the # entry gives none, what it gives for the pair posejoint="" + # a tool offset x,y,z both sides evaluate under, as motion hands it over + tool="-" for tok in $parms; do case "$tok" in joints=*) joints=${tok#joints=} ;; @@ -46,6 +48,7 @@ run() { jnt=*) jnt=${tok#jnt=} ;; orient=*) orient=${tok#orient=} ;; posejoint=*) posejoint=${tok#posejoint=} ;; + tool=*) tool=${tok#tool=} ;; esac done [ -n "$posejoint" ] || posejoint=$orient @@ -57,10 +60,14 @@ run() { printf '%s\n' "$setp" printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" # halcmd keeps quotes, so an absent value travels as a dash - printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" "$tool" } > "$hal" - echo "=== $loadrt type ${ktype:-0}" + if [ "$tool" = "-" ]; then + echo "=== $loadrt type ${ktype:-0}" + else + echo "=== $loadrt type ${ktype:-0} tool $tool" + fi halrun -f "$hal" rm -f "$hal" } @@ -92,7 +99,7 @@ setp matrixkins.C_zz 1.001" \ # tables and heads, offsets set so no term drops out run "maxkins" \ "setp maxkins.pivot-length 100" \ - "joints=9 jnt=10,20,30,0,15,25,7,0,3" + "joints=9 jnt=10,20,30,0,15,25,7,0,3 orient=- posejoint=4" run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" 1 @@ -123,7 +130,7 @@ setp xyzab_tdr_kins.tool-offset-z 7 setp xyzab_tdr_kins.x-rot-point 1 setp xyzab_tdr_kins.y-rot-point 2 setp xyzab_tdr_kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" 1 + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=4" 1 run "xyzacb_trsrn" \ "setp xyzacb_trsrn_kins.nut-angle 45 @@ -162,6 +169,45 @@ setp xyzbca_trsrn_kins.primary-angle 20 setp xyzbca_trsrn_kins.secondary-angle 35" \ "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3" 1 +# a tool offset across the tool, a tool held off the spindle axis: its X +# and Y turn with the tool frame the module reports, the same in realtime +# and outside it +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3 tool=4,3,12" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3 tool=4,3,12" 1 +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3 tool=4,3,7" +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3 tool=4,3,7" +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,4 posejoint=4 tool=4,3,50" 1 +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3 tool=4,3,50" 1 + # polar run "rosekins" "" "joints=3 jnt=10,5,30" @@ -170,8 +216,8 @@ run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" -run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" -run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0 orient=-" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0 orient=-" # parallel machines, from a pose the forward can be seeded with run "tripodkins" \ diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e426b4d7554..e409e2b3cf9 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -8,5 +8,11 @@ the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the -kinematics with nothing netted to its pin, and that G13.1 in an -ON_ABORT_COMMAND routine does not swallow the rest of the routine. +kinematics with nothing netted to its pin and, the head being tilted, +moves the programmed point rather than the joints, with the interpreter +agreeing on where the point went, that under G43.5 a tool vector I J K +lands on the joints and the point the rotary words reach, is not +incremental, keeps the free joint on the pole and is refused with a +rotary word, as a zero vector, on the identity kinematics and outside +G43.5, and that G13.1 in an ON_ABORT_COMMAND routine does not swallow +the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index e62800c5cd7..1b284ccdf8c 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -163,38 +163,180 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- the tool length reaches the kinematics with nothing netted ---------- +# ---- a tool length under a tilt keeps the joints ------------------------ # -# Motion hands the module the offset G43 puts in effect. The head is -# tilted, so the offset moves the joints, by the tilt term of the length: -# the same length along the tool axis instead of along Z. +# Motion hands the module the offset G43 puts in effect, with nothing +# netted. The head is tilted, so the length lies along the tool axis +# instead of along Z: the point the joints stand on moves by the tilt +# term of the length, and the joints stay where they are. The +# interpreter works out the same point ahead of motion, so a move to +# where it thinks the machine is goes nowhere. import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) drain() + +def pose(): + s.poll() + return list(s.position[:3]) + +def tool_length_check(what, before_joints, before_pose, after_joints, after_pose, want): + if max(abs(after_joints[j] - before_joints[j]) for j in range(JOINTS)) > 1e-6: + error("%s moved the joints by %s" % (what, + " ".join("%.4f" % (after_joints[j] - before_joints[j]) for j in range(JOINTS)))) + got = [after_pose[j] - before_pose[j] for j in (0, 1, 2)] + if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("%s moved the point by %s, not %s" % (what, + " ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) + said = [m[1].strip() for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s raised %s" % (what, said)) + # the interpreter's point: a move of nothing from it goes nowhere + mdi("G91") + stayed = mdi("G0 X0 Y0 Z0") + mdi("G90") + if max(abs(stayed[j] - after_joints[j]) for j in range(JOINTS)) > 1e-6: + error("after %s the interpreter has the point elsewhere: a move of nothing moved the joints by %s" + % (what, " ".join("%.4f" % (stayed[j] - after_joints[j]) for j in range(JOINTS)))) + mdi("G12.1 P0") mdi("G0 X10 Y10 Z-5 B-22.5 C45") mdi("G49") +errors_before = errors before = mdi("G0 X10 Y10 Z-5") +before_pose = pose() after = mdi("G43.4 H1") +pose_after = pose() L = 12.5 # tool 1 in tool.tbl b, cc = math.radians(-22.5), math.radians(45) -want = [-L * math.sin(math.pi - b) * math.cos(cc), - -L * math.sin(math.pi - b) * math.sin(cc), - -L * (1 + math.cos(math.pi - b))] -got = [after[j] - before[j] for j in (0, 1, 2)] -if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: - error("G43.4 in the tilted pose moved the joints by %s, not %s" - % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) -else: - print("G43.4 moved the joints by the tilt term of the tool length") +# the length along the tool axis, less the length along Z the offset stands for +want = [L * math.sin(math.pi - b) * math.cos(cc), + L * math.sin(math.pi - b) * math.sin(cc), + L * (1 + math.cos(math.pi - b))] +tool_length_check("G43.4 in the tilted pose", before, before_pose, after, pose_after, want) # G49 would drop to identity and hold the joints where they are; a zero # offset without a switch takes the length back out back = mdi("G43.1 Z0") -if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: - error("a zero tool length did not take the length back out of the joints") +tool_length_check("a zero tool length", after, pose_after, back, pose(), [-w for w in want]) +if errors == errors_before: + print("a tool length under a tilt moved the point, not the joints") + +# a point-to-point move ends on joints motion holds while the point +# stays; a tool length moves the point, and the joints are held on +errors_before = errors +p2p = mdi("G53.4 G0 X10 Y10 Z-5 B-22.5 C45") +p2p_pose = pose() +after = mdi("G43.1 Z%g" % L) +tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) +if errors == errors_before: + print("a tool length after a point-to-point move keeps the joints too") + +# An offset across the tool, a tool held off the spindle axis: its X and Y +# are the tool's own and lie along the tool's X and Y under the tilt. The +# joints stay and the point moves by the offset less its turn into the tool +# frame; a move then puts the tip, the point the tool hangs from less the +# whole offset turned with the tool, on the point programmed. +errors_before = errors +sin_b, cos_b, sin_c, cos_c = math.sin(b), math.cos(b), math.sin(cc), math.cos(cc) +# 5axiskins' tool frame Rz(C) Ry(-B), its columns +tool_x = [cos_b * cos_c, cos_b * sin_c, sin_b] +tool_y = [-sin_c, cos_c, 0.0] +tool_z = [-sin_b * cos_c, -sin_b * sin_c, cos_b] +T = [4.0, 3.0, L] +turned = [T[0] * tool_x[a] + T[1] * tool_y[a] for a in range(3)] +standing_pose = pose() +across = mdi("G43.1 X4 Y3") +tool_length_check("an offset across the tool under the tilt", after, standing_pose, across, pose(), + [T[a] - turned[a] if a < 2 else -turned[a] for a in range(3)]) +target = [20.0, 5.0, -10.0] +moved = mdi("G0 X%g Y%g Z%g" % tuple(target)) +pivot = hal.get_value("5axiskins.pivot-length") +hangs = [moved[a] + pivot * ((a == 2) - tool_z[a]) for a in range(3)] +tip = [hangs[a] - turned[a] - T[2] * tool_z[a] for a in range(3)] +if max(abs(tip[a] - target[a]) for a in range(3)) > 1e-6: + error("with an offset across the tool the tip is at %s, not %s" + % (" ".join("%.4f" % v for v in tip), " ".join("%.4f" % v for v in target))) +here = pose() +back = mdi("G43.1 X0 Y0") +tool_length_check("taking the offset across the tool out", moved, here, back, pose(), + [turned[a] - T[a] if a < 2 else turned[a] for a in range(3)]) +if errors == errors_before: + print("an offset across the tool turns with the tool") +mdi("G43.1 Z0") +mdi("G49") + +# ---- G43.5: the tool axis as a vector ------------------------------------ +# +# Under G43.5 a G0 or G1 line gives the direction of the tool axis as I J K +# and the interpreter finds the rotaries. The head's tool axis at B, C is +# (-sin B cos C, -sin B sin C, cos B), so the vector for the tilted pose +# above has to land on the joints and the point the rotary words reach. + +def refused(cmd, needle): + drain() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +errors_before = errors +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0") +mdi("G43.5 H1") +s.poll() +if 435 not in s.gcodes: + error("G43.5 is not among the active G-codes %s" % (s.gcodes,)) +by_words = mdi("G0 X10 Y10 Z-5 B-22.5 C45") +by_words_pose = pose() +mdi("G0 X0 Y0 Z0 B0 C0") +vec = (-math.sin(b) * math.cos(cc), -math.sin(b) * math.sin(cc), math.cos(b)) +by_vector = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +if max(abs(by_vector[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("the vector put the joints at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in by_vector), " ".join("%.4f" % v for v in by_words))) +if max(abs(p - q) for p, q in zip(pose(), by_words_pose)) > 1e-5: + error("the vector put the point at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in pose()), " ".join("%.4f" % v for v in by_words_pose))) +# a direction is not incremental +mdi("G91") +held = mdi("G1 X0 I%.9f J%.9f K%.9f F1000" % vec) +mdi("G90") +if max(abs(held[j] - by_vector[j]) for j in range(JOINTS)) > 1e-6: + error("the same vector under G91 moved the joints by %s" + % " ".join("%.4f" % (held[j] - by_vector[j]) for j in range(JOINTS))) +# the pole: the tool vertical leaves C where it is, and the point stays +pole = mdi("G0 K1") +if abs(pole[3]) > 1e-6 or abs(pole[4] - 45) > 1e-3: + error("the tool vertical put B, C at %.4f, %.4f, not 0, 45" % (pole[3], pole[4])) +if max(abs(p - q) for p, q in zip(pose()[:3], by_words_pose[:3])) > 1e-5: + error("the tool vertical moved the point to %s" % " ".join("%.4f" % v for v in pose())) +# a rotary offset renames the angles, the direction is the same: the same +# joints, called something else by the program +mdi("G10 L2 P1 C30") +mdi("G0 X0 Y0 Z0 B0 C0") +offset = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +mdi("G10 L2 P1 C0") +if max(abs(offset[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("under a C offset the vector put the joints at %s, not %s" + % (" ".join("%.4f" % v for v in offset), " ".join("%.4f" % v for v in by_words))) +if errors == errors_before: + print("G43.5 turned the tool along the vector, onto the joints the rotary words reach") +refused("G0 X0 K1 B5", "twice") +refused("G0 X0 I0 J0 K0", "zero") +mdi("G13.1") +refused("G0 X0 K1", "G12.1 first") +mdi("G43.4 H1") +refused("G0 X0 K1", "K word with no") +mdi("G43.5 H1") mdi("G49") +refused("G0 X0 K1", "K word with no") # ---- a negative kinematics number is refused ----------------------------- diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..d114c3a2afc --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,11 @@ +The C kinematics against the tilted work plane maths. + +The tilted work plane maths for the two nutating-head machines was first +written in python, remap_funcs_twp.py, independently of the kinematics +modules, for the remap the configs used before the interpreter learned +the codes. It lives on here, one copy per machine, as the oracle. +This test loads each module in realtime and puts its tool frame next to +the python transformation matrix over a grid of head angles, and its +tool frame inverse next to the python candidate joint angles and virtual +rotation for a set of requested tool axes. Where they disagree, one of +the two has the sign or the order of a rotation wrong. diff --git a/tests/kins-twp/check.py b/tests/kins-twp/check.py new file mode 100755 index 00000000000..2b8d52dd683 --- /dev/null +++ b/tests/kins-twp/check.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +# The python half of the tilted work plane cross-check. +# +# Imports the machine's remap_funcs_twp.py, the maths the tilted work +# plane remap orients the head with, and drives twpcheck, loaded after +# the kinematics module, to get the module's answers to the same +# questions. Two comparisons: +# +# frames over a grid of primary angle, secondary angle, virtual +# rotation and table angle, the module's tool frame in +# machine coordinates against the python transformation +# matrix Rp * Rs * Rtc +# inverse for a set of requested tool axes, with the table held as +# the remap holds it, the joint angle pairs the module's +# kinematicsToolFrameInverse() finds against the pairs the +# python candidate search keeps, and the spin about the tool +# the module reports for the python's horizontal tool x +# against the python's own virtual rotation; then with nothing +# held, where the module may turn the table, each side judged +# by the other's maths +# +# Usage: check.py MACHINE CONFIGDIR INIFILE +# MACHINE is xyzacb or xyzbca; CONFIGDIR holds remap_funcs_twp.py; +# INIFILE is what that file reads its letters and limits from. + +import os +import sys +import time +from math import radians, degrees, pi, sin, cos, atan2 + +import numpy as np +import hal + +machine, cfgdir, inifile = sys.argv[1:4] +os.environ["INI_FILE_NAME"] = inifile +sys.path.insert(0, cfgdir) +import remap_funcs_twp as twp + +# joint numbers: the table, the secondary and the primary rotary +TABLE, SECONDARY, PRIMARY = {"xyzacb": (3, 4, 5), "xyzbca": (4, 3, 5)}[machine] +PREROT = "%s_trsrn_kins.pre-rot" % machine +TOL = 1e-9 +ANGLE_TOL = 1e-6 # degrees + +failures = 0 +def fail(what): + global failures + failures += 1 + print("kins-twp: FAIL %s: %s" % (machine, what)) + +class Log: + def debug(self, *a, **k): pass + def error(self, *a, **k): print("kins-twp: python error:", a[0] % tuple(a[1:]) if len(a) > 1 else a[0]) +log = Log() + +# ---- driving twpcheck + +request = 0 +def ask(j, axis=None, xdir=None, held=0): + """set the joints and the request, wait for the answer""" + global request + for i, v in enumerate(j): + hal.set_p("twpcheck.j-%d" % i, str(v)) + hal.set_p("twpcheck.held", str(held)) + for i, c in enumerate("xyz"): + hal.set_p("twpcheck.axis-%s" % c, str(axis[i] if axis is not None else 0.0)) + hal.set_p("twpcheck.xdir-%s" % c, str(xdir[i] if xdir is not None else 0.0)) + hal.set_p("twpcheck.have-x", "1" if xdir is not None else "0") + request += 1 + hal.set_p("twpcheck.request", str(request)) + deadline = time.time() + 5 + while hal.get_value("twpcheck.done") != request: + if time.time() > deadline: + print("kins-twp: FAIL twpcheck did not answer") + sys.exit(1) + time.sleep(0.002) + +def read_matrix(name): + return np.array([[hal.get_value("twpcheck.%s-%d%d" % (name, r, c)) for c in range(3)] + for r in range(3)]) + +def read_solutions(): + n = hal.get_value("twpcheck.nsol") + sols = [] + for k in range(max(n, 0)): + sols.append(([hal.get_value("twpcheck.sol-%d-%d" % (k, i)) for i in range(6)], + hal.get_value("twpcheck.spin-%d" % k), + hal.get_value("twpcheck.free-%d" % k))) + return n, sols + +def joints_at(table, secondary, primary): + j = [10.0, 20.0, 30.0, 0.0, 0.0, 0.0] + j[TABLE], j[SECONDARY], j[PRIMARY] = table, secondary, primary + return j + +# ---- the python's answers + +def py_matrix(primary_deg, secondary_deg, tc): + m = twp.kins_calc_transformation_matrix(radians(primary_deg), radians(secondary_deg), tc, + np.asmatrix(np.identity(4)), 'inv') + return np.array(m)[:3, :3] + +def py_pairs(z): + """the (primary, secondary) pairs in degrees the remap would keep for a + tool axis, following remap.py: every combination of the candidate + lists, kept where it reaches the axis""" + t1, t2 = twp.kins_calc_possible_joint_angles(log, np.array(z), None) + if t1 is None or t2 is None: + return [] + pairs = [] + for a in set(t1): + for b in set(t2): + m = py_matrix(degrees(a), degrees(b), 0.0) + if np.allclose(m[:, 2], z, atol=1e-6): + pairs.append((degrees(a), degrees(b))) + return pairs + +def same_angle(a, b): + d = (a - b + 180.0) % 360.0 - 180.0 + return abs(d) < ANGLE_TOL + +def same_pair(p, q): + return same_angle(p[0], q[0]) and same_angle(p[1], q[1]) + +def fmt(m): + return np.array2string(m, precision=6, suppress_small=True) + +# ---- frames +# +# The module's frame is the head's rotation from its joints alone, so it +# is compared with the python matrix at zero virtual rotation; whether the +# frame should carry the virtual rotation too is a convention question the +# test does not settle. + +frames = 0 +hal.set_p(PREROT, "0") +for table in (0.0, 20.0): + for primary in (0.0, 30.0, -25.0, 90.0, 180.0, -135.0): + for secondary in (0.0, 30.0, -25.0, 90.0, -90.0, 180.0): + ask(joints_at(table, secondary, primary)) + if hal.get_value("twpcheck.frame-rc") != 0: + fail("no frame at primary %g secondary %g" % (primary, secondary)) + continue + tool = read_matrix("tool") + want = py_matrix(primary, secondary, 0.0) + frames += 1 + if not np.allclose(tool, want, atol=TOL): + fail("tool frame differs at primary %g secondary %g table %g\n module:\n%s\n python:\n%s" + % (primary, secondary, table, fmt(tool), fmt(want))) + +# ---- inverse, the table held +# +# The remap holds the table and orients the head, so ask the module the +# same: the joint pairs must then be the python's, and the spin about the +# tool for the python's horizontal tool x must be the python's virtual +# rotation. + +def rz(a): + return np.array([[cos(a), -sin(a), 0.0], [sin(a), cos(a), 0.0], [0.0, 0.0, 1.0]]) + +def frames_at(j): + ask(j) + return read_matrix("work"), read_matrix("tool") + +def in_work(work, tool): + return work.T @ tool + +HOLD_TABLE = 1 << TABLE +REQUESTS = ((30.0, 30.0), (-25.0, 60.0), (120.0, -45.0), (180.0, 90.0), + (0.0, 0.0), (90.0, 135.0), (45.0, 170.0), (-100.0, -20.0)) + +requests = 0 +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + if not any(same_pair(p, (primary, secondary)) for p in pairs): + fail("the python does not find the pair (%g, %g) the axis was made from" % (primary, secondary)) + + ask(seed, axis=z, held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("with the table held, the module finds no solution for " + where) + continue + found = [(s[0][PRIMARY], s[0][SECONDARY]) for s in sols] + for s in sols: + j, spin, free = s + if abs(j[TABLE] - seed[TABLE]) > 1e-12: + fail("the held table moved for " + where) + if free != 0 and (primary, secondary) != (0.0, 0.0): + fail("with the table held a solution is still a family for " + where) + for p in pairs: + if not any(same_pair(p, f) for f in found): + fail("python pair (%.6f, %.6f) not among the module's %s for %s" + % (p[0], p[1], ["(%.6f, %.6f)" % f for f in found], where)) + for f in found: + if not any(same_pair(p, f) for p in pairs): + fail("module pair (%.6f, %.6f) not among the python's %s for %s" + % (f[0], f[1], ["(%.6f, %.6f)" % p for p in pairs], where)) + + # tool x as the python's virtual rotation places it, horizontal: the + # module, holding the table, must answer the same pair with that spin + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + if abs(full[2, 0]) > 1e-9: + fail("python virtual rotation %g leaves tool x off horizontal for pair (%.6f, %.6f)" % (tc, p[0], p[1])) + ask(seed, axis=z, xdir=full[:, 0], held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + match = [s for s in sols if same_pair((s[0][PRIMARY], s[0][SECONDARY]), p)] + if not match: + fail("with tool x given and the table held, pair (%.6f, %.6f) is gone from the module's answers" % p) + continue + spin = match[0][1] + if abs((spin - tc + pi) % (2 * pi) - pi) > 1e-6: + fail("module spin %.9f and python virtual rotation %.9f differ for pair (%.6f, %.6f)" + % (spin, tc, p[0], p[1])) + +# ---- inverse, nothing held +# +# The module may now turn the table, since it turns the tool against the +# work as surely as the head does, and reports one member of the family +# that results. Not the python's answer, so each is judged by the other's +# maths: a module solution must reach the axis through the python head +# matrix composed with the module's table frame, and with tool x given it +# must reach the whole frame. + +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + + ask(seed, axis=z) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution for " + where) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + if not np.allclose(tool, py_matrix(j[PRIMARY], j[SECONDARY], 0.0), atol=TOL): + fail("module frame at its own solution differs from the python head matrix for " + where) + if not np.allclose(in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0))[:, 2], z, atol=1e-6): + fail("module solution %s does not reach %s by the python head matrix" % (fmt(np.array(j)), where)) + for i in (0, 1, 2): + if abs(j[i] - seed[i]) > 1e-9: + fail("solution moved linear joint %d for %s" % (i, where)) + + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + ask(seed, axis=z, xdir=full[:, 0]) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution with tool x given for pair (%.6f, %.6f)" % p) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + achieved = in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0) @ rz(spin)) + if not np.allclose(achieved, full, atol=1e-6): + fail("module solution %s spin %.6f does not reach the python frame for pair (%.6f, %.6f)\n achieved:\n%s\n wanted:\n%s" + % (fmt(np.array(j)), spin, p[0], p[1], fmt(achieved), fmt(full))) + +if failures: + sys.exit(1) +print("kins-twp: %s agrees, %d frames, %d requests" % (machine, frames, requests)) diff --git a/tests/kins-twp/checkresult b/tests/kins-twp/checkresult new file mode 100755 index 00000000000..011ea9232ad --- /dev/null +++ b/tests/kins-twp/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'kins-twp: .* agrees' "$1")" = 2 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-twp/skip b/tests/kins-twp/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-twp/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh new file mode 100755 index 00000000000..d50c9bea68e --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,26 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install twpcheck.c >/dev/null + +# One hal file per machine. twpcheck answers frame and inverse requests +# from check.py over HAL pins; check.py holds the python maths. +run() { + local machine=$1 hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s_trsrn\n' "$machine" + printf 'setp %s_trsrn_kins.nut-angle 45\n' "$machine" + printf 'loadrt twpcheck joints=6 ktype=1\n' + printf 'loadrt threads name1=t1 period1=1000000\n' + printf 'addf twpcheck t1\n' + printf 'start\n' + printf 'loadusr -w python3 check.py %s %s/%s %s/twp-%s.ini\n' \ + "$machine" "$PWD" "$machine" "$PWD" "$machine" + } > "$hal" + echo "=== $machine" + halrun -f "$hal" + rm -f "$hal" +} + +run xyzacb +run xyzbca diff --git a/tests/kins-twp/twp-xyzacb.ini b/tests/kins-twp/twp-xyzacb.ini new file mode 100644 index 00000000000..7d276d86bf8 --- /dev/null +++ b/tests/kins-twp/twp-xyzacb.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzacb_trsrn + +[TWP] +PRIMARY = C +SECONDARY = B + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_B] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twp-xyzbca.ini b/tests/kins-twp/twp-xyzbca.ini new file mode 100644 index 00000000000..b6bfd198e60 --- /dev/null +++ b/tests/kins-twp/twp-xyzbca.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzbca_trsrn + +[TWP] +PRIMARY = C +SECONDARY = A + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_A] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twpcheck.c b/tests/kins-twp/twpcheck.c new file mode 100644 index 00000000000..7113dff88b5 --- /dev/null +++ b/tests/kins-twp/twpcheck.c @@ -0,0 +1,165 @@ +/* + * twpcheck: the realtime half of the tilted work plane cross-check. + * + * Loaded after a kinematics module, it answers requests made over HAL + * pins: for the joint values on its inputs it reports the module's tool + * frame and work frame, and for the tool axis (and optionally tool x) + * on its inputs it reports what kinematicsToolFrameInverse() finds, the + * joint sets and the spin about the tool each needs, with the joints + * named on the held pin kept where they are. check.py drives it and + * holds the python maths the answers are compared with. + * + * A request is made by raising the request pin; done follows it when + * the answers are on the pins. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + */ +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 6; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); + +static int comp_id = -1; + +#define NSOL TOOL_FRAME_MAX_SOLUTIONS + +static struct { + hal_real_t j[EMCMOT_MAX_JOINTS]; + hal_real_t axis[3]; + hal_real_t xdir[3]; + hal_bool_t have_x; + hal_uint_t held; /* bit per joint the inverse may not move */ + hal_uint_t request; + hal_uint_t done; + hal_real_t tool[3][3]; /* [row][column], columns are the frame's axes */ + hal_real_t work[3][3]; + hal_sint_t frame_rc; + hal_sint_t nsol; + hal_real_t sol[NSOL][EMCMOT_MAX_JOINTS]; + hal_real_t spin[NSOL]; + hal_sint_t free[NSOL]; +} *pins; + +static void publish(hal_real_t out[3][3], const PmRotationMatrix *m) +{ + hal_set_real(out[0][0], m->x.x); hal_set_real(out[0][1], m->y.x); hal_set_real(out[0][2], m->z.x); + hal_set_real(out[1][0], m->x.y); hal_set_real(out[1][1], m->y.y); hal_set_real(out[1][2], m->z.y); + hal_set_real(out[2][0], m->x.z); hal_set_real(out[2][1], m->y.z); hal_set_real(out[2][2], m->z.z); +} + +static void update(void *arg, long period) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, work; + PmCartesian axis, xdir; + double j[EMCMOT_MAX_JOINTS]; + double sols[NSOL * EMCMOT_MAX_JOINTS]; /* rows of joints doubles, packed */ + double spin[NSOL]; + int freed[NSOL]; + int i, k, n, rc; + (void)arg; + (void)period; + + if (hal_get_ui32(pins->request) == hal_get_ui32(pins->done)) { return; } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = i < joints ? hal_get_real(pins->j[i]) : 0.0; + } + + rc = kinematicsToolFrame(j, &tool, &ff); + if (!rc) { rc = kinematicsWorkFrame(j, &work, &ff); } + hal_set_si32(pins->frame_rc, rc); + if (!rc) { + publish(pins->tool, &tool); + publish(pins->work, &work); + } + + axis.x = hal_get_real(pins->axis[0]); + axis.y = hal_get_real(pins->axis[1]); + axis.z = hal_get_real(pins->axis[2]); + xdir.x = hal_get_real(pins->xdir[0]); + xdir.y = hal_get_real(pins->xdir[1]); + xdir.z = hal_get_real(pins->xdir[2]); + n = -1; + if (axis.x != 0 || axis.y != 0 || axis.z != 0) { + n = kinematicsToolFrameInverse(&axis, hal_get_bool(pins->have_x) ? &xdir : NULL, + j, hal_get_ui32(pins->held), sols, NSOL, + freed, spin); + } + hal_set_si32(pins->nsol, n); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + hal_set_real(pins->sol[k][i], k < n ? sols[k * joints + i] : 0.0); + } + hal_set_real(pins->spin[k], k < n ? spin[k] : 0.0); + hal_set_si32(pins->free[k], k < n ? freed[k] : 0); + } + + hal_set_ui32(pins->done, hal_get_ui32(pins->request)); +} + +int rtapi_app_main(void) +{ + static const char letter[3] = { 'x', 'y', 'z' }; + int i, k, r, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("twpcheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->j[i], 0.0, "twpcheck.j-%d", i); + } + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->axis[i], 0.0, "twpcheck.axis-%c", letter[i]); + res += hal_pin_new_real(comp_id, HAL_IN, &pins->xdir[i], 0.0, "twpcheck.xdir-%c", letter[i]); + } + res += hal_pin_new_bool(comp_id, HAL_IN, &pins->have_x, 0, "twpcheck.have-x"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->held, 0, "twpcheck.held"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->request, 0, "twpcheck.request"); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &pins->done, 0, "twpcheck.done"); + for (r = 0; r < 3; r++) { + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->tool[r][i], 0.0, "twpcheck.tool-%d%d", r, i); + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->work[r][i], 0.0, "twpcheck.work-%d%d", r, i); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->frame_rc, 0, "twpcheck.frame-rc"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->nsol, 0, "twpcheck.nsol"); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->sol[k][i], 0.0, "twpcheck.sol-%d-%d", k, i); + } + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->spin[k], 0.0, "twpcheck.spin-%d", k); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->free[k], 0, "twpcheck.free-%d", k); + } + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + if (hal_export_funct("twpcheck", update, NULL, 0, comp_id)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzacb/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzacb/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzbca/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzbca/remap_funcs_twp.py diff --git a/tests/ptp-iterative/README b/tests/ptp-iterative/README new file mode 100644 index 00000000000..456921354f4 --- /dev/null +++ b/tests/ptp-iterative/README @@ -0,0 +1,9 @@ +The point-to-point moves on a kinematics module that answers the inverse +by iterating, and the preview of them. + +genserkins takes the Jacobian at the joints it is handed, so the seed +decides whether there is an answer at all: this machine homes to a pose +that is nowhere near all zeros, and all zeros is the singular pose the +inverse cannot start from. The interpreter reads the joints the machine +stands in, and the preview reads them from the status buffer the way a GUI +does, so the same program previews and runs to the same place. diff --git a/tests/ptp-iterative/checkresult b/tests/ptp-iterative/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-iterative/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-iterative/sim.hal b/tests/ptp-iterative/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-iterative/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-iterative/test-ui.py b/tests/ptp-iterative/test-ui.py new file mode 100755 index 00000000000..7fb988a976e --- /dev/null +++ b/tests/ptp-iterative/test-ui.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# A module that answers the inverse by iterating has to be started +# somewhere, and the pose this machine homes to is not the one it would be +# started from by default. The moves are checked twice: through the +# interpreter, which reads the joints from motion, and through the preview, +# which reads them from the status buffer the way a GUI does. +import gcode +import linuxcnc +import preview_helpers +import os +import sys +import time +from rs274.interpret import StatMixin + +JOINTS = 6 +PROGRAM = "test.ngc" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + + +def drain(): + while e.poll(): + pass + + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + + +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.points = [] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + + def straight_traverse(self, *pos): + self.points.append(pos) + + def straight_feed(self, *pos): + self.points.append(pos) + + +# the canon protocol carries lengths in the interpreter's own units, which +# a GUI turns into the machine's; the angles are already there +def in_machine_units(pos): + s.poll() + scale = (s.linear_units or 1) * 25.4 + return [v * scale for v in pos[:3]] + list(pos[3:6]) + + +def preview(program=PROGRAM): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return None, "line %d: %s" % (line, gcode.strerror(result)) + return canon.points, None + + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +home = settled() +print("homed at %s" % " ".join("%.4f" % v for v in home)) +if abs(home[1] + 90) > 1e-6 or abs(home[4] - 90) > 1e-6: + error("the machine did not home to the pose the test is written for") + +# the preview runs first, from the pose the machine stands in, and its last +# point is where the program ends up +points, refused = preview() +if refused: + error("the preview refused %s, %s" % (PROGRAM, refused)) +elif not points: + error("the preview of %s reported no move at all" % PROGRAM) +previewed = points[-1] if points else None + +# the interpreter takes the same program, one line at a time +for line in open(PROGRAM): + line = line.strip() + if not line or line.startswith("m2"): + continue + reached = mdi(line) + print("%-20s %s" % (line, " ".join("%.4f" % v for v in reached))) + drain() + +if abs(reached[0] - 10) > 1e-6 or abs(reached[4] - 80) > 1e-6: + error("the program left joints 0 and 4 at %.6f and %.6f" + % (reached[0], reached[4])) +for j in (1, 2, 3, 5): + if abs(reached[j] - home[j]) > 1e-6: + error("the program moved joint %d from %.6f to %.6f" + % (j, home[j], reached[j])) + +# and both agree on where that is +s.poll() +if previewed: + for name, i, got in zip("XYZABC", range(6), in_machine_units(previewed)): + if abs(got - s.position[i]) > 1e-3: + error("the preview put %s at %.6f, the machine at %.6f" + % (name, got, s.position[i])) + print("preview and machine agree on %s" + % " ".join("%.4f" % v for v in in_machine_units(previewed))) + +# a preview taken now starts where the machine stands, so a program that +# names the joints it is already in asks for no move at all +after, refused = preview() +if refused: + error("the second preview refused %s, %s" % (PROGRAM, refused)) +if after and max(abs(a - b) for a, b in zip(in_machine_units(after[0]), s.position[:6])) > 1e-3: + error("the second preview started at %s, not at %s" + % (["%.4f" % v for v in in_machine_units(after[0])], + ["%.4f" % v for v in s.position[:6]])) + +# a point out of the arm's reach: the module says so through the HAL +# library it prints with, which has to be within reach of the process the +# preview runs in, or the answer is the process going down +# all joints at zero is the pose this arm cannot be inverted from, and the +# module says so through the HAL library it prints with. That library has +# to be within reach of the process the preview runs in: a GUI has it only +# underneath the interpreter it loaded, and out of reach the answer is the +# process going down rather than a refusal. +mdi("g53.7 g0 j0=0 j1=0 j2=0 j3=0 j4=0 j5=0") +out, refused = preview() +if not refused: + error("the preview answered from the pose the arm cannot be inverted from") +elif "invert" not in refused: + error("the preview said %r, which does not mention the inverse" % refused) +else: + print("the preview refused from the singular pose: %s" % refused) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-iterative/test.ini b/tests/ptp-iterative/test.ini new file mode 100644 index 00000000000..bc942288e36 --- /dev/null +++ b/tests/ptp-iterative/test.ini @@ -0,0 +1,140 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = genserkins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# the modified DH parameters of the RV-6SDL, as the melfa-sim config has them +HALCMD = setp genserkins.A-1 85 +HALCMD = setp genserkins.A-2 380 +HALCMD = setp genserkins.A-3 100 +HALCMD = setp genserkins.ALPHA-1 -1.570796326 +HALCMD = setp genserkins.ALPHA-3 -1.570796326 +HALCMD = setp genserkins.ALPHA-4 1.570796326 +HALCMD = setp genserkins.ALPHA-5 -1.570796326 +HALCMD = setp genserkins.D-0 350 +HALCMD = setp genserkins.D-3 425 +HALCMD = setp genserkins.D-5 235 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = -90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-iterative/test.ngc b/tests/ptp-iterative/test.ngc new file mode 100644 index 00000000000..0afcad5d314 --- /dev/null +++ b/tests/ptp-iterative/test.ngc @@ -0,0 +1,3 @@ +g53.7 g0 j4=80 +g53.7 g0 j0=10 +m2 diff --git a/tests/ptp-iterative/test.sh b/tests/ptp-iterative/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-iterative/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-iterative/tool.tbl b/tests/ptp-iterative/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-iterative/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README new file mode 100644 index 00000000000..08480a0f1e2 --- /dev/null +++ b/tests/ptp-machine-frame/README @@ -0,0 +1,23 @@ +The point-to-point moves on a machine whose slides do not line up with +its frame. + +slantkins.comp is built here, out of tree the way the switchkinscomp +template builds: XYZBC on five joints, joint 1 a slide slanted 30 degrees +in the XY plane, so machine X and Y are made by joints 0 and 1 together. +Type 0 is the carriage in machine coordinates, declared the machine frame +type and not an identity; type 1 the tool tip with the head tilted about Y +and the tool length from the tool table; type 2 a plain identity. + +The test checks that G13.1 and G49 select the machine frame type rather +than the identity, that G53.5 puts the carriage at a machine frame point, +the letters not given holding their machine coordinate and not their +joint, from the machine frame type and from under the tilted tip +kinematics alike, that G28.5 goes to a position stored on the machine +frame, through a waypoint when given one, that MACHINE_MOVES_IN_MACHINE_FRAME +runs G53, G28 and G30 as G53.5, G28.5 and G30.5 under the tip and under the +plain identity, G28.1 and G30.1 storing the carriage, that a plain move +after a machine frame move starts from the right place, and that a preview +reads the type the machine is on and what each type is from the status +buffer: the G53 a GUI restates the position with ahead of the program stays +in the world of that type, and the program's G53 ends where G53.5 would, +through the switches the program makes. diff --git a/tests/ptp-machine-frame/checkresult b/tests/ptp-machine-frame/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-machine-frame/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-machine-frame/g13-g53.ngc b/tests/ptp-machine-frame/g13-g53.ngc new file mode 100644 index 00000000000..cb6a840de63 --- /dev/null +++ b/tests/ptp-machine-frame/g13-g53.ngc @@ -0,0 +1,3 @@ +G13.1 +G53 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g13-g535.ngc b/tests/ptp-machine-frame/g13-g535.ngc new file mode 100644 index 00000000000..47f51f5d187 --- /dev/null +++ b/tests/ptp-machine-frame/g13-g535.ngc @@ -0,0 +1,3 @@ +G13.1 +G53.5 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g53.ngc b/tests/ptp-machine-frame/g53.ngc new file mode 100644 index 00000000000..b2f7efa7e05 --- /dev/null +++ b/tests/ptp-machine-frame/g53.ngc @@ -0,0 +1,2 @@ +G53 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/g535.ngc b/tests/ptp-machine-frame/g535.ngc new file mode 100644 index 00000000000..6f4ee09e007 --- /dev/null +++ b/tests/ptp-machine-frame/g535.ngc @@ -0,0 +1,2 @@ +G53.5 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/here.ngc b/tests/ptp-machine-frame/here.ngc new file mode 100644 index 00000000000..836d5abe2a0 --- /dev/null +++ b/tests/ptp-machine-frame/here.ngc @@ -0,0 +1,2 @@ +G91 G0 X0 +M2 diff --git a/tests/ptp-machine-frame/sim.hal b/tests/ptp-machine-frame/sim.hal new file mode 100644 index 00000000000..fbd3d125327 --- /dev/null +++ b/tests/ptp-machine-frame/sim.hal @@ -0,0 +1,16 @@ +loadrt switchkins_core +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-machine-frame/skip b/tests/ptp-machine-frame/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-machine-frame/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-machine-frame/slantkins.comp b/tests/ptp-machine-frame/slantkins.comp new file mode 100644 index 00000000000..65b8daa55a3 --- /dev/null +++ b/tests/ptp-machine-frame/slantkins.comp @@ -0,0 +1,170 @@ +component slantkins "Test kinematics whose machine frame is not its joints"; + +description +""" +A switchable kinematics for the ptp-machine-frame test: XYZBC on five +joints, where joint 1 is a slide slanted in the XY plane by the 'slant' +angle, so that machine X and Y are made by joints 0 and 1 together, the way +a mill-turn with a composite Y slide is built. + +type0 is the machine frame: the carriage in machine coordinates, the two +rotaries as joints, declared as the machine frame type and not an identity. + +type1 is the working transform: the tool tip, with the head tilted about Y +by B and the tool length from the tool table along the tool axis, folded so +that tip and carriage agree at B zero. + +type2 is a plain identity, the joints as the axes, declared identity. +"""; +pin out si32 dummy=0 "one pin, which halcompile requires"; +option period no; +option extra_setup; +license "GPL"; +author "Luca Toniolo"; +;; + +#include +// out of tree, the way the switchkinscomp template builds: the switchkins +// implementation is switchkins_core, loaded ahead of this module +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +static const kins_param_desc slant_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "slant", KINS_PARAM_FLOAT, KINS_IN, 0, 30.0 }, +}; +enum { P_TOOL, P_SLANT }; + +static void unused_axes(EmcPose *pos) +{ + pos->a = 0; + pos->u = 0; + pos->v = 0; + pos->w = 0; +} + +// the carriage: joint 1 runs at the slant angle from X, in the XY plane +static int frame_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)fflags; (void)iflags; + + pos->tran.x = j[0] + j[1] * cos(th); + pos->tran.y = j[1] * sin(th); + pos->tran.z = j[2]; + pos->b = j[3]; + pos->c = j[4]; + unused_axes(pos); + return 0; +} + +static int frame_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)iflags; (void)fflags; + + j[1] = pos->tran.y / sin(th); + j[0] = pos->tran.x - j[1] * cos(th); + j[2] = pos->tran.z; + j[3] = pos->b; + j[4] = pos->c; + return 0; +} + +// the tip: the head tilts about Y by B and carries the tool length below +// the pivot, folded so that the tip is the carriage at B zero +static void tip_offset(const kins_params *p, double b, double *dx, double *dz) +{ + double L = p->tool.tran.z; + double rb = b * PM_PI / 180.0; + + *dx = -L * sin(rb); + *dz = L * (1.0 - cos(rb)); +} + +static int tip_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dx, dz; + + frame_forward(p, s, j, pos, fflags, iflags); + tip_offset(p, j[3], &dx, &dz); + pos->tran.x += dx; + pos->tran.z += dz; + return 0; +} + +static int tip_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose carriage = *pos; + double dx, dz; + + tip_offset(p, pos->b, &dx, &dz); + carriage.tran.x -= dx; + carriage.tran.z -= dz; + return frame_inverse(p, s, &carriage, j, iflags, fflags); +} + +static const kins_ops frame_ops = { + .forward = frame_forward, + .inverse = frame_inverse, + .machine = 1, +}; + +static const kins_ops tip_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "slantkins"; + kp->halprefix = "slantkins"; + kp->required_coordinates = "xyzbc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = slant_params; + kp->nparams = sizeof(slant_params)/sizeof(slant_params[0]); + + switchkinsRegisterOps(0, &frame_ops); + switchkinsRegisterOps(1, &tip_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + return 0; +} + +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} + +EXPORT_SYMBOL(kinsDescribe); + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } + return switchkinsInit(comp_id, &kp, coordinates); +} diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py new file mode 100755 index 00000000000..ab16f9bff89 --- /dev/null +++ b/tests/ptp-machine-frame/test-ui.py @@ -0,0 +1,308 @@ +#!/usr/bin/env python3 +# The point-to-point moves on a machine whose slides do not line up with its +# frame: see README. + +import linuxcnc +import hal +import gcode +import preview_helpers +import sys +import os +import time +import math +from rs274.interpret import StatMixin + +JOINTS = 5 +SLANT = math.radians(30.0) +TOOL = 50.0 # T1 in tool.tbl + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def show(what, j): + print("%-34s %s" % (what, " ".join("%.4f" % v for v in j))) + +def expect(what, j, want, tol=1e-4): + show(what, j) + if max(abs(a - b) for a, b in zip(j, want)) > tol: + error("%s: expected %s" % (what, " ".join("%.4f" % v for v in want))) + +# the joints of a carriage position: joint 1 is the slanted slide +def carriage(x, y, z, b=0.0, cc=0.0): + j1 = y / math.sin(SLANT) + return [x - j1 * math.cos(SLANT), j1, z, b, cc] + +def expect_type(what, want): + k = kins_type() + print("%-34s kins-type %d" % (what, k)) + if k != want: + error("%s: kins-type %d, expected %d" % (what, k, want)) + +# --- G13.1 and G49 select the machine frame type, not the identity --------- +mdi("G12.1 P2") +expect_type("G12.1 P2, the plain identity", 2) +mdi("G13.1") +expect_type("G13.1", 0) +mdi("G43.4 H1") +expect_type("G43.4", 1) +mdi("G49") +expect_type("G49", 0) +mdi("G12.1 P1", "G13.1") +expect_type("G12.1 P1 then G13.1", 0) +drain() + +# --- G53.5 is the machine frame, not the slides ------------------------------ +# on the machine frame type G53 and G53.5 agree, and a Y word alone moves +# both slides: X is held, which the slanted slide would otherwise pull +j = mdi("G53 G0 X5 Y0 Z0 B0 C0") +expect("G53 to (5, 0, 0)", j, carriage(5, 0, 0)) +j = mdi("G53.5 G0 Y10") +expect("G53.5 Y10 holds machine X", j, carriage(5, 10, 0)) +j = mdi("G53.5 G0 X0") +expect("G53.5 X0 holds machine Y", j, carriage(0, 10, 0)) + +# under the tip kinematics with the head tilted and a tool on, G53.5 still +# means the carriage: the tilt and the tool length are left out. The tilt +# holds the tip, so the carriage has moved by the tool's swing, and that is +# where X is held +mdi("G43.4 H1") +j = mdi("G0 B30") +expect_type("G43.4 with the head at B30", 1) +swing_x = TOOL * math.sin(math.radians(30)) +swing_z = TOOL * (1 - math.cos(math.radians(30))) +expect("the tilt holds the tip", j, carriage(swing_x, 10, -swing_z, 30)) +j = mdi("G53.5 G0 Y0 Z0") +expect("G53.5 Y0 Z0 under the tip, tilted", j, carriage(swing_x, 0, 0, 30)) +j = mdi("G53.5 G0 X20 Y10 Z-5") +expect("G53.5 X20 Y10 Z-5 under the tip", j, carriage(20, 10, -5, 30)) +j = mdi("G53.5 G0 B0") +expect("G53.5 B0 is the joint", j, carriage(20, 10, -5, 0)) +# and the config reads machine moves in the machine frame: under the tip G53 +# is G53.5, the carriage at the words whatever the tilt and the tool, where +# the tip's G53 would take the tool length off along machine Z +mdi("G0 B30") +j = mdi("G53 G0 X0 Y0 Z0") +expect("G53 X0 Y0 Z0 under the tip, tilted", j, carriage(0, 0, 0, 30)) +j = mdi("G53 G0 Y10") +expect("G53 Y10 under the tip holds machine X", j, carriage(0, 10, 0, 30)) +# G28.1 stores where the carriage stands and G28 and G30 go back to it the +# same way, after a plain move under the tip has taken the carriage along +j = mdi("G53 G0 X20 Y10 Z-5", "G28.1", "G30.1", "G0 X0 Y0 Z10") +j = mdi("G28") +expect("G28 under the tip, tilted", j, carriage(20, 10, -5, 30)) +b = mdi("G0 X0 Y0 Z10") +j = mdi("G30 Y0") +expect("G30 Y0, Y alone back to the store", j, + carriage(b[0] + b[1] * math.cos(SLANT), 10, b[2], 30)) +# the plain identity is not the machine frame type either: its letters name +# the joints, where the machine frame holds machine X as Y moves +mdi("G49", "G0 B0", "G12.1 P2") +j = mdi("G53 G0 X5 Y0 Z0 B0 C0") +expect("G53 to (5, 0, 0) on the plain identity", j, carriage(5, 0, 0)) +j = mdi("G53 G0 Y10") +expect("G53 Y10 on the plain identity holds X", j, carriage(5, 10, 0)) +mdi("G13.1") +drain() + +# --- G28.5 goes to a machine frame position stored on the machine frame ---- +# with no words every letter goes, B to its stored zero included; with words +# the machine passes through that machine frame point and only those +# letters go on to the stored position. Under G43.4 a program Z is the +# tool length above the carriage, the offset the interpreter carries, so the +# waypoint's Z is 10 + 50 +mdi("G49", "G53 G0 X20 Y10 Z0 B0 C0", "G28.1") +mdi("G43.4 H1", "G0 B30", "G0 X0 Y0 Z10") +j = mdi("G28.5") +expect("G28.5 with the head tilted", j, carriage(20, 10, 0, 0)) +mdi("G0 X0 Y0 Z10") +j = mdi("G28.5 X40") +expect("G28.5 X40, through X then to X only", j, carriage(20, 0, 10 + TOOL, 0)) +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +# --- the program position agrees with the move ------------------------------ +# the interpreter reports the tip where the joints put it, so a plain move +# after a machine frame move starts from the right place +mdi("G43.4 H1", "G0 B30", "G53.5 G0 X20 Y10 Z0") +s.poll() +before = list(s.position[:3]) +j = mdi("G91 G0 X1", "G90") +after = carriage(20, 10, 0, 30) +after[0] += 1.0 +expect("G91 X1 after G53.5", j, after) +s.poll() +d = [s.position[i] - before[i] for i in range(3)] +print("%-34s dx %.4f dy %.4f dz %.4f" % ("the tip moved", d[0], d[1], d[2])) +if abs(d[0] - 1.0) > 1e-4 or abs(d[1]) > 1e-4 or abs(d[2]) > 1e-4: + error("the tip did not move by X1 alone") +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +# --- the preview reads the codes on the type the machine is on ------------- +# the status buffer says which type that is and what each type is; a GUI +# restates where the machine stands with G53 ahead of the program, in the +# world of the type in force, which the flag leaves alone, and the program +# after it reads G53 as running it would, through the switches it makes. +# AXIS hands the restating codes over as a list, the Gremlin based screens +# as a unit code and an init code. +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.end = None + + # where the last move ends, in the program's terms + def straight_traverse(self, *args): + self.end = args[-9:] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + +def preview(program, listed): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + if listed: + # as AXIS builds them: the tool, the position, the modal codes, and + # the offset in force, which under the tip the kinematics evaluates + axes = [i for i in range(9) if s.axis_mask & (1 << i)] + codes = ["G21", "G90", "T%d M6" % s.tool_in_spindle] + codes += ["G53 G0 %s%.8f" % ("XYZABCUVW"[i], s.position[i]) for i in axes] + codes += ["G%.1f" % (g * .1) for i, g in enumerate(s.gcodes) + if i not in (0, 1, 2, 12) and g != -1] + codes.append("G43.1 " + " ".join("%s%.8f" % ("XYZABCUVW"[i], s.tool_offset[i]) for i in axes)) + result, line = gcode.parse(program, canon, codes, "") + else: + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return line, gcode.strerror(result), None + return None, None, canon.end + +# The program's G53 ends where G53.5 to the same words ends, in each form, +# a list or an init code; line 0 is the codes ahead of the program +def far(a, b): + return a is None or b is None or max(abs(x - y) for x, y in zip(a, b)) > 1e-9 + +def expect_preview(what, program, same): + for listed in (True, False): + form = "list" if listed else "init code" + line, why, end = preview(program, listed) + print("%-34s %s" % ("%s, %s" % (what, form), + "line %d: %s" % (line, why) if why else "accepted")) + if why: + error("%s: the preview refused line %d" % (what, line)) + line, why, want = preview(same, listed) + if why or far(end, want): + error("%s, %s: the preview ends at %s, %s at %s" % (what, form, end, same, want)) + +# The restating G53 is the type in force's, never read in the machine frame: +# a move that goes nowhere ends on the position the machine reports. The +# list restates the tool offset after the position, which moves it. +def expect_restated(what, forms): + s.poll() + want = [v if i in (3, 4, 5) else v / 25.4 for i, v in enumerate(s.position)] + for listed in forms: + line, why, end = preview("here.ngc", listed) + if why or far(end, want): + error("%s, %s: the restated position reads %s, not %s" % + (what, "list" if listed else "init code", end, want)) + +# and running it ends there too: with no tool offset in force the restated +# position after the run is where the program's preview ended +def expect_run(what, program, *lines): + ends = [preview(program, listed)[2] for listed in (True, False)] + here = mdi() + mdi(*lines) + for listed, end in zip((True, False), ends): + there = preview("here.ngc", listed)[2] + if far(end, there): + error("%s, %s: the preview ends at %s, running it at %s" % + (what, "list" if listed else "init code", end, there)) + mdi("G53.7 G0 " + " ".join("J%d=%.9f" % (j, v) for j, v in enumerate(here))) + +mdi("G13.1") +s.poll() +flags = s.kins_type_flags +print("%-34s kins_type %d, flags %s" % ("the status buffer", s.kins_type, flags)) +if s.kins_type != kins_type(): + error("stat.kins_type %d, motion.kins-type %d" % (s.kins_type, kins_type())) +if (flags[0] & (linuxcnc.KINSTYPE_MACHINE | linuxcnc.KINSTYPE_IDENTITY) != linuxcnc.KINSTYPE_MACHINE + or not flags[1] & linuxcnc.KINSTYPE_PRIMARY + or not flags[2] & linuxcnc.KINSTYPE_IDENTITY or flags[-1] != -1): + error("stat.kins_type_flags does not say what slantkins declares") +mdi("G53 G0 X5 Y10 Z0") +expect_preview("G53 on the machine frame type", "g53.ngc", "g535.ngc") +expect_restated("on the machine frame type", (True, False)) +mdi("G43.4 H1", "G0 B30") +expect_type("G43.4 for the preview", 1) +expect_preview("G53 under the tip, tilted", "g53.ngc", "g535.ngc") +expect_preview("G13.1 then G53 under the tip", "g13-g53.ngc", "g13-g535.ngc") +expect_restated("under the tip, tilted", (False,)) +mdi("G49", "G0 B0", "G12.1 P2") +expect_type("G12.1 P2 for the preview", 2) +expect_preview("G53 on the plain identity", "g53.ngc", "g535.ngc") +expect_restated("on the plain identity", (True, False)) +expect_run("G53 on the plain identity", "g53.ngc", "G53 G0 X0") +mdi("G13.1") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-machine-frame/test.ini b/tests/ptp-machine-frame/test.ini new file mode 100644 index 00000000000..0d273a3d2ba --- /dev/null +++ b/tests/ptp-machine-frame/test.ini @@ -0,0 +1,116 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var +# G53, G28, G30 and the stores in the machine frame off the machine frame type +MACHINE_MOVES_IN_MACHINE_FRAME = 1 + +[KINS] +KINEMATICS = slantkins +JOINTS = 5 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-machine-frame/test.sh b/tests/ptp-machine-frame/test.sh new file mode 100755 index 00000000000..48c8eebedaf --- /dev/null +++ b/tests/ptp-machine-frame/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash -e +${SUDO} halcompile --install slantkins.comp >/dev/null +# a failed run leaves the var file behind, and it carries stored positions +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-machine-frame/tool.tbl b/tests/ptp-machine-frame/tool.tbl new file mode 100644 index 00000000000..67f23122e9e --- /dev/null +++ b/tests/ptp-machine-frame/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D6 diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README new file mode 100644 index 00000000000..72b4454251e --- /dev/null +++ b/tests/ptp-robot/README @@ -0,0 +1,8 @@ +The point-to-point moves on a serial robot. + +pumakins declares its arm kinematics the machine frame as well as the +working transform, since its world is the flange in the base frame with +no tool, so G53.5 moves the flange along a base axis and holds the rest, +from the arm kinematics and from the identity alike, and G13.1, G43.4 +and G49 all leave the robot on the arm kinematics. G53.7 names joints +by number, and G53.4 takes a Cartesian destination. diff --git a/tests/ptp-robot/checkresult b/tests/ptp-robot/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-robot/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-robot/sim.hal b/tests/ptp-robot/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-robot/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py new file mode 100755 index 00000000000..375de49eaf2 --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# A serial robot has no axis letter that names the joint it looks like, so +# the letter form of the point-to-point move is the machine frame, the flange +# in the base frame with no tool, and the joint form names the joints. +import hal +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def drain(): + while e.poll(): + pass + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the joint form moves the joints it names and leaves the rest alone +before = mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +after = mdi("G53.7 G0 J1=-20 J4=35") +print("G53.7 G0 J1=-20 J4=35 %s" % " ".join("%.4f" % v for v in after)) +drain() +if abs(after[1] + 20) > 1e-6 or abs(after[4] - 35) > 1e-6: + error("G53.7 left joints 1 and 4 at %.6f and %.6f" % (after[1], after[4])) +for j in (0, 2, 3, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) + +# a wrist reaches the same point with the forearm turned half a revolution +# and the wrist joints reversed, so a joint driven through zero must not +# come back as the other set: the joints not named stay where they are +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +held = mdi("G53.7 G0 J4=90") +for value in (-10, 90, -45): + now = mdi("G53.7 G0 J4=%d" % value) + print("G53.7 G0 J4=%-4d %s" % (value, " ".join("%.4f" % v for v in now))) + drain() + if abs(now[4] - value) > 1e-6: + error("G53.7 J4=%d left joint 4 at %.6f" % (value, now[4])) + for j in (0, 1, 2, 3, 5): + if abs(now[j] - held[j]) > 1e-6: + error("G53.7 J4=%d moved joint %d from %.6f to %.6f" + % (value, j, held[j], now[j])) + +# the letter form is the machine frame: on a robot the arm kinematics with +# no tool, the flange in the base frame, so a letter moves that coordinate +# of the flange and every other one holds, the orientation included +def flange(): + s.poll() + return list(s.position[:6]) + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def check_held(what, before, after, moved): + for i, name in enumerate("XYZABC"): + want = before[i] + moved.get(name, 0.0) + off = after[i] - want + if i >= 3: + # the flange angles come back in (-180, 180] + off = (off + 180.0) % 360.0 - 180.0 + if abs(off) > 1e-3: + error("%s left %s at %.4f, not %.4f" % (what, name, after[i], want)) + +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +after_joints = mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +after = flange() +print("G53.5 G0 Z+50 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z", before, after, {"Z": 50}) +if abs(after_joints[0]) > 1e-6: + error("G53.5 Z turned the waist to %.6f" % after_joints[0]) + +before = flange() +mdi("G53.5 G0 X%.6f A%.6f" % (before[0] - 40, before[3] + 15)) +after = flange() +print("G53.5 G0 X-40 A+15 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 X A", before, after, {"X": -40, "A": 15}) + +# the same move asked from the identity kinematics lands the flange at the +# same place: the letters are the machine frame whatever type is in force, +# and G13.1 comes back to the arm kinematics, which is that frame +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +mdi("G12.1 P1") +if kins_type() != 1: + error("G12.1 P1 left kins-type %d" % kins_type()) +mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +mdi("G13.1") +if kins_type() != 0: + error("G13.1 left kins-type %d, the arm kinematics is type 0" % kins_type()) +after = flange() +print("G53.5 G0 Z+50 from the identity %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z from the identity", before, after, {"Z": 50}) + +# G43.4 and G49 leave a robot on its arm kinematics: there is no other +# frame to come back to +mdi("G43.4 H1") +if kins_type() != 0: + error("G43.4 left kins-type %d" % kins_type()) +mdi("G49") +if kins_type() != 0: + error("G49 left kins-type %d" % kins_type()) +drain() + +# a letter of the wrong unit class is not a joint any more: A is the flange +# roll, in degrees, and Z is a length, so neither is refused +mdi("G53.5 G0 A0") +drain() + +# and the code that takes a Cartesian target still works: the point the +# robot is standing on is reachable by definition, so ask for it +s.poll() +here = list(s.position[:3]) +mdi("G53.7 G0 J1=0 J4=0") +# a serial robot reaches one point with more than one set of joints, so +# only the point is checked, not the pose it comes back in +back = mdi("G53.4 G0 X%.6f Y%.6f Z%.6f" % (here[0], here[1], here[2])) +drain() +s.poll() +if max(abs(a - b) for a, b in zip(s.position[:3], here)) > 1e-3: + error("G53.4 landed at %s, not at %s" + % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) +print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-robot/test.ini b/tests/ptp-robot/test.ini new file mode 100644 index 00000000000..4570f1389e8 --- /dev/null +++ b/tests/ptp-robot/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = pumakins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp pumakins.A2 300 +HALCMD = setp pumakins.A3 50 +HALCMD = setp pumakins.D3 70 +HALCMD = setp pumakins.D4 400 +HALCMD = setp pumakins.D6 80 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-robot/test.sh b/tests/ptp-robot/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-robot/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-robot/tool.tbl b/tests/ptp-robot/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-robot/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 2f33b4bbe08..206bc4d96af 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/README b/tests/twp-native/README new file mode 100644 index 00000000000..3280aceebea --- /dev/null +++ b/tests/twp-native/README @@ -0,0 +1,23 @@ +The tilted work plane on the xyzacb nutating-head sim, natively. + +G12.1 P1 selects the TCP kinematics, G68.2 defines a plane, and the +orientation moves are checked against the python maths in +tests/kins-twp/xyzacb: G53.1 lands the head on one of the oracle's +angle pairs and leaves the linear joints where they were all through the +move; G53.6 leaves the tool tip where it was; G53.3 ends at the point +asked for in the plane; a move along plane X goes along plane X in the +world; G68.3 reads the plane back off the oriented tool; G69 cancels. +G53.6 standing near the end of C's travel reaches the plane by the nearest pose inside it, never running C past its limit. Q1 lets the table take part. The point-to-point moves are checked too: +G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with +the head tilted, G28.5 and G30.5 to a slide position stored on the identity +kinematics, through a slide waypoint when given one, G53.4 G1 taking the +time the straight move would in G94 and in G93, and what they refuse. The +config sets MACHINE_MOVES_IN_MACHINE_FRAME, so under TCP G53, G28 and G30 land +where G53.5, G28.5 and G30.5 do, G28.1 and G30.1 storing the slides, and on +the identity kinematics they are the world's. + +The path of a move is read from a sampler on the servo thread, every cycle +of the joint commands and the world position, logged by halsampler to +samples.log while the test runs: a status snapshot per task cycle misses the +turn of a move that stops and comes back. The log is left behind when a +check fails. diff --git a/tests/twp-native/abort.ngc b/tests/twp-native/abort.ngc new file mode 100644 index 00000000000..217d67e407a --- /dev/null +++ b/tests/twp-native/abort.ngc @@ -0,0 +1,11 @@ +(a plane, then a long move to be stopped in the middle) +(the cancel at the end is one the read ahead reaches long before the machine does) +G21 G90 G94 +G12.1 P1 +G0 X0 Y0 Z0 A0 B0 C0 +G68.2 P1 Q123 I30 J20 K0 +G53.1 +G1 F300 Z-40 +G1 Z0 +G69 +M2 diff --git a/tests/twp-native/checkresult b/tests/twp-native/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-native/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal new file mode 100644 index 00000000000..792895c2bed --- /dev/null +++ b/tests/twp-native/sim.hal @@ -0,0 +1,32 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# every servo cycle of the joints and the world position, logged for the +# test to read the path of a move, not a task-cycle snapshot of it +loadrt sampler depth=4000 cfg=fffffffff +addf sampler.0 servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net J0 => sampler.0.pin.0 +net J1 => sampler.0.pin.1 +net J2 => sampler.0.pin.2 +net J3 => sampler.0.pin.3 +net J4 => sampler.0.pin.4 +net J5 => sampler.0.pin.5 +net Xcmd axis.x.pos-cmd => sampler.0.pin.6 +net Ycmd axis.y.pos-cmd => sampler.0.pin.7 +net Zcmd axis.z.pos-cmd => sampler.0.pin.8 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py new file mode 100755 index 00000000000..ae5acc6b8ca --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,706 @@ +#!/usr/bin/env python3 +# The tilted work plane on the xyzacb nutating-head sim, natively: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math +import numpy as np + +TOPDIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")) +sys.path.insert(0, os.path.join(TOPDIR, "tests", "kins-twp", "xyzacb")) +import remap_funcs_twp as twp + +JOINTS = 6 +TABLE, SECONDARY, PRIMARY = 3, 4, 5 # A, B, C +I4 = np.asmatrix(np.identity(4)) + +class Log: + def debug(self, *a): pass + def error(self, *a): print("oracle:", a) +log = Log() + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +# the path of a move is read from the sampler log, every servo cycle of the +# joint commands and the world position, which halsampler writes to +# samples.log as it runs; status would give a snapshot per task cycle, which +# misses the turn of a move that stops and comes back +LOG = "samples.log" + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: # the last piece is a line still being written + v = line.split() + if len(v) != 1 + JOINTS + 3: + continue # an "overrun" line, counted from the pin at the end + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +# run one command and return the settled joints and the log of the way there: +# the log is block buffered, so it is read until it has caught up with the +# machine at rest +def sampled(cmd): + n0 = len(log_samples()) + c.mdi(cmd) + c.wait_complete(60) + end = settled() + deadline = time.time() + 10 + while True: + samples = log_samples() + if len(samples) > n0 and close(samples[-1][0], end, 2e-6): + break + if time.time() > deadline: + error("%s: the sampler log did not catch up with the machine" % cmd) + break + time.sleep(0.02) + return end, samples[n0:] + +# a point-to-point move runs every joint on a straight line in joint space, +# all together: the fraction of the way each moving joint has gone is the +# same for all of them at every sample, never goes back, and reaches one; +# the log prints six decimals, which on a short move is 1e-5 of the way +def joint_line(what, samples, start, end): + moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] + if not moving: + error("%s: no joint moved" % what) + return + last = 0.0 + for n, (j, p) in enumerate(samples): + fs = [(j[i] - start[i]) / (end[i] - start[i]) for i in moving] + f = sum(fs) / len(fs) + if max(abs(x - f) for x in fs) > 1e-5: + error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) + return + if f < last - 1e-6: + error("%s: the joint path ran backwards at sample %d of %d (%.4f after %.4f)" % (what, n, len(samples), f, last)) + return + if f < -1e-6 or f > 1 + 1e-6: + error("%s: a joint left its segment at sample %d of %d (fraction %.4f)" % (what, n, len(samples), f)) + return + last = f + if last < 1 - 1e-6: + error("%s: the last sample is short of the end (fraction %.4f)" % (what, last)) + print("%s: %d joints on one line in joint space through %d samples" % (what, len(moving), len(samples))) + +def show(what, j): + print("%-26s %s" % (what, " ".join("%.4f" % v for v in j))) + +def wrap(d): + return (d + 180.0) % 360.0 - 180.0 + +def close(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +# the pairs the oracle would keep for a tool axis, in machine coordinates, +# as (b, c) in degrees, and which is nearest to the head's present angles +def oracle_pairs(z): + t1s, t2s = twp.kins_calc_possible_joint_angles(log, list(z), None) + pairs = [] + for t1 in t1s or []: + for t2 in t2s or []: + m = twp.kins_calc_transformation_matrix(t1, t2, 0, I4, 'inv') + got = [m[0, 2], m[1, 2], m[2, 2]] + if close(got, z, 1e-6): + pairs.append((math.degrees(t2), math.degrees(t1))) + return pairs + +def nearest_pair(pairs, b_now, c_now): + return min(pairs, key=lambda p: abs(wrap(p[0] - b_now)) + abs(wrap(p[1] - c_now))) + +def tool_axis(joints): + # the tool axis in work coordinates: the head as the oracle models it, + # brought into the table's frame the way the module reports the work + m = twp.kins_calc_transformation_matrix(math.radians(joints[PRIMARY]), + math.radians(joints[SECONDARY]), 0, I4, 'inv') + zm = np.array([m[0, 2], m[1, 2], m[2, 2]]) + a = math.radians(joints[TABLE]) + W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) + return W.T.dot(zm) + +def plane_axes(): + s.poll() + r = s.g68_rotation + return ([r[0], r[3], r[6]], [r[1], r[4], r[7]], [r[2], r[5], r[8]]) + +def rot_x(d): + r = math.radians(d) + return np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]]) + +def rot_y(d): + r = math.radians(d) + return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) + +# --- the plane, and G53.1 with the table held --------------------------- +# no G53.2 has run yet, so the pose parameters must refuse to be read +c.mdi("G0 X#<_orient_x>") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading #<_orient_x> before the first G53.2 was accepted") +drain() +start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") +show("start", start) +R = rot_y(20).dot(rot_x(30)) +mdi("G68.2 P1 Q123 I30 J20 K0") +after, samples = sampled("G53.1") +show("G53.1", after) +drain() +s.poll() +if not s.g68_active: + error("G68.2 did not leave a plane active") +if not close(plane_axes()[2], list(R[:, 2]), 1e-9): + error("status reports a different plane normal than the program defined") +pairs = oracle_pairs(list(R[:, 2])) +print("oracle pairs (b, c):", ["(%.4f, %.4f)" % p for p in pairs]) +want = nearest_pair(pairs, start[SECONDARY], start[PRIMARY]) +if not pairs or abs(wrap(after[SECONDARY] - want[0])) > 1e-3 or abs(wrap(after[PRIMARY] - want[1])) > 1e-3: + error("G53.1 landed on (%.4f, %.4f), the oracle's nearest pair is (%.4f, %.4f)" + % (after[SECONDARY], after[PRIMARY], want[0], want[1])) +if abs(after[TABLE] - start[TABLE]) > 1e-9: + error("G53.1 moved the table with Q0") +worst = 0.0 +where = (0, 0, start[0], len(samples)) +for n, smp in enumerate(samples): + for i in range(3): + d = abs(smp[0][i] - start[i]) + if d > worst: + worst = d + where = (n, i, smp[0][i], len(samples)) +print("linear joints moved at most %.9f through G53.1 (sample %d of %d, joint %d at %.9f); ended at %s" + % ((worst,) + (where[0], where[3], where[1], where[2]) + (" ".join("%.9f" % v for v in after[:3]),))) +if worst > 1e-6: + error("G53.1 moved a linear joint") +if not close(tool_axis(after), list(R[:, 2]), 1e-6): + error("the tool axis after G53.1 is not the plane normal") +joint_line("G53.1", samples, start, after) + +# P names the pose rather than its rank: P1 is the one with the secondary +# rotary positive and P2 the one with it negative, from wherever the +# machine is standing, while no P is the nearest and so does depend on it +poses = {} +for where in ("G0 A0 B0 C0", "G0 A0 B-40 C170"): + for word in ("", "P1", "P2"): + mdi("G69") + mdi(where) + mdi("G68.2 P1 Q123 I30 J20 K0") + got = mdi("G53.1 %s" % word) + poses.setdefault(word, []).append(got) + drain() +for word, sign in (("P1", 1), ("P2", -1)): + a, b = poses[word] + rot = lambda j: [j[TABLE], j[SECONDARY], j[PRIMARY]] + if max(abs(wrap(x - y)) for x, y in zip(rot(a), rot(b))) > 1e-4: + error("G53.1 %s landed differently from two starting poses: %s and %s" + % (word, rot(a), rot(b))) + if sign * a[SECONDARY] <= 0: + error("G53.1 %s put the secondary rotary at %.4f" % (word, a[SECONDARY])) +if abs(poses["P1"][0][SECONDARY] - poses["P2"][0][SECONDARY]) < 1e-6: + error("G53.1 P1 and P2 chose the same pose") +if abs(poses[""][0][SECONDARY] - poses[""][1][SECONDARY]) < 1e-6: + error("G53.1 with no P gave the same pose from both starts, so it is not the nearest") +print("G53.1 P1 %.4f, P2 %.4f, no P %.4f then %.4f (secondary rotary)" + % (poses["P1"][0][SECONDARY], poses["P2"][0][SECONDARY], + poses[""][0][SECONDARY], poses[""][1][SECONDARY])) +c.mdi("G53.1 P3") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("G53.1 P3 was accepted") +drain() +mdi("G69") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +mdi("G68.2 P1 Q123 I30 J20 K0") +after = mdi("G53.1") + +# --- moves in the plane go along the plane's axes in the world ---------- +# G53.1 swung the tool tip, so it is somewhere in the plane; a move to X10 +# travels along the plane's X by the difference +def in_plane(): + s.poll() + return R.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])), list(s.position[:3]) +q0, p0 = in_plane() +mdi("G0 X10") +q1, p1 = in_plane() +d = [p1[i] - p0[i] for i in range(3)] +want = list((10 - q0[0]) * R[:, 0]) +if not close(d, want, 1e-3) or abs(q1[0] - 10) > 1e-3: + error("G0 X10 in the plane moved the tool by %s, expected %s" % (d, want)) +mdi("G0 Z5") +q2, p2 = in_plane() +d = [p2[i] - p1[i] for i in range(3)] +want = list((5 - q1[2]) * R[:, 2]) +if not close(d, want, 1e-3) or abs(q2[2] - 5) > 1e-3: + error("G0 Z5 in the plane moved the tool by %s, expected %s" % (d, want)) + +# --- G53.6 keeps the tool centre point -------------------------------- +mdi("G69") +R2 = rot_y(20).dot(rot_x(-30)) +mdi("G68.2 P1 Q123 I-30 J20 K0") +s.poll(); before = list(s.position) +after, samples = sampled("G53.6") +show("G53.6", after) +drain() +worst = max(abs(smp[1][i] - before[i]) for smp in samples for i in range(3)) +print("tool tip moved at most %.6f through G53.6" % worst) +if worst > 1e-3: + error("G53.6 moved the tool tip") +if not close(tool_axis(after), list(R2[:, 2]), 1e-6): + error("the tool axis after G53.6 is not the plane normal") + +# --- G53.2 solves without moving, the pose lands on the parameters ---- +# same plane as G53.6 above, but from a pose that is not the answer: +# G53.2 must not move, and the pose it publishes must put the tool on the +# plane normal, which is where G53.6 already stands, so the nearest +# solution is the present rotary position +stay = after +after2, samples = sampled("G53.2") +drain() +if not close(after2, stay, 1e-9): + error("G53.2 moved the machine: %s became %s" % (stay, after2)) +# read the pose back through the parameters: a move to the published +# rotary words is a move to the present B and C, with the table held +before3 = mdi("G0 B0 C0") +after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") +show("G0 to #<_orient_b/c>", after3) +drain() +if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): + error("the tool axis at the pose G53.2 published is not the plane normal") +# the numbered parameters carry the same pose +before4 = mdi("G0 B0 C0") +after4, samples = sampled("G0 B#5075 C#5076") +show("G0 to #5075/#5076", after4) +drain() +if abs(wrap(after4[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after4[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#5075 #5076 held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after4[SECONDARY], after4[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +c.mdi("#5075 = 0") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("writing #5075 was accepted; the G53.2 pose is not read-only") +drain() +c.mdi("G0 A#<_orient_a>") +c.wait_complete(30) +m = e.poll() +if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("#<_orient_a> after G53.2: %s" % m[1]) +drain() + +# --- the orientation stays inside the rotary travel -------------------- +# the primary rotary C travels -320 to 320: standing at C300, a plane whose +# nearest pose puts C on the turn past 320 is reached the long way round or +# by the other pose, whichever is nearer inside the travel, and never by +# running C out of it +C_MIN, C_MAX, B_MIN, B_MAX = -320.0, 320.0, -185.0, 185.0 + +def turn_near(v, now, lo=None, hi=None): + turns = math.floor((now - v) / 360.0 + 0.5) + if lo is not None: + turns = min(max(turns, math.ceil((lo - v) / 360.0)), math.floor((hi - v) / 360.0)) + return v + 360.0 * turns + +def pose_near(pairs, b_now, c_now, limited): + best = None + for b, cc in pairs: + if limited: + bb, cc = turn_near(b, b_now, B_MIN, B_MAX), turn_near(cc, c_now, C_MIN, C_MAX) + else: + bb, cc = turn_near(b, b_now), turn_near(cc, c_now) + d = abs(bb - b_now) + abs(cc - c_now) + if best is None or d < best[0]: + best = (d, bb, cc) + return best[1], best[2] + +mdi("G69") +stand = mdi("G0 A0 B0 C300") +plane = None +for i in range(-60, 61, 5): + for j in range(-60, 61, 5): + z = list(rot_y(j).dot(rot_x(i))[:, 2]) + pairs = oracle_pairs(z) + if pairs and pose_near(pairs, stand[SECONDARY], stand[PRIMARY], False)[1] > C_MAX: + plane = (i, j, z, pose_near(pairs, stand[SECONDARY], stand[PRIMARY], True)) + break + if plane: + break +if not plane: + error("no plane on the grid puts the nearest C past the travel") +else: + i, j, z, want = plane + mdi("G68.2 P1 Q123 I%d J%d K0" % (i, j)) + after, samples = sampled("G53.6") + show("G53.6 at the C limit", after) + drain() + top = max(smp[0][PRIMARY] for smp in samples) + bottom = min(smp[0][PRIMARY] for smp in samples) + print("plane I%d J%d from C300: C went %.4f to %.4f, the nearest pose inside the travel is B %.4f C %.4f" + % (i, j, bottom, top, want[0], want[1])) + if top > C_MAX + 1e-6 or bottom < C_MIN - 1e-6: + error("G53.6 ran C out of its travel") + if abs(after[SECONDARY] - want[0]) > 1e-3 or abs(after[PRIMARY] - want[1]) > 1e-3: + error("G53.6 ended at B %.4f C %.4f, not the nearest pose inside the travel" + % (after[SECONDARY], after[PRIMARY])) + if not close(tool_axis(after), z, 1e-6): + error("the tool axis after G53.6 at the C limit is not the plane normal") + +# --- G53.3 goes to a point in the plane with the tool oriented ---------- +before = mdi("G69") +R3 = rot_y(-25).dot(rot_x(35)) +mdi("G68.2 P1 Q123 I35 J-25 K0") +after, samples = sampled("G53.3 X5 Y5 Z5") +show("G53.3", after) +drain() +joint_line("G53.3", samples, before, after) +s.poll() +prog = R3.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])) +if not close(list(prog), [5, 5, 5], 1e-3): + error("G53.3 ended at %s in the plane, not 5 5 5" % list(prog)) +if not close(tool_axis(after), list(R3[:, 2]), 1e-6): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool --------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +drain() +s.poll() +x, y, z = plane_axes() +if not s.g68_active or not close(list(s.g68_offset[:3]), [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(z, list(R3[:, 2]), 1e-6): + error("G68.3's normal %s is not the tool axis %s" % (z, list(R3[:, 2]))) +if abs(x[2]) > 1e-6: + error("G68.3's X is not parallel to the machine XY plane: %s" % x) +mdi("G69", "G68.3 R90") +xr, yr, zr = plane_axes() +if not close(xr, y, 1e-6): + error("G68.3 R90 did not turn the plane about its normal") + +# the words a plane code does not use are refused, not ignored +for cmd in ("G68.3 I1 J0 K0", "G68.3 P1", "G68.3 Q123", "G69 R45", "G69 I1", "G69 P1"): + mdi("G69") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + +# --- Q1 lets the table take part --------------------------------------- +mdi("G69", "G0 X0 Y0 Z0 A0 B0 C0") +R4 = rot_x(30) +mdi("G68.2 P1 Q123 I30 J0 K0") +after = mdi("G53.1 Q1") +show("G53.1 Q1", after) +drain() +if not close(tool_axis(after), list(R4[:, 2]), 1e-6): + error("the tool axis after G53.1 Q1 is not the plane normal") +# with the plane X requested as well and the table free, three joints +# place three constraints: the plane's X is reached by the machine rather +# than by the frame +m = twp.kins_calc_transformation_matrix(math.radians(after[PRIMARY]), + math.radians(after[SECONDARY]), 0, I4, 'inv') +a = math.radians(after[TABLE]) +W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) +xm = np.array([m[0, 0], m[1, 0], m[2, 0]]) +if not close(list(W.T.dot(xm)), list(R4[:, 0]), 1e-6): + error("with Q1 the machine did not place the plane's X") + +# --- G53.4, G53.5 and G69 ------------------------------------------------ +before = mdi("G69") +after, samples = sampled("G53.4 G0 X0 Y0 Z0 A0 B0 C0") +show("G53.4 G0", after) +drain() +joint_line("G53.4 G0", samples, before, after) +s.poll() +if s.g68_active: + error("G69 left the plane active") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("G69 left a rotation in status") +if not close(list(s.position[:3]), [0, 0, 0], 1e-6) or any(abs(after[j]) > 1e-6 for j in (TABLE, SECONDARY, PRIMARY)): + error("G53.4 G0 did not bring the tool and the rotaries back to zero") + +# G53.7 takes joint values by joint number: with the head tilted, J2=-5 +# puts joint 2 at -5 whatever that does to the tool tip, and touches no +# other joint; the value is the joint's own, untouched by G20 +before = mdi("G0 B30") +show("before G53.7", before) +after = mdi("G20 G53.7 G0 J2=-5") +mdi("G21") +show("G53.7 G0 J2=-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.7 J2=-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 J2=-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G53.7 G0 J2=0 J[2+2]=0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.7 J2=0 J4=0 did not put joints 2 and 4 at zero") + +# G53.5 takes the same destination by axis letter, in program units: Z-5 +# is joint 2 in millimetres, and under G20 the same words are inches +before = mdi("G0 B30") +after = mdi("G53.5 G0 Z-5") +show("G53.5 G0 Z-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.5 Z-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.5 Z-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G20 G53.5 G0 Z-1") +mdi("G21") +show("G20 G53.5 G0 Z-1", after) +if abs(after[2] + 25.4) > 1e-6: + error("an inch of G53.5 Z left joint 2 at %.6f, not -25.4" % after[2]) +after = mdi("G53.5 G0 Z0 B0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") + +# --- G28.5 and G30.5 ------------------------------------------------------ +# the stored positions are saved on the identity kinematics, where the world +# is the slides: G28.1 at X10 Y20 Z-5 with the head straight, G30.1 at the +# same slides with the head at B10 +mdi("G12.1 P0", "G0 X10 Y20 Z-5 A0 B0 C0", "G28.1", "G0 B10", "G30.1", + "G0 X0 Y0 Z0 B0", "G12.1 P1") +# with the head tilted under TCP, G28.5 alone takes every slide to the +# stored position, on one joint-space move +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G28.5") +show("G28.5", after) +drain() +joint_line("G28.5", samples, before, after) +if not close(after, [10, 20, -5, 0, 0, 0], 1e-6): + error("G28.5 put the joints at %s, not at the stored slides" % (after,)) +# an axis word is a slide to pass through first, and then only that letter +# goes to its stored value: Z3 on the way, then Z-5, the rest untouched +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G30.5 Z3") +show("G30.5 Z3", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G30.5 Z3 left joint 2 at %.6f, not at the stored -5" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G30.5 Z3 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# the first move ends on Z3 and the second starts there inside the same +# servo cycle, so the turn falls between two samples: joint 2 decelerates +# into it at 700 mm/s^2, which over one 1 ms cycle is 3.5e-4 short of it, +# and it never goes past it +peak = max(j[2] for j, p in samples) +if peak > 3 + 1e-6 or peak < 3 - 1e-3: + error("G30.5 Z3 did not pass through the Z3 slide (joint 2 peaked at %.6f)" % peak) +after = mdi("G30.5") +if not close(after, [10, 20, -5, 0, 10, 0], 1e-6): + error("G30.5 put the joints at %s, not at the stored slides with B10" % (after,)) +mdi("G0 X0 Y0 Z0 A0 B0 C0") + +# a point-to-point feed takes the time the straight move would: 10 mm at +# F600 is one second, and F30 in G93 is two +def timed(cmd): + c.mdi(cmd) + first = last = None + t0 = time.time() + s.poll(); start = [s.joint_position[i] for i in range(JOINTS)] + while time.time() - t0 < 60: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if now != start: + if first is None: + first = time.time() + last = time.time() + start = now + elif first is not None and s.inpos and not s.queue and time.time() - last > 0.3: + break + time.sleep(0.005) + c.wait_complete(60) + settled() + return (last - first) if first else 0.0 +mdi("G0 X0 Y0 Z0 A0 B0 C0") +took = timed("G53.4 G1 X10 F600") +print("G53.4 G1 X10 F600 took %.3f s" % took) +if not 0.7 < took < 1.5: + error("a 10 mm point-to-point feed at F600 took %.3f s, not about one" % took) +took = timed("G93 G53.4 G1 X0 F30") +mdi("G94") +print("G93 G53.4 G1 X0 F30 took %.3f s" % took) +if not 1.6 < took < 2.6: + error("a point-to-point feed at G93 F30 took %.3f s, not about two" % took) +drain() + +# what the point-to-point codes refuse +for cmd in ("G53.4 G2 X1 I1", "G91 G53.7 G0 J0=1", "G53.7 G0 J9=1", "G53.7 G0 X1", + "G53.7 G0 J1", "G53.7 G0", "G0 J0=1", "G53.7 G0 J0.5=1", "G53.7 G0 J0=1 J0=2", + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1", + "G91 G28.5 X1", "G30.5 G1 X1"): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) +mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") + +# The config sets MACHINE_MOVES_IN_MACHINE_FRAME: under TCP G53 is G53.5, the +# slides at the words whatever the head is doing, on one joint-space move, +# where the tip's G53 would move every slide to keep the tip on machine Z +before = mdi("G12.1 P1", "G0 B30") +after, samples = sampled("G53 G0 Z-5") +show("G53 G0 Z-5 under TCP", after) +drain() +joint_line("G53 G0 Z-5", samples, before, after) +if abs(after[2] + 5) > 1e-6: + error("G53 Z-5 under TCP left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53 Z-5 under TCP moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# G28.1 and G30.1 store where the slides stand with the head tilted, and G28 +# and G30 go back to them after a plain move has taken the slides along +mdi("G53.5 G0 X10 Y20 Z-5 B-30", "G28.1", "G30.1") +here = mdi("G0 X0 Y0 Z0 B0") +after, samples = sampled("G28") +show("G28 under TCP", after) +drain() +joint_line("G28", samples, here, after) +if not close(after, [10, 20, -5, 0, -30, 0], 1e-6): + error("G28 under TCP put the joints at %s, not at the slides G28.1 stored" % (after,)) +before = mdi("G0 X0 Y0 Z0 B0") +after = mdi("G30 Z3") +if abs(after[2] + 5) > 1e-6: + error("G30 Z3 under TCP left joint 2 at %.6f, not at the stored -5" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G30 Z3 under TCP moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# on the identity kinematics the world is the slides and G53 its straight line +mdi("G0 X0 Y0 Z0 B0", "G12.1 P0") +before = mdi("G53 G0 X10 Y0 Z-5") +if not close(before, [10, 0, -5, 0, 0, 0], 1e-6): + error("G53 X10 Z-5 on the identity put the joints at %s" % (before,)) +mdi("G28.1", "G0 X0 Z0", "G28", "G12.1 P1") +if not close(settled(), [10, 0, -5, 0, 0, 0], 1e-6): + error("G28 on the identity did not go back to the G28.1 store") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + +# --- a plane refuses what would move the ground under it --------------- +# each refusal is an interpreter error, and the abort that follows cancels +# the plane, so it is defined afresh before every one +for cmd in ("G92 X1", "G52 A1", "G10 L2 P1 X1"): + mdi("G68.2 P1 Q123 I30 J0 K0") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted while a plane is active" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + s.poll() + if s.g68_active: + error("the abort after %s left the plane active" % cmd) +mdi("G69") + +# --- stopping a program that has a plane --------------------------------- +# The read ahead runs the program's own G69 long before the machine gets +# there, so the cancel is sitting in the queue when the stop button throws +# the queue away. The plane in status has to end up cancelled all the same, +# and a G69 typed afterwards has to be able to say so again. +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete(30) +c.program_open("abort.ngc") +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if s.g68_active and s.current_line >= 8: + break + time.sleep(0.02) +s.poll() +if not s.g68_active: + error("the program never reported a plane to stop in the middle of") +c.abort() +c.wait_complete(30) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() +s.poll() +if s.g68_active: + error("stopping the program left the plane active in status") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("stopping the program left a rotation in status") +mdi("G69") +s.poll() +if s.g68_active: + error("G69 after the stop did not clear the plane") +print("a stopped program leaves no plane behind") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini new file mode 100644 index 00000000000..a8060670ed6 --- /dev/null +++ b/tests/twp-native/test.ini @@ -0,0 +1,149 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var +# G53, G28, G30 and the stores in the machine frame off the machine frame type +MACHINE_MOVES_IN_MACHINE_FRAME = 1 + +[KINS] +KINEMATICS = xyzacb_trsrn +JOINTS = 6 + +# what the python oracle reads +[TWP] +PRIMARY = C +SECONDARY = B + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzacb_trsrn_kins.nut-angle 45 +HALCMD = setp xyzacb_trsrn_kins.y-pivot 100 +HALCMD = setp xyzacb_trsrn_kins.z-pivot 200 +HALCMD = setp xyzacb_trsrn_kins.x-offset 5 +HALCMD = setp xyzacb_trsrn_kins.y-offset 7 +HALCMD = setp xyzacb_trsrn_kins.y-rot-axis 300 +HALCMD = setp xyzacb_trsrn_kins.z-rot-axis 400 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -320 +MAX_LIMIT = 320 +HOME_SEQUENCE = 0 diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/twp-native/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/twp-native/tool.tbl b/tests/twp-native/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-native/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/twp-table/README b/tests/twp-table/README new file mode 100644 index 00000000000..f9b61239e6e --- /dev/null +++ b/tests/twp-table/README @@ -0,0 +1,12 @@ +G53.1 on a table-table machine, xyzac-trt-kins: both rotaries carry the +work, so the pose is named by the sign of the one that carries the other, +A here, as Heidenhain's SEQ names its master axis. For each plane, from +several starting poses: G53.1 with no P takes the nearer of the two poses, +P1 always the one with A positive and P2 the one with A negative, the same +pose from every start; each leaves the tool along the plane's normal; a +move in the plane after the turn ends where the plane puts it. + +Then again with A tilting one way only, [AXIS_A] 0 to 100 while [JOINT_3] +runs on to 9999: G53.1 to each side plane goes to A90 with C carrying the +side round, never to A270, a whole turn the joint has travel for and the +axis has not. diff --git a/tests/twp-table/checkresult b/tests/twp-table/checkresult new file mode 100755 index 00000000000..a9e60354d4f --- /dev/null +++ b/tests/twp-table/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures, once per configuration +[ "$(grep -c "^Exiting with 0 errors" "$1")" = 2 ] diff --git a/tests/twp-table/sim.hal b/tests/twp-table/sim.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/twp-table/sim.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-table/test-ui.py b/tests/twp-table/test-ui.py new file mode 100755 index 00000000000..3f4c6f95207 --- /dev/null +++ b/tests/twp-table/test-ui.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +# G53.1 P1 and P2 on the xyzac-trt sim: see README. + +import linuxcnc +import sys +import os +import time +import math + +JOINTS = 5 +A, C = 3, 4 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + j = settled() + drain() + return j + +# the tool axis in the work frame, conventional-directions 0: the table +# turns C then A, so the tool is Rz(-C) Rx(-A) along Z seen from the part +def tool_axis(j): + a, cc = math.radians(j[A]), math.radians(j[C]) + return (math.sin(cc)*math.sin(a), math.cos(cc)*math.sin(a), math.cos(a)) + +# the plane's Z for G68.2 I J with K0, Euler Z X Z +def normal(i, j): + i, j = math.radians(i), math.radians(j) + return (math.sin(i)*math.sin(j), -math.cos(i)*math.sin(j), math.cos(j)) + +def angle(u, v): + return math.degrees(math.acos(max(-1.0, min(1.0, sum(p*q for p, q in zip(u, v)))))) + +def travel(j, start): + return abs(j[A] - start[0]) + abs(j[C] - start[1]) + +mdi("G12.1 P1", "G0 X0 Y0 Z60 A0 C0") + +# half.ini: A tilts one way only, its [AXIS_A] travel 0 to 100 while +# [JOINT_3] runs on to 9999, so a pose turned whole turns along to fit the +# joint must still fit the axis: each side plane goes to A90, C carrying it +one_sided = linuxcnc.ini(os.environ["INI_FILE_NAME"]).find("AXIS_A", "MIN_LIMIT") == "0" +if one_sided: + for i in (0, 90, -90, 180): + mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J90 K0" % i) + j = mdi("G53.1") + off = angle(tool_axis(j), normal(i, 90)) + if off > 1e-6 or abs(j[A] - 90.0) > 1e-6: + error("G53.1 on I%g J90 went to A%.3f C%.3f, %.6f degrees off the normal" + % (i, j[A], j[C], off)) + print("I%g J90 G53.1 on the one-sided A: A%.3f C%.3f" % (i, j[A], j[C])) + +planes = [] if one_sided else [(0, 30), (60, 30), (-90, 45), (45, -30), (150, 20)] +starts = [(0, 0), (-40, 200), (40, -100)] +for i, jj in planes: + nearest = [] + for word, sign in (("", 0), ("P1", 1), ("P2", -1)): + landed = [] + for start in starts: + mdi("G69", "G0 X0 Y0 Z60 A%g C%g" % start, + "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj)) + j = mdi("G53.1 %s" % word) + off = angle(tool_axis(j), normal(i, jj)) + if off > 1e-6: + error("G53.1 %s on I%g J%g from A%g C%g left the tool %.6f degrees off the normal" + % (word, i, jj, start[0], start[1], off)) + if sign and j[A] * sign <= 0: + error("G53.1 %s on I%g J%g from A%g C%g put A at %.4f" + % (word, i, jj, start[0], start[1], j[A])) + landed.append(j) + if not sign: + nearest.append((j, start)) + if sign: + # one pose, C on whichever turn is nearest the start + for j in landed[1:]: + d = (j[C] - landed[0][C]) % 360.0 + if abs(j[A] - landed[0][A]) > 1e-6 or min(d, 360.0 - d) > 1e-6: + error("G53.1 %s on I%g J%g landed on different poses: %s and %s" + % (word, i, jj, landed[0], j)) + print("I%g J%g G53.1 %s: %s" % (i, jj, word, + ", ".join("A%.3f C%.3f" % (j[A], j[C]) for j in landed))) + # no P: the nearer of the two poses, by rotary travel + for (j, start) in nearest: + mdi("G69", "G0 X0 Y0 Z60 A%g C%g" % start, "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj)) + other = mdi("G53.1 %s" % ("P2" if j[A] > 0 else "P1")) + if travel(other, start) < travel(j, start) - 1e-6: + error("G53.1 on I%g J%g from A%g C%g went to A%.3f C%.3f, A%.3f C%.3f is nearer" + % (i, jj, start[0], start[1], j[A], j[C], other[A], other[C])) + +# after the turn, a move in the plane ends where the plane puts it +turns = () if one_sided else ((60, 30, "P1"), (60, 30, "P2"), (-90, 45, "P1")) +for i, jj, word in turns: + mdi("G69", "G0 X0 Y0 Z60 A0 C0", "G68.2 X0 Y0 Z0 I%g J%g K0" % (i, jj), + "G53.1 %s" % word, "G0 X10 Y5 Z20", "G1 X-10 F2000") + s.poll() + ri, rj = math.radians(i), math.radians(jj) + px, py, pz = -10.0, 5.0, 20.0 + y1 = py*math.cos(rj) - pz*math.sin(rj) + z1 = py*math.sin(rj) + pz*math.cos(rj) + want = (px*math.cos(ri) - y1*math.sin(ri), px*math.sin(ri) + y1*math.cos(ri), z1) + got = s.position[:3] + if max(abs(p - q) for p, q in zip(got, want)) > 1e-6: + error("after G53.1 %s on I%g J%g the tip is at %s, the plane puts it at %s" + % (word, i, jj, got, want)) + +mdi("G69", "G0 X0 Y0 Z60 A0 C0") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-table/test.ini b/tests/twp-table/test.ini new file mode 100644 index 00000000000..44fb0768e9f --- /dev/null +++ b/tests/twp-table/test.ini @@ -0,0 +1,122 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzac-trt-kins.x-rot-point 100 +HALCMD = setp xyzac-trt-kins.y-rot-point -50 +HALCMD = setp xyzac-trt-kins.z-rot-point -80 + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -9999 +MAX_LIMIT = 9999 +HOME_SEQUENCE = 0 diff --git a/tests/twp-table/test.sh b/tests/twp-table/test.sh new file mode 100755 index 00000000000..1a5d8c386c2 --- /dev/null +++ b/tests/twp-table/test.sh @@ -0,0 +1,11 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini +# A tilting one way only, its joint left free to turn on +sed -e '/^\[AXIS_A\]/,/^\[/s/^MIN_LIMIT = .*/MIN_LIMIT = 0/' -e '/^\[AXIS_A\]/,/^\[/s/^MAX_LIMIT = .*/MAX_LIMIT = 100/' \ + -e '/^\[JOINT_3\]/,/^\[/s/^MIN_LIMIT = .*/MIN_LIMIT = 0/' -e '/^\[JOINT_3\]/,/^\[/s/^MAX_LIMIT = .*/MAX_LIMIT = 9999/' \ + test.ini > half.ini +rm -f sim.var sim.var.bak +linuxcnc -r half.ini +rm -f half.ini diff --git a/tests/twp-table/tool.tbl b/tests/twp-table/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-table/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6