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..3439a71fae7 --- /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_rot2> c#<_orient_rot1> ; one TCP turn and travel, C orients first + 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/mcodes/M128 b/configs/sim/axis/vismach/puma/mcodes/M128 index 26375100367..145f76e0015 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M128 +++ b/configs/sim/axis/vismach/puma/mcodes/M128 @@ -9,12 +9,11 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# RESTORE INI axis limits +# RESTORE INI axis velocity and acceleration limits (the position +# limits are never changed: a box on the machine frame, the flange) foreach l $coords { set L [string toupper $l] - catch {hal setp ini.$l.min_limit [set ::AXIS_[set L](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::AXIS_[set L](MAX_LIMIT)]} - catch {hal setp ini.$l.min_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} + catch {hal setp ini.$l.max_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::AXIS_[set L](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M129 b/configs/sim/axis/vismach/puma/mcodes/M129 index f9f9a2d6b84..7fe3c4e7e19 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M129 +++ b/configs/sim/axis/vismach/puma/mcodes/M129 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits to equiv joint limits +# switch kinematics and change axis velocity limits to equiv joint limits set kinstype 1 ;# 1: IDENTITY kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M130 b/configs/sim/axis/vismach/puma/mcodes/M130 index 8051f2a2237..dcdede0fa04 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M130 +++ b/configs/sim/axis/vismach/puma/mcodes/M130 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits +# switch kinematics and change axis velocity limits set kinstype 2 ;# 2: TYPE 2 kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index 99c0fa33112..f40fb5849c9 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -33,7 +33,7 @@ RS274NGC_STARTUP_CODE = G21 G10L2P0 x450 y100 z-495 a-180 (debug, ini: startup o MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 046f5d118d5..3d181caff47 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -36,7 +36,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index a6b9f8544e9..b4d84d0e71c 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -35,7 +35,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 8397ce3ea7a..2742f6d1059 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -113,7 +113,7 @@ POSTGUI_HALFILE = puma_postgui.hal MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/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/configs/sim/axis/vismach/twinspindle/README b/configs/sim/axis/vismach/twinspindle/README new file mode 100644 index 00000000000..3b75fd5cfb6 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/README @@ -0,0 +1,37 @@ +A mill-turn with a tilting head between two spindles, on twinspindlekins. + +The head carries the tool on X Y Z and tilts it on B, about Y. The main +spindle turns its part on C; the sub spindle faces it along Z, 500 away, +and turns its own part on W. W is an angle, so [AXIS_W] declares it +TYPE = ANGULAR, and both spindles are WRAPPED_ROTARY. The vismach window +draws the spindle line across the screen as on a lathe: main spindle on +the left, Z to the right, X up. + +Kinematics type 0 works on the part in the main spindle and orients the +tool with C and B; type 1 works on the part in the sub spindle and orients +it with W and B. Each part's frame has its Z out of its own spindle's face, +so a program reads the same on either part. Type 2 is the identity, the +machine frame, which G13.1 selects. + +The INI sets MACHINE_MOVES_IN_MACHINE_FRAME, so G53, G28 and G30 move +the slides in that machine frame whichever spindle's type is in force. + +Demo program: + + twin-spindle-holes.ngc -- one subroutine drills a ring of six holes, + each tilted 20 degrees out from the part axis, + into the face of the part the kinematics type + in force works on; the program runs it on the + main spindle (G12.1 P0, G54) and then on the + sub spindle (G12.1 P1, G55). The subroutine + names no rotary: G53.3 turns the ones the type + orients with, and the report on the terminal + reads them by number, #<_kins_orient_1> and + #<_kins_orient_2>, 5 and 4 (C and B) on the + main spindle, 8 and 4 (W and B) on the sub + spindle, with their angles #<_orient_rot1> and + #<_orient_rot2>. + +The tool table provides tool 1, length 50. The drawing hands its pivot +length and spindle distance to the kinematics (twinspindle.hal), so the +two agree. diff --git a/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc new file mode 100644 index 00000000000..86b120350c1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc @@ -0,0 +1,64 @@ +; twin-spindle-holes.ngc - one subroutine drills the same ring of tilted +; holes into the part in the main spindle and into the part in the sub +; spindle. +; +; Kinematics type 0 works on the main spindle's part and orients the tool +; with C and B; type 1 works on the sub spindle's part and orients it with +; W and B. The subroutine names neither: G53.3 turns whatever rotaries the +; type in force orients with, and the report reads them by number, +; #<_kins_orient_1> and #<_kins_orient_2>, with their angles in the same +; order, #<_orient_rot1> and #<_orient_rot2>. Each part's frame has its Z +; out of its own spindle, so both faces are at Z60 and the offsets read +; the same. + +o sub + # = 6 ; holes + # = 30 ; hole circle radius on the face + # = 20 ; tilt of each hole out from the part axis + # = 0 + o101 while [# lt #] + # = [# * 360 / #] + ; the hole's plane: origin on the face, X along the hole circle, + ; Z the hole axis + g68.2 p3 q1 x[#*cos[#]] y[#*sin[#]] z0 i[-sin[#]] j[cos[#]] k0 + g68.2 p3 q2 i[sin[#]*cos[#]] j[sin[#]*sin[#]] k[cos[#]] + g53.2 ; solve the pose, report it + (print, hole #: axis #<_kins_orient_1> to #<_orient_rot1>, axis #<_kins_orient_2> to #<_orient_rot2>) + g53.3 x0 y0 z5 ; orient and go, 5 above the hole + g1 z-10 + g0 z5 + # = [# + 1] + o101 endwhile + g0 z40 + g69 +o endsub + +; from one part to the other in the machine frame, the identity type: +; up, clear of both parts, and the head pointing down +o sub + g13.1 + g53 g0 x300 + g53 g0 z50 b90 +o endsub + +g21 g90 g94 g17 g40 +t1 m6 +g43 +f300 + +g10 l2 p1 x0 y0 z60 c0 w0 ; G54, the main spindle's part +g10 l2 p2 x0 y0 z60 c0 w0 ; G55, the sub spindle's part + +o call +g12.1 p0 ; the main spindle's part +g54 +o call + +o call +g12.1 p1 ; the sub spindle's part +g55 +o call + +o call +g54 +m2 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.hal b/configs/sim/axis/vismach/twinspindle/twinspindle.hal new file mode 100644 index 00000000000..16044ff4de1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.hal @@ -0,0 +1,15 @@ +loadusr -W ./twinspindlegui.py + +net :jx joint.0.pos-fb twinspindlegui.jx +net :jy joint.1.pos-fb twinspindlegui.jy +net :jz joint.2.pos-fb twinspindlegui.jz +net :jb joint.3.pos-fb twinspindlegui.jb +net :jc joint.4.pos-fb twinspindlegui.jc +net :jw joint.5.pos-fb twinspindlegui.jw + +# the drawing and the kinematics share the geometry +net :pivot-len twinspindlegui.pivot_len twinspindlekins.pivot-length +net :spindle-distance twinspindlegui.spindle_distance twinspindlekins.spindle-distance + +net :tool-len motion.tooloffset.z twinspindlegui.tool_length +net :tool-diam halui.tool.diameter twinspindlegui.tool_diam diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.ini b/configs/sim/axis/vismach/twinspindle/twinspindle.ini new file mode 100644 index 00000000000..d03f27002e7 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.ini @@ -0,0 +1,166 @@ +[EMC] +VERSION = 1.1 +MACHINE = Sim-twinspindlekins (XYZBCW mill-turn, two spindles) + DEBUG = 0 + +[DISPLAY] +# X Y Z are already the tip in the frame of the part the type works on, so +# the plot takes them as they are: a rotary in GEOMETRY would turn them +# again, and W, a translation there, would draw as a move along Z + GEOMETRY = XYZ + DISPLAY = axis + OPEN_FILE = ./twin-spindle-holes.ngc + INCREMENTS = 10 mm, 1 mm, .1 mm + JOG_AXES = XYZ + CYCLE_TIME = 0.100 + POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_FEED_OVERRIDE = 2.0 + PROGRAM_PREFIX = ../../nc_files/ + INTRO_GRAPHIC = linuxcnc.gif + INTRO_TIME = 2 + TOOL_EDITOR = tooledit z diam + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 + PARAMETER_FILE = twinspindle.var +# Off the machine frame kinematics (on either spindle's type), G53, G28 and +# G30 move the slides in the machine frame, as G53.5, G28.5 and G30.5 do, +# wherever the tool and the head point, and G28.1 and G30.1 store that +# position. Unset, they move in the world of the spindle's type. See +# MACHINE_MOVES_IN_MACHINE_FRAME in the INI file documentation. +MACHINE_MOVES_IN_MACHINE_FRAME = 1 + +[EMCMOT] + EMCMOT = motmod +COMM_TIMEOUT = 1.0 +SERVO_PERIOD = 1000000 + +[TASK] + TASK = milltask +CYCLE_TIME = 0.010 + +[HAL] + HALUI = halui +HALFILE = LIB:basic_sim.tcl +HALFILE = twinspindle.hal + +[TRAJ] + COORDINATES = XYZBCW + LINEAR_UNITS = mm + ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 + MAX_LINEAR_VELOCITY = 200.0 +MAX_LINEAR_ACCELERATION = 800.0 +DEFAULT_LINEAR_ACCELERATION = 800.0 + MAX_ANGULAR_VELOCITY = 360.0 + +[EMCIO] +TOOL_TABLE = twinspindle.tbl + +[KINS] +# type 0: the part in the main spindle, C and B orient +# type 1: the part in the sub spindle, W and B orient +# type 2: identity, the machine frame +KINEMATICS = twinspindlekins + JOINTS = 6 + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.tbl b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill diff --git a/configs/sim/axis/vismach/twinspindle/twinspindlegui.py b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py new file mode 100755 index 00000000000..4d4f25178cb --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. + +# The twinspindlekins machine: a B tilting head on X Y Z between a main +# spindle (C) and a sub spindle (W) that face each other along Z. The model +# is built in machine coordinates and turned at the end so that Z, the +# spindle line, runs across the screen as on a lathe: main spindle on the +# left, Z to the right, X up. +# +# pivot_len and spindle_distance are handed to the kinematics by +# twinspindle.hal, so the drawing and the maths agree; they can be set at +# invocation, example: twinspindlegui.py pivot_len=200 + +from vismach import * +import hal +import sys + +pivot_len = 150 # gauge line to the B axis +spindle_distance = 500 # main spindle face to sub spindle face +lat, lon = -80, 0 # the starting view, from the front + +for setting in sys.argv[1:]: + exec(setting) + +c = hal.component("twinspindlegui") +for pin in ("jx", "jy", "jz", "jb", "jc", "jw", "tool_length", "tool_diam"): + c.newpin(pin, hal.Type.REAL, hal.Dir.IN) +c.newpin("pivot_len", hal.Type.REAL, hal.Dir.OUT) +c.newpin("spindle_distance", hal.Type.REAL, hal.Dir.OUT) +c["pivot_len"] = pivot_len +c["spindle_distance"] = spindle_distance +c.ready() + +class HalToolCylinder(CylinderZ): + def __init__(self, comp, *args): + CylinderZ.__init__(self, *args) + self.comp = comp + + def coords(self): + r = 5 # default if hal pin not set + if c.tool_diam > 0: r = c.tool_diam/2 + return -self.comp.tool_length, r, 0, r + +# a chuck and a part, face at z 0, the part standing on +z; the flat on +# the part shows the spindle turning +def spindle(work=None): + part = [CylinderZ(0, 50, 60, 50), + Color([1, 0.2, 0.2, 1], [Box(40, -8, 0, 52, 8, 60)])] + if work: + part.append(Translate([work], 0, 0, 60)) + return Collection([Color([0.6, 0.6, 0.7, 1], [CylinderZ(-100, 90, 0, 90)]), + Color([0.9, 0.8, 0.5, 1], part)]) + +# the main spindle turns the work by -C: its frame is Rz(-C) +work = Capture() +main_spindle = HalRotate([spindle(work)], c, "jc", -1, 0, 0, 1) +main_spindle = Collection([main_spindle, + Box(-160, -160, -400, 160, 160, -100)]) + +# the sub spindle is the same, turned by -W, then half a turn about X to +# face the main spindle from spindle_distance +sub_spindle = HalRotate([spindle()], c, "jw", -1, 0, 0, 1) +sub_spindle = Collection([sub_spindle, + Box(-160, -160, -400, 160, 160, -100)]) +sub_spindle = Rotate([sub_spindle], 180, 1, 0, 0) +sub_spindle = HalTranslate([sub_spindle], c, "spindle_distance", 0, 0, 1) + +# the head, gauge line at z 0: the tool below it, the spindle motor above +# up to the B axis at pivot_len, which is then put at the origin +tooltip = Capture() +head = Collection([HalTranslate([tooltip], c, "tool_length", 0, 0, -1), + Color([0.9, 0.9, 0.2, 1], [HalToolCylinder(c)]), + CylinderZ(0, 30, 40, 45), + CylinderZ(40, 45, pivot_len - 45, 45), + Translate([CylinderY(-55, 55, 55, 55)], 0, 0, pivot_len)]) +head = Translate([head], 0, 0, -pivot_len) +head = HalRotate([head], c, "jb", 1, 0, 1, 0) + +# the ram carries the B axis from behind, along -Y, clear of the head +# whichever way it tilts; the slides read the gauge line at B 0, pivot_len +# below the B axis +head = Collection([head, + Box(-50, -700, -50, 50, -60, 50)]) +head = Translate([head], 0, 0, pivot_len) +head = HalTranslate([head], c, "jx", 1, 0, 0) +head = HalTranslate([head], c, "jy", 0, 1, 0) +head = HalTranslate([head], c, "jz", 0, 0, 1) + +bed = Box(-420, -900, -450, -400, 300, 1000) + +model = Collection([bed, main_spindle, sub_spindle, head]) +# Z across the screen to the right, X up, as on a lathe +model = Rotate([model], -90, 0, 1, 0) +model = Rotate([model], 180, 0, 0, 1) + +main(model, tooltip, work, size=900, lat=lat, lon=lon) diff --git a/docs/src/code/nml-messages.adoc b/docs/src/code/nml-messages.adoc index 307066ea39d..5b1bf4a702d 100644 --- a/docs/src/code/nml-messages.adoc +++ b/docs/src/code/nml-messages.adoc @@ -92,6 +92,7 @@ EMC_TRAJ_SET_G92_TYPE EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE EMC_TRAJ_PROBE_TYPE EMC_TRAJ_SET_TELEOP_ENABLE_TYPE +EMC_TRAJ_SET_JOG_FRAME_TYPE EMC_TRAJ_SET_SPINDLESYNC_TYPE EMC_TRAJ_SET_SPINDLE_SCALE_TYPE EMC_TRAJ_SET_FO_ENABLE_TYPE diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index ed9acdbbbfe..675618e6ac1 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -435,6 +435,8 @@ See <> document for GMOCCAPY details. . The proper GEOMETRY string depends on the machine configuration and the kinematics used to control it. The order of the letters is important. For example, rotating around C then B is different than rotating around B then C. +. The tool marker does not use GEOMETRY where the kinematics type in force reports its tool and work frames: it is drawn along the tool as the kinematics places it against the work, the `tool_frame` the Python interface reports. + GEOMETRY orients it under an identity kinematics type and with a module that reports no frames. . Rotations are by default applied with respect to the machine origin. Example: `GEOMETRY=CXYZ` first translates the control point to X, Y, Z and then performs a C rotation about the Z axis centered at the machine origin. . UVW translation example: `GEOMETRY=XYZUVW` causes UVW to move in the coordinate system of the tool and XYZ to move in the coordinate system of the material. @@ -639,6 +641,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) @@ -1034,6 +1041,10 @@ The __ specifies one of: X Y Z A B C U V W The preview, the DROs, touch off and jogging of the GUIs follow the type too. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. ++ +`MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, and a world jog the same way every servo cycle, so on a kinematics that is not the identity a move or a jog runs at the pace of its slowest joint whatever these say. +See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. When set to 0 (default), no per-axis jerk limiting is applied. @@ -1045,6 +1056,11 @@ The __ specifies one of: X Y Z A B C U V W When this limit is exceeded, the controller aborts axis motion. The axis must be homed before MAX_LIMIT is in force. For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. ++ +`MIN_LIMIT` and `MAX_LIMIT` together are a box in the machine frame: the carriage, or the flange of a robot, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. +On a kinematics module that declares its machine frame type (see the <> chapter) every move is checked against the box where its joints put the machine frame, whatever kinematics type is in force: a tool centre point move whose tip lies past the box with the carriage inside it is accepted, and a 'G53.5' to a carriage position past the box is refused. +The travel itself is the `[JOINT_N]` limits; the box is a restriction the configuration adds inside it. +A module that declares no machine frame type keeps the box on the world of the kinematics in force. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. It is ignored on a `LINEAR` axis. @@ -1189,9 +1205,10 @@ These settings are verified at startup when the trivkins kinematics modules is s The `[JOINT_N]MIN_LIMIT`/`MAX_LIMIT` settings are enforced while jogging in joint mode prior to homing. After homing, `[AXIS_L]MIN_LIMIT`/`MAX_LIMIT` coordinate limits are used as constraints for axis (coordinate letter) jogging and by the trajectory planning used for G-code moves (programs and MDI commands). -The trajectory planner works in Cartesian space (XYZABCUVW) and has no information about the motion of joints implemented by *any* kinematics module. -It is possible for joint limit violations to occur for G-code that obeys trajectory planning position limits when non identity kinematics are used. -The motion module always detects joint position limit violations and faults if they occur during the execution of G-code commands. +The trajectory planner works in Cartesian space (XYZABCUVW). +On a kinematics that is not the identity, every move of a program or an MDI command is also followed through the kinematics module ahead of motion, and a move that would take a joint past `MIN_LIMIT` or `MAX_LIMIT` anywhere along it is refused before it runs, its line failing with the joint and the limit; see the <> chapter, INI file limit settings. +A kinematics module that can only be evaluated in realtime is checked at the ends of each move. +The motion module detects a joint position limit violation during the execution of G-code commands and faults, for what cannot be seen ahead of it. See also related https://github.com/LinuxCNC/linuxcnc/issues/97[GitHub issue #97]. ==== diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index f55ccb764a2..ffd0e25e08d 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -182,8 +182,14 @@ see <> for an example. *g68_active*:: '(returns integer)' - a tilted work plane (G68.2) is in effect. +*g68_kins_offset*:: '(returns tuple of floats)' - + under the machine frame kinematics, the shift of the second feature coordinate system the tilted work plane is cut through (3+2), in machine units: the part as the primary kinematics places it, carried onto the machine frame with the rotaries held, after the whole offset chain. Zero where there is none. + +*g68_kins_rotation*:: '(returns tuple of floats)' - + the rotation of that second feature coordinate system, nine values row by row; the identity where there is none. + *g68_offset*:: '(returns tuple of floats)' - - origin of the tilted work plane, in the coordinate system it was defined in. + origin of the tilted work plane, in the coordinate system it was defined in, the same under every kinematics type. *g68_rotation*:: '(returns tuple of floats)' - rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. @@ -224,6 +230,12 @@ see <> for an example. INTERP_FILE_NOT_OPEN, INTERP_ERROR. see src/emc/nml_intf/interp_return.hh +*jog_frame*:: '(returns integer)' - + the frame world jogs of X, Y and Z move along, set by `jog_frame()`. One of: + * JOG_FRAME_MACHINE + * JOG_FRAME_PLANE + * JOG_FRAME_TOOL + *joint*:: '(returns tuple of dicts)' - reflecting current joint values. See <>. @@ -244,6 +256,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. @@ -284,6 +303,11 @@ see <> for an example. *optional_stop*:: '(returns integer)' - option stop flag. +*part_position*:: '(returns tuple of floats)' - + the commanded joints as the module's primary kinematics type, the one that follows the part, reads them: where the tool stands against the part, in machine units. + The same as *position*, except under another type of a switchable module, the machine frame type <> selects, where *position* is in machine coordinates. + The backplot of the G-code preview is drawn from it, minus the tool offset. + *paused*:: '(returns boolean)' - `motion paused` flag. @@ -367,6 +391,11 @@ see <> for an example. task main loop heartbeat counter. Increments every task cycle. Rate is determined by `[TASK]CYCLE_TIME` in the INI file. +*tool_frame*:: '(returns tuple of floats or None)' - + orientation of the tool in the work frame at the commanded joints, nine values row by row; its columns are the tool's X, Y and Z, Z pointing from the tip towards the holder. + Under an identity type of a switchable module the frames are the primary type's, the one that follows the part, so a display can draw the tool against the part there too. + None where no type reports tool and work frames. + *tool_from_pocket*:: '(returns integer)' - pocket number for the currently loaded tool (0 if no tool loaded). @@ -703,6 +732,13 @@ c.tool_offset(toolno, z_offset, x_offset, diameter, frontangle, backangle, orie the known coordinate letters XYZABCUVW (x=>0,y=>1,z=>2,a=>3,b=>4,c=>5,u=>6,v=>7,w=>8) +`jog_frame(int)`:: + set the frame an axis jog of X, Y and Z moves along: the machine's axes, the axes of the active tilted work plane, or the tool's own axes; see <>. Refused while a jog is under way. The other axis letters jog along their own axes in every frame. + + Constants;; + JOG_FRAME_MACHINE + + JOG_FRAME_PLANE + + JOG_FRAME_TOOL + `load_tool_table()`:: reload the tool table. diff --git a/docs/src/gcode.html.in b/docs/src/gcode.html.in index 02353bbcf36..b573eafa3be 100644 --- a/docs/src/gcode.html.in +++ b/docs/src/gcode.html.in @@ -65,6 +65,11 @@ tr.head td, tr.head th { background: black; color: white; } G33K ($)Spindle Synchronized Motion G33.1K ($)Rigid Tapping G80 Cancel Canned Cycle + G53.4Point-to-Point Move, Program Coordinates + G53.5Point-to-Point Move, Machine Frame + G53.7J<n>=Point-to-Point Move, Joint Values + G53.1, G53.3, G53.6(P) (Q)Orient the Tool to the Work Plane + G53.2(P) (Q)Solve the Tool Orientation, No Motion Canned cycles(X Y Z or U V W apply to canned cycles, depending on active plane) G70Q (X) (Z) (D) (E) (P)Lathe finishing cycle @@ -100,6 +105,8 @@ tr.head td, tr.head th { background: black; color: white; } G43 H Tool Length Offset G43.1 Dynamic Tool Length Offset G43.2 H Apply additional Tool Length Offset + G43.4 (H) Tool Length Offset on Primary Kinematics + G43.5 (H) Tool Length Offset, Tool Axis as a Vector G49 Cancel Tool Length Compensation Stopping @@ -136,6 +143,10 @@ tr.head td, tr.head th { background: black; color: white; } M52P0 (off) or P1 (on)Adaptive Feed Control M53P0 (off) or P1 (on)Feed Stop Control G54-G59.3Select Coordinate System + G68.2, G68.4P Q I J K RTilted Work Plane + G68.3(R)Tilted Work Plane from the Tool + G69Cancel Tilted Work Plane + G12.1, G13.1PSelect Kinematics Flow-control Codes o subSubroutines, sub/endsub call @@ -167,6 +178,7 @@ tr.head td, tr.head th { background: black; color: white; } G28, G28.1Go/Set Predefined Position G28.2PHome from G-code G30, G30.1Go/Set Predefined Position + G28.5, G30.5Go to Predefined Machine Frame Position G53Move in Machine Coordinates G52, G92Coordinate System Offset G92.1, G92.2Reset G92 Offsets diff --git a/docs/src/gcode/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..13912f733fa 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 Machine Frame 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,17 +972,31 @@ 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 readout can change even though nothing has moved. +The G-code preview, the backplot and the tool shown in a display stay on +the part whichever kinematics is selected: the module's primary +kinematics, the one that follows the part, places the tool against the +part from the joints, so a switch moves nothing on the screen. Under the +machine frame kinematics with a head or a table tilted, the slides alone +do not say where the tool cuts the part; the preview draws each move +where it does, as the primary kinematics would, and a rotary move as the +curve the tool sweeps across the part. This is what the standard controls +show: the Siemens simulation keeps the workpiece fixed and moves only the +tool, whatever the machine type, and the Heidenhain graphic shows the +workpiece as it is machined. Status has the point as `part_position`. The +preview evaluates the kinematics outside realtime for this; with a module +that runs only in realtime it draws the coordinates of the type in force. + The active kinematics is available to the program as the read-only parameter '#<_kins_type>', which lets a subroutine put back whatever was selected before it ran: @@ -992,14 +1012,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 +1028,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 +1089,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 +1211,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 +in the machine frame, and with `MACHINE_MOVES_IN_MACHINE_FRAME` set 'G30' and +'G30.1' are 'G30.5' and its store whenever the kinematics in force is not the +machine frame kinematics. + It is an error if : * Cutter Compensation is turned on +[[gcode:g28.5]] +== G28.5, G30.5 Go to Predefined Machine Frame Position(((G28.5 Go to Predefined Machine Frame 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 +1720,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, twinspindlekins) apply an X and Y of +the offset along the tool's own X and Y, as they apply the length along +the tool axis. The offset is given in the tool's axes, which are the +machine's with the rotaries at zero, so a tool held off the spindle axis +keeps its offset as the head or the table turns. Under the identity and +the other modules, X and Y apply along the work axes. + [NOTE] 'G43 H0' is a little special. Its behavior is different on random tool changer machines and nonrandom tool changer machines (see the @@ -1758,15 +1861,18 @@ 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 (KINSTYPE_PRIMARY, see the <> chapter), the number is not the answer; a module that -declares none rejects 'G43.4' at read time. +declares none rejects 'G43.4' at read time. A module with one working +kinematics per tool head declares each one primary: 'G43.4' stays on the +one selected with 'G12.1' and, from any other kinematics, switches to the +lowest numbered. The switch is a queue synchronisation point like '<>': when 'G43.4' or 'G49' changes the @@ -1779,22 +1885,100 @@ 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 words of +the two axes the kinematics type orients the tool with, the ones +'#<_kins_orient_1>' and '#<_kins_orient_2>' name (A B C for a type that +orients with three angles, a robot's wrist), in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +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 word of an orienting axis on the same line, +* the kinematics type orients the tool with an axis that is not rotary, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* 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 +2012,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 +2049,257 @@ 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_rot1>', '#<_orient_rot2>' and '#<_orient_rot3>', and on the + numbered parameters '#5071' to '#5076' in the same order, in program + units in the plane, and sets '#<_orient_valid>' and '#5080' to 1. The + rotaries come in the order the kinematics orients the tool with, the + axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, so a program reads + the angles the same way whatever letters the machine gives them; + '#<_orient_rot3>' is 0 on a machine that orients with two rotaries. The program can then reach + the pose with a move of its own making, for instance a single 'G0' that + names 'X Y Z' and the rotary words together, so the turn and the travel + are one Cartesian move under the TCP kinematics. This is what Heidenhain + 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 <>. Under the module's machine frame kinematics, which <> selects, the interpreter orients the tool with the TCP kinematics' frames, for 3+2 machining with the slides alone moving; see <>. + +.G53.1 Example +[source,ngc] +---- +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 an identity kinematics the module declares + no TCP kinematics beside, the module cannot be evaluated by the + interpreter, or it reports no frames, so it cannot say where its joints + point the tool. +* The plane's normal cannot be reached by the rotary joints. +* 'P' is anything but 0, 1 or 2, no reachable pose has the rotary that + names it the way 'P' asks for, or the machine has no pair of poses a + 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 +2475,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 +2487,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 +2537,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 +2555,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, and under the machine frame kinematics it reads the tool on the part, as the TCP kinematics places it. + 'G68.4' takes any 'G68.2' form and composes it onto the active plane: the words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. @@ -2133,7 +2605,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 +2623,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 +2636,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,10 +2658,96 @@ 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 an identity + kinematics the module declares no TCP kinematics beside, or reports no + frames. +* 'G68.3' is given 'I', 'J', 'K', 'P' or 'Q', or 'G69' any of those or 'R': + a word the code does not use is an error, not ignored. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. +[[sec:twp-machine-frame]] +=== 3+2 under the machine frame kinematics(((3+2 machining))) + +Under a module's machine frame kinematics, the type <> selects, a plane is cut where the module's TCP kinematics would cut it with the rotary joints held where they stand. The plane sits on the part, turned with the table, and the moves under it are made by the slides alone, so a rotary move never drags them along as it does under the TCP kinematics. This is 3+2 machining with the rotaries set once, what Fanuc calls the second feature coordinate system. + +When 'G68.2', 'G68.3' or 'G68.4' defines a plane, the interpreter takes the map from the part, as the TCP kinematics places it, to the machine frame, at the rotaries as they stand. With the rotaries held that map is a fixed rotation and shift, and it carries the tool length along the tilted tool, which the TCP kinematics applies and the machine frame kinematics does not. 'G53.1', 'G53.3' and 'G53.6' orient the tool to the plane on the part with the TCP kinematics' frames, so a table rotary helps where the plane needs it, and take the map again at the rotaries they end at. 'G53.6' keeps the tool tip where it is at the ends of its move; in between, the slides and the rotaries move in straight lines in the machine frame and the tip leaves the point while they turn. 'G53.2' moves nothing and leaves the map as it was. + +A rotary word under the plane moves the rotary alone and the map stays, so the moves after it are still cut where the plane sat on the part before the turn, which is now somewhere else. Fanuc and Heidenhain do the same, and Fanuc's manual tells the programmer not to turn a rotary in a tilted working plane. The preview draws those moves where they land on the turned part. To work on the turned part, give the plane again after the turn: 'G68.4 X0 Y0 Z0 I0 J0 K0' is the same plane at the rotaries as they now stand, and 'G53.1' turns the rotaries to the plane and takes the map where they end. + +.3+2 Example +[source,ngc] +---- +G13.1 (the machine frame kinematics) +G43 H1 +G68.2 X0 Y0 Z0 I0 J30 K0 (a plane on the part, tilted 30 degrees about X) +G53.1 (the rotaries to it, the table helping) +G0 X0 Y0 Z5 (above the plane's origin, the slides alone) +G1 Z-2 F200 (down along the plane's normal) +G0 Z5 +C90 (the table alone; the plane stays where it was) +G68.4 X0 Y0 Z0 I0 J0 K0 (the plane again, on the part as it now stands) +G0 X0 Y0 Z5 +G69 +---- + +A change of tool length offset in the plane, 'G43' or 'G49', keeps the slides where they are and takes the map again with the new length. 'G53', 'G28' and 'G30' move in the machine frame, as Fanuc's do. A change of kinematics type in the plane keeps the plane on the part: under the TCP kinematics it follows the table as the rotaries turn, and back under the machine frame kinematics the map is taken at the rotaries as they stand. + +Status keeps the two apart, as the Siemens swivel cycle keeps its workpiece reference frame apart from its table and tool reference frames: 'g68_offset' and 'g68_rotation' are the plane as defined, the same under every kinematics type, and 'g68_kins_offset' and 'g68_kins_rotation' are the map, the identity where none is in effect. A display composes the two after the offset chain to show plane coordinates, and the plane jog frame moves along the plane turned by the map. The G-code preview restates the plane from status with the map status has, so a program run after a plane set in MDI moves where the machine does, a rotary turned since included, and so does an abort with `RETAIN_WORK_PLANE`. + +The G-code preview draws the plane as defined, on the part, and the moves in it where they cut the part, the same under both types, as <> describes. + +A module that declares no TCP kinematics, or whose machine frame kinematics is its TCP kinematics, a robot's, has no map to take: there a plane is in the machine frame, and only head rotaries can orient the tool to it. + +[[sec:jog-frame]] +=== Jogging along the plane or the tool(((Jog frame))) + +In world mode a jog of X, Y or Z moves along one of three frames: + +* *machine*: the machine's own X, Y and Z, the slides on a machine whose + slides line up with its frame. The machine does not turn when its table + does: under a tool centre point kinematics, where X, Y and Z are the + part's and turn with a rotary table or a work spindle, a jog of X still + moves the X slide alone, whatever the rotaries stand at, and the + position the GUI shows changes along the part's axes it crosses. +* *plane*: the axes of the active plane, the ones a program under it moves + along, so a jog of Z backs the tool straight out of a hole drilled into + the plane. With no plane active it is the X, Y and Z of the kinematics in + force: under a tool centre point kinematics the part's axes, turned with + the table. +* *tool*: the tool's own axes at the current pose as the kinematics module + reports them (`tool_frame` in status), Z along the tool from the tip + towards the holder: for a spindle head set at an angle, or a tool that is + not normal to the plane. A plane in effect does not change it: to jog + along the plane's axes, select the plane frame. Under an identity + kinematics type, the machine frame kinematics G13.1 selects, the tool's + axes are the ones the module's tool centre point kinematics has at the + joints, so a tool jog moves the machine as it does under the tool centre + point kinematics, along the tool as the head stands: Fanuc's tool axis + direction feed and Heidenhain's Manual Operation Tool Axis move so + without the tool centre point too. With a module that reports no frames + it is the machine frame. + +The operator selects the frame and a program never changes it: Python +`jog_frame()`, with `jog_frame` in status; the halui pins +`halui.jog-frame.machine`, `halui.jog-frame.plane` and `halui.jog-frame.tool`, +with `halui.jog-frame.is-machine`, `-plane` and `-tool` reporting it; in AXIS +the View menu or the `%` key. `motion.jog-frame` reports the frame in HAL, so +a pendant can, for instance, let only Z through in the tool frame. The jog +pins `axis.L.jog-*` jog the letter of the selected frame. A, B, C, U, V and W +jog along their own axes in every frame, and joint mode is not affected. + +The frame is taken when a jog starts and kept until the jog ends, so a jog +in the tool frame does not bend while a rotary jogs with it, and the frame +cannot change while a jog is under way. One of X, Y and Z moves at a time in +a frame: a jog of another waits until the one under way has stopped. Every +world axis keeps to its own `[AXIS_L]` velocity, acceleration and travel: a +jog along a turned axis runs as fast as the slowest axis it moves allows, a +continuous jog stops where the turned axis meets the travel, and an +incremental jog that would end outside it is refused. The joint limits cap +it as they cap any world jog. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 3aeb7ba083e..ae352b92262 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,13 @@ 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-5076' - Pose last solved by <> for X, Y, Z + and the first, second and third orienting rotary, in program units in + the tilted work plane. Same values as `#<_orient_x>` and kin. Read-only, + volatile. +* '5077-5079' - Written 0 by 'G53.2'. Read-only, volatile. +* '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. + Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, 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 +515,34 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_kins_orient_1>', '#<_kins_orient_2>', '#<_kins_orient_3>' - The + axes the kinematics type in force orients the tool with, as axis numbers, + 0 for X to 8 for W: the first rotation, the one whose axis stays put, then + the second. A head and a table give the table first. The type's module + declares the letters, or they are those its joint map gives the joints + that turn. '#<_kins_orient_3>' is -1 on those. A type with three + rotaries, a table and a two axis head say, gives all three, the tables + first; a robot arm, whose joints all turn the tool, reads 3, 4 and 5, the + A B C of its pose. All three are -1 where the type orients otherwise, the identity + type among them, or the interpreter cannot evaluate the kinematics. An axis + named here must be 'ANGULAR' in the INI file; the interpreter refuses a + module whose type orients with a 'LINEAR' axis when it first loads it. + +* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>', + '#<_kins_orient_3_head>' - 1 where that axis turns the head, 0 where it + turns the table, -1 as above. + +* '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_rot1>', '#<_orient_rot2>', '#<_orient_rot3>' - The pose 'G53.2' + last solved, in program units in the tilted work plane: X Y Z, then the + angles of the axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, in + that order, 0 for a third the machine does not have. This is how Fanuc + and Siemens number the rotaries of a five axis machine, first and second, + whatever their letters. '#<_orient_valid>' is 1 once a 'G53.2' has run; + reading the others before that is an error. The same values are on the + numbered parameters '#5071' to '#5076' and '#5080'. See + <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] @@ -965,7 +1000,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 +1009,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/gui/axis.adoc b/docs/src/gui/axis.adoc index caa94c9d104..5a1d5e438b1 100644 --- a/docs/src/gui/axis.adoc +++ b/docs/src/gui/axis.adoc @@ -278,6 +278,12 @@ So be familiar with your machine's design and interpret the display as needed. tuning, physical constraints, or position quantization. * 'Show Machine Position' - This is the position in unoffset coordinates, as established by Homing. * 'Show Relative Position' - This is the Machine Position modified by 'G5x', 'G92', and 'G43' offsets. +* 'Jog in machine frame', 'Jog in work plane frame', 'Jog in tool frame' - The + frame a jog of X, Y and Z moves along in world mode, see + <>. Outside the machine frame + the status bar shows the frame next to the position type. +* 'Joint mode', 'World mode' - Jog the joints one by one, or the axis letters. + Not shown with an identity kinematics. ==== Help Menu @@ -663,6 +669,7 @@ The most frequently used keyboard shortcuts are shown in the following table: |Shift-PgUp, PgDn |Rapid Z Axis |Manual |@ |Toggle Actual/Commanded |Any |# |Toggle Relative/Machine |Any +|% |Cycle jog frame Machine/Plane/Tool |Any |=== == Show LinuxCNC Status (`linuxcnctop`) @@ -1356,6 +1363,7 @@ print(vars.emcini.get()) interp_state = IntVar ja_rbutton = StringVar jog_aspeed = DoubleVar + jog_frame = IntVar jog_speed = DoubleVar kinematics_type = IntVar linuxcnctop_command = StringVar diff --git a/docs/src/gui/halui.adoc b/docs/src/gui/halui.adoc index e9c87f8c1ae..ba70bbaaf50 100644 --- a/docs/src/gui/halui.adoc +++ b/docs/src/gui/halui.adoc @@ -240,6 +240,17 @@ _L_ = axis letter (xyzabcuvw) * 'halui.axis.selected.minus' (bool in) - pin for jogging the selected axis in negative direction at the halui.axis.jog-speed velocity * 'halui.axis.selected.plus' (bool in) - for jogging the selected axis in positive direction at the halui.axis.jog-speed velocity +=== Jog Frame + +The frame X, Y and Z jog along in coordinated jog mode, see <>. + +* 'halui.jog-frame.is-machine' (bool, out) - indicates X, Y and Z jog along the machine axes +* 'halui.jog-frame.is-plane' (bool, out) - indicates X, Y and Z jog along the axes of the tilted work plane +* 'halui.jog-frame.is-tool' (bool, out) - indicates X, Y and Z jog along the axes of the tool +* 'halui.jog-frame.machine' (bool, in) - pin for requesting jogs along the machine axes +* 'halui.jog-frame.plane' (bool, in) - pin for requesting jogs along the axes of the tilted work plane +* 'halui.jog-frame.tool' (bool, in) - pin for requesting jogs along the axes of the tool + === Mode * 'halui.mode.auto' (bool, in) - pin for requesting auto mode diff --git a/docs/src/man/man1/halui.1.adoc b/docs/src/man/man1/halui.1.adoc index 861abcd04c0..f8e05f3a7d3 100644 --- a/docs/src/man/man1/halui.1.adoc +++ b/docs/src/man/man1/halui.1.adoc @@ -370,6 +370,21 @@ _L_ = axis letter (xyzabcuvw) *halui.axis.selected.plus* bool in:: pin for jogging the selected axis in positive direction at the halui.axis.jog-speed velocity +=== Jog frame + +*halui.jog-frame.is-machine* bool out:: + pin showing X, Y and Z jog along the machine axes +*halui.jog-frame.is-plane* bool out:: + pin showing X, Y and Z jog along the axes of the tilted work plane +*halui.jog-frame.is-tool* bool out:: + pin showing X, Y and Z jog along the axes of the tool +*halui.jog-frame.machine* bool in:: + pin for requesting jogs along the machine axes +*halui.jog-frame.plane* bool in:: + pin for requesting jogs along the axes of the tilted work plane +*halui.jog-frame.tool* bool in:: + pin for requesting jogs along the axes of the tool + === Flood coolant *halui.flood.is-on* bool out:: diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 79a3a116e21..16ebf850e4b 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -44,6 +44,8 @@ kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakin *loadrt 5axiskins* +*loadrt twinspindlekins* + == DESCRIPTION Rather than exporting HAL pins and functions, these components provide @@ -207,7 +209,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 +258,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 +295,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,12 +422,46 @@ 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. +=== twinspindlekins - mill-turn with two spindles + +XYZBCW -- a head on X Y Z tilts the tool on B, about Y, between a main +spindle turning its part on C and a sub spindle facing it along Z and +turning its own part on W. Declare W `TYPE = ANGULAR` in its `[AXIS_W]` +section; C and W may be `WRAPPED_ROTARY`. Coordinates XYZBCW are required; +A, U and V may be added with the coordinates parameter and map one to one +to their joints. + +Type 0 works in the frame of the part in the main spindle, and orients the +tool with C and B. Type 1 works in the frame of the part in the sub spindle, +which is the main spindle's turned half a turn about X, so its Z too points +out of the face towards the tool, and orients the tool with W and B. Both +are primary: G12.1 selects the spindle, and a program runs the same on +either part. Type 2 is the identity and the machine frame. The rotary +directions are the conventional ones. + +With B at 0 the tool points along -Z and the slides read the gauge line; the +tool length is applied along the tool, as on 5axiskins. The sim config +configs/sim/axis/vismach/twinspindle drills one subroutine's holes into +both parts. + +*twinspindlekins.pivot-length*:: + Distance from the B rotation point to the tool holder gauge line. + +*twinspindlekins.tool-length*:: + Tool length, applied along the tool. Motion hands the module the offset + in effect (G43, G49), so the pin needs no connection. An X and Y of the + offset are applied along the tool's own X and Y. + +*twinspindlekins.spindle-distance*:: + Distance along Z from the main spindle face to the sub spindle face. + == SEE ALSO For additional information, see following subsections of the section 'Advanced Topics' of the LinuxCNC documentation: diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index d24119c1ff6..84bdbc08630 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -215,6 +215,11 @@ Note: feed-inhibit applies to G-code commands -- not jogs. will be stopped immediately. *motion.jog-is-active* OUT BOOL:: TRUE if any joint or axis is jogging. +*motion.jog-frame* OUT SINT:: + The frame a jog of X, Y and Z moves along in world mode: 0 the machine + axes, 1 the axes of the tilted work plane, 2 the axes of the tool. The + **axis.**_L_**.jog-*** pins of X, Y and Z jog the letter of that frame; + a HAL net can read the pin to let only some of them through in a frame. *motion.in-position* OUT BOOL:: TRUE if the machine is in position (i.e., not currently moving towards the commanded position). **motion.misc-error-**_NN_ IN BOOL:: @@ -355,6 +360,7 @@ These pins represent the *commanded geometric intent* from the interpreter and a Values greater than 1 or less than zero are ignored. **axis.**_L_**.jog-counts** IN SINT:: Connect to the "counts" pin of an external encoder to use a physical jog wheel. + For X, Y and Z the wheel jogs that letter of the frame *motion.jog-frame* reports. **axis.**_L_**.jog-enable** IN BOOL:: When TRUE (and in manual mode), any change to "jog-counts" will result in motion. When false, "jog-counts" is ignored. diff --git a/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..e2d3a5aef22 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. @@ -301,9 +337,11 @@ every block. It is the inverse of the tool frame: not where the tool points for these joints, but which joints point it there. It has more than one answer. A five-axis machine reaches a given tool axis two -ways, one with the secondary rotary positive and one with it negative, and -which of the two is wanted depends on the joint limits and on what the operator -asked for, shortest move or one direction of rotation only. So the module +ways, which differ in the sign of the first rotary met going from the tool to +the work: the secondary rotary of two in the head, the one carrying the other +of two in the table, the head rotary of one of each. Which of the two is wanted +depends on the joint limits and on what the operator asked for, shortest move +or one direction of rotation only. So the module reports every set of joint values that reaches the orientation and stops there. It does not apply the joint limits and it does not rank the answers, because neither is geometry: a module that picked for the caller would be picking with @@ -329,6 +367,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 +572,22 @@ 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` a working transform +`G43.4` and `G43.5` switch to, which a module with one per tool head sets +on each, `G12.1` choosing the head; `machine` the machine frame type `G13.1` and +`G49` cancel to and `G53.5` moves in, which a module leaves unset when its +identity type is that frame and sets on its working transform when that +reports the flange with no tool, as the robot modules do (see the Switchable +Kinematics chapter). A type may also name the axis letters that orient +the tool, first rotation then second, in `orient`: "AC" for instance, or three +letters, tables first, for a type with three rotaries. Left out, they are the +letters the joint map gives the joints that turn the frames, which is right +for a module whose forward puts each joint in its own letter; a module that +writes an orienting joint into another letter declares them. A robot arm, all +of whose joints turn the tool, orients with the A B C of its pose. `G43.5` +writes its solution into these letters, the interpreter reads them back as +`#<_kins_orient_1>` to `#<_kins_orient_3>`, and `G53.2` publishes the angles +it solves in the same order as `#<_orient_rot1>` to `#<_orient_rot3>`. A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. @@ -547,6 +606,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()`, @@ -557,7 +628,9 @@ names. Its tool is the caller's where the caller gives one through `kinematicsUserSetTool()`, since a planner knows what a segment runs under better than the machine does; otherwise it is motion's, from motion's own offset pins, and the loader says once when the module's tool pin disagrees with -them. `kinslimits` is built on it. +them. `kinslimits` is built on it, and so is the cap canon puts on every +segment's velocity and acceleration from the joint limits through the Jacobian +(see the Switchable Kinematics chapter, INI file limit settings). A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. @@ -581,6 +654,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 +674,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 +719,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..2e5b9623fb8 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -50,6 +50,7 @@ The following kinematics modules support switchable kinematics: . *xyzab_tdr_kins* (type0:identity type1:tcp) . *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) . *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) +. *twinspindlekins* (type0:main spindle type1:sub spindle type2:identity) Every module listed above uses its own kinematics for type0 and identity kinematics for type1. Each accepts the module string @@ -72,6 +73,9 @@ type0 is then reached without running the failing kinematics again. The 'sparm' setting exchanges type0 and type1, so any G-code or HAL logic that selects a kinematics type by number must match. +twinspindlekins has no 'sparm': its type0 and type1 are both working +kinematics, one per spindle, and type2 is the identity. + Kinematics that solve the forward direction iteratively, genhexkins among them, need a pose to start from. While another type is running they take the estimate the caller supplies, which motion seeds from @@ -226,9 +230,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 @@ -299,12 +304,60 @@ MAX_VELOCITY = MIN_ACCELERATION = ---- -The INI file limits specified apply to the type 0 default -kinematics type that is activated at startup. These limits may -*not* be applicable when switching to alternative kinematics. -However, since an interpreter-motion synchronization is required -when switching kinematics, INI-HAL pins can be used to setup -limits for a pending kinematics type. +The position limits, `MIN_LIMIT` and `MAX_LIMIT`, are a box in the +machine frame, the world of the kinstype the module declares +KINSTYPE_MACHINE (its identity kinstype where it declares none): +the carriage in machine coordinates, or a robot's flange in its base +frame. Every move is checked against that box where its joints put +the machine frame, whichever kinstype is in force, so the box means +the same thing after a switch as before it and needs no resetting. +The travel is the `[JOINT_N]` limits, checked on every move as well; +the box is a restriction inside it. A module that declares no +kinstype flags keeps the box on the world of the kinstype in force, +as before. + +The travel is followed along each move, not only at its end. Ahead +of motion every segment is sampled through the module, on the +kinstype and the tool offset the program is under, and a move that +would take a homed joint past its `MIN_LIMIT` or `MAX_LIMIT` anywhere +along it is refused before it is sent: a turn of a tilted head under +the tool centre point that swings the carriage past its travel and +back, with both ends of the move inside it, fails its line with the +joint, the limit and where along the move it would be passed, and +nothing of it runs. Between samples the joint is followed on the +cubic through their positions and rates, and a peak that comes near +the limit is read through the module itself; only a point the module +puts past the limit refuses a move. A module that can only be +evaluated in realtime is checked at the ends of each move; motion's +own check of the joints while they move stays behind both. + +The velocity and acceleration limits, `MAX_VELOCITY` and +`MAX_ACCELERATION`, bound the coordinate letter in the world of the +kinstype in force. What the joints can follow is bound separately: +every segment is sampled through the module's Jacobian ahead of +motion, on the kinstype and the tool offset the program is under, and +its velocity and acceleration are lowered to what the slowest joint +can give within its own `[JOINT_N]` limits, the share a carriage on a +circle feels as centripetal included. A turn of a tilted head under +the tool centre point, which swings the carriage through a circle the +head's own limit never mentions, runs at the carriage's pace. A +module that can only be evaluated in realtime (one without the +parameter block form, see <>) +caps nothing; on it the `[AXIS_L]` limits are all there is. A world +jog is bounded the same way, in motion, every servo cycle: the Jacobian +at the joints the machine stands in caps the jog's planners so that no +joint is asked for more than its limits, and a jog of C under the tool +centre point with the head laid over runs at the pace of the carriage +it swings. + +So a switch does not need the velocity and acceleration limits reset +to keep the joints within their limits. It may still want them reset +where a letter names a joint of another kind after the switch: a +robot's identity kinstype puts joint 0, a rotary, on X, and the +`[AXIS_X]` limit in mm/s says nothing useful about it. Since an +interpreter-motion synchronization is required when switching +kinematics, INI-HAL pins can be used to set up limits for a pending +kinematics type. [NOTE] INI-HAL pins are typically not recognized during a G-code program @@ -339,7 +392,7 @@ kinematics (trivkins). See the kins man page for more information ($ man kins). A user-provided M-code can alter any or all of the axis coordinate -limits prior to changing the motion.switchkins-type pin and +velocity and acceleration limits prior to changing the motion.switchkins-type pin and synchronizing the interpreter and motionparts of LinuxCNC. As an example, a bash script invoking halcmd can be 'hardcoded' to set any number of HAL pins: @@ -365,24 +418,25 @@ with a complex (non-identity) kinematics (type0) after homing. The system is configured so that it can be switched to identity kinematics (type1) in order to manipulate individual joints using the conventional letters from the set 'XYZABCUVW'. The INI file -settings ([AXIS_L]) are *not* applicable when operating with -identity (type1) kinematics. To address this use case, the user -M-code scripts can be designed as follows: +velocity and acceleration settings ([AXIS_L]) are *not* applicable +when operating with identity (type1) kinematics. To address this +use case, the user M-code scripts can be designed as follows: *M129* (Switch to identity type1) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the 'identity-referenced' joint number INI file - setting ([JOINT_N]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the 'identity-referenced' joint + number INI file setting ([JOINT_N]) . HAL: `setp motion.switchkins-type 1` . MDI: execute a syncing G-code (M66E0L0) *M128* (restore robot default kinematics type 0) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the appropriate INI file setting ([AXIS_L]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the appropriate INI file setting + ([AXIS_L]) . HAL: `setp motion.switchkins-type 0` . MDI: execute a syncing G-code (M66E0L0) @@ -428,6 +482,7 @@ configs/sim/axis/vismach/ . . 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins) . 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn) . 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn) +. twinspindle/twinspindle.ini (twinspindlekins) == User kinematics provisions @@ -573,8 +628,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 +641,45 @@ 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. A tilted work plane under the machine frame kinstype is read on the +part through the lowest numbered primary kinstype with the rotaries held, +for 3+2 machining with the slides alone moving, so a table machine needs +a primary kinstype for it; see the G-code documentation for 'G68.2'. A +machine with two tool heads, a saw blade and a waterjet say, declares +one primary kinstype per head and selects the head with 'G12.1 P-': +'G43.4' stays on the kinstype in force when it is primary, and from any +other switches to the lowest numbered primary. The +machine frame kinstype is the machine with the tool left out, XYZ the +pivot or the flange in machine coordinates and the rotary letters the +orientation as that kinstype reports it; on a machine whose slides line +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 and at most one machine frame, any +number primary, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +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 +705,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/docs/src/remap/remap.adoc b/docs/src/remap/remap.adoc index 75e2a0980fa..02c225dda19 100644 --- a/docs/src/remap/remap.adoc +++ b/docs/src/remap/remap.adoc @@ -1829,8 +1829,8 @@ All the listed G-codes are already defined in the current implementation of Linu |# |Gxx |Gxx.1 |Gxx.2 |Gxx.3 |Gxx.4 |Gxx.5 |Gxx.6 |Gxx.7 |Gxx.8 |Gxx.9 |10 |G10 | | | | | | | | | |11 | | | | | | | | | | -|12 | | | | | | | | | | -|13 | | | | | | | | | | +|12 | |G12.1 | | | | | | | | +|13 | |G13.1 | | | | | | | | |14 | | | | | | | | | | |15 | | | | | | | | | | |16 | | | | | | | | | | @@ -1851,7 +1851,7 @@ All the listed G-codes are already defined in the current implementation of Linu |25 | | | | | | | | | | |26 | | | | | | | | | | |27 | | | | | | | | | | -|28 |G28 |G28.1 |G28.2 | | | | | | | +|28 |G28 |G28.1 |G28.2 | | |G28.5 | | | | |29 | | | | | | | | | | |=== @@ -1859,10 +1859,10 @@ All the listed G-codes are already defined in the current implementation of Linu [width="90%",align="center",options="header,strong,unbreakable",cols="1*2^em,10*1 joint positions) * @@ -144,6 +156,19 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, const EmcPose* world, double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); +/** + * The same at joints the caller already holds for the pose, from its own + * kinematicsUserInverse(): the derivative is taken there and no inverse is + * run first. What a caller sampling a path wants, having just inverted + * each sample. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + /** * The parameter block as it stands, refreshed from HAL first. For * reporting; the block belongs to the context. @@ -257,6 +282,23 @@ int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, int* joint); +/** + * The axes that orient the tool on the active type, as axis numbers, 0 X + * to 8 W: the first rotation then the second, and whether each turns the + * head (1) or the table (0). Two heads or two tables are ordered as + * kinematicsUserOrientJoints() orders them, the one whose axis stays put + * first; a table and a head, the table first. The letters are the type's + * declared orient, else those the joint map gives the joints. Three + * rotaries come tables first, each group in joint order. A type whose + * tool is turned by more than three joints and no table, a robot arm, + * orients with the A B C of its pose, all three head. Returns how many + * axes, 2 or 3, the unused entries -1; or -1 with every entry -1 where the + * type turns other rotaries between tool and work, or a letter cannot be + * found. + */ +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[3], int head[3]); + /** * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the * HAL file hands it to loadrt: the module name first, then any of diff --git a/include/kins_module.h b/include/kins_module.h index 13dc8f62411..26f803d72e0 100644 --- a/include/kins_module.h +++ b/include/kins_module.h @@ -329,10 +329,19 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, forward starts from the pose it is handed, so the shared code seeds it with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is - the module's working transform, the type G43.4 switches to. machine says - 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. */ + a working transform, the type G43.4 switches to; a module with one per + tool head flags each, G12.1 choosing the head. machine says + this is the machine frame type, the tool left out, the pivot or the flange + in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a + module that leaves it unset on every type has its identity type stand in. + A machine frame type does not read the tool offset in the parameter block, + so a working transform that leaves the tool out anyway, a robot's flange, + carries primary and machine both. orient names the axis letters that + orient the tool, first rotation then second, "AC" for instance, or three, + tables first, on a type with three rotaries; left + NULL they are the letters the joint map gives the joints that turn the + frames, which is right for a module whose forward puts each joint in its + own letter, and a module that puts one elsewhere declares them. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -344,6 +353,7 @@ typedef struct kins_ops { int identity; /* joints are axes */ int primary; /* the working transform */ int machine; /* the machine frame */ + const char *orient; /* orienting letters, NULL: from the joint map */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/include/motion_types.h b/include/motion_types.h index 60cadaa05ed..32a6426b8ea 100644 --- a/include/motion_types.h +++ b/include/motion_types.h @@ -27,4 +27,9 @@ #define EMC_SO_OVERRIDE_ON 1 #define EMC_SO_OVERRIDE_LOCK 2 +/* the frame a world jog of X, Y and Z moves along, EMC_TRAJ_SET_JOG_FRAME */ +#define EMC_JOG_FRAME_MACHINE 0 +#define EMC_JOG_FRAME_PLANE 1 +#define EMC_JOG_FRAME_TOOL 2 + #endif diff --git a/lib/python/common/hal_glib.py b/lib/python/common/hal_glib.py index 6227a3dae3b..99a1870c1af 100644 --- a/lib/python/common/hal_glib.py +++ b/lib/python/common/hal_glib.py @@ -209,6 +209,7 @@ class _GStat(GObject.GObject): 'gcode-group6-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), 'gcode-group7-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), 'gcode-group8-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), + 'gcode-group9-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), 'gcode-group10-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), 'gcode-group12-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), 'gcode-group13-changed': (GObject.SignalFlags.RUN_FIRST, GObject.TYPE_NONE, (GObject.TYPE_STRING,)), @@ -293,7 +294,8 @@ class _GStat(GObject.GObject): STATE_ON = linuxcnc.STATE_ON STATE_OFF = linuxcnc.STATE_OFF - group0 = ('G4', 'G10','G28', 'G28.2', 'G30', 'G52', 'G53', 'G92', 'G92.1', 'G92.2', 'G92.3') + group0 = ('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') group1 = ('G0', 'G1', 'G2', 'G3', 'G33', 'G38.n', 'G73', 'G76', 'G80', 'G81',\ 'G82', 'G83', 'G84', 'G85', 'G86', 'G87', 'G88', 'G89') group2 = ('G17', 'G18', 'G19', 'G17.1', 'G18.1', 'G19.1') @@ -302,7 +304,8 @@ class _GStat(GObject.GObject): group5 = ('G93', 'G94', 'G95') group6 = ('G20', 'G21') group7 = ('G40', 'G41', 'G42', 'G41.1', 'G42.1') - group8 = ('G43', 'G43.1', 'G49') + group8 = ('G43', 'G43.1', 'G43.2', 'G43.4', 'G43.5', 'G49') + group9 = ('G68.2', 'G68.3', 'G68.4', 'G69') group10 = ('G98', 'G99') group12 = ('G54', 'G55', 'G56', 'G57', 'G58', 'G59', 'G59.1', 'G59.2', 'G59.3') group13 = ('G61', 'G61.1', 'G64') @@ -509,7 +512,7 @@ def merge(self): itime = fpm = fpr = css = rpm = metric = False radius = diameter = adm = idm = False group0 = group1 = group2 = group3 = group4 = group5 = group6 = group7 = '' - group8 = group10 = group12 = group13 = group14 = group15 ='' + group8 = group9 = group10 = group12 = group13 = group14 = group15 ='' for num,i in enumerate(active_gcodes): if i == 'G90': adm = True @@ -532,6 +535,7 @@ def merge(self): elif i in self.group6: group6 = i elif i in self.group7: group7 = i elif i in self.group8: group8 = i + elif i in self.group9: group9 = i elif i in self.group10: group10 = i elif i in self.group12: group12 = i elif i in self.group13: group13 = i @@ -547,6 +551,7 @@ def merge(self): self.old['group6'] = group6 self.old['group7'] = group7 self.old['group8'] = group8 + self.old['group9'] = group9 self.old['group10'] = group10 self.old['group12'] = group12 self.old['group13'] = group13 @@ -915,7 +920,7 @@ def update(self): # G modal #################################### for g in ('group0','group1','group2','group3','group4','group5', - 'group6','group7','group8','group10','group12','group13','group14','group15'): + 'group6','group7','group8','group9','group10','group12','group13','group14','group15'): gold = old.get(g, None) gnew = self.old[g] if gold != gnew: @@ -1067,7 +1072,7 @@ def forced_update(self): self.emit('g-code-changed',g_code_new) for g in ('group0','group1','group2','group3','group4','group5', - 'group6','group7','group8','group10','group12','group13','group14','group15'): + 'group6','group7','group8','group9','group10','group12','group13','group14','group15'): gnew = self.old[g] self.emit('gcode-{}-changed'.format(g), gnew) diff --git a/lib/python/preview_helpers.py b/lib/python/preview_helpers.py index bcbb191ba1c..d87dfee0f77 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -36,7 +36,10 @@ def workplane_code(s): words are in machine units, so the block goes where the machine's G20 or G21 is in force; a later unit change converts the plane. The block selects the machine's coordinate system first, since the plane sits on - it and a later block may reselect it but not change it.""" + it and a later block may reselect it but not change it, and the tool + length offset, since under the machine frame kinematics the plane is + cut through the machine's second feature coordinate system, which the + interpreter takes from status and which comes after the tool.""" if not getattr(s, "g68_active", 0): return None r = s.g68_rotation @@ -51,5 +54,7 @@ def workplane_code(s): i = 0.0 k = atan2(-r[1], r[4]) system = next((g for g in s.gcodes[1:] if 540 <= g <= 593), 540) - return "G%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( - system // 10 if system % 10 == 0 else system / 10, o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) + length = "G43 " if s.gcodes[9] == 430 else "" + return "%sG%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( + length, system // 10 if system % 10 == 0 else system / 10, + o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) diff --git a/lib/python/qtvcp/lib/mdi_text.py b/lib/python/qtvcp/lib/mdi_text.py index 3b397c26c4b..d849229a90b 100644 --- a/lib/python/qtvcp/lib/mdi_text.py +++ b/lib/python/qtvcp/lib/mdi_text.py @@ -16,6 +16,8 @@ def gcode_titles(): 'G10L10': 'Set Tool Table, Calculated, Fixture', 'G10L11': 'Coordinate System Origin Setting', 'G10L20': 'Coordinate System Origin Setting Calculated', + 'G12.1': 'Select Kinematics', + 'G13.1': 'Select Machine Frame Kinematics', 'G17': 'Plane Select XY', 'G18': 'Plane Select XZ', 'G19': 'Plane Select YZ', @@ -28,8 +30,10 @@ def gcode_titles(): 'G28': 'Go to Predefined Position', 'G28.1': 'Set Predefined Position', 'G28.2': 'Home from G-code', + 'G28.5': 'Go to Predefined Machine Frame Position', 'G30': 'Go to Predefined Position', 'G30.1': 'Set Predefined Position', + 'G30.5': 'Go to Predefined Machine Frame Position', 'G33': 'Spindle Synchronized Motion', 'G33.1': 'Rigid Tapping', 'G38.2': 'Probing Toward Workpiece, Signal Error', @@ -44,9 +48,18 @@ def gcode_titles(): 'G43': 'Use Tool Length Offset from Tool Table', 'G43.1': 'Dynamic Tool Length Offset', 'G43.2': 'Apply additional Tool Length Offset', + 'G43.4': 'Tool Length Offset on Primary Kinematics', + 'G43.5': 'Tool Length Offset with the Tool Axis as a Vector', 'G49': 'Cancel Tool Length Offset', 'G52': 'Local Coordinate System Offset', 'G53': 'Move in Machine Coordinates', + 'G53.1': 'Orient the Tool to the Work Plane', + 'G53.2': 'Solve the Tool Orientation without Moving', + 'G53.3': 'Orient the Tool and Move in the Work Plane', + 'G53.4': 'Point-to-Point Move in Program Coordinates', + 'G53.5': 'Point-to-Point Move in the Machine Frame', + 'G53.6': 'Orient the Tool, Holding the Tool Centre Point', + 'G53.7': 'Point-to-Point Move to Joint Values', 'G54': 'Select Coordinate System 1', 'G55': 'Select Coordinate System 2', 'G56': 'Select Coordinate System 3', @@ -59,6 +72,10 @@ def gcode_titles(): 'G61': 'Exact Path Mode', 'G61.1': 'Exact Stop Mode', 'G64': 'Path Control Mode with Tolerance', + 'G68.2': 'Tilted Work Plane', + 'G68.3': 'Tilted Work Plane from the Tool Direction', + 'G68.4': 'Tilted Work Plane on the Active Plane', + 'G69': 'Cancel Tilted Work Plane', 'G73': 'Drilling Cycle with Chip Breaking', 'G74': 'Left-hand Tapping Cycle with Dwell', 'G76': 'Multi-pass Threading Cycle (Lathe)', @@ -135,6 +152,9 @@ def gcode_words(): 'G10L10': ['P', 'X', 'Y', 'Z', 'R', 'I', 'J', 'Q'], 'G10L11': ['P', 'X', 'Y', 'Z', 'R', 'I', 'J', 'Q'], 'G10L20': ['P', 'X', 'Y', 'Z'], + 'G12.1': ['P'], + 'G28.5': ['X', 'Y', 'Z', 'A', 'B', 'C'], + 'G30.5': ['X', 'Y', 'Z', 'A', 'B', 'C'], 'G33': ['X', 'Y', 'Z', 'K', 'D', '$'], 'G33.1': ['X', 'Y', 'Z', 'K', '$'], 'G38.2': ['X', 'Y', 'Z'], @@ -148,9 +168,21 @@ def gcode_words(): 'G43': ['H'], 'G43.1': ['X', 'Y', 'Z'], 'G43.2': ['H'], + 'G43.4': ['H'], + 'G43.5': ['H'], 'G52': ['X', 'Y', 'Z'], 'G53': ['X', 'Y', 'Z'], + 'G53.1': ['P', 'Q'], + 'G53.2': ['P', 'Q'], + 'G53.3': ['P', 'Q', 'X', 'Y', 'Z'], + 'G53.4': ['X', 'Y', 'Z', 'A', 'B', 'C'], + 'G53.5': ['X', 'Y', 'Z', 'A', 'B', 'C'], + 'G53.6': ['P', 'Q'], + 'G53.7': ['J'], 'G64': ['P', 'Q'], + 'G68.2': ['P', 'Q', 'X', 'Y', 'Z', 'I', 'J', 'K', 'R'], + 'G68.3': ['X', 'Y', 'Z', 'R'], + 'G68.4': ['P', 'Q', 'X', 'Y', 'Z', 'I', 'J', 'K', 'R'], 'G73': ['X', 'Y', 'Z', 'R', 'Q', 'L'], 'G74': ['X', 'Y', 'Z', 'R', 'L', 'P', '$'], 'G76': ['P', 'Z', 'I', 'J', 'K', 'Q', 'H', 'E', 'L', 'D', '$'], @@ -200,6 +232,8 @@ def gcode_descriptions(gcode): 'G10L10': G10L10, 'G10L11': G10L11, 'G10L20': G10L20, + 'G12.1': G12_1, + 'G13.1': G13_1, 'G17': G17, 'G18': G18, 'G19': G19, @@ -211,8 +245,10 @@ def gcode_descriptions(gcode): 'G28': G28, 'G28.1': G28_1, 'G28.2': G28_2, + 'G28.5': G28_5, 'G30': G30, 'G30.1': G30_1, + 'G30.5': G30_5, 'G33': G33, 'G33.1': G33_1, 'G38.2': G38_2, @@ -227,9 +263,18 @@ def gcode_descriptions(gcode): 'G43': G43, 'G43.1': G43_1, 'G43.2': G43_2, + 'G43.4': G43_4, + 'G43.5': G43_5, 'G49': G49, 'G52': G52, 'G53': G53, + 'G53.1': G53_1, + 'G53.2': G53_2, + 'G53.3': G53_3, + 'G53.4': G53_4, + 'G53.5': G53_5, + 'G53.6': G53_6, + 'G53.7': G53_7, 'G54': G54, 'G55': G55, 'G56': G56, @@ -242,6 +287,10 @@ def gcode_descriptions(gcode): 'G61': G61, 'G61.1': G61_1, 'G64': G64, + 'G68.2': G68_2, + 'G68.3': G68_3, + 'G68.4': G68_4, + 'G69': G69, 'G73': G73, 'G74': G74, 'G76': G76, @@ -411,6 +460,29 @@ def gcode_descriptions(gcode): current coordinates become the given value. """ +G12_1 = """G12.1 Select Kinematics +G12.1 P- +P = the kinematics number the module lists + +Selects one of the kinematics a switchable module +provides. Nothing moves: the mapping between joint +and coordinate positions changes, so the readout +can change. A queue synchronisation point: queued +motion finishes first, blending stops. The +selection survives the end of the program and an +abort. The active number is #<_kins_type>. +""" + +G13_1 = """G13.1 Select Machine Frame Kinematics +Cancels back to the kinematics the module declares +the machine frame: the identity on a machine whose +slides line up with its frame, the arm kinematics +with no tool on a robot. Which number that is, the +module declares, so G13.1 means the same whatever +order the module lists its types in. A queue +synchronisation point like G12.1. +""" + G17 = """G17 Plane Select G17 = XY Plane """ @@ -492,6 +564,28 @@ def gcode_descriptions(gcode): or if Pn names a joint the machine does not have. """ +G28_5 = """G28.5 Go to Predefined Machine Frame Position +G28.5 + +G28.5 is to G28 what G53.5 is to G53: the position +stored in #5161-#5169 is read in the machine frame +with the orientation left out, the carriage or the +flange in machine coordinates, and the move is a +point-to-point move there, rapid, with no offset, +tool length, rotation or work plane applied. With +axes, those letters go to the given machine frame +position first, then to their stored positions. +Store the position with G28.1 while the machine +frame kinematics is in force (G13.1). +""" + +G30_5 = """G30.5 Go to Predefined Machine Frame Position +G30.5 + +As G28.5, with the position stored in #5181-#5189 +by G30.1. +""" + G30 = """G30 Go to Predefined Position G30 uses the values stored in parameters 5181-5189 as the X Y Z A B C U V W final point to move to. @@ -698,6 +792,37 @@ def gcode_descriptions(gcode): location. """ +G43_4 = """G43.4 Tool Length Offset on Primary Kinematics +G43.4 Hn +n = tool number, Hn is optional + +G43 together with a switch to the kinematics the +module declares its working transform, tool centre +point control, so the program runs with tool length +compensation in that kinematics. The switch happens +first, the offset applies after it. G49 cancels the +offset and switches back to the machine frame +kinematics. Rejected on a switchable module that +declares no primary kinematics; a plain G43 on a +machine that does not switch. +""" + +G43_5 = """G43.5 Tool Length Offset with the Tool Axis as a Vector +G43.5 Hn +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- + +G43.4 with one thing more: while it is in effect a +G0 or G1 line may give the tool axis direction as a +vector I J K, from the tip towards the holder, in +place of rotary words, and the interpreter works +out where the rotaries have to go. The pose nearest +where the rotaries stand is taken. G2 and G3 keep +I J K as the arc centre. It is an error to give +I J K with a rotary word, or a zero vector, or on +the identity kinematics. +""" + G49 = """G49 Cancel Tool Length Compensation """ @@ -719,6 +844,103 @@ def gcode_descriptions(gcode): line if one is currently active. """ +G53_1 = """G53.1 Orient the Tool to the Work Plane +G53.1 + +Turns the rotaries until the tool axis is normal to +the active tilted work plane (G68.2). The linear +joints stay where they are and the tip swings with +them: a point-to-point move. P picks among the +poses that reach the plane, nearest first: P0 (the +default) the nearest, P1 and P2 the secondary +rotary positive or negative. Q0 (the default) holds +the joints that carry the work, Q1 frees them. An +error with no plane active, or on the identity +kinematics. +""" + +G53_2 = """G53.2 Solve the Tool Orientation without Moving +G53.2 + +Solves the same pose as G53.1 and moves nothing. +The pose is published on #<_orient_x> #<_orient_y> +#<_orient_z> and the rotaries in the order the +kinematics orients with, #<_orient_rot1> to +#<_orient_rot3>, and on #5071 to #5076, in program +units in the plane, with #<_orient_valid> (#5080) +set to 1, so the program can reach it with a move +of its own, for instance a single G0 naming X Y Z +and the rotary words together. P and Q as for +G53.1. +""" + +G53_3 = """G53.3 Orient the Tool and Move in the Work Plane +G53.3 X- Y- Z- + +Turns the rotaries as G53.1 does and takes the tool +to X Y Z, given in the plane, in one point-to-point +move. A word left out keeps the present value. P +and Q as for G53.1. +""" + +G53_4 = """G53.4 Point-to-Point Move in Program Coordinates +G53.4 G0 +G53.4 G1 F- + +A point-to-point move defines its two ends and +leaves the path to the joints: the inverse +kinematics runs once at the destination and every +joint travels from where it is to where it must be, +all together, the slowest setting the pace. The +tool does not follow a straight line. G53.4 takes +the destination in program coordinates, through +the offsets and the work plane like any other +move: the move to cross a singularity or turn a +head right round without leaving the coordinate +system. Non-modal, on a line with G0 or G1 in +force; with G1 it takes the time the straight move +would at F. +""" + +G53_5 = """G53.5 Point-to-Point Move in the Machine Frame +G53.5 G0 +G53.5 G1 F- + +A point-to-point move (see G53.4) to a position in +the machine frame with the tool left out: X Y Z the +point the tool hangs from, the pivot of a head or +the flange of a robot, in machine coordinates, the +rotary letters the orientation as the machine frame +kinematics reports it, on a mill the rotary joints. +Program units, no offset, tool length, rotation or +work plane. A letter not given holds its machine +frame coordinate. The form for parking and tool +change positions, whatever the head is doing. +Absolute distance mode only. +""" + +G53_6 = """G53.6 Orient the Tool, Holding the Tool Centre Point +G53.6 + +Turns the rotaries until the tool is normal to the +active work plane, as G53.1, keeping the tool +centre point where it is: a Cartesian move of the +rotary words, the kinematics compensating the +linear joints all along. P and Q as for G53.1. +""" + +G53_7 = """G53.7 Point-to-Point Move to Joint Values +G53.7 G0 J= ... +G53.7 G1 F- J= ... + +A point-to-point move (see G53.4) to joint values, +one word per joint: J2=-5 sends joint 2 to -5 in +the joint's own units, nothing converted, no offset +of any kind. A joint not named holds. The form that +works on every machine, a robot included. Absolute +distance mode only; the J= word is read only here. +""" + G54 = """G54 Select Coordinate System G54 = select coordinate system 1 """ @@ -788,6 +1010,51 @@ def gcode_descriptions(gcode): linear move. """ +G68_2 = """G68.2 Tilted Work Plane +G68.2 X- Y- Z- I- J- K- (Euler angles) +G68.2 P1 X- Y- Z- I- J- K- (angles about fixed axes) +G68.2 P2 Q0..Q3 X- Y- Z- (three points, one per block) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: origin and +X,) +G68.2 P3 Q2 I- J- K- (then +Z, the normal) + +Defines a plane on top of the active coordinate +system, with its own cancel (G69), so the blocks +that follow are programmed in the tilted plane +while G54 itself is untouched. X Y Z is the plane's +origin, the rest of the words its rotation, both in +the coordinate system active when the plane is +defined. P selects the form, numbered as Fanuc +numbers them; Q names the axis order of the angle +forms (Q313 and Q123 the defaults) or the block of +the point and vector forms. R turns the plane about +its own Z after everything else. The plane does not +move the tool: G53.1, G53.3 or G53.6 orient it. +""" + +G68_3 = """G68.3 Tilted Work Plane from the Tool Direction +G68.3 X- Y- Z- + +Defines a plane whose Z is the tool axis as the +rotary joints stand now, with the origin at X Y Z +and R turning it about that Z. An error where the +kinematics cannot be evaluated by the interpreter, +is the identity, or reports no tool frame. +""" + +G68_4 = """G68.4 Tilted Work Plane on the Active Plane +G68.4 (any G68.2 form) + +Takes any G68.2 form and composes it onto the plane +already active, so the new plane is given in the +coordinates of the old one. An error with no plane +active. +""" + +G69 = """G69 Cancel Tilted Work Plane +Cancels the plane. So does the end of the program, +M2 or M30, and an abort. +""" + G73 = """G73 Drilling Cycle with Chip Breaking G73 X Y Z R Q L R = retract position along the Z axis diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 264570e48c9..6873530cb4e 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -1265,6 +1265,19 @@ def posstrs(self): positions = s.actual_position if self.get_show_relative(): + k = getattr(s, 'g68_kins_rotation', None) if s.g68_active else None + if k: + # off the second feature coordinate system a plane is + # cut through under the machine frame kinematics, which + # comes after the whole offset chain + positions = list(positions) + o = s.g68_kins_offset + _x = positions[X] - o[X] + _y = positions[Y] - o[Y] + _z = positions[Z] - o[Z] + positions[X] = k[0]*_x + k[3]*_y + k[6]*_z + positions[Y] = k[1]*_x + k[4]*_y + k[7]*_z + positions[Z] = k[2]*_x + k[5]*_y + k[8]*_z positions = [(i-j) for i, j in zip(positions, s.tool_offset)] positions = [(i-j) for i, j in zip(positions, s.g5x_offset)] diff --git a/lib/python/rs274/glcanon_bake.py b/lib/python/rs274/glcanon_bake.py index d42ff29ea97..b866239d65b 100644 --- a/lib/python/rs274/glcanon_bake.py +++ b/lib/python/rs274/glcanon_bake.py @@ -381,8 +381,9 @@ def axis_positions(self) -> npt.NDArray[np.float32]: and ``kindtool[i]`` - event vertices included, since axes are dropped as columns and never as rows. The coordinates are machine-frame, after g92 -> XY rotation -> g5x, which is the point the drawn planes are - built from; they are not the numbers as written in the program, and an - arc has already been segmented by the time it reaches here. + built from, read back onto the part first under the machine frame + kinematics; they are not the numbers as written in the program, and + an arc has already been segmented by the time it reaches here. ``(N, 0)`` unless the parse was configured with ``want_axis_positions``. diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 66a491ed2bc..517bd17548b 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -1808,13 +1808,28 @@ def _draw_foam(self, ctx: FrameContext, pos: Sequence[float], def _draw_at_tool(self, ctx: FrameContext, pos: Sequence[float], rx: float, ry: float, rz: float) -> None: ctx.mv.translate(*pos) - self._apply_rotary(ctx, rx, ry, rz) + if not self._apply_tool_frame(ctx): + self._apply_rotary(ctx, rx, ry, rz) current_tool = ctx.current_tool() if current_tool is None or current_tool.diameter <= ctx.view_tool_min_dia: self._draw_cone(ctx) else: self.draw_solid(ctx, current_tool) + @staticmethod + def _apply_tool_frame(ctx: FrameContext) -> bool: + """Turn the marker into the tool frame the kinematics reports, in + the work frame, and say whether it did. A module that reports its + frames knows where the tool points better than GEOMETRY does, so + GEOMETRY is only the fallback for one that does not.""" + frame = getattr(ctx.stat, 'tool_frame', None) + if frame is None: + return False + m = glnav.identity_matrix() + m[:3, :3] = np.asarray(frame, dtype=np.float64).reshape(3, 3) + ctx.mv.mult(m) + return True + @staticmethod def _apply_rotary(ctx: FrameContext, rx: float, ry: float, rz: float) -> None: @@ -2743,10 +2758,14 @@ def active_workplane(ctx: FrameContext) -> "WorkPlane | None": Status carries it as the interpreter gave it, the origin in the coordinate system the plane was defined in and in machine units, so it goes through the g92 offset, the XY rotation and the g5x offset the way - the canon takes a definition, in the canon's units. Where it is one the - loaded program defined, that record is returned, extents and all; a - plane from MDI, or from a program no longer loaded, comes back as a - record of its own with nothing under it. + the canon takes a definition, in the canon's units. It is drawn on the + part, as defined: under the machine frame kinematics the second feature + coordinate system status has apart carries it onto the slides, but the + renderer draws the program under it on the part too, and the backplot + and the tool follow. Where it is one the loaded program defined, that + record is returned, extents and all; a plane from MDI, or from a program + no longer loaded, comes back as a record of its own with nothing under + it. """ s = ctx.stat if s is None or not getattr(s, 'g68_active', 0): diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 3c83b5502a8..9f86a99e1ac 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -193,5 +193,22 @@ 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_external_g68_kins_frame(self): + # the second feature coordinate system the machine's plane is cut + # through, which a plane restated from status is cut through too + return (tuple(self.s.g68_kins_offset[:3]), tuple(self.s.g68_kins_rotation)) + + def get_kinematics_type(self): + return self.s.kinematics_type + + 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/share/axis/tcl/axis.tcl b/share/axis/tcl/axis.tcl index 2b99ad86ae8..f1654e8f8f3 100644 --- a/share/axis/tcl/axis.tcl +++ b/share/axis/tcl/axis.tcl @@ -424,6 +424,31 @@ setup_menu_accel .menu.view end [_ "Show relative position"] .menu.view add separator +.menu.view add radiobutton \ + -value 0 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in machine frame"] + +.menu.view add radiobutton \ + -value 1 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in work plane frame"] + +.menu.view add radiobutton \ + -value 2 \ + -variable jog_frame \ + -accelerator % \ + -command set_jog_frame +setup_menu_accel .menu.view end [_ "Jog in tool frame"] + +# kept last: AXIS takes the separator and the two entries out on an +# identity kinematics +.menu.view add separator + .menu.view add radiobutton \ -value 0 \ -variable teleop_mode \ @@ -1378,6 +1403,14 @@ label .info.program_time \ -textvariable ::program_remaining \ -width 22 +# packed after .info.position while jogs move along a frame other than +# the machine's +label .info.jog_frame \ + -anchor w \ + -borderwidth 2 \ + -relief sunken \ + -textvariable ::jog_frame_string + # Pack widget .info.task_state pack .info.task_state \ -side left @@ -1885,6 +1918,14 @@ proc update_state {args} { set ::position [concat [_ "Position:"] $coord_str $display_str] } + if {$::jog_frame == 0} { + pack forget .info.jog_frame + } else { + set ::jog_frame_string [concat [_ "Jog:"] \ + [lindex [list [_ Machine] [_ Plane] [_ Tool]] $::jog_frame]] + pack .info.jog_frame -side left -after .info.position + } + if {$::task_state == $::STATE_ON && $::interp_state == $::INTERP_IDLE} { if { ($::last_interp_state != $::INTERP_IDLE || $::last_task_state != $::task_state) \ && $::task_mode == $::TASK_MODE_AUTO} { @@ -2005,6 +2046,7 @@ set coord_type 1 set display_type 0 set spindledir {} set motion_mode 0 +set jog_frame 0 set kinematics_type -1 set metric 0 set max_speed 1 @@ -2026,6 +2068,7 @@ trace add variable spindledir write queue_update_state trace add variable coord_type write queue_update_state trace add variable display_type write queue_update_state trace add variable motion_mode write queue_update_state +trace add variable jog_frame write queue_update_state trace add variable kinematics_type write queue_update_state trace add variable on_any_limit write queue_update_state trace add variable motion_mode write joint_mode_switch diff --git a/src/Makefile b/src/Makefile index e24509ada47..d2ed535f478 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1224,6 +1224,15 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) + +obj-m += twinspindlekins.o +twinspindlekins-objs := emc/kinematics/twinspindlekins.o +twinspindlekins-objs += libposemath/_posemath.o +twinspindlekins-objs += $(MATHSTUB) +twinspindlekins-objs += emc/kinematics/kins_util.o +twinspindlekins-objs += emc/kinematics/switchkins.o +twinspindlekins-objs += emc/kinematics/switchkins_main.o +twinspindlekins-objs += emc/kinematics/switchkins_setup.o #---------------------------------------------------------------- obj-$(CONFIG_MOTMOD) += motmod.o @@ -1390,6 +1399,7 @@ endif ../rtlib/5axiskins$(MODULE_EXT): $(addprefix objects/rt,$(5axiskins-objs)) ../rtlib/switchkins_core$(MODULE_EXT): $(addprefix objects/rt,$(switchkins_core-objs)) ../rtlib/kins_single_core$(MODULE_EXT): $(addprefix objects/rt,$(kins_single_core-objs)) +../rtlib/twinspindlekins$(MODULE_EXT): $(addprefix objects/rt,$(twinspindlekins-objs)) ../rtlib/maxkins$(MODULE_EXT): $(addprefix objects/rt,$(maxkins-objs)) ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs)) ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs)) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index c43156490c9..fee3a206ab8 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -333,6 +333,65 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* What compute_jfwd() gives, straight from the DH frames in the base + frame: joint i moves along or turns about z_i, the axis of frame i + through its origin o_i, so the last frame's origin moves z_i or + z_i x (o_n - o_i) and turns 0 or z_i. The planner asks for many + Jacobians along each move, and this one skips the quaternions. */ +static void genser_jfwd_frames(const genser_struct *genser, const go_real *jest, + go_matrix *Jfwd) +{ + double R[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + double o[3] = {0, 0, 0}; + double z[GENSER_MAX_JOINTS][3], oi[GENSER_MAX_JOINTS][3]; + int n = genser->link_num, i, r, c; + + for (i = 0; i < n; i++) { + const go_link *link = &genser->links[i]; + int prismatic = GO_QUANTITY_LENGTH == link->quantity; + double th = prismatic ? link->u.dh.theta : jest[i]; + double d = prismatic ? jest[i] : link->u.dh.d; + double sal = sin(link->u.dh.alpha), cal = cos(link->u.dh.alpha); + double sth = sin(th), cth = cos(th); + // as go_dh_pose_convert(): Rx(alpha) Tx(a) Rz(theta) Tz(d) + double L[3][3] = {{cth, -sth, 0}, + {sth * cal, cth * cal, -sal}, + {sth * sal, cth * sal, cal}}; + double t[3] = {link->u.dh.a, -sal * d, cal * d}; + double N[3][3]; + + for (r = 0; r < 3; r++) { + o[r] += R[r][0] * t[0] + R[r][1] * t[1] + R[r][2] * t[2]; + } + for (r = 0; r < 3; r++) { + for (c = 0; c < 3; c++) { + N[r][c] = R[r][0] * L[0][c] + R[r][1] * L[1][c] + R[r][2] * L[2][c]; + } + } + memcpy(R, N, sizeof(R)); + for (r = 0; r < 3; r++) { + z[i][r] = R[r][2]; + oi[i][r] = o[r]; + } + } + for (i = 0; i < n; i++) { + double v[3] = {o[0] - oi[i][0], o[1] - oi[i][1], o[2] - oi[i][2]}; + if (GO_QUANTITY_LENGTH == genser->links[i].quantity) { + for (r = 0; r < 3; r++) { + Jfwd->el[r][i] = z[i][r]; + Jfwd->el[3 + r][i] = 0; + } + continue; + } + Jfwd->el[0][i] = z[i][1] * v[2] - z[i][2] * v[1]; + Jfwd->el[1][i] = z[i][2] * v[0] - z[i][0] * v[2]; + Jfwd->el[2][i] = z[i][0] * v[1] - z[i][1] * v[0]; + for (r = 0; r < 3; r++) { + Jfwd->el[3 + r][i] = z[i][r]; + } + } +} + /* The Jacobian in the terms of kinematics.h: joints in degrees per pose word in EmcPose units, the derivative of genser_inverse(). @@ -356,8 +415,6 @@ static int genser_jacobian(const kins_params *p, const double *joint, genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; - go_link linkout[GENSER_MAX_JOINTS] = {}; go_real jest[GENSER_MAX_JOINTS]; double E[3][3]; double sb, cb, sc, cc; @@ -379,14 +436,7 @@ static int genser_jacobian(const kins_params *p, const double *joint, go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); - for (link = 0; link < genser->link_num; link++) { - retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); - if (GO_RESULT_OK != retval) - return -1; - } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) - return -1; + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GO_RESULT_OK != retval) return -1; // singular: no finite joint rate follows the pose @@ -530,7 +580,6 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; @@ -575,12 +624,7 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) { - rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", - joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - return retval; - } + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GO_RESULT_OK != retval) { rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", @@ -631,6 +675,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 +733,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_single.c b/src/emc/kinematics/kins_single.c index 839313ef965..6ff60667663 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -150,6 +150,14 @@ int kinematicsTypeFlags(int ktype) return -1; } +// and so no machine frame type either +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + (void)joint; + (void)pos; + return -1; +} + // The module's description, for a copy of it loaded outside RT. A module // with one type does not depend on its parameters for its shape, so this // is the table as declared. @@ -171,5 +179,6 @@ EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(kinsSingleInit); EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/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/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index e90b35d14c8..4bbf55613c3 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -310,45 +310,59 @@ int kinematicsInverse(const EmcPose * pos, return r; } // kinematicsInverse() -int kinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +int kinematicsToolFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { int r; - if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } - if (kops[switchkins_type]) { - read_block(switchkins_type); - return kinsOpsToolFrame(kops[switchkins_type], &rt_params, - joint, rot, fflags); + if (ktype < 0 || ktype >= kins_count) { return -1; } + if (kops[ktype]) { + read_block(ktype); + return kinsOpsToolFrame(kops[ktype], &rt_params, joint, rot, fflags); } - if (!ktools[switchkins_type]) { + if (!ktools[ktype]) { return -1; // this type does not supply one; not an error } - r = ktools[switchkins_type](joint, rot, fflags); + r = ktools[ktype](joint, rot, fflags); if (r) { return r; } // the type answers in its own frame; put it in the convention here so // no module has to get the half turn right for itself - return toolFrameApplyNative(rot, &knative[switchkins_type]); -} // kinematicsToolFrame() + return toolFrameApplyNative(rot, &knative[ktype]); +} // kinematicsToolFrameOf() -int kinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +int kinematicsWorkFrameOf(int ktype, + const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { - if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } - if (kops[switchkins_type]) { - read_block(switchkins_type); - return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, - joint, rot, fflags); + if (ktype < 0 || ktype >= kins_count) { return -1; } + if (kops[ktype]) { + read_block(ktype); + return kinsOpsWorkFrame(kops[ktype], &rt_params, joint, rot, fflags); } - if (!kworks[switchkins_type]) { + if (!kworks[ktype]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the // wrong way, so there are not two conventions for it to be caught between - return kworks[switchkins_type](joint, rot, fflags); + return kworks[ktype](joint, rot, fflags); +} // kinematicsWorkFrameOf() + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinematicsToolFrameOf(switchkins_type, joint, rot, fflags); +} // kinematicsToolFrame() + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinematicsWorkFrameOf(switchkins_type, joint, rot, fflags); } // kinematicsWorkFrame() int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, @@ -461,6 +475,30 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + int k; + + for (k = 0; k < kins_count; k++) { + int f = kinematicsTypeFlags(k); + if (f < 0 || !(f & KINSTYPE_MACHINE)) { continue; } + return call_forward(k, joint, pos, &fflags, &iflags) == 0 ? 0 : -2; + } + return -1; +} // kinematicsMachineFrame() + +int kinematicsForwardOf(int ktype, + const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (kinematicsTypeFlags(ktype) < 0) { return -1; } + return call_forward(ktype, joint, pos, fflags, iflags); +} // kinematicsForwardOf() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -561,6 +599,8 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrameOf); +EXPORT_SYMBOL(kinematicsWorkFrameOf); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsSetTool); @@ -569,6 +609,8 @@ EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); +EXPORT_SYMBOL(kinematicsForwardOf); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); @@ -701,7 +743,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities, primaries, machines; + int identities, machines; int res = 0; char* emsg = "other"; @@ -729,13 +771,13 @@ int switchkinsInit(const int comp_id, } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } - // declarations must name provided types, and each flag is unique: - // G13.1 and G49 resolve the machine frame type from the flags and - // G43.4 the primary, so two answers is a load error. A module that + // declarations must name provided types, and identity and machine + // are unique: G13.1 and G49 resolve the machine frame type from the + // flags, so two answers is a load error. Several types may be + // primary, one per tool head, G12.1 choosing among them. A module that // names no machine frame type has its identity type stand in, which // is the truth on every machine whose slides line up with its frame. identities = 0; - primaries = 0; machines = 0; for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } @@ -752,7 +794,6 @@ int switchkinsInit(const int comp_id, identities++; if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } } - if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", @@ -761,9 +802,6 @@ int switchkinsInit(const int comp_id, if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } - if (primaries > 1) { - emsg = "more than one primary switchkins-type declared"; goto error; - } if (machines > 1) { emsg = "more than one machine frame switchkins-type declared"; goto error; } diff --git a/src/emc/kinematics/twinspindlekins.c b/src/emc/kinematics/twinspindlekins.c new file mode 100644 index 00000000000..97019c25305 --- /dev/null +++ b/src/emc/kinematics/twinspindlekins.c @@ -0,0 +1,278 @@ +/******************************************************************** +* Description: twinspindlekins.c +* kinematics for a mill-turn with a tilting head and two spindles +* +* License: GPL Version 2 +* +* Notes: +* 1) The head carries the tool on X Y Z and tilts it on B, about Y, from +* a pivot pivot-length above the gauge line. With B at 0 the tool +* points along -Z, tool axis +Z, and the slides read the gauge line. +* The pose is the tip plus the tool length along the part's Z, which +* G43 takes off again, so a tilt moves the tip by the length along +* the tool axis less the length along Z the offset stands for. +* 2) The main spindle turns the work about machine Z on C, its face in +* the plane Z 0. The sub spindle faces it from spindle-distance along +* +Z and turns its own work on W, which the INI makes ANGULAR. +* 3) Type 0 works on the part in the main spindle and type 1 on the part +* in the sub spindle, each in that part's own frame: X Y Z of the part +* and B, C or W the orientation. Both are primary: G12.1 picks the +* spindle. Type 2 is the identity and stands in for the machine frame. +* 4) The sub spindle part frame is the main spindle's turned half a turn +* about X: X stays, Y and Z reverse, so Z still points out of the face +* towards the tool. Moved to the other spindle, a program reads the +* same. +* 5) The rotary directions are the conventional ones: C and W turn the +* work the way that turns the tool positively about the part's Z. +* 6) Coordinates XYZBCW are required, A U V may be added with the +* coordinates parameter and map one to one to their joints. +********************************************************************/ + +#define REQUIRED_COORDINATES "XYZBCW" + +#include +#include +#include +#include + +#include + +static const kins_param_desc twin_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 150.0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "spindle-distance", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH, P_SPINDLE_DISTANCE }; + +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the tool tip in the machine frame, from the slides and B (note 1) +static PmCartesian tip_from_joints(const kins_params *p, const double *joints) +{ + const double P = p->geometry[P_PIVOT_LENGTH]; + const double L = P + p->tool.tran.z; + const double b = joints[JB]*TO_RAD; + PmCartesian t; + + t.x = joints[JX] - L*sin(b); + t.y = joints[JY]; + t.z = joints[JZ] + P - L*cos(b); + return t; +} // tip_from_joints() + +// the part frame of each spindle, the machine seen from the part: machine +// = O + R part, with R = Rz(-C) for the main spindle and Rx(180) Rz(-W) for +// the sub spindle (notes 2, 4, 5) +static PmCartesian part_from_machine(const kins_params *p, int sub, + double angle, PmCartesian m) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, r; + + q = m; + if (sub) { + q.y = -m.y; + q.z = p->geometry[P_SPINDLE_DISTANCE] - m.z; + } + r.x = c*q.x - s*q.y; + r.y = s*q.x + c*q.y; + r.z = q.z; + return r; +} // part_from_machine() + +static PmCartesian machine_from_part(const kins_params *p, int sub, + double angle, PmCartesian r) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, m; + + q.x = c*r.x + s*r.y; + q.y = -s*r.x + c*r.y; + q.z = r.z; + m = q; + if (sub) { + m.y = -q.y; + m.z = p->geometry[P_SPINDLE_DISTANCE] - q.z; + } + return m; +} // machine_from_part() + +static int twin_forward(const kins_params *p, int sub, const double *joints, + EmcPose *pos) +{ + const double angle = sub ? joints[JW] : joints[JC]; + + pos->tran = part_from_machine(p, sub, angle, tip_from_joints(p, joints)); + pos->tran.z += p->tool.tran.z; + pos->b = joints[JB]; + pos->c = joints[JC]; + pos->w = joints[JW]; + + // optional letters (specify with coordinates module parameter) + pos->a = (JA != -1)? joints[JA] : 0; + pos->u = (JU != -1)? joints[JU] : 0; + pos->v = (JV != -1)? joints[JV] : 0; + + return 0; +} // twin_forward() + +static int twin_inverse(const kins_params *p, int sub, const EmcPose *pos, + double *joints) +{ + const double T = p->tool.tran.z; + const double L = p->geometry[P_PIVOT_LENGTH] + T; + const double b = pos->b*TO_RAD; + PmCartesian tip = pos->tran; + PmCartesian t; + EmcPose P; // computed position + + tip.z -= T; + t = machine_from_part(p, sub, sub ? pos->w : pos->c, tip); + P.tran.x = t.x + L*sin(b); + P.tran.y = t.y; + P.tran.z = t.z - p->geometry[P_PIVOT_LENGTH] + L*cos(b); + P.b = pos->b; + P.c = pos->c; + P.w = pos->w; + + // optional letters (specify with coordinates module parameter) + P.a = (JA != -1)? pos->a : 0; + P.u = (JU != -1)? pos->u : 0; + P.v = (JV != -1)? pos->v : 0; + + return kinsPoseToMappedJoints(p, &P, joints); +} // twin_inverse() + +static int main_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 0, joints, pos); +} + +static int main_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 0, pos, joints); +} + +static int sub_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 1, joints, pos); +} + +static int sub_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 1, pos, joints); +} + +// the tool frame, Ry(B), is the same on both spindles +static int twin_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + + rot->x.x = cb; rot->y.x = 0; rot->z.x = sb; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = -sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} // twin_tool_frame() + +// the work frames, the R of part_from_machine(), its columns the part axes +static int main_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JC]*TO_RAD), c = cos(joints[JC]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = -s; rot->y.y = c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; + + return 0; +} // main_work_frame() + +static int sub_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JW]*TO_RAD), c = cos(joints[JW]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = s; rot->y.y = -c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = -1; + + return 0; +} // sub_work_frame() + +// each spindle orients with its own rotary, the table first, and B; the +// joint map gives the same letters, declared so the table reads +static const kins_ops main_ops = { + .forward = main_forward, + .inverse = main_inverse, + .work = main_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "CB", +}; + +static const kins_ops sub_ops = { + .forward = sub_forward, + .inverse = sub_inverse, + .work = sub_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "WB", +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "twinspindlekins"; // !!! must agree with filename + kp->halprefix = "twinspindlekins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = twin_params; + kp->nparams = sizeof(twin_params)/sizeof(twin_params[0]); + + switchkinsRegisterOps(0, &main_ops); + switchkinsRegisterOps(1, &sub_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + + return 0; +} // switchkinsSetup() diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 92de369697a..24631d01df0 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -25,6 +25,7 @@ ********************************************************************/ #include +#include #include #include #include @@ -61,6 +62,10 @@ struct KinematicsUserContext { int warned_tool; EmcPose caller_tool; /* from kinematicsUserSetTool() */ int have_caller_tool; + double motion_tool[AXIS_COUNT]; /* motion.tooloffset.*, as last read */ + int have_motion_tool; + int hold; /* kinematicsUserHoldPins() */ + int held; /* and the pins read since */ double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; @@ -163,15 +168,15 @@ static int note_all(KinematicsUserContext *ctx) } /* The block sees the pins as they are now, and the tool of whoever - knows it best: the caller, then motion, then the module's own pin. */ + knows it best: the caller, then motion, then the module's own pin. + Held, it sees the pins as they read when the hold began. */ static void refresh(KinematicsUserContext *ctx) { int i; - double tool[AXIS_COUNT]; - int have_motion_tool = 0; + int fresh = !ctx->hold || !ctx->held; /* a pin that stops answering, its module unloaded, keeps its last value */ - for (i = 0; i < ctx->info.nparams; i++) { + for (i = 0; fresh && i < ctx->info.nparams; i++) { int c = ctx->pin_of_param[i]; double value; if (c < 0) continue; @@ -188,17 +193,22 @@ static void refresh(KinematicsUserContext *ctx) if (ctx->tool_param >= 0) { ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; } + ctx->held = ctx->hold; return; } - for (i = 0; i < AXIS_COUNT; i++) { - int c = ctx->pin_of_tool[i]; - tool[i] = 0.0; - if (c < 0) continue; - if (pin_value(ctx->pin_name[c], HAL_REAL, &tool[i]) != 0) continue; - have_motion_tool = 1; + if (fresh) { + ctx->have_motion_tool = 0; + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->pin_of_tool[i]; + ctx->motion_tool[i] = 0.0; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], HAL_REAL, &ctx->motion_tool[i]) != 0) continue; + ctx->have_motion_tool = 1; + } } - if (!have_motion_tool) return; + ctx->held = ctx->hold; + if (!ctx->have_motion_tool) return; /* the module's pin and motion disagree: the config lost the tool somewhere between them. Say so once; motion's value is the one @@ -206,17 +216,17 @@ static void refresh(KinematicsUserContext *ctx) disagreement. */ if (ctx->tool_param >= 0 && !ctx->warned_tool && ctx->params.geometry[ctx->tool_param] != 0.0 - && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + && fabs(ctx->motion_tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { fprintf(stderr, "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" " using motion's value\n", ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, - ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->params.geometry[ctx->tool_param], ctx->motion_tool[AXIS_Z]); ctx->warned_tool = 1; } - for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, ctx->motion_tool[i]); if (ctx->tool_param >= 0) { - ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; + ctx->params.geometry[ctx->tool_param] = ctx->motion_tool[AXIS_Z]; } } @@ -469,10 +479,20 @@ int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) ctx->have_caller_tool = 1; } else { ctx->have_caller_tool = 0; + /* motion's tool, which a hold begun with the caller's never read */ + ctx->held = 0; } return 0; } +int kinematicsUserHoldPins(KinematicsUserContext* ctx, int hold) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + ctx->hold = hold != 0; + ctx->held = 0; + return 0; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) @@ -532,9 +552,7 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, { KINEMATICS_INVERSE_FLAGS iflags = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; double j[EMCMOT_MAX_JOINTS]; - int r, a; if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; @@ -546,6 +564,24 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, world, j, &iflags, &fflags) != 0) { return -1; } + return kinematicsUserJacobianAt(ctx, j, world, J); +} + +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS] = {0}; + int r, a; + + if (!ctx || !ctx->initialized || !joints || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) j[r] = joints[r]; if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, jac, &iflags) != 0) { return -1; @@ -583,6 +619,7 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + ctx->held = 0; refresh(ctx); return 0; } @@ -751,6 +788,102 @@ int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, return r; } +// the letter of joint j: the principal one, else any that maps to it +static int joint_letter(const kins_params *p, int j) +{ + int a; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joint_of_axis[a] == j) { return a; } + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joints_of_axis[a] & (1u << j)) { return a; } + } + return -1; +} + +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[3], int head[3]) +{ + static const char letters[] = "XYZABCUVW"; + const kins_ops *ops; + double j[EMCMOT_MAX_JOINTS]; + unsigned int table_mask = 0, head_mask = 0; + int joints[2] = {-1, -1}; + int i, r = -1; + + if (!axes || !head) return -1; + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + ops = ctx->info.ops[ctx->ktype]; + if (!ops->tool || !ops->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + if (toolFrameWorkJoints(frame_work, ctx->num_joints, j, &table_mask) == 0 + && toolFrameWorkJoints(frame_tool, ctx->num_joints, j, &head_mask) == 0 + && !(table_mask & head_mask)) { + int tables = __builtin_popcount(table_mask), heads = __builtin_popcount(head_mask); + if (heads == 2 && tables == 0) { + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 1; + } else if (tables == 2 && heads == 0) { + r = toolFrameOrientJoints(frame_work, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 0; + } else if (tables == 1 && heads == 1) { + joints[0] = __builtin_ctz(table_mask); + joints[1] = __builtin_ctz(head_mask); + head[0] = 0; + head[1] = 1; + r = 0; + } else if (tables + heads == 3) { + // three rotaries: the tables first, each group in joint order + int n = 0, jn; + for (jn = 0; jn < ctx->num_joints; jn++) { + if (table_mask & (1u << jn)) { axes[n] = jn; head[n++] = 0; } + } + for (jn = 0; jn < ctx->num_joints; jn++) { + if (head_mask & (1u << jn)) { axes[n] = jn; head[n++] = 1; } + } + frame_ctx = NULL; + for (n = 0; n < 3; n++) { + if (ops->orient && strlen(ops->orient) >= 3) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[n])); + axes[n] = at ? (int)(at - letters) : -1; + } else { + axes[n] = joint_letter(&ctx->params, axes[n]); + } + } + if (axes[0] < 0 || axes[1] < 0 || axes[2] < 0) { + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + return -1; + } + return 3; + } else if (heads > 3 && tables == 0) { + // an arm: the wrist is one of many joints that turn the tool, + // and the pose gives the orientation as A B C + for (i = 0; i < 3; i++) { axes[i] = 3 + i; head[i] = 1; } + frame_ctx = NULL; + return 3; + } + } + frame_ctx = NULL; + for (i = 0; i < 2 && r == 0; i++) { + if (ops->orient && ops->orient[0] && ops->orient[1]) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[i])); + axes[i] = at ? (int)(at - letters) : -1; + } else { + axes[i] = joint_letter(&ctx->params, joints[i]); + } + if (axes[i] < 0) { r = -1; } + } + if (r != 0) { + axes[0] = axes[1] = head[0] = head[1] = -1; + return -1; + } + return 2; +} + KinematicsUserContext* kinematicsUserInitString(const char* kinematics, int num_joints, int comp_id, diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 28978ae80e8..52e7b219159 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -700,6 +700,20 @@ int main(int argc, char* argv[]) { c->probe_home_err_inhibit); break; + case EMCMOT_SET_WORK_PLANE: + log_print( + "SET_WORK_PLANE active=%d, x=(%.6g %.6g %.6g), y=(%.6g %.6g %.6g), z=(%.6g %.6g %.6g)\n", + c->work_plane_active, + c->work_plane[0], c->work_plane[3], c->work_plane[6], + c->work_plane[1], c->work_plane[4], c->work_plane[7], + c->work_plane[2], c->work_plane[5], c->work_plane[8] + ); + break; + + case EMCMOT_SET_JOG_FRAME: + log_print("SET_JOG_FRAME %d\n", c->jog_frame); + break; + default: log_print("ERROR: unknown command %d\n", c->command); diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c index f6dd8d4aef0..e60ea61d316 100644 --- a/src/emc/motion/axis.c +++ b/src/emc/motion/axis.c @@ -1,6 +1,7 @@ #include #include +#include // memset #include // EMCMOT_MAX_AXIS #include "axis.h" @@ -26,6 +27,8 @@ typedef struct { double acc_limit; /* upper limit of axis accel */ double jerk_limit; /* upper limit of axis jerk */ simple_tp_t teleop_tp; /* planner for teleop mode motion */ + double teleop_vel_req; /* the speed the jog asked for */ + double teleop_acc_req; /* and the acceleration it may use */ rtapi_sint old_ajog_counts; /* prior value, used for deltas */ int kb_ajog_active; /* non-zero during a keyboard jog */ @@ -69,6 +72,42 @@ typedef struct { static emcmot_axis_t axis_array[EMCMOT_MAX_AXIS]; static axis_hal_data_t *hal_data = NULL; +/* A world jog of X, Y and Z along the axes of a frame turned against the + world, the work plane or the tool. A planner per frame axis plans the + jog along it; each cycle its step is turned into the world and added to + the X, Y and Z teleop planners, position and command alike, so those + always hold where the machine is. One frame axis moves at a time, and a + jog of another waits until the one under way has stopped: two at once + would move each world axis by their sum, past what the limits along + either allow, and into the travel along neither one's way. The rotation + is taken when a frame jog starts and kept until the planners are at + rest: a tool frame turning with the rotaries would bend the jog under + way. A frame axis gets the limits of the world axes over the share of + the jog each takes. */ +typedef struct { + int selected; /* X Y Z jogs go to the frame */ + double rot_now[3][3]; /* the frame as last reported, columns + its axes in world coordinates */ + double rot[3][3]; /* the frame the planners run in */ + int latched; /* rot is in use */ + int owner; /* the planner that moves, -1 none */ + unsigned int asked; /* counts the jogs asked for */ + simple_tp_t tp[3]; /* planners along the frame's axes */ + unsigned int order[3]; /* when each was asked */ + int cont[3]; /* a continuous jog, to the travel */ + double vel_req[3]; /* the speed the jog asked for */ + double acc_req[3]; /* and the acceleration it may use */ + double vel_cap[3]; /* the most the joints allow of them */ + double acc_cap[3]; + double vel_limit[3]; /* the world axes' limits along the */ + double acc_limit[3]; /* frame axis */ + double jerk_limit[3]; + double min_lim[3]; /* the travel of X, Y and Z, taken */ + double max_lim[3]; /* with the frame */ +} jog_frame_t; + +static jog_frame_t frame; + // Mark strings for translation, but defer translation to userspace #define _(s) (s) @@ -80,6 +119,10 @@ void axis_init_all(void) emcmot_axis_t *axis = &axis_array[n]; axis->locking_joint = -1; } + for (n = 0; n < 3; n++) { + frame.rot_now[n][n] = 1.0; + frame.rot[n][n] = 1.0; + } } void axis_initialize_external_offsets(void) @@ -173,7 +216,8 @@ void axis_output_to_hal(double *pcmd_p[]) hal_set_real(axis_data->teleop_vel_cmd, axis->teleop_vel_cmd); hal_set_real(axis_data->teleop_pos_cmd, axis->teleop_tp.pos_cmd); hal_set_real(axis_data->teleop_vel_lim, axis->teleop_tp.max_vel); - hal_set_bool(axis_data->teleop_tp_enable, axis->teleop_tp.enable); + hal_set_bool(axis_data->teleop_tp_enable, axis->teleop_tp.enable + || (n < 3 && frame.tp[n].enable)); hal_set_bool(axis_data->kb_ajog_active, axis->kb_ajog_active); hal_set_bool(axis_data->wheel_ajog_active, axis->wheel_ajog_active); @@ -261,7 +305,7 @@ double axis_get_compound_velocity(void) for (n = 0; n < EMCMOT_MAX_AXIS; n++) { emcmot_axis_t *axis = &axis_array[n]; - if (axis->teleop_tp.active) { + if (axis->teleop_tp.active || (n < 3 && frame.latched)) { v2 += axis->teleop_vel_cmd * axis->teleop_vel_cmd; } } @@ -320,6 +364,299 @@ static void axis_jog_limits(int axis_num, double *min_lim, double *max_lim) } } +// The frame for jogs of X, Y and Z, its axes in world coordinates as the +// columns of rot, row major; selected 0 jogs them along the world axes +void axis_set_jog_frame(int selected, const double rot[9]) +{ + int i, k; + + frame.selected = selected; + for (i = 0; i < 3; i++) { + for (k = 0; k < 3; k++) { frame.rot_now[i][k] = rot[3 * i + k]; } + } +} + +// Whether frame planner k moves, or has been asked to +static int frame_busy(int k); + +// Whether any teleop planner moves, a frame jog's included +bool axis_jog_is_moving(void) +{ + int n; + + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + if (axis_array[n].teleop_tp.active || axis_array[n].teleop_tp.curr_vel != 0.0) { + return 1; + } + } + return frame.latched; +} + +// Whether a planner has somewhere to go this cycle, and which way +static int planner_request(const simple_tp_t *tp, double *dir) +{ + double togo = tp->pos_cmd - tp->curr_pos; + + if (!tp->enable) { return 0; } + if (fabs(togo) < TINY_DP(tp->max_acc, 0.001) && tp->curr_vel == 0.0) { return 0; } + *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : tp->curr_vel > 0.0 ? 1.0 : -1.0; + return 1; +} + +static int frame_busy(int k) +{ + double dir; + + return frame.tp[k].active || frame.tp[k].curr_vel != 0.0 + || planner_request(&frame.tp[k], &dir); +} + +// Where world axis n is commanded, external offset included +static double world_pos(int n) +{ + return axis_array[n].teleop_tp.curr_pos + axis_array[n].ext_offset_tp.curr_pos; +} + +// Take the frame for a jog about to start, unless one is under way +static void frame_latch(void) +{ + int i, k; + + if (frame.latched) { return; } + for (k = 0; k < 3; k++) { + double vel = 1e99, acc = 1e99, jerk = 1e99; + for (i = 0; i < 3; i++) { + double u = frame.rot_now[i][k]; + // a rounding error is no share of the jog, and would hold an + // axis standing on its limit + if (fabs(u) < 1e-12) { u = 0.0; } + frame.rot[i][k] = u; + u = fabs(u); + if (u == 0.0) { continue; } + if (axis_array[i].vel_limit / u < vel) { vel = axis_array[i].vel_limit / u; } + if (axis_array[i].acc_limit / u < acc) { acc = axis_array[i].acc_limit / u; } + if (axis_array[i].jerk_limit / u < jerk) { jerk = axis_array[i].jerk_limit / u; } + } + frame.vel_limit[k] = vel; + frame.acc_limit[k] = acc; + frame.jerk_limit[k] = jerk; + } + // an axis not homed jogs within one range of where it stands, as a + // plain jog of it does + for (i = 0; i < 3; i++) { axis_jog_limits(i, &frame.min_lim[i], &frame.max_lim[i]); } + memset(frame.tp, 0, sizeof(frame.tp)); + memset(frame.cont, 0, sizeof(frame.cont)); + frame.owner = -1; + frame.asked = 0; + frame.latched = 1; +} + +// Stop the frame planners where they are and let the frame go +static void frame_release(void) +{ + memset(frame.tp, 0, sizeof(frame.tp)); + memset(frame.cont, 0, sizeof(frame.cont)); + frame.owner = -1; + frame.latched = 0; +} + +// How far frame axis k may go, s the way, before the jog leaves the box +static double frame_reach(int k, double s) +{ + double reach = 1e99; + int i; + + for (i = 0; i < 3; i++) { + double u = s * frame.rot[i][k], room; + if (u == 0.0) { continue; } + room = ((u > 0.0 ? frame.max_lim[i] : frame.min_lim[i]) + - world_pos(i)) / u; + if (room < reach) { reach = room; } + } + return reach > 0.0 ? reach : 0.0; +} + +// The planner that moves: the one under way until it has stopped, then the +// one asked for first of those waiting. A continuous jog runs to the box +// from where it starts +static int frame_owner(void) +{ + int k, next = -1; + + if (frame.owner >= 0 && frame_busy(frame.owner)) { return frame.owner; } + for (k = 0; k < 3; k++) { + if (frame_busy(k) && (next < 0 || frame.order[k] < frame.order[next])) { next = k; } + } + if (next >= 0 && frame.cont[next]) { + simple_tp_t *tp = &frame.tp[next]; + double s = tp->pos_cmd > tp->curr_pos ? 1.0 : -1.0; + tp->pos_cmd = tp->curr_pos + s * frame_reach(next, s); + } + frame.owner = next; + return next; +} + +// Whether the box holds the frame jogs once frame axis k is sent to target, +// each jog waiting its turn: where each ends, in the order they were asked +// for, inside it or no further out than now. A continuous jog waiting is +// left out, it stops at the box from wherever it starts +static int frame_ends_inside(int k, double target) +{ + double now[3], end[3]; + int i, j, done[3] = {0, 0, 0}; + + for (i = 0; i < 3; i++) { now[i] = end[i] = world_pos(i); } + for (;;) { + int next = -1; + double to; + for (j = 0; j < 3; j++) { + if (done[j] || (j != k && (!frame_busy(j) || frame.cont[j]))) { continue; } + if (next < 0 || (j != k && (next == k || frame.order[j] < frame.order[next]))) { + next = j; + } + } + if (next < 0) { return 1; } + done[next] = 1; + to = next == k ? target : frame.tp[next].pos_cmd; + for (i = 0; i < 3; i++) { + end[i] += frame.rot[i][next] * (to - frame.tp[next].curr_pos); + if (end[i] > frame.max_lim[i] && end[i] > now[i]) { return 0; } + if (end[i] < frame.min_lim[i] && end[i] < now[i]) { return 0; } + } + } +} + +// A jog of frame axis k under way, or one waiting its turn, keeps its +// place; a new one goes after those asked for before it +static void frame_start(int k, int queued, double vel, double acc) +{ + simple_tp_t *tp = &frame.tp[k]; + + if (!queued) { frame.order[k] = ++frame.asked; } + frame.vel_req[k] = vel < frame.vel_limit[k] ? vel : frame.vel_limit[k]; + frame.acc_req[k] = acc; + frame.vel_cap[k] = frame.vel_req[k]; + frame.acc_cap[k] = frame.acc_req[k]; + tp->max_vel = frame.vel_req[k]; + tp->max_acc = frame.acc_req[k]; + tp->enable = 1; +} + +static void frame_jog_cont(int k, double vel) +{ + double s = vel > 0.0 ? 1.0 : -1.0; + simple_tp_t *tp = &frame.tp[k]; + int queued; + + frame_latch(); + queued = frame_busy(k); + frame.cont[k] = 1; + // how far is read from the box when the jog gets to move + tp->pos_cmd = tp->curr_pos + s * (frame.owner == k ? frame_reach(k, s) : 1.0); + frame_start(k, queued, fabs(vel), frame.acc_limit[k]); + axis_array[k].kb_ajog_active = 1; +} + +static void frame_jog_incr(int k, double offset, double vel) +{ + simple_tp_t *tp = &frame.tp[k]; + double target; + int queued; + + frame_latch(); + queued = frame_busy(k); + target = (frame.cont[k] ? tp->curr_pos : tp->pos_cmd) + (vel > 0.0 ? offset : -offset); + if (!frame_ends_inside(k, target)) { return; } + frame.cont[k] = 0; + tp->pos_cmd = target; + frame_start(k, queued, fabs(vel), frame.acc_limit[k]); + axis_array[k].kb_ajog_active = 1; +} + +// A jog wheel's counts on X, Y or Z, as axis_handle_jogwheels() takes them +// in the world +static int frame_jog_wheel(int k, double distance, double fraction, bool vel_mode) +{ + simple_tp_t *tp = &frame.tp[k]; + double pos, acc; + int queued; + + frame_latch(); + queued = frame_busy(k); + acc = fraction * frame.acc_limit[k]; + pos = (frame.cont[k] ? tp->curr_pos : tp->pos_cmd) + distance; + if (vel_mode) { + double v = frame.vel_limit[k]; + double stop_dist = v * v / (2 * acc); + if (pos > tp->curr_pos + stop_dist) { + pos = tp->curr_pos + stop_dist; + } else if (pos < tp->curr_pos - stop_dist) { + pos = tp->curr_pos - stop_dist; + } + } + if (!frame_ends_inside(k, pos)) { return 0; } + frame.cont[k] = 0; + tp->pos_cmd = pos; + frame_start(k, queued, frame.vel_limit[k], acc); + axis_array[k].wheel_ajog_active = 1; + return 1; +} + +// Run the frame planner whose turn it is and carry its step into the world +// planners; 1 if the step would have taken an axis out of its box and was +// held back +static int frame_step(double period) +{ + simple_tp_t *tp; + double was, step[3]; + int i, k, out = 0; + + if (!frame.latched) { return 0; } + k = frame_owner(); + if (k < 0) { + // all at rest: the next frame jog takes the frame afresh + frame_release(); + return 0; + } + tp = &frame.tp[k]; + tp->max_vel = frame.vel_cap[k] < frame.vel_req[k] ? frame.vel_cap[k] : frame.vel_req[k]; + tp->max_acc = frame.acc_cap[k] < frame.acc_req[k] ? frame.acc_cap[k] : frame.acc_req[k]; + tp->max_jerk = frame.jerk_limit[k]; + was = tp->curr_pos; + simple_tp_update(tp, period); + for (i = 0; i < 3; i++) { + double to; + step[i] = frame.rot[i][k] * (tp->curr_pos - was); + to = world_pos(i) + step[i]; + if ((step[i] > 0.0 && to >= frame.max_lim[i]) + || (step[i] < 0.0 && to <= frame.min_lim[i])) { + out = 1; + } + } + if (out) { + // the jog ends where it is + tp->curr_pos = tp->pos_cmd = was; + tp->curr_vel = tp->curr_acc = 0.0; + tp->active = 0; + return 1; + } + for (i = 0; i < 3; i++) { + axis_array[i].teleop_tp.curr_pos += step[i]; + axis_array[i].teleop_tp.pos_cmd += step[i]; + } + return 0; +} + +// The velocity the frame planners give world axis n +static double frame_vel(int n) +{ + double v = 0.0; + int k; + + if (n >= 3 || !frame.latched) { return 0.0; } + for (k = 0; k < 3; k++) { v += frame.rot[n][k] * frame.tp[k].curr_vel; } + return v; +} void axis_jog_cont(int axis_num, double vel, long servo_period) { @@ -327,6 +664,10 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) emcmot_axis_t *axis = &axis_array[axis_num]; double min_lim, max_lim; + if (frame.selected && axis_num < 3) { + frame_jog_cont(axis_num, vel); + return; + } axis_jog_limits(axis_num, &min_lim, &max_lim); if (vel > 0.0) { axis->teleop_tp.pos_cmd = max_lim; @@ -336,6 +677,8 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -346,6 +689,10 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) emcmot_axis_t *axis = &axis_array[axis_num]; double tmp1, min_lim, max_lim; + if (frame.selected && axis_num < 3) { + frame_jog_incr(axis_num, offset, vel); + return; + } if (vel > 0.0) { tmp1 = axis->teleop_tp.pos_cmd + offset; } else { @@ -359,6 +706,8 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -381,6 +730,8 @@ void axis_jog_abs(int axis_num, double offset, double vel) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -398,6 +749,16 @@ bool axis_jog_abort(int axis_num, bool immediate) if (immediate) { axis->teleop_tp.curr_vel = 0.0; } + if (axis_num < 3) { + simple_tp_t *tp = &frame.tp[axis_num]; + if (tp->enable) { + aborted = 1; + } + tp->enable = 0; + if (immediate) { + tp->curr_vel = 0.0; + } + } return aborted; } @@ -472,6 +833,17 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo } distance = delta * hal_get_real(axis_data->ajog_scale); + if (frame.selected && axis_num < 3) { + if ( (ajog_accel_fraction > 1) + || (ajog_accel_fraction < 0) ) { + ajog_accel_fraction = 1; + } + if (!frame_jog_wheel(axis_num, distance, ajog_accel_fraction, + hal_get_bool(axis_data->ajog_vel_mode))) { + break; + } + continue; + } pos = axis->teleop_tp.pos_cmd + distance; if ( hal_get_bool(axis_data->ajog_vel_mode) ) { double v = axis->vel_limit; @@ -490,6 +862,8 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo axis->teleop_tp.pos_cmd = pos; axis->teleop_tp.max_vel = axis->vel_limit; axis->teleop_tp.max_acc = aaccel_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->wheel_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -517,6 +891,7 @@ void axis_sync_teleop_tp_to_carte_pos(int extfactor, double *pcmd_p[]) axis_array[n].teleop_tp.curr_vel = 0.0; axis_array[n].teleop_tp.curr_acc = 0.0; } + frame_release(); } void axis_sync_carte_pos_to_teleop_tp(int extfactor, double *pcmd_p[]) @@ -746,12 +1121,71 @@ static int update_teleop_with_check(int axis_num, simple_tp_t *the_tp, double se return 0; } +// What the teleop planners that have somewhere to go this cycle ask of +// the world axes, the velocity and the acceleration each jog asked for +// along its way, and the velocity they are at: what the cap on the joints +// reads before the planners run. 0 if none has anywhere to go +int axis_teleop_request(double vel[], double acc[], double now[]) +{ + double dir; + int n, k, any = 0; + + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + emcmot_axis_t *axis = &axis_array[n]; + vel[n] = acc[n] = now[n] = 0.0; + if (planner_request(&axis->teleop_tp, &dir)) { + vel[n] = dir * axis->teleop_vel_req; + acc[n] = dir * axis->teleop_acc_req; + now[n] = axis->teleop_tp.curr_vel; + any = 1; + } + } + // the frame planner whose turn it is, the others wait + k = frame.latched ? frame_owner() : -1; + if (k >= 0 && planner_request(&frame.tp[k], &dir)) { + for (n = 0; n < 3; n++) { + vel[n] += frame.rot[n][k] * dir * frame.vel_req[k]; + acc[n] += frame.rot[n][k] * dir * frame.acc_req[k]; + now[n] += frame.rot[n][k] * frame.tp[k].curr_vel; + } + any = 1; + } + return any; +} + +// The most the planners with somewhere to go may do this cycle: the given +// share of what their jog asked, where the joints cannot follow all of it +void axis_teleop_cap(double vel_scale, double acc_scale) +{ + double dir; + int n, k; + + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + for (n = 0; n < EMCMOT_MAX_AXIS; n++) { + emcmot_axis_t *axis = &axis_array[n]; + if (!planner_request(&axis->teleop_tp, &dir)) { continue; } + axis->teleop_tp.max_vel = vel_scale * axis->teleop_vel_req; + axis->teleop_tp.max_acc = acc_scale * axis->teleop_acc_req; + } + k = frame.latched ? frame.owner : -1; + if (k >= 0 && planner_request(&frame.tp[k], &dir)) { + frame.vel_cap[k] = vel_scale * frame.vel_req[k]; + frame.acc_cap[k] = acc_scale * frame.acc_req[k]; + } +} + int axis_calc_motion(double servo_period) { int axis_num; int violated_teleop_limit = 0; emcmot_axis_t *axis; + // the frame planner first, so the frame's state is this cycle's when + // the jog flags it shares with X, Y and Z are read below + if (frame_step(servo_period)) { + violated_teleop_limit = 1; + } for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { axis = &axis_array[axis_num]; @@ -763,10 +1197,10 @@ int axis_calc_motion(double servo_period) violated_teleop_limit = 1; } - axis->teleop_vel_cmd = axis->teleop_tp.curr_vel; + axis->teleop_vel_cmd = axis->teleop_tp.curr_vel + frame_vel(axis_num); axis->pos_cmd = axis->teleop_tp.curr_pos; - if (!axis->teleop_tp.active) { + if (!axis->teleop_tp.active && !(axis_num < 3 && frame.latched && frame_busy(axis_num))) { axis->kb_ajog_active = 0; axis->wheel_ajog_active = 0; } diff --git a/src/emc/motion/axis.h b/src/emc/motion/axis.h index 5d61f0b13f0..834d548e69a 100644 --- a/src/emc/motion/axis.h +++ b/src/emc/motion/axis.h @@ -51,8 +51,13 @@ void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); +int axis_teleop_request(double vel[], double acc[], double now[]); +void axis_teleop_cap(double vel_scale, double acc_scale); int axis_calc_motion(double servo_period); +void axis_set_jog_frame(int selected, const double rot[9]); +bool axis_jog_is_moving(void); + #ifdef __cplusplus } diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index fd9760e59ac..35ba4578d5b 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -74,6 +74,8 @@ // module written before the call exports no such symbol, and the weak // reference leaves it NULL rather than refusing to load motion #pragma weak kinematicsSetTool +// old modules export no kinematicsMachineFrame; keep it optional +#pragma weak kinematicsMachineFrame #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -120,6 +122,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) @@ -226,62 +272,83 @@ void apply_spindle_limits(spindle_status_t *s){ } -/* inRange() returns non-zero if the position lies within the joint - limits, or 0 if not. It also reports an error for each joint limit - violation. It's possible to get more than one violation per move. */ -STATIC int inRange(EmcPose pos, int id, char *move_type) +/* The [AXIS_L] box is the machine frame envelope: the carriage, or the + flange, in machine coordinates, the frame that does not move when the + head tilts or a tool is loaded. Where the module names its machine + frame type, the endpoint's joints go through that type's forward and + the box is read there, whatever type is in force; a module that names + none keeps the box on the world of the type in force, as before. + Returns non-zero when the endpoint is inside the box, and reports each + letter outside it. */ +STATIC int box_ok(const double *joint_pos, const EmcPose *pos, int id, const char *move_type) { - double joint_pos[EMCMOT_MAX_JOINTS]; - int joint_num, axis_num; - emcmot_joint_t *joint; - int in_range = 1; + EmcPose frame = *pos; /* the estimate in, and the fallback */ int failing_axes[EMCMOT_MAX_AXIS]; double targets[EMCMOT_MAX_AXIS]; const char axis_letters[] = "XYZABCUVW"; + int axis_num, in_box = 1; if (EMCMOT_MAX_AXIS != 9) { rtapi_print_msg(RTAPI_MSG_ERR, "BUG: %s(): invalid number of axes defined", __func__); - } else { - targets[0] = pos.tran.x; - targets[1] = pos.tran.y; - targets[2] = pos.tran.z; - targets[3] = pos.a; - targets[4] = pos.b; - targets[5] = pos.c; - targets[6] = pos.u; - targets[7] = pos.v; - targets[8] = pos.w; - axis_check_constraints(targets, failing_axes); - for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { - if (failing_axes[axis_num] == -1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("negative")); - in_range = 0; - } - if (failing_axes[axis_num] == 1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("positive")); - in_range = 0; - } + return 1; + } + if (kinematicsMachineFrame && kinematicsMachineFrame(joint_pos, &frame) == -2) { + reportError(_("%s move on line %d cannot be placed in the machine frame"), + move_type, id); + return 0; + } + targets[0] = frame.tran.x; + targets[1] = frame.tran.y; + targets[2] = frame.tran.z; + targets[3] = frame.a; + targets[4] = frame.b; + targets[5] = frame.c; + targets[6] = frame.u; + targets[7] = frame.v; + targets[8] = frame.w; + axis_check_constraints(targets, failing_axes); + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { + if (failing_axes[axis_num] == -1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("negative")); + in_box = 0; + } + if (failing_axes[axis_num] == 1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("positive")); + in_box = 0; } } + return in_box; +} - /* Now, check that the endpoint puts the joints within their limits too */ +/* inRange() returns non-zero if the position lies within the joint + limits and the [AXIS_L] box, or 0 if not. It also reports an error for + each limit violation. It's possible to get more than one violation per + move. */ +STATIC int inRange(EmcPose pos, int id, char *move_type) +{ + double joint_pos[EMCMOT_MAX_JOINTS]; + int joint_num; + emcmot_joint_t *joint; + int in_range = 1; - /* 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; } + /* the box, read where the joints put the machine frame */ + if (!box_ok(joint_pos, &pos, id, move_type)) { in_range = 0; } + + /* and the joints within their limits */ for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { /* point to joint data */ joint = &joints[joint_num]; @@ -309,6 +376,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 +1198,169 @@ 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; + } + /* the joints named still have to keep the machine frame in the box */ + if (!box_ok(target, &end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } else { + if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + 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 +2261,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: @@ -2090,6 +2335,42 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotConfig->switchkins_seq++; break; + case EMCMOT_SET_WORK_PLANE: + /* in program order, after the moves before it; a frame jog under + way keeps the frame it started in */ + rtapi_print_msg(RTAPI_MSG_DBG, "SET_WORK_PLANE"); + for (n = 0; n < 9; n++) { + emcmotInternal->work_plane[n] = emcmotCommand->work_plane[n]; + } + emcmotInternal->work_plane_active = emcmotCommand->work_plane_active; + emcmotUpdateJogFrame(); + break; + + case EMCMOT_SET_JOG_FRAME: + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOG_FRAME"); + if (emcmotCommand->jog_frame < EMC_JOG_FRAME_MACHINE + || emcmotCommand->jog_frame > EMC_JOG_FRAME_TOOL) { + reportError(_("unknown jog frame %d"), emcmotCommand->jog_frame); + break; + } + if (emcmotCommand->jog_frame == emcmotStatus->jog_frame) { + break; + } + /* not while a jog is under way, nor while one slows down: a + jog in the new frame would move the same world axes at once. + Planners left moving by leaving teleop mode are put at rest + when teleop mode is entered again */ + if (axis_jog_is_active() + || (emcmotStatus->motion_state == EMCMOT_MOTION_TELEOP + && axis_jog_is_moving())) { + reportError(_("the jog frame cannot change while jogging")); + break; + } + emcmotStatus->jog_frame = emcmotCommand->jog_frame; + /* in force for a jog wheel count in this very cycle */ + emcmotUpdateJogFrame(); + break; + default: rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); reportError(_("unrecognized command %d"), emcmotCommand->command); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 7c9a35c237b..276324f69cf 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -46,6 +46,19 @@ static int switchkins_type = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags = 0; +// old modules export no kinematicsJacobian; a jog on them is bounded by +// the axis limits alone, as before +#pragma weak kinematicsJacobian +// nor the frames; the status then reports none and a GUI keeps its own +// idea of the tool orientation +#pragma weak kinematicsToolFrame +#pragma weak kinematicsWorkFrame +// and only modules built on switchkins.c those of another type, and its +// forward +#pragma weak kinematicsToolFrameOf +#pragma weak kinematicsWorkFrameOf +#pragma weak kinematicsForwardOf + /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ // #define WATCH_FLAGS 1 @@ -194,6 +207,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 +380,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 +391,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 +399,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 +410,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 +961,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; @@ -1252,6 +1357,109 @@ static void handle_jjogwheels(void) first_pass = 0; } +/* A world jog is planned per axis against the [AXIS_L] limits, which say + nothing about the joints once the kinematics is not the identity: a jog + of C under the tool centre point with the head laid over swings the + carriage through a circle. Each cycle, before the teleop planners run, + the module's Jacobian at the joints the machine stands in says how fast + every joint would move per unit of the jog the planners were asked for, + and how that rate has changed since the last cycle (what a carriage on a + circle feels as centripetal), and the planners are capped so that no + joint is asked for more than its own limits: the velocity by the joint + velocity limits, read a stopping distance ahead since the rate keeps + growing while the planners slow down, and by the acceleration budget + the changing rate takes at that velocity (at most half); the + acceleration by what the budget leaves. All active axes scale together + so the jog keeps its direction. Nothing is done on the identity, or on + a module without a Jacobian. */ +static void teleop_joint_cap(double period) +{ + static double jac_prev[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + static int have_prev = 0; + static double acc_scale_prev = 1.0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double joint_pos[EMCMOT_MAX_JOINTS]; + double vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS], now[EMCMOT_MAX_AXIS]; + double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; + double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; + double vel_scale, acc_scale; + const double tiny = 1e-12; + int flags, a, j; + + if (!kinematicsJacobian) { have_prev = 0; return; } + flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; + if (flags >= 0 ? (flags & KINSTYPE_IDENTITY) != 0 + : emcmotConfig->kinType == KINEMATICS_IDENTITY) { + have_prev = 0; + return; + } + /* the jog in world coordinates, a frame jog turned in already */ + if (!axis_teleop_request(vreq, areq, now)) { have_prev = 0; return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rnorm += vreq[a] * vreq[a]; + anorm += areq[a] * areq[a]; + speed += now[a] * now[a]; + } + rnorm = sqrt(rnorm); + anorm = sqrt(anorm); + speed = sqrt(speed); + if (rnorm < tiny || anorm < tiny) { have_prev = 0; return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsJacobian(joint_pos, &emcmotStatus->carte_pos_cmd, jac, &iflags) != 0) { + axis_teleop_cap(1.0, 1.0); + have_prev = 0; + return; + } + /* the direction the jog was asked in, and the one the planners + accelerate in, each at its own limit */ + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rhat[a] = vreq[a] / rnorm; + ahat[a] = areq[a] / anorm; + } + /* the path the planners need to stop from the speed they are at, + at the acceleration they were left last cycle */ + ahead = speed * speed / (2.0 * anorm * acc_scale_prev); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, change = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rate += jac[j][a] * rhat[a]; + if (have_prev) { change += (jac[j][a] - jac_prev[j][a]) * rhat[a]; } + } + rate = fabs(rate); + /* how the rate changes per unit of path, from the last cycle */ + change = (have_prev && speed > tiny) ? fabs(change) / (speed * period) : 0.0; + if (rate > tiny) { + cap = joints[j].vel_limit / (rate + change * ahead); + if (cap < vcap) { vcap = cap; } + } + if (change > tiny) { + cap = sqrt(joints[j].acc_limit / (2.0 * change)); + if (cap < vcap) { vcap = cap; } + } + joint_pos[j] = change; /* kept for the acceleration pass */ + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double arate = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { arate += jac[j][a] * ahat[a]; } + arate = fabs(arate); + if (arate > tiny) { + cap = (joints[j].acc_limit - joint_pos[j] * vcap * vcap) / arate; + if (cap < acap) { acap = cap; } + } + } + vel_scale = vcap / rnorm; + acc_scale = acap / anorm; + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + if (acc_scale < 1e-6) { acc_scale = 1e-6; } + axis_teleop_cap(vel_scale, acc_scale); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } + } + have_prev = 1; + acc_scale_prev = acc_scale; +} // teleop_joint_cap() + static void get_pos_cmds(long period) { int joint_num, result; @@ -1430,18 +1638,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 */ @@ -1508,6 +1771,7 @@ static void get_pos_cmds(long period) break; case EMCMOT_MOTION_TELEOP: + teleop_joint_cap(servo_period); ext_offset_teleop_limit = axis_calc_motion(servo_period); if (!ext_offset_teleop_limit) { ext_offset_coord_limit = 0; //in case was set in prior coord motion @@ -2183,7 +2447,183 @@ static void output_to_hal(void) axis_output_to_hal(pcmd_p); hal_set_bool(emcmot_hal_data->jog_is_active, axis_jog_is_active() || joint_jog_is_active()); + hal_set_sint(emcmot_hal_data->jog_frame, emcmotStatus->jog_frame); + +} + +static double frame_entry(const PmCartesian *c, int k) +{ + return (k == 0) ? c->x : (k == 1) ? c->y : c->z; +} + +/* Whether the work frame of the type in force turns with any joint: a + rotary table, or a work spindle, under a tool centre point type. A + property of the type, so worked out once per type, by turning each + joint a little from where the machine is. */ +static int work_frame_turns(double *joint_pos, const PmRotationMatrix *work) +{ + PmRotationMatrix turned; + const PmCartesian *w[3] = { &work->x, &work->y, &work->z }; + const PmCartesian *u[3] = { &turned.x, &turned.y, &turned.z }; + int j, a, k, turns = 0; + + for (j = 0; j < NO_OF_KINS_JOINTS && !turns; j++) { + joint_pos[j] += 7.0; + if (kinematicsWorkFrame(joint_pos, &turned, &fflags) == 0) { + for (a = 0; a < 3; a++) { + for (k = 0; k < 3; k++) { + if (fabs(frame_entry(u[a], k) - frame_entry(w[a], k)) > 1e-9) { turns = 1; } + } + } + } + joint_pos[j] -= 7.0; + } + return turns; +} + +/* The type that follows the part, the module's working transform, or -1 + where the module declares none. */ +static int primary_kins_type(void) +{ + int k, f; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + f = emcmotStatus->switchkins_flags[k]; + if (f >= 0 && (f & KINSTYPE_PRIMARY)) { return k; } + } + return -1; +} +/* The commanded joints as the primary type reads them, where the tool + stands against the part: the commanded position itself on that type, or + where no type is declared primary, and the primary type's forward under + any other. A display draws the tool there, so under the machine frame + type G13.1 selects it is on the part as under the tool centre point + type, and a switch between them moves nothing. Where the forward fails + the last answer stands. */ +static void update_part_pos(void) +{ + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inv = 0; + /* the last answer is the estimate an iterative forward starts from */ + EmcPose pos = emcmotStatus->part_pos_cmd; + int j, primary = primary_kins_type(); + + if (primary < 0 || primary == switchkins_type || !kinematicsForwardOf) { + emcmotStatus->part_pos_cmd = emcmotStatus->carte_pos_cmd; + return; + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsForwardOf(primary, joint_pos, &pos, &ff, &inv) == 0) { + emcmotStatus->part_pos_cmd = pos; + } +} + +/* The tool frame in the work frame, transpose(work) * tool as + toolFrameInWork() has it, at the commanded joints, for the status; and + the machine axes in the work frame, transpose(work), for the machine + jog frame on a type that turns the work. */ +static void update_tool_frame(void) +{ + static int turns_type = -1, turns = 0; + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + PmRotationMatrix work, tool; + const PmCartesian *w[3] = { &work.x, &work.y, &work.z }; + const PmCartesian *t[3] = { &tool.x, &tool.y, &tool.z }; + int j, a, b, f, primary; + + emcmotStatus->tool_frame_ok = 0; + emcmotInternal->work_frame_ok = 0; + emcmotInternal->tool_jog_ok = 0; + if (!kinematicsToolFrame || !kinematicsWorkFrame) { return; } + if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + /* an identity type turns neither frame whatever the rotaries do, so it + says nothing about where the tool points. The primary type, the one + that follows the part, still knows: a display under the machine frame + type shows the tool against the part as it has them, and a tool jog + moves along the tool as the head stands, the move it makes under the + primary type, as Fanuc's tool axis direction feed and Heidenhain's + Manual Operation Tool Axis move without the tool centre point. */ + if (switchkins_type >= 0 && switchkins_type < SWITCHKINS_MAX_TYPES) { + f = emcmotStatus->switchkins_flags[switchkins_type]; + if (f >= 0 && (f & KINSTYPE_IDENTITY)) { + if (!kinematicsToolFrameOf || !kinematicsWorkFrameOf) { return; } + primary = primary_kins_type(); + if (primary < 0 || (emcmotStatus->switchkins_flags[primary] & KINSTYPE_IDENTITY)) { + return; + } + if (kinematicsWorkFrameOf(primary, joint_pos, &work, &fflags) != 0 + || kinematicsToolFrameOf(primary, joint_pos, &tool, &fflags) != 0) { + return; + } + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotStatus->tool_frame[3 * a + b] = w[a]->x * t[b]->x + + w[a]->y * t[b]->y + w[a]->z * t[b]->z; + /* entry (a, b) is component a of column b of the tool + frame, the machine's world being the machine frame */ + emcmotInternal->tool_jog[3 * a + b] = frame_entry(t[b], a); + } + } + emcmotStatus->tool_frame_ok = 1; + emcmotInternal->tool_jog_ok = 1; + return; + } + } + if (kinematicsWorkFrame(joint_pos, &work, &fflags) != 0) { return; } + if (turns_type != switchkins_type) { + turns = work_frame_turns(joint_pos, &work); + turns_type = switchkins_type; + } + if (turns) { + /* entry (a, b) is component b of column a of the work frame */ + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotInternal->work_frame[3 * a + b] = frame_entry(w[a], b); + } + } + emcmotInternal->work_frame_ok = 1; + } + if (kinematicsToolFrame(joint_pos, &tool, &fflags) != 0) { return; } + /* entry (a, b) is column a of the work frame dotted with column b of + the tool frame */ + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + emcmotStatus->tool_frame[3 * a + b] = w[a]->x * t[b]->x + + w[a]->y * t[b]->y + w[a]->z * t[b]->z; + } + } + emcmotStatus->tool_frame_ok = 1; + for (a = 0; a < 9; a++) { emcmotInternal->tool_jog[a] = emcmotStatus->tool_frame[a]; } + emcmotInternal->tool_jog_ok = 1; +} + +/* The frame world jogs of X, Y and Z move along, for the teleop planners: + the machine axes, which a type turning the work sees turned (the + machine does not tilt when the table does); the tilted work plane, or + the world axes where there is none; or the tool frame as the kinematics + reports it, whatever plane is in effect, the world axes where there is + none. Every cycle, and when the frame or the plane changes. */ +void emcmotUpdateJogFrame(void) +{ + double r[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + const double *m = emcmotInternal->work_frame; + const double *p = emcmotInternal->work_plane; + const double *t = emcmotInternal->tool_jog; + int selected = (emcmotStatus->jog_frame != EMC_JOG_FRAME_MACHINE); + int i; + + if (emcmotStatus->jog_frame == EMC_JOG_FRAME_MACHINE && emcmotInternal->work_frame_ok) { + for (i = 0; i < 9; i++) { r[i] = m[i]; } + selected = 1; + } else if (emcmotStatus->jog_frame == EMC_JOG_FRAME_PLANE && emcmotInternal->work_plane_active) { + for (i = 0; i < 9; i++) { r[i] = p[i]; } + } else if (emcmotStatus->jog_frame == EMC_JOG_FRAME_TOOL && emcmotInternal->tool_jog_ok) { + for (i = 0; i < 9; i++) { r[i] = t[i]; } + } + axis_set_jog_frame(selected, r); } static void update_status(void) @@ -2265,6 +2705,9 @@ static void update_status(void) emcmotStatus->eoffset_pose.w = axis_get_ext_offset_curr_pos(8); emcmotStatus->external_offsets_applied = hal_get_bool(emcmot_hal_data->eoffset_active); + update_part_pos(); + update_tool_frame(); + emcmotUpdateJogFrame(); for (dio = 0; dio < emcmotConfig->numDIO; dio++) { emcmotStatus->synch_di[dio] = hal_get_bool(emcmot_hal_data->synch_di[dio]); diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 954de98dd84..b00547e3257 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -133,6 +133,7 @@ typedef struct { hal_bool_t jog_stop; /* RPI: set TRUE to stop jogging following accel values*/ hal_bool_t jog_stop_immediate; /* RPI: set TRUE to stop jogging immediately*/ hal_bool_t jog_is_active; /* RPI: TRUE if active jogging*/ + hal_sint_t jog_frame; /* RPI: the frame world jogs move along */ hal_bool_t tp_reverse; /* Set true if trajectory planner is running in reverse*/ hal_bool_t motion_enabled; /* RPI: motion enable for all joints */ hal_bool_t is_all_homed; /* RPI: TRUE if all active joints is homed */ @@ -271,6 +272,9 @@ 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 emcmotUpdateJogFrame(void); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 4a096507d64..39bfd58f0b1 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -15,6 +15,7 @@ #include /* memset */ #include #include /* decls for HAL implementation */ +#include /* EMC_JOG_FRAME_MACHINE */ #include "../tp/tp.h" #include "../tp/sp_scurve.h" /* sp_scurve_cleanup() at module exit */ @@ -610,6 +611,7 @@ static int init_hal_io(void) CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &(emcmot_hal_data->distance_to_go), 0.0, "motion.distance-to-go")); CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->program_line), 0, "motion.program-line")); CALL_CHECK(hal_pin_new_bool(mot_comp_id, HAL_OUT, &(emcmot_hal_data->jog_is_active), 0, "motion.jog-is-active")); + CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->jog_frame), 0, "motion.jog-frame")); /* Standard Interp State Pins */ CALL_CHECK(hal_pin_new_sint(mot_comp_id, HAL_OUT, &(emcmot_hal_data->interp_line_number), 0, "motion.interp.line-number")); @@ -914,6 +916,9 @@ static int init_comm_buffers(void) emcmotStatus->activeDepth = 0; emcmotStatus->paused = 0; emcmotStatus->overrideLimitMask = 0; + emcmotStatus->jog_frame = EMC_JOG_FRAME_MACHINE; + emcmotInternal->work_plane_active = 0; + emcmotInternal->work_frame_ok = 0; SET_MOTION_INPOS_FLAG(1); SET_MOTION_ENABLE_FLAG(0); /* record the kinematics type of the machine */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1a48aa02f57..e3579aaa8ca 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 */ @@ -179,6 +180,8 @@ extern "C" { EMCMOT_SELECT_KINS_TYPE, /* select the switchkins type (G12.1) */ EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ + EMCMOT_SET_WORK_PLANE, /* the tilted work plane in effect */ + EMCMOT_SET_JOG_FRAME, /* the frame world jogs of X Y Z move along */ } cmd_code_t; @@ -278,6 +281,22 @@ 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; + + /* SET_WORK_PLANE: the plane's axes in world coordinates, row major */ + double work_plane[9]; + int work_plane_active; + int jog_frame; /* SET_JOG_FRAME: EMC_JOG_FRAME_* */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -608,6 +627,14 @@ Suggestion: Split this in to an Error and a Status flag register.. int carte_pos_cmd_ok; /* non-zero if command is valid */ EmcPose carte_pos_fb; /* actual Cartesian position */ int carte_pos_fb_ok; /* non-zero if feedback is valid */ + EmcPose part_pos_cmd; /* the commanded joints as the primary type + reads them: where the tool stands against + the part, carte_pos_cmd on that type */ + double tool_frame[9]; /* the tool frame in the work frame at the + commanded joints, row major */ + int tool_frame_ok; /* non-zero if the module reports its frames */ + int jog_frame; /* the frame world jogs of X Y Z move along, + EMC_JOG_FRAME_* */ EmcPose world_home; /* cartesean coords of home position */ emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ @@ -793,6 +820,17 @@ typedef struct emcmot_internal_t { move while overriding limits */ TP_STRUCT coord_tp; /* coordinated mode planner */ int idForStep; /* status id while stepping */ + double work_plane[9]; /* the tilted work plane's axes in world + coordinates, row major */ + int work_plane_active; /* non-zero while a plane is in effect */ + double work_frame[9]; /* the machine axes in world coordinates, + transpose(work), row major */ + int work_frame_ok; /* non-zero on a type whose work frame turns + with a joint, a table under TCP */ + double tool_jog[9]; /* the tool's axes in world coordinates of + the type in force, row major: the frame + a tool jog moves along */ + int tool_jog_ok; /* non-zero while tool_jog is known */ } emcmot_internal_t; /* error ring buffer access functions */ diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 54a9b384935..5ec4c036a9f 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -6,6 +6,7 @@ INCLUDES += emc/kinematics_userspace LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ jacobian.cc \ joint_limits.cc \ + segment_cap.cc \ ) # kins_util.c is the shared kinematics code the modules link; the loader @@ -45,7 +46,8 @@ USERSRCS += $(KINSLIMITS_SRCS) TARGETS += ../bin/kinslimits -MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh \ + emc/motion_planning/segment_cap.hh $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh cp $^ $@ HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc index 1b750be6776..31731fb22c4 100644 --- a/src/emc/motion_planning/kinslimits.cc +++ b/src/emc/motion_planning/kinslimits.cc @@ -6,8 +6,8 @@ * * The tool attaches to a running HAL instance, loads the kinematics * module through the non-RT interface, samples the move, and reports - * the most restrictive cap found along it. The sampling loop here is - * the same one the trajectory planner uses to cap a segment. + * the most restrictive cap found along it, then the cap canon puts on + * the segment through segmentCap(), which is the one the planner sees. * * Example (in a terminal with a running config, or under halrun): * @@ -36,6 +36,7 @@ #include #include "jacobian.hh" #include "joint_limits.hh" +#include "segment_cap.hh" using namespace motion_planning; @@ -91,11 +92,13 @@ static void usage(const char *argv0) "usage: %s --module NAME --joints N --coords LETTERS\n" " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--min m0,m1,... --max m0,m1,...]\n" " [--samples N] [--singularity COND]\n" "\n" "Prints the Jacobian and the world-space caps the joint limits imply\n" - "for a straight move from --start to --end. Requires a running HAL\n" - "instance with the kinematics module loaded.\n", argv0); + "for a straight move from --start to --end, and with --min and --max\n" + "the first place along it a joint leaves its travel. Requires a\n" + "running HAL instance with the kinematics module loaded.\n", argv0); } int main(int argc, char **argv) @@ -105,7 +108,7 @@ int main(int argc, char **argv) int num_joints = 0; int samples = 11; double singularity = 100.0; - std::vector start_v, end_v, vel_v, acc_v, jerk_v; + std::vector start_v, end_v, vel_v, acc_v, jerk_v, min_v, max_v; for (int i = 1; i < argc; i++) { const char *a = argv[i]; @@ -120,6 +123,8 @@ int main(int argc, char **argv) else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else if (!strcmp(a, "--min") && next) { min_v = parse_list(next); i++; } + else if (!strcmp(a, "--max") && next) { max_v = parse_list(next); i++; } else { usage(argv[0]); return 1; } } @@ -263,6 +268,41 @@ int main(int argc, char **argv) min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); printf("worst cond : %.3f\n", max_cond); + /* the cap canon reads for the same move, along the same tangent, with + the change of the joint rates along the move charged as well */ + { + struct { EmcPose start, end; } line = { start, end }; + SegmentCapLimits lim; + SegmentCap cap; + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + lim.joints = num_joints; + for (int j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + lim.vel[j] = j < num_joints ? vel_v[j] : SEGMENT_CAP_NONE; + lim.acc[j] = j < num_joints ? acc_v[j] : SEGMENT_CAP_NONE; + lim.min[j] = j < (int)min_v.size() ? min_v[j] : -HUGE_VAL; + lim.max[j] = j < (int)max_v.size() ? max_v[j] : HUGE_VAL; + } + auto pose_at = [](double f, EmcPose *p, void *arg) { + const EmcPose *se = (const EmcPose *)arg; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(p, ax, pose_axis(se[0], ax) + f * (pose_axis(se[1], ax) - pose_axis(se[0], ax))); + } + }; + if (segmentCap(ctx, &lim, target, samples, pose_at, &line, joints, &cap) == 0) { + printf("canon's cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), %d of %d samples unanswered\n", + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, cap.samples); + if (cap.travel_joint >= 0) { + printf("travel : joint %d past its %s at s=%.3f, at %.6f\n", + cap.travel_joint, cap.travel_side > 0 ? "MAX_LIMIT" : "MIN_LIMIT", + cap.travel_at, cap.travel_pos); + } else if (!min_v.empty() || !max_v.empty()) { + printf("travel : every joint inside it\n"); + } + } else { + printf("canon's cap : none, no sample could be evaluated\n"); + } + } + kinematicsUserFree(ctx); hal_exit(comp_id); return 0; diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc new file mode 100644 index 00000000000..761669e68c3 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.cc @@ -0,0 +1,419 @@ +/******************************************************************** + * Description: segment_cap.cc + * The velocity and acceleration cap a segment gets from the joint + * limits through the kinematics module's Jacobian, and the first + * place along it a joint would leave its travel. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ + +#include "segment_cap.hh" +#include + +namespace motion_planning { + +/* at each sample: the joints, the rate of every joint per unit of path + parameter, the path parameter, and whether the module answered there */ +static double q[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; +static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; +static double s[SEGMENT_CAP_MAX_SAMPLES]; +static bool ok[SEGMENT_CAP_MAX_SAMPLES]; + +static double pose_axis(const EmcPose &p, int ax) +{ + switch (ax) { + case AXIS_X: return p.tran.x; + case AXIS_Y: return p.tran.y; + case AXIS_Z: return p.tran.z; + case AXIS_A: return p.a; + case AXIS_B: return p.b; + case AXIS_C: return p.c; + case AXIS_U: return p.u; + case AXIS_V: return p.v; + default: return p.w; + } +} + +/* The joints at fraction f of a segment sampled at K points, the inverse + seeded from what joints holds, and the rate of each there per unit of + path parameter: the module's Jacobian along the tangent, taken by a + central difference of the geometry, exact for a line and a small + fraction of a sample step for an arc. False where the module cannot + answer. */ +static bool sample_at(KinematicsUserContext *ctx, double length, int K, + SegmentPoseFn pose_at, void *arg, int njoints, + double f, double *joints, double *rate) +{ + double df = 1.0 / (8.0 * (K - 1)); + double fp = f + df > 1.0 ? 1.0 : f + df; + double fm = f - df < 0.0 ? 0.0 : f - df; + EmcPose p, pp, pm; + double t[AXIS_COUNT]; + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; + int j, a; + + pose_at(f, &p, arg); + pose_at(fp, &pp, arg); + pose_at(fm, &pm, arg); + for (a = 0; a < AXIS_COUNT; a++) { + t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); + } + if (kinematicsUserInverse(ctx, &p, joints) != 0 + || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { + return false; + } + for (j = 0; j < njoints; j++) { + rate[j] = 0.0; + for (a = 0; a < AXIS_COUNT; a++) { rate[j] += J[j][a] * t[a]; } + } + return true; +} + +//---------------------------------------------------------------------- +// The travel. Every joint is followed from sample to sample against its +// MIN_LIMIT and MAX_LIMIT, the samples themselves and, between two, the +// cubic through their positions and rates, which is where the joint goes +// between them as long as the samples are close enough to see its turns. +// The cubic only says where to look: a joint is reported past a limit +// where the inverse puts it there, never on the cubic's word. +//---------------------------------------------------------------------- + +/* a hair past a limit is the inverse rounding, not travel */ +static const double TRAVEL_HAIR = 1e-6; +/* the share of its bulge past both ends the cubic may be out by at its + peak; a peak that close to the limit is read through the inverse */ +static const double TRAVEL_DOUBT = 0.05; +/* how deep a stretch is split at its peaks, and how many points beyond + the samples a segment may be read at in all */ +enum { TRAVEL_DEPTH = 6, TRAVEL_READS = 32 }; + +enum { WATCH_MAX = 1, WATCH_MIN = 2 }; +enum { TRAVEL_CLEAR, TRAVEL_CROSSED, TRAVEL_UNSURE }; + +/* a point the joints are known at */ +struct TravelPoint { + double s; + double q[KINEMATICS_USER_MAX_JOINTS]; + double g[KINEMATICS_USER_MAX_JOINTS]; +}; + +static void travel_load(TravelPoint *p, int k, int njoints) +{ + p->s = s[k]; + for (int j = 0; j < njoints; j++) { + p->q[j] = q[k][j]; + p->g[j] = g[k][j]; + } +} + +/* whether some joint moves between two points further than its rates at + them account for: the inverse has changed branch, or the joint swings + through more between the two than either shows */ +static bool travel_jumped(const TravelPoint &a, const TravelPoint &b, int njoints) +{ + double ds = b.s - a.s; + + for (int j = 0; j < njoints; j++) { + double moved = b.q[j] - a.q[j]; + double trapezoid = 0.5 * (a.g[j] + b.g[j]) * ds; + double rate = fmax(fabs(a.g[j]), fabs(b.g[j])) * ds; + if (fabs(moved - trapezoid) > rate + TRAVEL_HAIR) { return true; } + } + return false; +} + +/* whether a followed joint is past its travel at a point, and if so the + first one, into out */ +static bool travel_past(const TravelPoint &p, const SegmentCapLimits *lim, + const int *watch, int njoints, double length, SegmentCap *out) +{ + for (int j = 0; j < njoints; j++) { + int side = 0; + if ((watch[j] & WATCH_MAX) && p.q[j] > lim->max[j] + TRAVEL_HAIR) { side = 1; } + if ((watch[j] & WATCH_MIN) && p.q[j] < lim->min[j] - TRAVEL_HAIR) { side = -1; } + if (side) { + out->travel_joint = j; + out->travel_side = side; + out->travel_at = p.s / length; + out->travel_pos = p.q[j]; + return true; + } + } + return false; +} + +/* The peak toward one side of the cubic through two points, the joint at + qa and qb, its rates there scaled to the stretch ma and mb: where it is, + u inside (0, 1), its value, and how far it bulges past both ends. False + where the cubic has no peak inside the stretch on that side. */ +static bool travel_peak(double qa, double ma, double qb, double mb, int side, + double *u, double *peak, double *bulge) +{ + /* p(t) = qa + ma t + c2 t^2 + c3 t^3, p'(t) = 3 c3 t^2 + 2 c2 t + ma */ + double c2 = 3.0 * (qb - qa) - 2.0 * ma - mb; + double c3 = 2.0 * (qa - qb) + ma + mb; + double roots[2]; + int i, n = 0; + bool found = false; + + if (c3 == 0.0) { + if (c2 != 0.0) { roots[n++] = -ma / (2.0 * c2); } + } else { + double disc = c2 * c2 - 3.0 * c3 * ma; + if (disc >= 0.0) { + /* the pair that keeps its digits */ + double r = c2 >= 0.0 ? -(c2 + sqrt(disc)) : -(c2 - sqrt(disc)); + roots[n++] = r / (3.0 * c3); + if (r != 0.0) { roots[n++] = ma / r; } + } + } + for (i = 0; i < n; i++) { + double t = roots[i], v; + if (!(t > 0.0 && t < 1.0)) { continue; } + v = qa + t * (ma + t * (c2 + t * c3)); + if (!found || side * (v - *peak) > 0.0) { + *u = t; + *peak = v; + found = true; + } + } + if (!found) { return false; } + *bulge = side * (*peak - (side > 0 ? fmax(qa, qb) : fmin(qa, qb))); + return *bulge > 0.0; +} + +/* What lies between two points: where the cubic between them peaks past a + limit, or close enough that it may be wrong, the inverse is read at the + peak; a joint past a limit there is CROSSED, else each half is looked at + the same way. UNSURE where the module cannot answer at a peak or a + joint jumps to it. */ +static int travel_between(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int K, SegmentPoseFn pose_at, void *arg, + int njoints, const int *watch, + const TravelPoint &a, const TravelPoint &b, + int *reads, SegmentCap *out) +{ + struct Stretch { + TravelPoint a, b; + int depth; + }; + /* split depth first, the half nearer the start on top */ + static Stretch stack[TRAVEL_DEPTH + 2]; + static TravelPoint m; + int n = 0, j, side; + + stack[n].a = a; + stack[n].b = b; + stack[n++].depth = 0; + while (n > 0) { + const Stretch st = stack[--n]; + double ds = st.b.s - st.a.s; + double worst = 0.0, at = -1.0; + + for (j = 0; j < njoints; j++) { + for (side = 1; side >= -1; side -= 2) { + double limit, u, peak, bulge, past; + if (!(watch[j] & (side > 0 ? WATCH_MAX : WATCH_MIN))) { continue; } + if (!travel_peak(st.a.q[j], st.a.g[j] * ds, st.b.q[j], st.b.g[j] * ds, + side, &u, &peak, &bulge)) { + continue; + } + /* how far past the limit the true peak can be */ + limit = side > 0 ? lim->max[j] : lim->min[j]; + past = side * (peak - limit) + TRAVEL_DOUBT * bulge - TRAVEL_HAIR; + if (past > worst) { + worst = past; + at = u; + } + } + } + if (at < 0.0) { continue; } + /* out of reads: what is left is let through */ + if (*reads <= 0) { return TRAVEL_CLEAR; } + (*reads)--; + m.s = st.a.s + at * ds; + for (j = 0; j < njoints; j++) { m.q[j] = st.a.q[j]; } + if (!sample_at(ctx, length, K, pose_at, arg, njoints, m.s / length, m.q, m.g) + || travel_jumped(st.a, m, njoints) || travel_jumped(m, st.b, njoints)) { + return TRAVEL_UNSURE; + } + if (travel_past(m, lim, watch, njoints, length, out)) { return TRAVEL_CROSSED; } + if (st.depth < TRAVEL_DEPTH) { + stack[n].a = m; + stack[n].b = st.b; + stack[n++].depth = st.depth + 1; + stack[n].a = st.a; + stack[n].b = m; + stack[n++].depth = st.depth + 1; + } + } + return TRAVEL_CLEAR; +} + +/* the first place along the sampled segment a joint leaves its travel */ +static void travel(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int K, SegmentPoseFn pose_at, void *arg, + int njoints, SegmentCap *out) +{ + static int watch[KINEMATICS_USER_MAX_JOINTS]; + static TravelPoint a, b; + int k, j, any = 0, reads = TRAVEL_READS; + + if (!ok[0]) { return; } + /* a side of the travel the joint starts past is not followed: the move + may be the one bringing it back */ + for (j = 0; j < njoints; j++) { + watch[j] = 0; + if (std::isfinite(lim->max[j]) && q[0][j] <= lim->max[j] + TRAVEL_HAIR) { watch[j] |= WATCH_MAX; } + if (std::isfinite(lim->min[j]) && q[0][j] >= lim->min[j] - TRAVEL_HAIR) { watch[j] |= WATCH_MIN; } + any |= watch[j]; + } + if (!any) { return; } + travel_load(&a, 0, njoints); + for (k = 1; k < K; k++) { + if (!ok[k]) { return; } + travel_load(&b, k, njoints); + if (travel_jumped(a, b, njoints)) { return; } + if (travel_between(ctx, lim, length, K, pose_at, arg, njoints, watch, + a, b, &reads, out) != TRAVEL_CLEAR) { + return; + } + if (travel_past(b, lim, watch, njoints, length, out)) { return; } + a = b; + } +} + +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out) +{ + const double tiny = 1e-12; + /* the most the rate of every joint changes per unit of parameter on + either side of a sample, and the most it can bulge past the samples + on either side */ + static double h[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double bulge[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + int K = samples < 2 ? 2 : samples; + int njoints; + int k, j, prev, answered = 0; + + out->vel = SEGMENT_CAP_NONE; + out->acc = SEGMENT_CAP_NONE; + out->vel_joint = -1; + out->acc_joint = -1; + out->vel_at = 0.0; + out->samples = 0; + out->unanswered = 0; + out->travel_joint = -1; + out->travel_side = 0; + out->travel_at = 0.0; + out->travel_pos = 0.0; + if (!ctx || !lim || !pose_at || !joints || length <= 0.0) { return -1; } + if (K > SEGMENT_CAP_MAX_SAMPLES) { K = SEGMENT_CAP_MAX_SAMPLES; } + njoints = lim->joints; + if (njoints > KINEMATICS_USER_MAX_JOINTS) { njoints = KINEMATICS_USER_MAX_JOINTS; } + out->samples = K; + + for (k = 0; k < K; k++) { + double f = (double)k / (double)(K - 1); + + s[k] = f * length; + /* the joints at the sample, seeded from the sample before so the + whole segment stays on one solution branch, then the Jacobian + on that branch */ + ok[k] = sample_at(ctx, length, K, pose_at, arg, njoints, f, joints, g[k]); + if (!ok[k]) { + out->unanswered++; + continue; + } + for (j = 0; j < njoints; j++) { + q[k][j] = joints[j]; + h[k][j] = 0.0; + bulge[k][j] = 0.0; + } + answered++; + } + if (!answered) { return -1; } + + /* how far the rate can rise between two samples: the parabola + through three in a row bulges by an eighth of its second + difference over its middle, and a peak between samples is + covered by charging that to all three */ + for (k = 1; k + 1 < K; k++) { + if (!ok[k - 1] || !ok[k] || !ok[k + 1]) { continue; } + for (j = 0; j < njoints; j++) { + double b = fabs(g[k + 1][j] - 2.0 * g[k][j] + g[k - 1][j]) / 8.0; + if (b > bulge[k][j]) { bulge[k][j] = b; } + if (b > bulge[k - 1][j]) { bulge[k - 1][j] = b; } + if (b > bulge[k + 1][j]) { bulge[k + 1][j] = b; } + } + } + + /* the joint velocity limits */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny && lim->vel[j] / rate < out->vel) { + out->vel = lim->vel[j] / rate; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + + /* how the rates change along the segment, read between neighbours + and charged to both */ + for (prev = -1, k = 0; k < K; k++) { + if (!ok[k]) { continue; } + if (prev >= 0) { + for (j = 0; j < njoints; j++) { + double dh = fabs(g[k][j] - g[prev][j]) / (s[k] - s[prev]); + if (dh > h[k][j]) { h[k][j] = dh; } + if (dh > h[prev][j]) { h[prev][j] = dh; } + } + } + prev = k; + } + + /* the changing rate at the capped velocity takes at most half a + joint's acceleration budget */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + if (h[k][j] > tiny) { + double cap = sqrt(lim->acc[j] / (2.0 * h[k][j])); + if (cap < out->vel) { + out->vel = cap; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + } + + /* the joint acceleration limits, less what the changing rate takes */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny) { + double cap = (lim->acc[j] - h[k][j] * out->vel * out->vel) / rate; + if (cap < out->acc) { + out->acc = cap; + out->acc_joint = j; + } + } + } + } + + /* and the travel, on the same samples */ + travel(ctx, lim, length, K, pose_at, arg, njoints, out); + return 0; +} // segmentCap() + +} // namespace motion_planning diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh new file mode 100644 index 00000000000..76407abd991 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.hh @@ -0,0 +1,96 @@ +/******************************************************************** + * Description: segment_cap.hh + * The velocity and acceleration a segment can be run at without + * asking any joint for more than its own limits, read off the + * kinematics module's Jacobian along the segment. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ +#ifndef SEGMENT_CAP_HH +#define SEGMENT_CAP_HH + +#include +#include + +namespace motion_planning { + +/* The joint limits the cap is read against, in the units motion commands + the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION, and the + travel between MIN_LIMIT and MAX_LIMIT, -HUGE_VAL and HUGE_VAL for a + joint whose travel is not to be followed. */ +struct SegmentCapLimits { + int joints; + double vel[KINEMATICS_USER_MAX_JOINTS]; + double acc[KINEMATICS_USER_MAX_JOINTS]; + double min[KINEMATICS_USER_MAX_JOINTS]; + double max[KINEMATICS_USER_MAX_JOINTS]; +}; + +/* No joint binds. */ +#define SEGMENT_CAP_NONE 1e9 + +/* The most a segment was sampled at. */ +#define SEGMENT_CAP_MAX_SAMPLES 129 + +/* What the joints allow along the segment: the fastest the path parameter + may advance, and the hardest it may accelerate, per second. */ +struct SegmentCap { + double vel; + double acc; + int vel_joint; /* the joint setting each, -1 for none */ + int acc_joint; + double vel_at; /* the fraction of the segment the velocity cap is read at */ + int samples; /* points the segment was sampled at */ + int unanswered; /* of them, the ones the module could not invert or differentiate */ + int travel_joint; /* the first joint found leaving its travel along the segment, -1 for none */ + int travel_side; /* 1 past its MAX_LIMIT, -1 past its MIN_LIMIT */ + double travel_at; /* the fraction of the segment it is found at */ + double travel_pos; /* where the joint is there */ +}; + +/* The pose at a fraction of the segment, 0 the start and 1 the end, in + the units the module takes, which are motion's. A line interpolates, + an arc walks its circle. */ +typedef void (*SegmentPoseFn)(double fraction, EmcPose *pose, void *arg); + +/* Sample the segment at `samples` points, the ends among them, and read + at each the rate every joint moves at per unit of path parameter: the + module's Jacobian along the local tangent. Between neighbouring samples + read how that rate changes with the parameter, and from three in a row + how much it can bulge between two of them, which is added to the rate so + that a peak falling between samples is covered. A joint's velocity + limit then caps the path velocity, and its acceleration limit caps the + path acceleration once the share the changing rate takes at the capped + velocity (what a carriage on a circle feels as centripetal) is set + aside: at most half the joint's budget goes to it, and the velocity is + lowered where it would take more. + + The joints are followed against their travel along the same samples: a + sample past a limit, or, between two samples, the peak of the cubic + through their positions and rates, read again through the inverse at + the peak and found past the limit there, is reported, the first one + along the segment. Where the cubic puts a peak near the limit without + the inverse confirming it, the stretch is split at the peak and each + half looked at the same way, a bounded number of times. Only a point + the inverse itself puts past a limit is reported, and not on a side of + the travel the joint starts the segment past, nor beyond a sample the + module cannot invert or a joint moving between two points further than + its rates allow, as where the inverse changes branch: past those the + segment is not known well enough to be refused. + + length is the segment's extent in the path parameter, in the units the + caps come back in. joints seeds the inverse at the start and comes back + holding the joints at the end, so the next segment starts on the same + solution branch. Returns 0, or -1 where no sample could be evaluated, + the caps then NONE and no joint found leaving its travel. */ +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out); + +} // namespace motion_planning + +#endif // SEGMENT_CAP_HH diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 9a0a4de911c..a4cde216fa9 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -260,9 +260,18 @@ extern void HOME_CYCLE_JOINT(int joint); /* The tilted work plane. Origin in program units and a row major rotation matrix, both in the coordinate system active when the plane was defined; - active 0 cancels it. */ + active 0 cancels it. Under the machine frame kinematics a plane is cut on + the part through the second feature coordinate system (3+2), and x, y, z + and rotation are the plane with it folded in, the one canon applies. The + rest is what status reports: the plane as defined, and the second feature + coordinate system on its own, a row major rotation and a shift in the + machine's length units, the identity and zero where there is none. */ extern void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active); + const double rotation[9], int active, + const double defined_origin[3], + const double defined_rotation[9], + const double kins_shift[3], + const double kins_rotation[9]); /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real @@ -275,6 +284,12 @@ extern void CANON_UPDATE_END_POINT(double x, double y, double z, /* Called from emctask to update the canon position during skipping through programs started with start-from-line > 0. */ +extern int CANON_MOVE_REFUSED(char *message, int max); +/* Called from emctask after the interpreter has read or executed a line: + 1 and the reason in message where canon refused a move since the last + call, because a joint would leave its travel along it, 0 otherwise. + The refused move and every move after it are dropped until then. */ + extern void USE_LENGTH_UNITS(CANON_UNITS u); /* Use the specified units for length. Conceptually, the units must @@ -298,6 +313,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 +713,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,12 +972,20 @@ 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); - -/* The tilted work plane the machine last executed, in the form - SET_G68_FRAME takes: 1 with origin and rotation filled in, 0 when there - is none or the coordinate system under it is no longer the one the - interpreter has, -1 when there is no machine to ask (sai, preview). */ +/* 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, as defined: 1 with + origin and rotation filled in, 0 when there is none or the coordinate + system under it is no longer the one the interpreter has, -1 when there + is no machine to ask (sai, preview). */ extern int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]); +/* The second feature coordinate system the machine's plane is cut through, + as SET_G68_FRAME gave it: 1 with shift and rotation filled in, the + identity where there is none, -1 when nothing can say. */ +extern int GET_EXTERNAL_G68_KINS_FRAME(double shift[3], double rotation[9]); // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); @@ -1006,6 +1052,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..ce03e3451cd 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -295,6 +295,12 @@ 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_JOG_FRAME_TYPE: + ((EMC_TRAJ_SET_JOG_FRAME *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -510,6 +516,10 @@ 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_JOG_FRAME_TYPE: + return "EMC_TRAJ_SET_JOG_FRAME"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1368,6 +1378,8 @@ void EMC_TASK_STAT::update(CMS * cms) EmcPose_update(cms, &g68_offset); cms->update(g68_rotation, 9); cms->update(g68_active); + EmcPose_update(cms, &g68_kins_offset); + cms->update(g68_kins_rotation, 9); EmcPose_update(cms, &toolOffset); cms->update(activeGCodes, ACTIVE_G_CODES); cms->update(activeMCodes, ACTIVE_M_CODES); @@ -1563,6 +1575,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 @@ -1658,6 +1672,25 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) EmcPose_update(cms, &origin); cms->update(rotation, 9); cms->update(active); + EmcPose_update(cms, &kins_offset); + cms->update(kins_rotation, 9); +} + +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_JOG_FRAME::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + cms->update(frame); +} + +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); } /* @@ -1700,6 +1733,10 @@ void EMC_TRAJ_STAT::update(CMS * cms) cms->update(rapid_scale); EmcPose_update(cms, &position); EmcPose_update(cms, &actualPosition); + EmcPose_update(cms, &part_position); + cms->update(tool_frame, 9); + cms->update(tool_frame_ok); + cms->update(jog_frame); cms->update(velocity); cms->update(acceleration); cms->update(maxVelocity); diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index d251e08abc6..3ce852cf480 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -109,6 +109,8 @@ 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_SET_JOG_FRAME_TYPE ((NMLTYPE) 241) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -313,6 +315,8 @@ extern int emcJointSetHomingParams(int joint, double home, double offset, double extern int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence); extern int emcJointSetMaxVelocity(int joint, double vel); extern int emcJointSetMaxAcceleration(int joint, double acc); +extern double emcJointGetMaxVelocity(int joint); +extern double emcJointGetMaxAcceleration(int joint); extern int emcJointInit(int joint); extern int emcJointHalt(int joint); @@ -375,12 +379,15 @@ 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 emcTrajSetWorkPlane(const double rotation[9], int active); +extern int emcTrajSetJogFrame(int frame); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index f71030156bb..8a851102e80 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 { @@ -869,7 +873,8 @@ class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_G68() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_G68_TYPE, sizeof(EMC_TRAJ_SET_G68)), - origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0) + origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0), + kins_offset{}, kins_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1} {}; // For internal NML/CMS use only. @@ -880,6 +885,24 @@ class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { EmcPose origin; double rotation[9]; // row major int active; + EmcPose kins_offset; // the second feature coordinate system (3+2) + double kins_rotation[9]; // row major +}; + +// the frame world jogs of X, Y and Z move along, EMC_JOG_FRAME_* +class EMC_TRAJ_SET_JOG_FRAME:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SET_JOG_FRAME() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_JOG_FRAME_TYPE, sizeof(EMC_TRAJ_SET_JOG_FRAME)), + frame(0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + int frame; }; class EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG:public EMC_TRAJ_CMD_MSG { @@ -935,6 +958,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() @@ -1014,6 +1058,11 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { EmcPose position; // current commanded position EmcPose actualPosition; // current actual position, from forward kins + EmcPose part_position; // commanded joints as the primary type reads + // them, the tool against the part + double tool_frame[9]; // tool frame in the work frame, row major + bool tool_frame_ok; // the kinematics reports its frames + int jog_frame; // the frame world jogs move along, EMC_JOG_FRAME_* double velocity; // system velocity, for subsequent motions double acceleration; // system acceleration, for subsequent // motions @@ -1531,6 +1580,8 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { EmcPose g68_offset; // tilted work plane origin, in user units double g68_rotation[9]; // tilted work plane rotation, row major int g68_active; // a tilted work plane is in effect + EmcPose g68_kins_offset; // under the machine frame kinematics, the second + double g68_kins_rotation[9]; // feature coordinate system the plane is cut through EmcPose toolOffset; // tool offset, in general pose form int activeGCodes[ACTIVE_G_CODES]; int activeMCodes[ACTIVE_M_CODES]; diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index e40a1818cf9..d9a0f3e931d 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -84,6 +84,10 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() position{}, actualPosition{}, + part_position{}, + tool_frame{1, 0, 0, 0, 1, 0, 0, 0, 1}, + tool_frame_ok(false), + jog_frame(0), velocity(1.0), acceleration(1.0), @@ -156,6 +160,8 @@ EMC_TASK_STAT::EMC_TASK_STAT() g68_offset{}, g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, g68_active(0), + g68_kins_offset{}, + g68_kins_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, toolOffset{}, activeSettings{}, programUnits(CANON_UNITS_MM), diff --git a/src/emc/rs274ngc/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/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 801a4364ba7..b061c9762e2 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -58,24 +58,26 @@ void GCodeRenderer::set_spindle_speed(double rpm) { / (2 * M_PI) * rpm; } -// The plane's origin and axes on the machine, through the transform the way -// a move endpoint on this line goes, so the drawn plane and the moves in it -// agree. Recorded here rather than at hand over because the offsets in -// force at the definition are what place it, and they move on. -void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { +// The plane's origin and axes as the program defined it, through the +// offsets the way a move endpoint on this line goes, so the drawn plane and +// the moves in it agree. Recorded here rather than at hand over because the +// offsets in force at the definition are what place it, and they move on. +// Under the machine frame kinematics the moves go through the second +// feature coordinate system onto the slides and are drawn back on the part, +// where this is. +void GCodeRenderer::set_g68_drawn(const WorkFrame &drawn) { if(parse_state.interp_error) return; - frame_ = frame; workplane_ = -1; lo_unreached_ = false; - if(!frame.active || !data_) return; + if(!drawn.active || !data_) return; Point9 at; - transform({}, at); + chain(drawn, {}, at); Point3 origin = {at[P9_X], at[P9_Y], at[P9_Z]}; std::array axes; for(int i = 0; i < P3_COUNT; i++) { Point9 unit = {}; unit[i] = 1.0; - transform(unit, at); + chain(drawn, unit, at); for(int j = 0; j < P3_COUNT; j++) axes[i][j] = at[j] - origin[j]; } std::vector &planes = data_->workplanes; @@ -95,6 +97,17 @@ void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { workplane_ = (long)planes.size() - 1; } +// A kinematics switch moves no joint, so the point the next move starts +// from, in the world of the type in force, is where the new type reads the +// joints; drawn on the part it stays where it was. Where nothing can be +// evaluated here it keeps its numbers, as it always has. +void GCodeRenderer::select_kins_type(int type) { + if(parse_state.interp_error) return; + Point9 at; + if(point_to_kins_type(lo_, tool_, type, at)) lo_ = at; + if(timer_ && point_to_kins_type(time_lo_, tool_, type, at)) time_lo_ = at; +} + void GCodeRenderer::arc_feed(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c, @@ -336,8 +349,9 @@ void preview_geometry_register(py::module_ &m) { triple(r.extents[BOX_MAX]))); return out; }, "(lineno, origin, (x, y, z) axes, min, max) per tilted work " - "plane; origin and axes in the machine frame, the extents " - "of the moves made under it in the plane's own coordinates") + "plane; origin and axes where it is drawn, on the part, the " + "extents of the moves made under it in the plane's own " + "coordinates") .def("tool_offsets", [](const PreviewData &d) { py::list out; for(const ToolOffsetRecord &r : d.tool_offsets) @@ -883,9 +897,10 @@ void GCodeRenderer::publish_line() { canon_guard([&]{ canon_.attr("lineno") = last_line_; }); } -void GCodeRenderer::transform(const Point9 &in, Point9 &out) const { +void GCodeRenderer::chain(const WorkFrame &frame, const Point9 &in, + Point9 &out) const { out = in; - frame_.apply(out); + frame.apply(out); out += g92_; if(rotation_xy_ != 0.0) { double rotx = out[P9_X] * rotation_cos_ - out[P9_Y] * rotation_sin_; @@ -907,30 +922,32 @@ void GCodeRenderer::untransform(const Point9 &in, Point9 &out) const { out -= g92_; } -// Points through the live transform, by the two steps a move endpoint takes: -// the g92 -> XY rotation -> g5x chain, and then the GEOMETRY string of the -// plane drawn first. Nothing is sampled, linearised or rebuilt, so a point -// carrying an A/B/C value is turned by its own angle exactly as its move -// would be, and a GEOMETRY naming U/V/W reads them off the point rather than -// losing them. +// Points through the live transform, by the steps a move endpoint takes: +// the g92 -> XY rotation -> g5x chain, onto the part under the machine +// frame kinematics, and then the GEOMETRY string of the plane drawn first. +// Nothing is sampled, linearised or rebuilt, so a point carrying an A/B/C +// value is turned by its own angle exactly as its move would be, and a +// GEOMETRY naming U/V/W reads them off the point rather than losing them. bool GCodeRenderer::transform_points(const Point9 *in, size_t n, double *machine, double *display) const { for(size_t i = 0; i < n; i++) { - Point9 at; + Point9 at, part; transform(in[i], at); machine[i * P3_COUNT + P3_X] = at[P9_X]; machine[i * P3_COUNT + P3_Y] = at[P9_Y]; machine[i * P3_COUNT + P3_Z] = at[P9_Z]; + on_part(at, part); Point3 drawn; - plane_point(data_->ops[0], at, drawn); + plane_point(data_->ops[0], part, drawn); for(int c = 0; c < P3_COUNT; c++) display[i * P3_COUNT + c] = drawn[c]; } return true; } -void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, - unsigned char kind, PlanePoints *points) { +void GCodeRenderer::write_vertex(const Point9 &axes, const Point9 &drawn, + int line_number, unsigned char kind, + PlanePoints *points) { if(!data_->reserve(1)) { if(!parse_state.interp_error) PyErr_NoMemory(); parse_state.interp_error ++; @@ -939,7 +956,7 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, size_t at = data_->n; for(int i = 0; i < data_->nplanes; i++) { Point3 p; - plane_point(data_->ops[i], pts9, p); + plane_point(data_->ops[i], drawn, p); for(int j = 0; j < P3_COUNT; j++) { if(p[j] < data_->drawn[BOX_MIN][j]) data_->drawn[BOX_MIN][j] = p[j]; if(p[j] > data_->drawn[BOX_MAX][j]) data_->drawn[BOX_MAX][j] = p[j]; @@ -953,7 +970,7 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, if(data_->axis_pos) for(int k = 0; k < data_->n_axes; k++) data_->axis_pos[at * (size_t)data_->n_axes + k] = - (float)pts9[data_->axis_cols[k]]; + (float)axes[data_->axis_cols[k]]; data_->attrs[at * ATTRS_PER_VERTEX] = (uint32_t)line_number; data_->attrs[at * ATTRS_PER_VERTEX + 1] = (uint32_t)kind | (data_->tool << 8); @@ -962,36 +979,48 @@ void GCodeRenderer::write_vertex(const Point9 &pts9, int line_number, void GCodeRenderer::mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points) { - write_vertex(at, line_number, kind, points); + Point9 drawn; + on_part(at, drawn); + write_vertex(at, drawn, line_number, kind, points); } -// The four machine-frame extent pairs, from one move's raw endpoints. -void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2) { - Box3 box; - for(int j = 0; j < P3_COUNT; j++) { - box[BOX_MIN][j] = std::min(p1[j], p2[j]); - box[BOX_MAX][j] = std::max(p1[j], p2[j]); - } +// The four extent pairs, from one move's endpoints: as drawn, and with the +// tool offset added back to the endpoints as the slides' world has them, +// which is what the axis limits bound. Only the machine frame kinematics +// draws a point anywhere but where it is. +void GCodeRenderer::accumulate_extents(const Point9 &p1, const Point9 &p2, + const Point9 &d1, const Point9 &d2) { + // With no rotation to remove, the zero_rxy kinds are the plain ones. + bool rotated = rotation_xy_ != 0.0; + // A pair's box, and the same with the rotation taken out. + struct Boxes { Box3 box, rot; }; + auto boxes = [&](const Point9 &a, const Point9 &b) { + Boxes out; + for(int j = 0; j < P3_COUNT; j++) { + out.box[BOX_MIN][j] = std::min(a[j], b[j]); + out.box[BOX_MAX][j] = std::max(a[j], b[j]); + } + if(rotated) { + Point3 u1, u2; + unrotate_xy(a, u1); + unrotate_xy(b, u2); + for(int j = 0; j < P3_COUNT; j++) { + out.rot[BOX_MIN][j] = std::min(u1[j], u2[j]); + out.rot[BOX_MAX][j] = std::max(u1[j], u2[j]); + } + } + return out; + }; + Boxes slides = boxes(p1, p2), drawn = boxes(d1, d2); // The tool-corrected box is the raw box shifted: adding a constant is // monotonic, so this is the same box, not an approximation of it. Point3 shift = {tool_[P9_X], tool_[P9_Y], tool_[P9_Z]}; - Box3 rot; - if(rotation_xy_ != 0.0) { - Point3 u1, u2; - unrotate_xy(p1, u1); - unrotate_xy(p2, u2); - for(int j = 0; j < P3_COUNT; j++) { - rot[BOX_MIN][j] = std::min(u1[j], u2[j]); - rot[BOX_MAX][j] = std::max(u1[j], u2[j]); - } - } - // With no rotation to remove, the zero_rxy kinds are the plain ones. - bool rotated = rotation_xy_ != 0.0; for(int i = 0; i < EXTENT_KINDS; i++) { - bool unrotated = (i >= EXT_ZERO_RXY) && rotated; - const Point3 &lo = unrotated ? rot[BOX_MIN] : box[BOX_MIN]; - const Point3 &hi = unrotated ? rot[BOX_MAX] : box[BOX_MAX]; bool notool = (i == EXT_NOTOOL || i == EXT_NOTOOL_ZERO_RXY); + const Boxes &from = notool ? slides : drawn; + bool unrotated = (i >= EXT_ZERO_RXY) && rotated; + const Point3 &lo = unrotated ? from.rot[BOX_MIN] : from.box[BOX_MIN]; + const Point3 &hi = unrotated ? from.rot[BOX_MAX] : from.box[BOX_MAX]; for(int j = 0; j < P3_COUNT; j++) { double a = notool ? lo[j] + shift[j] : lo[j]; double b = notool ? hi[j] + shift[j] : hi[j]; @@ -1016,15 +1045,20 @@ void GCodeRenderer::unrotate_xy(const Point9 &p, Point3 &out) const { void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, unsigned char cat) { data_->moves ++; - accumulate_extents(p1, p2); + // Drawn where the tool stands against the part, which under the machine + // frame kinematics the slides' own world does not say. + Point9 d1, d2; + on_part(p1, d1); + on_part(p2, d2); + accumulate_extents(p1, p2, d1, d2); // A cut drawn from where traverses the drawing left out went under the // plane starts in it. - if(lo_unreached_ && cat != CAT_TRAVERSE) reach(p1); + if(lo_unreached_ && cat != CAT_TRAVERSE) reach(d1); lo_unreached_ = false; - reach(p2); + reach(d2); - double dx = p2[P9_X] - p1[P9_X], dy = p2[P9_Y] - p1[P9_Y], - dz = p2[P9_Z] - p1[P9_Z]; + double dx = d2[P9_X] - d1[P9_X], dy = d2[P9_Y] - d1[P9_Y], + dz = d2[P9_Z] - d1[P9_Z]; double len = sqrt(dx * dx + dy * dy + dz * dz); if(cat == CAT_TRAVERSE) data_->rapid_length += len; else data_->cut_length += len; @@ -1033,7 +1067,8 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, // at its start; the shaders discard the segment into it. bool jump = !data_->has_cur || p1 != data_->cur9; // Rotary subdivision: a move that turns A, B or C is drawn as a polyline, - // since the tool's path through the machine's frame is not a straight line. + // since the tool's path through the machine's frame is not a straight + // line, nor on the part where the slides carry it there. long steps = 1; bool turning = false; double dc = 0.0; @@ -1049,15 +1084,18 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, long count = steps + (jump ? 1 : 0); for(long i = 0; i < count; i++) { long sub = i - (jump ? 1 : 0) + 1; - Point9 pt; - if(steps == 1 && !jump) { - pt = p2; + if(sub == 0) { + write_vertex(p1, d1, line_number, KIND_NOOP, nullptr); + } else if(sub == steps) { + write_vertex(p2, d2, line_number, cat, nullptr); } else { double t = (double)sub / (double)steps; + Point9 pt, drawn; for(int k = 0; k < P9_COUNT; k++) pt[k] = t * p2[k] + (1.0 - t) * p1[k]; + on_part(pt, drawn); + write_vertex(pt, drawn, line_number, cat, nullptr); } - write_vertex(pt, line_number, sub == 0 ? KIND_NOOP : cat, nullptr); } data_->cur9 = p2; data_->has_cur = true; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index e5131e80cb1..83bb1fb135d 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -135,13 +135,14 @@ struct ToolOffsetRecord { }; // A tilted work plane the program defined, G68.2, G68.3 or G68.4: where it -// sits on the machine, and what the program did in it. The origin and the -// axes are in the machine frame, through the offsets in force at the -// definition the way a move endpoint on the same line gets them; the -// extents are in the plane's own coordinates, over every move made while -// it was in effect, empty (min > max) while nothing moved. A definition -// that names the plane in effect again is the same record, so a program -// that restates its plane in a loop has one plane, not one per pass. +// is drawn, and what the program did in it. The origin and the axes are +// through the offsets in force at the definition the way a move endpoint +// on the same line gets them, and on the part, where the moves are drawn, +// under the machine frame kinematics too; the extents are in the plane's +// own coordinates, over every move made while it was in effect, empty +// (min > max) while nothing moved. A definition that names the plane in +// effect again is the same record, so a program that restates its plane in +// a loop has one plane, not one per pass. struct WorkPlaneRecord { int lineno; Point3 origin; @@ -156,7 +157,7 @@ struct WorkPlaneRecord { } return true; } - // Take in a machine point the program reached under the plane. + // Take in a point the program reached under the plane, as drawn. void extend(const Point9 &p) { Point3 d = {p[P9_X] - origin[P3_X], p[P9_Y] - origin[P3_Y], p[P9_Z] - origin[P3_Z]}; @@ -409,7 +410,14 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; - void set_g68_frame(const WorkFrame &frame) override; + // The moves go through the frame canon applies; the plane is drawn as + // the program defined it, which set_g68_drawn() brings. + void set_g68_frame(const WorkFrame &frame) override { + if(parse_state.interp_error) return; + frame_ = frame; + } + void set_g68_drawn(const WorkFrame &drawn) override; + void select_kins_type(int type) override; // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -512,9 +520,18 @@ private: // One record vertex at `at`, writing its per-plane position to `points`. void mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points); - void write_vertex(const Point9 &pts9, int line_number, unsigned char kind, - PlanePoints *points); - void accumulate_extents(const Point9 &p1, const Point9 &p2); + // A vertex at `axes`, drawn at `drawn`: the same point but under the + // machine frame kinematics, where on_part() has moved it. + void write_vertex(const Point9 &axes, const Point9 &drawn, int line_number, + unsigned char kind, PlanePoints *points); + void accumulate_extents(const Point9 &p1, const Point9 &p2, + const Point9 &d1, const Point9 &d2); + // Where a point of the chain is drawn: where the tool stands against the + // part, which under the machine frame kinematics the slides' own world + // does not say, and the point itself everywhere else. + void on_part(const Point9 &at, Point9 &out) const { + if(!point_on_part(at, tool_, out)) out = at; + } bool read_planes(); // program_geometry.machine_limits -> the estimator, or none. bool read_limits(); @@ -532,7 +549,11 @@ private: // where this came from, though that method no longer runs on a rendered // parse. Not bit-identical to it by construction: the compiler is free to // contract the rotation's multiply-add, so the tests allow a few ULPs. - void transform(const Point9 &in, Point9 &out) const; + void transform(const Point9 &in, Point9 &out) const { + chain(frame_, in, out); + } + // The same through a plane other than the one moves go through. + void chain(const WorkFrame &frame, const Point9 &in, Point9 &out) const; // The inverse, to recover an arc segment's program X. void untransform(const Point9 &in, Point9 &out) const; void event(Kind kind, int line_number, const Point9 &axes); diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f725a708dff..61c247c2cc6 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, @@ -554,13 +577,25 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; +// The plane canon applies, which carries the moves onto the slides, and the +// plane as defined, which the renderer draws on the part: under the machine +// frame kinematics the one has the second feature coordinate system folded +// in, and the renderer reads the moves back onto the part, where the plane +// is. A callback canon has only the first. void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) { - WorkFrame frame; - frame.active = active != 0; + const double rotation[9], int active, + const double *defined_origin, + const double *defined_rotation, + const double * /*kins_shift*/, + const double * /*kins_rotation*/) { + WorkFrame frame, drawn; + frame.active = drawn.active = active != 0; frame.origin = ensure_inch({x, y, z}); std::copy(rotation, rotation + 9, frame.rotation.begin()); + drawn.origin = ensure_inch({defined_origin[0], defined_origin[1], defined_origin[2]}); + std::copy(defined_rotation, defined_rotation + 9, drawn.rotation.begin()); parse_state.canon->set_g68_frame(frame); + parse_state.canon->set_g68_drawn(drawn); }; void USE_LENGTH_UNITS(CANON_UNITS u) { parse_state.metric = u == CANON_UNITS_MM; } @@ -638,6 +673,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*/) {} @@ -668,7 +707,9 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {parse_state.selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void SELECT_KINS_TYPE(int /*switchkins_type*/) {} +void SELECT_KINS_TYPE(int switchkins_type) { + parse_state.canon->select_kins_type(switchkins_type); +} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} @@ -753,6 +794,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,11 +978,138 @@ 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 machine's second feature coordinate system, from a canon that watches +// the status buffer, for the plane the preview restates from it +int GET_EXTERNAL_G68_KINS_FRAME(double *shift, double *rotation) { + if(parse_state.interp_error) return -1; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_external_g68_kins_frame")) return -1; + try { + py::object got = canon.attr("get_external_g68_kins_frame")(); + if(got.is_none()) return -1; + py::sequence pair = got.cast(); + py::sequence o = py::reinterpret_borrow(pair[0]).cast(); + py::sequence r = py::reinterpret_borrow(pair[1]).cast(); + if(py::len(o) < 3 || py::len(r) != 9) return -1; + for(int i = 0; i < 3; i++) shift[i] = o[i].cast(); + for(int i = 0; i < 9; i++) rotation[i] = r[i].cast(); + } catch(py::error_already_set &) { + return -1; // the error goes with the exception + } catch(py::builtin_exception &) { + return -1; + } + return 1; +} + +// the kind of transform the machine runs, from a canon that watches the +// status buffer; one that cannot answer has no machine, and the joints +// 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*/) { } +// The renderer's points on the part, through the interpreter of the parse: +// whether the type in force reads back onto the part is asked once per +// type, then each point is mapped. The answer belongs to one parse. +struct OnPart { + int kins_type = -1; // the type the answer is for + bool ready = false; +}; +static OnPart on_part; + +// A renderer's point, in inches with the tool offset out, through one of +// the interpreter's kinematics maps, which work in the machine's units with +// the tool offset in. +template +static bool through_kins(const Point9 &at, const Point9 &tool, Point9 &out, Map map) { + // inches to the machine's units; the angles are degrees either way + double scale = 25.4 * GET_EXTERNAL_LENGTH_UNITS(); + double world[P9_COUNT], got[P9_COUNT]; + for(int i = 0; i < P9_COUNT; i++) { + double v = at[i] + tool[i]; + world[i] = (parse_state.angular >> i) & 1 ? v : v * scale; + } + if(!map(world, got)) return false; + for(int i = 0; i < P9_COUNT; i++) { + double v = (parse_state.angular >> i) & 1 ? got[i] : got[i] / scale; + out[i] = v - tool[i]; + } + return true; +} + +bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return false; + setup_pointer s = &interp->_setup; + if(s->kins_type != on_part.kins_type) { + on_part.kins_type = s->kins_type; + on_part.ready = interp->kins_part_ready(s); + } + if(!on_part.ready) return false; + return through_kins(at, tool, out, [&](const double *world, double *part) { + return interp->kins_on_part(s, world, part); + }); +} + +bool point_to_kins_type(const Point9 &at, const Point9 &tool, int type, Point9 &out) { + Interp *interp = dynamic_cast(parse_state.pinterp); + if(!interp || parse_state.interp_error) return false; + setup_pointer s = &interp->_setup; + if(type == s->kins_type || !interp->kins_preview(s)) return false; + return through_kins(at, tool, out, [&](const double *world, double *got) { + return interp->kins_carry(s, s->kins_type, type, world, got); + }); +} + #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) // The parse. `initcodes` is the list of the new signature or null for the @@ -931,6 +1123,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 +1214,33 @@ 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(); + on_part = OnPart(); 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) { + // no motion reports the point after a kinematics switch here: the + // interpreter reads it back from the joints itself + own->_setup.kins_switch_reads_back = true; + machine_moves = own->_setup.machine_moves_in_machine_frame; + own->_setup.machine_moves_in_machine_frame = false; + // and a tilted work plane they restate is the one status has + own->_setup.g68_as_sent = true; + } + if(initcodes) { 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; + own->_setup.g68_as_sent = false; + } while(!parse_state.interp_error && RESULT_OK) { error_line_offset = 1; @@ -1080,7 +1299,10 @@ static py::object parse_file(const char *f, py::handle canon, if(parse_state.pinterp) { auto interp = dynamic_cast(parse_state.pinterp); - if(interp) interp->_setup.use_lazy_close = false; + if(interp) { + interp->_setup.use_lazy_close = false; + interp->_setup.g68_as_sent = false; + } parse_state.pinterp->close(); } if(parse_state.interp_error) { diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index db21a558a0a..2eede43fad6 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -179,6 +179,17 @@ public: // The tilted work plane, or its cancellation; the origin already in // inches. virtual void set_g68_frame(const WorkFrame &frame) = 0; + // The same plane as the program defined it, on the part. Under the + // machine frame kinematics the frame canon applies has the second + // feature coordinate system folded in, and carries the moves onto the + // slides; a preview draws the plane on the part instead, where it reads + // the moves back to. Elsewhere the two are the same. Only the renderer + // listens. + virtual void set_g68_drawn(const WorkFrame & /*drawn*/) {} + // A kinematics switch, before the interpreter takes it: the machine + // does not move, and the point a renderer chains from is read back + // from the joints under the new type. Only the renderer listens. + virtual void select_kins_type(int /*type*/) {} virtual void set_plane(int plane) = 0; virtual void set_feed_rate(double rate) = 0; virtual void set_traverse_rate(double rate) = 0; @@ -318,5 +329,16 @@ static inline void forward(const char *method, A &&...args) { // external_*_units. Defined in gcodemodule.cc. double read_external_length_units(); double read_external_angle_units(); +// Where the tool tip stands against the part for a point of the +// kinematics type the program is in, in inches with the tool offset out as +// the renderer has its points: the primary type's reading of the joints +// the slides stand in there. False, and out left alone, where the two are +// the same: under the primary type, on a machine without one, and where +// the kinematics cannot be evaluated here. +bool point_on_part(const Point9 &at, const Point9 &tool, Point9 &out); +// The same point over a switch to another type, before the interpreter +// takes it: the joints stay, and the point is where the new type reads +// them. False, and out left alone, where nothing can be evaluated here. +bool point_to_kins_type(const Point9 &at, const Point9 &tool, int type, Point9 &out); #endif // GCODEMODULE_HH diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 14dd80bf570..31ac55ecb39 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 Z, rotaries 1 2 3, 0 0 0, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_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..f16d0e14de0 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -2477,6 +2477,9 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->w_axis_offset); SET_XY_ROTATION(settings->rotation_xy); + // canon's plane has the second feature coordinate system folded in, over + // the offsets that just changed + if (settings->g68_snap_active) { work_plane_send(settings); } return INTERP_OK; } @@ -3106,6 +3109,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 +4226,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 +4520,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 +4530,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 @@ -5632,6 +5645,7 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 if (settings->g68_active) { world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, &settings->current_y, &settings->current_z); + if (settings->g68_snap_active) { work_plane_send(settings); } } if (block->m_modes[4] == 30) @@ -5744,8 +5758,23 @@ 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) { + double *rotary_end[6] = {&AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end}; + CHP(tool_vector_ends(block, settings, rotary_end)); + } if (move == G_1) { inverse_time_rate_straight(end_x, end_y, end_z, @@ -5757,7 +5786,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 +6662,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; @@ -6623,10 +6674,10 @@ static int kins_type_info_available() return 0; } -// the type carrying a KINSTYPE_ flag, or -1 when the module declares none; +// the lowest type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test -static int flagged_kins_type(int flag) +int flagged_kins_type(int flag) { int k, f; @@ -6684,26 +6735,46 @@ 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); + int now = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With - // no kinematics attached there is nothing to switch to. + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. + // A module with a working transform per tool head flags each one + // primary: the one in force stays, from any other type the lowest. + if (now >= 0 && (now & KINSTYPE_PRIMARY)) { primary = settings->kins_type; } CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); - if (primary >= 0) { 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); + CHP(snap_after_switch(settings, standing, have_standing)); + } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { // the offset in effect is no longer G43.4's, so G49 has no switch to undo 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 +6854,103 @@ 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"); } - 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; + // 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; + if (!settings->g68_snap_active) { + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + } + if (settings->g68_snap_active) { + // Under the second feature coordinate system the machine frame type + // reads no tool, so the slides stay where they are, and the frame is + // taken again at its rotaries with the new length, which it carries + // along the tilted tool. + double wx, wy, wz; + EmcPose before = settings->tool_offset; + int status; - settings->tool_offset = tool_offset; + program_to_world_xyz(settings, settings->current_x, settings->current_y, + settings->current_z, &wx, &wy, &wz); + settings->tool_offset = tool_offset; + // the same offset again, as a GUI restates it, keeps the frame: one + // taken from status may stand on rotaries turned since + status = (tool_offset.tran.x == before.tran.x && tool_offset.tran.y == before.tran.y + && tool_offset.tran.z == before.tran.z && tool_offset.a == before.a + && tool_offset.b == before.b && tool_offset.c == before.c + && tool_offset.u == before.u && tool_offset.v == before.v + && tool_offset.w == before.w) + ? INTERP_OK : snap_take(settings, settings->g68_snap_joints); + if (status != INTERP_OK) { + settings->tool_offset = before; + return status; + } + world_to_program_xyz(settings, wx, wy, wz, &settings->current_x, + &settings->current_y, &settings->current_z); + settings->AA_current += before.a - tool_offset.a; + settings->BB_current += before.b - tool_offset.b; + settings->CC_current += before.c - tool_offset.c; + settings->u_current += before.u - tool_offset.u; + settings->v_current += before.v - tool_offset.v; + settings->w_current += before.w - tool_offset.w; + USE_TOOL_LENGTH_OFFSET(tool_offset); + work_plane_send(settings); + } else if (point_known) { + double prog[9]; + EmcPose in_program; + + 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_AX(AXIS_A, point.a); + in_program.b = USER_TO_PROGRAM_AX(AXIS_B, point.b); + in_program.c = USER_TO_PROGRAM_AX(AXIS_C, point.c); + in_program.u = USER_TO_PROGRAM_AX(AXIS_U, point.u); + in_program.v = USER_TO_PROGRAM_AX(AXIS_V, point.v); + in_program.w = USER_TO_PROGRAM_AX(AXIS_W, point.w); + USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); + } else { + double dx, dy, dz; + + USE_TOOL_LENGTH_OFFSET(tool_offset); + + // by the offset difference seen from the program: the XY rotation + // and the tilted work plane taken off it + dx = settings->tool_offset.tran.x - tool_offset.tran.x; + dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; + + rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); + + settings->current_x += dx; + settings->current_y += dy; + settings->current_z += dz; + settings->AA_current += settings->tool_offset.a - tool_offset.a; + settings->BB_current += settings->tool_offset.b - tool_offset.b; + settings->CC_current += settings->tool_offset.c - tool_offset.c; + settings->u_current += settings->tool_offset.u - tool_offset.u; + settings->v_current += settings->tool_offset.v - tool_offset.v; + settings->w_current += settings->tool_offset.w - tool_offset.w; + + settings->tool_offset = tool_offset; + } // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, @@ -6836,7 +6977,15 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // the program selected itself is left alone, and a module that // declares no such type keeps the plain cancel. int identity = flagged_kins_type(KINSTYPE_MACHINE); - if (identity >= 0) { switch_kins_type(identity, settings); } + if (identity >= 0 && identity != settings->kins_type) { + double joints[EMCMOT_MAX_JOINTS], kept[EMCMOT_MAX_JOINTS]; + bool have, back; + CHP(snap_before_switch(settings, joints, &have)); + switch_joints(settings, kept, &back); + switch_kins_type(identity, settings); + CHP(snap_after_switch(settings, joints, have)); + if (back) { switch_point(settings, kept); } + } settings->kins_by_g43_4 = false; } @@ -6922,8 +7071,19 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 NCE_KINS_TYPE_NOT_PROVIDED); } + // a tilted work plane stays; the joints stay, so its second feature + // coordinate system is taken where they are, or dropped, and a preview + // reads the point back from them + double joints[EMCMOT_MAX_JOINTS], kept[EMCMOT_MAX_JOINTS]; + bool have = false, back = false, changed = (kins_type != settings->kins_type); + if (changed) { + CHP(snap_before_switch(settings, joints, &have)); + switch_joints(settings, kept, &back); + } SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; + if (changed) { CHP(snap_after_switch(settings, joints, have)); } + if (back) { switch_point(settings, kept); } // the program has taken the kinematics over from G43.4 settings->kins_by_g43_4 = false; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.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..8832d8e937c 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,28 @@ struct setup unsigned g68_seq_have; // bit per Q received int g68_seq_r_q; // the P2 block that gave R, or -1 double g68_seq_word[4][7]; // per Q: x y z i j k r + // the second feature coordinate system: under the machine frame type a + // plane is cut on the part as the primary type places it with the + // rotaries held, a rigid map from the primary type's world to the + // machine frame, after the whole chain; machine units + bool g68_snap_active; + double g68_snap_rotation[3][3]; + double g68_snap_shift[3]; + double g68_snap_joints[EMCMOT_MAX_JOINTS]; // the joints it was taken at + bool g68_as_sent; // a plane given now is the machine's, as a GUI restates it from status + // the pose G53.2 last solved, in program words, for #<_orient_x> and kin + bool orient_valid; + double orient_pose[6]; // x y z, the rotaries in orient order + // 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 @@ -808,8 +844,11 @@ struct setup bool probe_flag; // flag indicating probing done bool input_flag; // flag indicating waiting for input done bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done + bool kins_switch_reads_back; // a preview: no motion reports the point after a switch, so it is read from the joints here 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 +917,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 +1150,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..19574483586 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,19 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_KINS_ORIENT_1, + NP_KINS_ORIENT_2, + NP_KINS_ORIENT_3, + NP_KINS_ORIENT_1_HEAD, + NP_KINS_ORIENT_2_HEAD, + NP_KINS_ORIENT_3_HEAD, + NP_ORIENT_VALID, + NP_ORIENT_X, + NP_ORIENT_Y, + NP_ORIENT_Z, + NP_ORIENT_ROT1, + NP_ORIENT_ROT2, + NP_ORIENT_ROT3, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -546,6 +559,39 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_KINS_ORIENT_1: // _kins_orient_1 and kin: the axes that orient the tool + case NP_KINS_ORIENT_2: + case NP_KINS_ORIENT_3: + case NP_KINS_ORIENT_1_HEAD: + case NP_KINS_ORIENT_2_HEAD: + case NP_KINS_ORIENT_3_HEAD: + { + int axes[3], head[3]; + kins_orient(&_setup, axes, head); + if (cmd <= NP_KINS_ORIENT_3) { + *value = axes[cmd - NP_KINS_ORIENT_1]; + } else { + *value = head[cmd - NP_KINS_ORIENT_1_HEAD]; + } + } + 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_ROT1: + case NP_ORIENT_ROT2: + case NP_ORIENT_ROT3: + 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 +969,25 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the axes that orient the tool on that type, 0 X to 8 W, first rotation + // then second, and whether each turns the head (1) or the table (0); -1 + // where the type does not orient with two rotaries + init_readonly_param("_kins_orient_1", NP_KINS_ORIENT_1, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2", NP_KINS_ORIENT_2, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3", NP_KINS_ORIENT_3, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_1_head", NP_KINS_ORIENT_1_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2_head", NP_KINS_ORIENT_2_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3_head", NP_KINS_ORIENT_3_HEAD, PA_USE_LOOKUP); + + // the pose G53.2 last solved: 1.0 once one has been, and its words + init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); + init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); + init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); + init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); + init_readonly_param("_orient_rot1", NP_ORIENT_ROT1, PA_USE_LOOKUP); + init_readonly_param("_orient_rot2", NP_ORIENT_ROT2, PA_USE_LOOKUP); + init_readonly_param("_orient_rot3", NP_ORIENT_ROT3, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_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..c095b313a34 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -114,6 +114,21 @@ setup::setup() : g68_seq_have(0), g68_seq_r_q(-1), g68_seq_word{}, + g68_snap_active(false), + g68_snap_rotation{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + g68_snap_shift{0.0, 0.0, 0.0}, + g68_snap_joints{}, + g68_as_sent(false), + orient_valid(false), + orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + kins_ctx(nullptr), + 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), @@ -127,8 +142,11 @@ setup::setup() : probe_flag(0), input_flag(0), kinsSwitch_flag(0), + kins_switch_reads_back(false), 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 +212,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 +239,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..9256fc2a9b5 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -43,6 +43,7 @@ #include "rs274ngc_return.hh" #include "interp_internal.hh" #include "rs274ngc_interp.hh" +#include "units.h" //---------------------------------------------------------------------- // small matrix helpers, row major double[3][3] @@ -151,7 +152,8 @@ void Interp::g68_unrotate(setup_pointer s, double *x, double *y, double *z) } // The whole chain for X Y Z, program coordinates to the absolute (G53) -// frame: the plane, G92, the XY rotation, G5x and the tool offset. +// frame: the plane, G92, the XY rotation, G5x and the tool offset, then +// the second feature coordinate system where one is in effect. void Interp::program_to_world_xyz(setup_pointer s, double px, double py, double pz, double *wx, double *wy, double *wz) @@ -163,15 +165,31 @@ void Interp::program_to_world_xyz(setup_pointer s, y += s->axis_offset_y; z += s->axis_offset_z; rotate(&x, &y, s->rotation_xy); - *wx = x + s->origin_offset_x + s->tool_offset.tran.x; - *wy = y + s->origin_offset_y + s->tool_offset.tran.y; - *wz = z + s->origin_offset_z + s->tool_offset.tran.z; + x += s->origin_offset_x + s->tool_offset.tran.x; + y += s->origin_offset_y + s->tool_offset.tran.y; + z += s->origin_offset_z + s->tool_offset.tran.z; + if (s->g68_snap_active) { + mat_apply(s->g68_snap_rotation, &x, &y, &z); + x += USER_TO_PROGRAM_LEN(s->g68_snap_shift[0]); + y += USER_TO_PROGRAM_LEN(s->g68_snap_shift[1]); + z += USER_TO_PROGRAM_LEN(s->g68_snap_shift[2]); + } + *wx = x; + *wy = y; + *wz = z; } void Interp::world_to_program_xyz(setup_pointer s, double wx, double wy, double wz, double *px, double *py, double *pz) { + if (s->g68_snap_active) { + wx -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[0]); + wy -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[1]); + wz -= USER_TO_PROGRAM_LEN(s->g68_snap_shift[2]); + mat_apply_transposed(s->g68_snap_rotation, &wx, &wy, &wz); + } + double x = wx - s->origin_offset_x - s->tool_offset.tran.x; double y = wy - s->origin_offset_y - s->tool_offset.tran.y; double z = wz - s->origin_offset_z - s->tool_offset.tran.z; @@ -190,29 +208,112 @@ void Interp::world_to_program_xyz(setup_pointer s, // setting and clearing the plane //---------------------------------------------------------------------- -// Tell canon the plane in effect. Canon knows no G52 in the plane: it -// gets the origin with L applied, which is the frame the moves are in. -static void work_plane_send(setup_pointer s) +// The plane as defined, the form status reports it in. Canon knows no G52 +// in the plane: it gets the origin with L applied, which is the frame the +// moves are in. +void Interp::work_plane_defined(setup_pointer s, double o[3], double flat[9]) { - double o[3] = { s->g68_local[0], s->g68_local[1], s->g68_local[2] }, flat[9]; - + for (int i = 0; i < 3; i++) { o[i] = s->g68_local[i]; } mat_apply(s->g68_rotation, &o[0], &o[1], &o[2]); for (int i = 0; i < 3; i++) { o[i] += s->g68_offset[i]; for (int j = 0; j < 3; j++) { flat[3*i + j] = s->g68_rotation[i][j]; } } - SET_G68_FRAME(o[0], o[1], o[2], flat, 1); +} + +// The plane canon applies. Canon knows no second feature coordinate system: +// the stages between the plane and it are rigid, so canon gets the plane +// that lands where the chain with it does, read off at four points. +void Interp::work_plane_sent(setup_pointer s, double o[3], double flat[9]) +{ + work_plane_defined(s, o, flat); + if (s->g68_snap_active) { + double v[4][3]; + + for (int k = 0; k < 4; k++) { + double x, y, z; + + program_to_world_xyz(s, k == 1, k == 2, k == 3, &x, &y, &z); + // back through the stages canon applies outside the plane + x -= s->origin_offset_x + s->tool_offset.tran.x; + y -= s->origin_offset_y + s->tool_offset.tran.y; + z -= s->origin_offset_z + s->tool_offset.tran.z; + rotate(&x, &y, -s->rotation_xy); + v[k][0] = x - s->axis_offset_x; + v[k][1] = y - s->axis_offset_y; + v[k][2] = z - s->axis_offset_z; + } + for (int i = 0; i < 3; i++) { + o[i] = v[0][i]; + for (int j = 0; j < 3; j++) { flat[3*i + j] = v[j + 1][i] - v[0][i]; } + } + } +} + +// tell canon the plane in effect, and status the plane as defined and the +// second feature coordinate system apart +void Interp::work_plane_send(setup_pointer s) +{ + double o[3], flat[9], def_o[3], def_flat[9], shift[3] = { 0, 0, 0 }; + double kins[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + work_plane_sent(s, o, flat); + work_plane_defined(s, def_o, def_flat); + if (s->g68_snap_active) { + for (int i = 0; i < 3; i++) { + shift[i] = s->g68_snap_shift[i]; + for (int j = 0; j < 3; j++) { kins[3*i + j] = s->g68_snap_rotation[i][j]; } + } + } + SET_G68_FRAME(o[0], o[1], o[2], flat, 1, def_o, def_flat, shift, kins); +} + +// tell canon there is no plane +static void work_plane_send_none() +{ + static const double zero[3] = { 0, 0, 0 }; + static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0, zero, identity, zero, identity); +} + +// The second feature coordinate system for a plane that comes from the +// machine, a GUI restating it or an abort keeping it: the one status has +// where the machine reports one, else one taken at the joints it stands in. +int Interp::snap_from_machine(setup_pointer s, const double *joints) +{ + double shift[3], flat[9]; + + if (GET_EXTERNAL_G68_KINS_FRAME(shift, flat) <= 0) { return snap_take(s, joints); } + for (int i = 0; i < 3; i++) { + s->g68_snap_shift[i] = shift[i]; + for (int j = 0; j < 3; j++) { s->g68_snap_rotation[i][j] = flat[3*i + j]; } + } + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->g68_snap_joints[i] = joints[i]; } + s->g68_snap_active = true; + return INTERP_OK; } // Install a plane. The tool does not move, so its program coordinates // change: take the current point through the old chain to the absolute -// frame and back through the new one. +// frame and back through the new one. Under the machine frame type the +// second feature coordinate system is taken at the rotaries as they stand, +// or for a plane a GUI restates, the machine's. int Interp::work_plane_set(setup_pointer s, int code, const double origin[3], const double rotation[3][3]) { - double wx, wy, wz; + double wx, wy, wz, joints[EMCMOT_MAX_JOINTS]; + bool have; program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + CHP(snap_standing(s, joints, &have, s->g68_as_sent)); + if (!have) { + s->g68_snap_active = false; + } else if (s->g68_as_sent) { + CHP(snap_from_machine(s, joints)); + } else { + CHP(snap_take(s, joints)); + } for (int i = 0; i < 3; i++) { s->g68_offset[i] = origin[i]; @@ -235,22 +336,22 @@ int Interp::work_plane_set(setup_pointer s, int code, int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) { double wx, wy, wz; - static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; s->g68_seq_code = 0; if (!s->g68_active) { - if (tell_canon_anyway) { SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); } + if (tell_canon_anyway) { work_plane_send_none(); } return INTERP_OK; } program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); s->g68_active = false; + s->g68_snap_active = false; s->g68_code = 0; for (int i = 0; i < 3; i++) { s->g68_offset[i] = s->g68_local[i] = 0.0; } mat_identity(s->g68_rotation); world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); - SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); + work_plane_send_none(); return INTERP_OK; } @@ -260,10 +361,12 @@ int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) // the shifted origin; a G52 in the plane is kept when the machine was // still in the plane the interpreter has, and becomes part of the origin // otherwise. A plane whose coordinate system the read ahead has changed -// is cancelled. +// is cancelled. Under the machine frame type the second feature coordinate +// system is the machine's too. int Interp::work_plane_restore(setup_pointer s) { - double origin[3], flat[9], here[3], wx, wy, wz; + double origin[3], flat[9], joints[EMCMOT_MAX_JOINTS], wx, wy, wz; + bool have; int got = GET_EXTERNAL_G68_FRAME(origin, flat); s->g68_seq_code = 0; @@ -275,14 +378,22 @@ int Interp::work_plane_restore(setup_pointer s) if (got == 0) { return work_plane_cancel(s, true); } if (s->g68_active) { + double def_o[3], def_flat[9], shift[3], kins[9]; bool same = true; - for (int i = 0; i < 3; i++) { here[i] = s->g68_local[i]; } - mat_apply(s->g68_rotation, &here[0], &here[1], &here[2]); + work_plane_defined(s, def_o, def_flat); for (int i = 0; i < 3; i++) { - if (fabs(here[i] + s->g68_offset[i] - origin[i]) > 1e-9) { same = false; } + if (fabs(def_o[i] - origin[i]) > 1e-9) { same = false; } for (int j = 0; j < 3; j++) { - if (fabs(s->g68_rotation[i][j] - flat[3*i + j]) > 1e-12) { same = false; } + if (fabs(def_flat[3*i + j] - flat[3*i + j]) > 1e-12) { same = false; } + } + } + if (s->g68_snap_active && GET_EXTERNAL_G68_KINS_FRAME(shift, kins) > 0) { + for (int i = 0; i < 3; i++) { + if (fabs(s->g68_snap_shift[i] - shift[i]) > 1e-9) { same = false; } + for (int j = 0; j < 3; j++) { + if (fabs(s->g68_snap_rotation[i][j] - kins[3*i + j]) > 1e-12) { same = false; } + } } } if (same) { @@ -292,6 +403,12 @@ int Interp::work_plane_restore(setup_pointer s) } program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + CHP(snap_standing(s, joints, &have, true)); + if (have) { + CHP(snap_from_machine(s, joints)); + } else { + s->g68_snap_active = false; + } for (int i = 0; i < 3; i++) { s->g68_offset[i] = origin[i]; s->g68_local[i] = 0.0; @@ -535,6 +652,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 +682,1420 @@ 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 +#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; } + CHP(kins_check_orient(s)); + } + return INTERP_OK; +} + +// Every type the module evaluates here orients the tool with angular axes: +// a type whose rotaries are letters the INI file makes LINEAR would have +// G43.5 and G68.2 write degrees into a length. +int Interp::kins_check_orient(setup_pointer s) +{ + static const char letters[] = "XYZABCUVW"; + KinematicsUserContext *ctx = KINS_CTX(s); + double zero[EMCMOT_MAX_JOINTS] = {0}; + int t, i, n, axes[3], head[3]; + + for (t = 0; t < kinematicsUserGetNumTypes(ctx); t++) { + if (kinematicsUserSetType(ctx, t) != 0) { continue; } + n = kinematicsUserOrientAxes(ctx, zero, axes, head); + for (i = 0; i < n; i++) { + if (axisKindsAngular(s->axis_kinds, axes[i])) { continue; } + kins_release(s); + ERS(_("kinematics type %d of %s orients the tool with %c, which [AXIS_%c] TYPE makes LINEAR"), + t, s->kins_module, letters[axes[i]], letters[axes[i]]); + } + } + return INTERP_OK; +} + +// The axes that orient the tool on the kinematics type the program is in, +// at the joints it stands in: axis numbers 0 X to 8 W, first rotation then +// second, a robot's wrist A B C, and 1 for a head, 0 for a table. All -1 +// where the type turns other rotaries, or no module can be evaluated here. +void Interp::kins_orient(setup_pointer s, int axes[3], int head[3]) +{ + void *vctx; + double now[EMCMOT_MAX_JOINTS]; + int i; + + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + if (kins_context(s, &vctx) != INTERP_OK) { return; } + if (current_joints(s, (KinematicsUserContext *)vctx, now) != INTERP_OK) { return; } + kinematicsUserOrientAxes((KinematicsUserContext *)vctx, now, axes, head); +} + +// 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_AX(AXIS_A, offset->a); + tool.b = PROGRAM_TO_USER_AX(AXIS_B, offset->b); + tool.c = PROGRAM_TO_USER_AX(AXIS_C, offset->c); + tool.u = PROGRAM_TO_USER_AX(AXIS_U, offset->u); + tool.v = PROGRAM_TO_USER_AX(AXIS_V, offset->v); + tool.w = PROGRAM_TO_USER_AX(AXIS_W, offset->w); + kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); +} + +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_AX(AXIS_A, abs_pos[3]); + pose->b = PROGRAM_TO_USER_AX(AXIS_B, abs_pos[4]); + pose->c = PROGRAM_TO_USER_AX(AXIS_C, abs_pos[5]); + pose->u = PROGRAM_TO_USER_AX(AXIS_U, abs_pos[6]); + pose->v = PROGRAM_TO_USER_AX(AXIS_V, abs_pos[7]); + pose->w = PROGRAM_TO_USER_AX(AXIS_W, abs_pos[8]); +} + +// and back: a machine pose as program coordinates, through the chain +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_AX(AXIS_A, pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_AX(AXIS_B, pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_AX(AXIS_C, pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_AX(AXIS_U, pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_AX(AXIS_V, pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_AX(AXIS_W, pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; +} + +// whether two machine points are the same, to a hair either way +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); +} + +// The type the preview reads the joints back through to draw the tool on +// the part: the primary type, the one that follows the part, where another +// is in force, as motion reports the part position. -1 under the primary +// type itself and where the module declares none. +int Interp::part_kins_type(setup_pointer s) +{ + int primary = flagged_kins_type(KINSTYPE_PRIMARY); + + return primary == s->kins_type ? -1 : primary; +} + +// The kinematics for a preview, loaded, its pins read once: the points +// come in their thousands and the pins do not move under a preview. NULL +// where nothing can be evaluated here. Nothing here is an error: a +// preview that cannot map a point draws it as it is. +void *Interp::kins_preview(setup_pointer s) +{ + void *vctx; + + if (kins_here(s, &vctx) != INTERP_OK || !vctx) { return NULL; } + kinematicsUserHoldPins((KinematicsUserContext *)vctx, 1); + return vctx; +} + +// Whether the preview can draw the tool on the part under the type in +// force, with part_kins_type() and a module evaluated here; asked again +// only when the type changes. +bool Interp::kins_part_ready(setup_pointer s) +{ + return part_kins_type(s) >= 0 && kins_preview(s) != NULL; +} + +// The joints type from puts the machine in at a point of its world, read +// back by type to. world and out are absolute and in the machine's units +// with the tool offset in, as each type's world has them. False where +// either type cannot answer, or nothing is loaded. +bool Interp::kins_carry(setup_pointer s, int from, int to, const double world[9], double out[9]) +{ + KinematicsUserContext *ctx = KINS_CTX(s); + double joints[EMCMOT_MAX_JOINTS]; + EmcPose pose; + bool ok; + int i; + + if (!ctx || kinematicsUserIsRtOnly(ctx)) { return false; } + pose.tran.x = world[0]; + pose.tran.y = world[1]; + pose.tran.z = world[2]; + pose.a = world[3]; + pose.b = world[4]; + pose.c = world[5]; + pose.u = world[6]; + pose.v = world[7]; + pose.w = world[8]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + kins_set_tool(ctx, &s->tool_offset); + ok = kinematicsUserSetType(ctx, from) == 0 + && kinematicsUserInverse(ctx, &pose, joints) == 0 + && kinematicsUserSetType(ctx, to) == 0 + && kinematicsUserForward(ctx, joints, &pose) == 0; + kinematicsUserSetType(ctx, s->kins_type); + if (!ok) { return false; } + out[0] = pose.tran.x; + out[1] = pose.tran.y; + out[2] = pose.tran.z; + out[3] = pose.a; + out[4] = pose.b; + out[5] = pose.c; + out[6] = pose.u; + out[7] = pose.v; + out[8] = pose.w; + return true; +} + +// Where the tool stands against the part at a point of the type in force: +// the joints that type puts the machine in there, read back by the type +// that follows the part. Under the machine frame type the slides put the +// tool on the part somewhere their own world does not say, and a preview +// draws it there, as under the TCP kinematics. Once kins_part_ready() +// has said yes. +bool Interp::kins_on_part(setup_pointer s, const double world[9], double part[9]) +{ + const int primary = part_kins_type(s); + + return primary >= 0 && kins_carry(s, s->kins_type, primary, world, part); +} + +// The joints a kinematics switch keeps, for a preview to read the point +// back from under the new type: on the machine the queue drains at the +// switch and task reads the point from motion, but a preview has no +// motion to ask. have stays false where nothing can be evaluated. +void Interp::switch_joints(setup_pointer s, double *joints, bool *have) +{ + void *vctx = s->kins_switch_reads_back ? kins_preview(s) : NULL; + + *have = vctx && current_joints(s, vctx, joints) == INTERP_OK; +} + +// After the switch: the point where the new type reads those joints, as +// motion reads it. In a tilted work plane taken again at the same joints +// it comes out where it was. +void Interp::switch_point(setup_pointer s, const double *joints) +{ + void *vctx = kins_preview(s); + EmcPose pose; + double prog[9]; + int i; + + if (!vctx) { return; } + // the point as it stood, the estimate an iterative forward starts from + current_machine_pose(s, &pose); + if (kinematicsUserForward((KinematicsUserContext *)vctx, joints, &pose) != 0) { return; } + machine_pose_to_program(s, &pose, prog); + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } +} + +// 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; +} + +//---------------------------------------------------------------------- +// The second feature coordinate system. Under the machine frame type a +// plane is cut where the primary type, the one that follows the part, +// would cut it with the rotaries held where they stood when the plane was +// defined or the tool oriented to it: Fanuc's second feature coordinate +// system, 3+2 with the slides alone moving. With the rotaries held the +// map from the primary type's world to the machine frame is rigid, +// Q = forward_machine(inverse_primary(W)), and it carries the tool length +// along the tilted tool, which the primary type applies and the machine +// frame type does not. A rotary word does not take it again: the moves +// after it keep the frame, as Fanuc's do. +//---------------------------------------------------------------------- + +// The primary type the plane is read through, where the type in force is +// the machine frame type and does not itself follow the part; -1 where the +// plane is in the frame of the type in force. +int Interp::snap_primary_type(setup_pointer s) +{ + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + + if (flags < 0 || !(flags & KINSTYPE_MACHINE) || (flags & KINSTYPE_PRIMARY)) { return -1; } + return flagged_kins_type(KINSTYPE_PRIMARY); +} + +// The joints a snapshot taken now is taken at: where the program is, or +// from_machine where the machine stands, for a plane read back from status, +// which the program's point may not have reached yet. have stays false +// where none is wanted or nothing can be evaluated here. +int Interp::snap_standing(setup_pointer s, double *joints, bool *have, bool from_machine) +{ + void *vctx; + + *have = false; + if (snap_primary_type(s) < 0) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + if (!vctx) { return INTERP_OK; } + if (from_machine) { + int n = GET_EXTERNAL_JOINT_POSITIONS(joints, EMCMOT_MAX_JOINTS); + if (n > 0) { + for (int i = n; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = 0.0; } + *have = true; + return INTERP_OK; + } + } + CHP(current_joints(s, vctx, joints)); + *have = true; + return INTERP_OK; +} + +// Take the second feature coordinate system at the joints given, with the +// tool offset the program is under. Nothing changes on an error. +int Interp::snap_take(setup_pointer s, const double *joints) +{ + void *vctx; + KinematicsUserContext *ctx; + const int primary = snap_primary_type(s); + double held[4][EMCMOT_MAX_JOINTS], qv[4][3], rotation[3][3], shift[3]; + EmcPose base, q[4]; + int k, i, j, njoints, pass, status = 0; + + if (primary < 0) { + s->g68_snap_active = false; + return INTERP_OK; + } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx) { + s->g68_snap_active = false; + return INTERP_OK; + } + njoints = kinematicsUserGetNumJoints(ctx); + CHKS((kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + // the part as the primary type has it, at the point the joints place + // and a unit along each axis from it, the rotaries held + current_machine_pose(s, &base); + if (kinematicsUserForward(ctx, joints, &base) != 0) { status = -1; } + for (k = 0; status == 0 && k < 4; k++) { + EmcPose pose = base; + + pose.tran.x += (k == 1); + pose.tran.y += (k == 2); + pose.tran.z += (k == 3); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { held[k][i] = joints[i]; } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = held[k][i]; } + if (kinematicsUserInverse(ctx, &pose, held[k]) != 0) { + status = -1; + break; + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(held[k][i] - prev[i])); } + if (worst < 1e-9) { break; } + } + } + // and the same joints in the machine frame + kinematicsUserSetType(ctx, s->kins_type); + for (k = 0; status == 0 && k < 4; k++) { + q[k] = base; + if (kinematicsUserForward(ctx, held[k], &q[k]) != 0) { status = -1; } + } + CHKS((status != 0), + _("the kinematics cannot carry the tilted work plane from type %d onto the machine frame"), primary); + for (k = 0; k < 4; k++) { + for (j = 0; j < njoints; j++) { + double turn; + if (!(s->kins_angular_joints & (1 << j))) { continue; } + turn = fmod(fabs(held[k][j] - joints[j]), 360.0); + CHKS((fmin(turn, 360.0 - turn) > 1e-6), + _("kinematics type %d turns joint %d to follow the part, so a tilted work plane" + " cannot be held under the machine frame type"), primary, j); + } + } + for (k = 0; k < 4; k++) { + qv[k][0] = q[k].tran.x; + qv[k][1] = q[k].tran.y; + qv[k][2] = q[k].tran.z; + } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { rotation[i][j] = qv[j + 1][i] - qv[0][i]; } + } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + double dot = rotation[0][i]*rotation[0][j] + rotation[1][i]*rotation[1][j] + + rotation[2][i]*rotation[2][j]; + CHKS((fabs(dot - (i == j)) > 1e-6), + _("kinematics type %d does not carry the part rigidly onto the machine frame" + " with the rotaries held, so a tilted work plane cannot be cut under the" + " machine frame type"), primary); + } + } + shift[0] = base.tran.x; + shift[1] = base.tran.y; + shift[2] = base.tran.z; + mat_apply(rotation, &shift[0], &shift[1], &shift[2]); + for (i = 0; i < 3; i++) { shift[i] = qv[0][i] - shift[i]; } + memcpy(s->g68_snap_rotation, rotation, sizeof(rotation)); + memcpy(s->g68_snap_shift, shift, sizeof(shift)); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->g68_snap_joints[i] = joints[i]; } + s->g68_snap_active = true; + return INTERP_OK; +} + +// the joints a kinematics switch keeps, read while the type they are known +// in is in force, for the plane to be carried over the switch +int Interp::snap_before_switch(setup_pointer s, double *joints, bool *have) +{ + void *vctx; + + *have = false; + if (!s->g68_active) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + if (!vctx) { return INTERP_OK; } + CHP(current_joints(s, vctx, joints)); + *have = true; + return INTERP_OK; +} + +// After the switch: the second feature coordinate system where the new type +// wants one, taken at the joints the switch kept, and canon told. +int Interp::snap_after_switch(setup_pointer s, const double *joints, bool have) +{ + if (!s->g68_active) { return INTERP_OK; } + s->g68_snap_active = false; + if (have) { CHP(snap_take(s, joints)); } + work_plane_send(s); + return INTERP_OK; +} + +// a direction the program gives, in world coordinates: through the +// tilted work plane's rotation where one is active, then the XY rotation +// of the coordinate system it sits on +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; + int primary, identity, framed; + + 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; + CHP(current_joints(s, ctx, joints)); + // under the machine frame type the tool is read on the part, as the + // primary type has it + primary = snap_primary_type(s); + CHKS((primary >= 0 && kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + identity = kinematicsUserIsIdentity(ctx); + framed = !identity + && kinematicsUserWorkFrame(ctx, joints, &work) == 0 + && kinematicsUserToolFrame(ctx, joints, &tool) == 0; + kinematicsUserSetType(ctx, s->kins_type); + CHKS((identity), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHKS((!framed), + _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); + toolFrameInWork(&work, &tool, &in_work); + zt = in_work.z; + 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); +} + +// The rotary letters a solution for the kinematics type in force is +// written into, as axis numbers: those it orients the tool with, else the +// A B C of the pose. Returns how many. +static int orienting_letters(KinematicsUserContext *ctx, const double *now, int orienting[3]) +{ + int head[3]; + int n = kinematicsUserOrientAxes(ctx, now, orienting, head); + + if (n > 0) { return n; } + orienting[0] = 3; + orienting[1] = 4; + orienting[2] = 5; + return 3; +} + +// 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, primary, identity, solved; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + 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; + CHP(current_joints(s, ctx, now)); + + // under the machine frame type the tool is oriented on the part, as the + // primary type has it, every rotary free to help + primary = snap_primary_type(s); + CHKS((primary >= 0 && kinematicsUserSetType(ctx, primary) != 0), + _("kinematics type %d is not available outside realtime"), primary); + identity = kinematicsUserIsIdentity(ctx); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + solved = INTERP_OK; + if (!identity) { + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + solved = orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name); + } + kinematicsUserSetType(ctx, s->kins_type); + CHKS((identity), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHP(solved); + + // where that puts the machine, and what the program calls it: in the + // second feature coordinate system taken at the rotaries it ends at. + // STAY moves nothing, so its own is put back after. + bool kept_active = s->g68_snap_active; + double kept_rotation[3][3], kept_shift[3], kept_joints[EMCMOT_MAX_JOINTS]; + memcpy(kept_rotation, s->g68_snap_rotation, sizeof(kept_rotation)); + memcpy(kept_shift, s->g68_snap_shift, sizeof(kept_shift)); + memcpy(kept_joints, s->g68_snap_joints, sizeof(kept_joints)); + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + if (primary >= 0) { CHP(snap_take(s, sol)); } + machine_pose_to_program(s, &end_pose, end_prog); + if (code == G_53_2) { + s->g68_snap_active = kept_active; + memcpy(s->g68_snap_rotation, kept_rotation, sizeof(kept_rotation)); + memcpy(s->g68_snap_shift, kept_shift, sizeof(kept_shift)); + memcpy(s->g68_snap_joints, kept_joints, sizeof(kept_joints)); + } else if (primary >= 0) { + work_plane_send(s); + } + + if (code == G_53_2) { + // STAY: solve only, nothing moves. X Y Z and the rotaries in the + // order the type orients with go to the named parameters + // #<_orient_x> and kin and to #5071-#5080, for the program to use + // in a move of its own making, the way Heidenhain's STAY fills + // Q120-122. The machine state does not change. + for (i = 0; i < 3; i++) { s->orient_pose[i] = end_prog[i]; } + for (i = 0; i < 3; i++) { + s->orient_pose[3 + i] = (i < count) ? end_prog[orienting[i]] : 0.0; + } + s->orient_valid = true; + for (i = 0; i < 6; i++) { s->parameters[5071 + i] = s->orient_pose[i]; } + for (i = 6; i < 9; i++) { s->parameters[5071 + i] = 0.0; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + + // the rotaries the solution turns, in the letters the type orients + // with; every other axis stays where it is + double rot[6] = {s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current}; + for (i = 0; i < count; i++) { + if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } + } + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // 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, + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + if (code == G_53_1) { + // the joints went where the solver put them, every letter with them + for (i = 0; i < 6; i++) { rot[i] = end_prog[3 + i]; } + } + s->AA_current = rot[0]; + s->BB_current = rot[1]; + s->CC_current = rot[2]; + s->u_current = rot[3]; + s->v_current = rot[4]; + s->w_current = rot[5]; + return INTERP_OK; +} + +// 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 +// coordinates of the axes the kinematics type orients with (A B C U V W +// ends, 0 A to 5 W), so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotary_end[6]) +{ + static const char letters[] = "XYZABCUVW"; + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + double v[3], prog[9]; + const bool rotary_flag[6] = {block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag}; + PmCartesian axis; + EmcPose pose; + int i; + + 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)); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + for (i = 0; i < count; i++) { + CHKS((orienting[i] < 3), + _("G43.5: kinematics type %d orients the tool with %c, not a rotary axis"), + s->kins_type, letters[orienting[i]]); + CHKS((rotary_flag[orienting[i] - 3]), + _("G43.5: a tool vector and a %c word on one line give the orientation twice"), + letters[orienting[i]]); + } + 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); + for (i = 0; i < count; i++) { + *rotary_end[orienting[i] - 3] = prog[orienting[i]]; + } + 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 = axisKindsAngular(s->axis_kinds, a); + 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++) { + 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 = PROGRAM_TO_USER_AX(a, 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++) { + if (!flags[a]) { continue; } + *coord[a] = PROGRAM_TO_USER_AX(a, 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++) { + home[a] = USER_TO_PROGRAM_AX(a, 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..a3ea58b330e 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -376,6 +376,57 @@ 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 *rotary_end[6]); + int kins_check_orient(setup_pointer s); + void kins_orient(setup_pointer s, int axes[3], int head[3]); + int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int convert_home_slides(int code, block_pointer block, setup_pointer settings); + int convert_savehome_slides(int code, setup_pointer settings); + 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); + int part_kins_type(setup_pointer settings); + void *kins_preview(setup_pointer settings); + bool kins_part_ready(setup_pointer settings); + bool kins_carry(setup_pointer settings, int from, int to, const double world[9], double out[9]); + bool kins_on_part(setup_pointer settings, const double world[9], double part[9]); + void switch_joints(setup_pointer settings, double *joints, bool *have); + void switch_point(setup_pointer settings, const double *joints); + bool kins_machine_frame(setup_pointer settings); + int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known); + 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); + int snap_primary_type(setup_pointer settings); + int snap_standing(setup_pointer settings, double *joints, bool *have, bool from_machine); + int snap_from_machine(setup_pointer settings, const double *joints); + int snap_take(setup_pointer settings, const double *joints); + int snap_before_switch(setup_pointer settings, double *joints, bool *have); + int snap_after_switch(setup_pointer settings, const double *joints, bool have); + void work_plane_defined(setup_pointer settings, double origin[3], double rotation[9]); + void work_plane_sent(setup_pointer settings, double origin[3], double rotation[9]); + void work_plane_send(setup_pointer settings); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/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..e4102c81eac 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -116,7 +116,11 @@ void HOME_CYCLE(void) { ECHO_WITH_ARGS(""); } void HOME_CYCLE_JOINT(int joint) { ECHO_WITH_ARGS("%d", joint); } void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) { + const double rotation[9], int active, + const double * /*defined_origin*/, + const double * /*defined_rotation*/, + const double * /*kins_shift*/, + const double * /*kins_rotation*/) { ECHO_WITH_ARGS("%.4f, %.4f, %.4f, " "[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], %d", x, y, z, @@ -222,6 +226,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 +609,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 +868,16 @@ extern int GET_EXTERNAL_G68_FRAME(double * /*origin*/, double * /*rotation*/) return -1; } +extern int GET_EXTERNAL_G68_KINS_FRAME(double * /*shift*/, double * /*rotation*/) +{ + return -1; +} + +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return true; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); @@ -895,6 +957,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/Submakefile b/src/emc/task/Submakefile index 9fdab7004d6..e775d36d7af 100644 --- a/src/emc/task/Submakefile +++ b/src/emc/task/Submakefile @@ -32,7 +32,7 @@ USERSRCS += $(MILLTASKSRCS) ../bin/milltask: $(call TOOBJS, $(MILLTASKSRCS)) ../lib/librs274.so.0 ../lib/liblinuxcnc.a \ ../lib/libnml.so.0 ../lib/liblinuxcncini.so.1 ../lib/libposemath.so.0 \ ../lib/liblinuxcnchal.so.0 ../lib/libpyplugin.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 $(ECHO) Linking $(notdir $@) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 5ad736c59e8..18161da78c4 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -65,6 +65,11 @@ #include "tooldata/tooldata.hh" #include #include +#include // getpid() +#include +#include +#include +#include using namespace linuxcnc; @@ -482,6 +487,13 @@ static double toExtVel(double vel) { static double toExtAcc(double acc) { return toExtVel(acc); } +static double fromExtVel(double vel) { + if (!canon.cartesian_move && canon.angular_move) { + return FROM_EXT_ANG(vel); + } + return FROM_EXT_LEN(vel); +} + static void send_g5x_msg(int index) { flush_segments(); @@ -552,7 +564,11 @@ void HOME_CYCLE_JOINT(int joint) } void SET_G68_FRAME(double x, double y, double z, - const double rotation[9], int active) + const double rotation[9], int active, + const double defined_origin[3], + const double defined_rotation[9], + const double kins_shift[3], + const double kins_rotation[9]) { flush_segments(); @@ -562,12 +578,18 @@ void SET_G68_FRAME(double x, double y, double z, for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } canon.g68Active = active; + // status has the plane as defined and the second feature coordinate + // system apart, as the standard controls report them auto msg = std::make_unique(); - msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); - msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); - msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); - for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->origin.tran.x = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[0])); + msg->origin.tran.y = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[1])); + msg->origin.tran.z = TO_EXT_LEN(FROM_PROG_LEN(defined_origin[2])); + for (int i = 0; i < 9; i++) { msg->rotation[i] = defined_rotation[i]; } msg->active = active; + msg->kins_offset.tran.x = kins_shift[0]; + msg->kins_offset.tran.y = kins_shift[1]; + msg->kins_offset.tran.z = kins_shift[2]; + for (int i = 0; i < 9; i++) { msg->kins_rotation[i] = kins_rotation[i]; } interp_list.append(std::move(msg)); } @@ -836,6 +858,409 @@ static double getStraightJerk(double x, double y, double z, return 0.0; // a move to nowhere } +//---------------------------------------------------------------------- +// The kinematics ahead of motion: what the joints allow a segment. +// +// A segment is planned in world coordinates against the [AXIS_L] +// velocity and acceleration limits, which say nothing about the joints +// once the kinematics is not the identity: a turn of a tilted head under +// the tool centre point swings a carriage through a circle the head's +// own limit never mentions. So every segment is also sampled through +// the module's Jacobian here, on the kinematics type and the tool offset +// the program is under (canon runs ahead of motion, so neither is what +// the machine has reached), and its velocity and acceleration are +// lowered to what the slowest joint can follow. The module is the one +// motion runs, loaded through the non-RT loader in kinematics_userspace/ +// the first time a segment asks. A module that runs only in realtime, +// and a kinematics type that is the identity, cap nothing. +// +// The same samples follow every joint against its travel. Motion checks +// the joints at the end of a move only, so a tilted head turning under +// the tool centre point can swing a carriage past its limit in the middle +// of a move whose ends are both inside; motion then stops on the soft +// limit partway through it, with the joint past the limit. Here such a +// move is refused before it is sent: it is dropped, every move after it +// is dropped too, since one sent after it would start from where the +// dropped one began, and task fails the line with the reason. +//---------------------------------------------------------------------- + +// where a joint would leave its travel along a segment, joint -1 for nowhere +struct KinsTravel { + int joint; + int side; // 1 past MAX_LIMIT, -1 past MIN_LIMIT + double limit; // the limit, as motion held it + double pos; // where the joint would be + double at; // the fraction of the segment it is found at +}; + +// the first move refused, until task takes it +static struct { + bool pending; + int line; // of the move dropped for it, -1 until then + KinsTravel travel; +} refusal = { false, -1, { -1, 0, 0.0, 0.0, 0.0 } }; + +static struct { + KinematicsUserContext *ctx; + int comp_id; + bool tried; // loading was attempted, once + int type; // the kinematics type the program is in, -1 for the machine's + double joints[EMCMOT_MAX_JOINTS]; // the joints at canon.endPoint, as far as canon knows + bool in_arc; // ARC_FEED caps the arc itself; the straight helpers stand back + bool debug; // EMCCANON_KINS_DEBUG in the environment: say what every segment got + motion_planning::SegmentCapLimits limits; + // the last straight segment's answer, since its velocity and its + // acceleration are asked for separately + struct { + bool valid; + EmcPose start, end; + bool capped; + double vel, acc; + KinsTravel travel; + } memo; +} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0, { -1, 0, 0.0, 0.0, 0.0 } } }; + +// the machine's own module, once +static bool kins_load(void) +{ + char name[HAL_NAME_LEN + 1]; + int joints, j; + + if (kins.tried) { return kins.ctx != NULL; } + kins.tried = true; + linuxcnc::IniFile ini(emc_inifile); + if (!ini) { return false; } + auto module = ini.findString("KINEMATICS", "KINS"); + joints = ini.findIntV("JOINTS", "KINS", 0); + if (!module || joints < 1) { return false; } + snprintf(name, sizeof(name), "canon.%d", (int)getpid()); + kins.comp_id = hal_init(name); + if (kins.comp_id < 0) { return false; } + kins.ctx = kinematicsUserInitString(module->c_str(), joints, kins.comp_id, name); + hal_ready(kins.comp_id); + if (kins.ctx && kinematicsUserIsRtOnly(kins.ctx)) { + kinematicsUserFree(kins.ctx); + kins.ctx = NULL; + } + if (!kins.ctx) { + hal_exit(kins.comp_id); + kins.comp_id = -1; + return false; + } + // the joint limits as the INI gives them; one left out binds nothing + kins.limits.joints = kinematicsUserGetNumJoints(kins.ctx); + for (j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + double vel = j < kins.limits.joints ? emcJointGetMaxVelocity(j) : 0.0; + double acc = j < kins.limits.joints ? emcJointGetMaxAcceleration(j) : 0.0; + kins.limits.vel[j] = vel > 0.0 ? vel : SEGMENT_CAP_NONE; + kins.limits.acc[j] = acc > 0.0 ? acc : SEGMENT_CAP_NONE; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { kins.joints[j] = 0.0; } + kins.debug = getenv("EMCCANON_KINS_DEBUG") != NULL; + if (kins.debug) { + fprintf(stderr, "canon kins: %s, %d joints, limits", module->c_str(), kins.limits.joints); + for (j = 0; j < kins.limits.joints; j++) { + fprintf(stderr, " %g/%g", kins.limits.vel[j], kins.limits.acc[j]); + } + fprintf(stderr, "\n"); + } + return true; +} + +// the module on the type and the tool offset the program is under, or +// NULL where there is nothing to cap with +static KinematicsUserContext *kins_here(void) +{ + int type = kins.type >= 0 ? kins.type : GET_EXTERNAL_KINS_TYPE(); + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(type); + EmcPose tool; + + // the identity, whether the module as a whole is one or the type in + // force is, needs no module loaded to know the joints are the axes + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return NULL; } + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return NULL; } + if (!kins_load()) { return NULL; } + if (kinematicsUserSetType(kins.ctx, type) != 0) { return NULL; } + if (kinematicsUserIsIdentity(kins.ctx)) { return NULL; } + tool = to_ext_pose(canon.toolOffset.tran.x, canon.toolOffset.tran.y, canon.toolOffset.tran.z, + canon.toolOffset.a, canon.toolOffset.b, canon.toolOffset.c, + canon.toolOffset.u, canon.toolOffset.v, canon.toolOffset.w); + kinematicsUserSetTool(kins.ctx, &tool); + return kins.ctx; +} + +// whether two machine points are the same, to a hair either way +static bool kins_same(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-6; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints at the start of a segment, as far as canon can know ahead of +// motion: the ones it holds while they still explain the point, since a +// point does not name one joint set, else the joints the machine stands +// in if those do, else the inverse iterated to a fixed point +static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) +{ + double standing[EMCMOT_MAX_JOINTS] = {0}; + EmcPose seeded = *start; + int i, n, pass; + + if (kinematicsUserForward(ctx, kins.joints, &seeded) == 0 && kins_same(&seeded, start)) { return; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + if (n > 0) { + seeded = *start; + if (kinematicsUserForward(ctx, standing, &seeded) == 0 && kins_same(&seeded, start)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { kins.joints[i] = standing[i]; } + return; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = kins.joints[i]; } + if (kinematicsUserInverse(ctx, start, kins.joints) != 0) { return; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(kins.joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } +} + +// how many points to sample a segment at: enough that a turn of the +// rotaries, a stretch of travel on a machine whose Jacobian changes with +// position, or a sweep of arc is seen every few degrees or centimetres +static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) +{ + const double turn[6] = {end->a - start->a, end->b - start->b, end->c - start->c, + end->u - start->u, end->v - start->v, end->w - start->w}; + double rot = 0.0; + for (int n = 0; n < 6; n++) { + if (AXIS_ANG(n + 3)) { rot = fmax(rot, fabs(turn[n])); } + } + double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + + pow(end->tran.z - start->tran.z, 2)); + int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); + + return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; +} + +// the travel motion holds the joints to now, MIN_LIMIT and MAX_LIMIT or +// what the ini.N pins have made of them since, on the joints that are +// homed: the position of one that is not means nothing to its limits +static void kins_travel_limits(void) +{ + for (int j = 0; j < kins.limits.joints; j++) { + bool homed = emcStatus->motion.joint[j].homed; + kins.limits.min[j] = homed ? emcStatus->motion.joint[j].minPositionLimit : -HUGE_VAL; + kins.limits.max[j] = homed ? emcStatus->motion.joint[j].maxPositionLimit : HUGE_VAL; + } +} + +// what the joints allow along a segment, in the machine's units per second +// of the path parameter canon plans it with, false where nothing binds, +// and where along it a joint would leave its travel +static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const EmcPose *end, + double length, int samples, motion_planning::SegmentPoseFn pose_at, void *arg, + double *vel, double *acc, KinsTravel *travel) +{ + motion_planning::SegmentCap cap; + + travel->joint = -1; + kins_seed(ctx, start); + kins_travel_limits(); + if (motion_planning::segmentCap(ctx, &kins.limits, length, samples, pose_at, arg, kins.joints, &cap) != 0) { + CANON_ERROR("the kinematics cannot reach the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f, the joints are not checked along it", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c); + return false; + } + if (cap.unanswered) { + CANON_ERROR("the kinematics cannot reach %d of %d points on the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f, the joints are not checked there", + cap.unanswered, cap.samples, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c); + } + // a pole: some joint would have to move at any speed at all for the + // path to advance, and the move crawls rather than being dropped, + // since a dropped segment would send the next one along another path + if (cap.vel_joint >= 0 && cap.vel < 1e-3) { + CANON_ERROR("joint %d cannot follow the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f near %.0f%% of its length: the kinematics is singular there and the move crawls", + cap.vel_joint, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, 100.0 * cap.vel_at); + } + if (cap.travel_joint >= 0) { + travel->joint = cap.travel_joint; + travel->side = cap.travel_side; + travel->limit = cap.travel_side > 0 ? kins.limits.max[cap.travel_joint] + : kins.limits.min[cap.travel_joint]; + travel->pos = cap.travel_pos; + travel->at = cap.travel_at; + } + if (kins.debug) { + fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered, travel left by joint %d at %.2f; seed", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, length, cap.samples, + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, + cap.travel_joint, cap.travel_at); + for (int j = 0; j < kins.limits.joints; j++) { fprintf(stderr, " %.3f", kins.joints[j]); } + fprintf(stderr, "\n"); + } + *vel = cap.vel; + *acc = cap.acc; + return cap.vel_joint >= 0 || cap.acc_joint >= 0; +} + +// a segment a joint would leave its travel along refuses the move, the +// first one only: the moves after it are dropped with it +static void kins_refuse(const KinsTravel *travel) +{ + if (travel->joint < 0 || refusal.pending) { return; } + refusal.pending = true; + refusal.line = -1; + refusal.travel = *travel; +} + +struct KinsLine { + EmcPose start, end; +}; + +static void kins_line_pose(double f, EmcPose *pose, void *arg) +{ + const KinsLine *line = (const KinsLine *)arg; + + pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); + pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); + pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); + pose->a = line->start.a + f * (line->end.a - line->start.a); + pose->b = line->start.b + f * (line->end.b - line->start.b); + pose->c = line->start.c + f * (line->end.c - line->start.c); + pose->u = line->start.u + f * (line->end.u - line->start.u); + pose->v = line->start.v + f * (line->end.v - line->start.v); + pose->w = line->start.w + f * (line->end.w - line->start.w); +} + +// the cap on a straight segment from canon.endPoint, in the machine's +// units per second of the parameter getStraightVelocity() plans it with, +// the length along the axes getStraightSpan() measures +static bool kins_straight_cap(double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double *vel, double *acc) +{ + KinematicsUserContext *ctx; + KinsLine line; + double length; + + if (kins.in_arc) { return false; } + line.start = to_ext_pose(canon.endPoint); + line.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + if (kins.memo.valid && kins_same(&kins.memo.start, &line.start) && kins_same(&kins.memo.end, &line.end)) { + kins_refuse(&kins.memo.travel); + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; + } + ctx = kins_here(); + if (!ctx) { return false; } + // the length getStraightVelocity() plans the segment along + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + if (!span.moving) { + return false; + } + length = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + kins.memo.valid = true; + kins.memo.start = line.start; + kins.memo.end = line.end; + kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, + kins_samples(&line.start, &line.end, 0.0), + kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc, + &kins.memo.travel); + kins_refuse(&kins.memo.travel); + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; +} + +struct KinsArc { + PmCircle circle; // in canon's units, as ARC_FEED builds it + bool line; // no turn at all: the chord + EmcPose start, end; +}; + +static void kins_arc_pose(double f, EmcPose *pose, void *arg) +{ + const KinsArc *arc = (const KinsArc *)arg; + PmCartesian p; + + kins_line_pose(f, pose, (void *)&arc->start); + if (arc->line) { return; } + pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); + pose->tran.x = TO_EXT_LEN(p.x); + pose->tran.y = TO_EXT_LEN(p.y); + pose->tran.z = TO_EXT_LEN(p.z); +} + +// the cap on an arc from canon.endPoint, in the machine's units per +// second of the XYZ length ARC_FEED plans it with +static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, + const PM_CARTESIAN &end_xyz, const CANON_POSITION &endpt, + int rotation, double length, double full_angle, + double *vel, double *acc) +{ + KinematicsUserContext *ctx = kins_here(); + KinsArc arc; + KinsTravel travel; + bool capped; + + if (!ctx || length <= 0.0) { return false; } + arc.start = to_ext_pose(canon.endPoint); + arc.end = to_ext_pose(endpt); + arc.line = rotation == 0; + if (!arc.line) { + PmCartesian s = { canon.endPoint.x, canon.endPoint.y, canon.endPoint.z }; + PmCartesian e = { end_xyz.x, end_xyz.y, end_xyz.z }; + PmCartesian c = { center.x, center.y, center.z }; + PmCartesian n = { normal.x, normal.y, normal.z }; + if (pmCircleInit(&arc.circle, &s, &e, &c, &n, rotation > 0 ? rotation - 1 : rotation) != 0) { + return false; + } + } + capped = kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), + kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), + kins_arc_pose, &arc, vel, acc, &travel); + kins_refuse(&travel); + return capped; +} + +// whether a move about to be sent is dropped: the one refused, or one +// after it, which would start from where the refused one began, until +// task has taken the refusal +static bool kins_dropped(int line_number) +{ + if (!refusal.pending) { return false; } + if (refusal.line < 0) { refusal.line = line_number; } + return true; +} + +int CANON_MOVE_REFUSED(char *message, int max) +{ + const KinsTravel &t = refusal.travel; + const char *limit = t.side > 0 ? "MAX_LIMIT" : "MIN_LIMIT"; + char move[32] = "the move"; + + if (!refusal.pending) { return 0; } + if (refusal.line > 0) { snprintf(move, sizeof(move), "the move on line %d", refusal.line); } + if (t.at > 1.0 - 1e-9) { + snprintf(message, max, "%s would end with joint %d past its limit, at %.4f against %s %.4f", + move, t.joint, t.pos, limit, t.limit); + } else { + snprintf(message, max, "%s would take joint %d past its limit, to %.4f against %s %.4f, %.0f%% of the way along", + move, t.joint, t.pos, limit, t.limit, 100.0 * t.at); + } + refusal.pending = false; + // the move may be asked for again, under other limits by then + kins.memo.valid = false; + return 1; +} + /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -865,6 +1290,14 @@ static AccelData getStraightAcceleration(double x, double y, double z, if (out.tmax > 0.0) { out.acc = out.dtot / out.tmax; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.acc > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kacc) < out.acc) { + out.acc = fromExtVel(kacc); + } + } if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; @@ -909,6 +1342,14 @@ static VelData getStraightVelocity(double x, double y, double z, } else { out.vel = canon.linearFeedRate; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.vel > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kvel) < out.vel) { + out.vel = fromExtVel(kvel); + } + } if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; @@ -958,6 +1399,10 @@ static void flush_segments(void) { #endif VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); + if (kins_dropped(line_no)) { + drop_segments(); + return; + } double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); double vel = linedata.vel; @@ -1091,12 +1536,17 @@ void FINISH() { void ON_RESET() { drop_segments(); + refusal.pending = false; } void SELECT_KINS_TYPE(int switchkins_type) { flush_segments(); + // the segments from here on run under it + kins.type = switchkins_type; + kins.memo.valid = false; + auto selectKinsMsg = std::make_unique(); selectKinsMsg->switchkins_type = switchkins_type; @@ -1146,6 +1596,48 @@ 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(); + if (kins_dropped(line_number)) { return; } + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = 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) @@ -1205,6 +1697,7 @@ void STRAIGHT_TRAVERSE(int line_number, VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); + if (kins_dropped(line_number)) { return; } vel = veldata.vel; acc = accdata.acc; @@ -1251,6 +1744,10 @@ void RIGID_TAP(int line_number, double x, double y, double z, double scale) from_prog(x,y,z,unused,unused,unused,unused,unused,unused); rotate_and_offset_pos(x,y,z,unused,unused,unused,unused,unused,unused); + // the segments before it first, so that the tap is read from where it + // starts + flush_segments(); + VelData veldata = getStraightVelocity(x, y, z, canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); @@ -1274,7 +1771,7 @@ void RIGID_TAP(int line_number, double x, double y, double z, double scale) rigidTapMsg->ini_maxjerk = toExtVel(jerk); rigidTapMsg->acc = toExtAcc(acc); rigidTapMsg->scale = scale; - flush_segments(); + if (kins_dropped(line_number)) { return; } if(ini_maxvel && acc) { interp_list.set_line_number(line_number); @@ -1324,6 +1821,7 @@ void STRAIGHT_PROBE(int line_number, AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); acc = accdata.acc; + if (kins_dropped(line_number)) { return; } probeMsg->vel = toExtVel(vel); probeMsg->ini_maxvel = toExtVel(ini_maxvel); @@ -2756,6 +3254,7 @@ void ARC_FEED(int line_number, // Find the equivalent maximum velocity for a linear displacement // This accounts for speed restrictions due to helical and other axes + kins.in_arc = true; VelData veldata = getStraightVelocity(endpt); // Compute spiral length, first by the minimum circular arc length @@ -2788,6 +3287,7 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) AccelData accdata = getStraightAcceleration(endpt); + kins.in_arc = false; double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; @@ -2797,6 +3297,17 @@ void ARC_FEED(int line_number, // circle plane and helical axis will still be within limits double a_max = total_xyz_length / tt_max; + // and what the joints allow along the arc, through the kinematics + { + double kvel, kacc; + if (kins_arc_cap(center_cart, normal_cart, end_cart, endpt, rotation, + total_xyz_length, full_angle, &kvel, &kacc)) { + v_max = std::min(v_max, FROM_EXT_LEN(kvel)); + a_max = std::min(a_max, FROM_EXT_LEN(kacc)); + } + } + if (kins_dropped(line_number)) { return; } + // Limit velocity by maximum double vel = std::min(canon.linearFeedRate, v_max); canon_debug("current F = %f\n",canon.linearFeedRate); @@ -2998,7 +3509,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(); @@ -3014,6 +3525,7 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) canon.toolOffset.u = FROM_PROG_AX(AXIS_U, offset.u); canon.toolOffset.v = FROM_PROG_AX(AXIS_V, offset.v); canon.toolOffset.w = FROM_PROG_AX(AXIS_W, offset.w); + kins.memo.valid = false; /* append it to interp list so it gets updated at the right time, not at read-ahead time */ @@ -3027,6 +3539,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 +3557,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() { @@ -3080,6 +3612,7 @@ void CHANGE_TOOL() double jerk = getStraightJerk(x, y, z, a, b, c, u, v, w); vel = veldata.vel; acc = accdata.acc; + if (kins_dropped(_tag.fields[GM_FIELD_LINE_NUMBER])) { return; } auto linearMoveMsg = std::make_unique(); linearMoveMsg->feed_mode = canon.feed_mode; @@ -3505,6 +4038,7 @@ void INIT_CANON() double units; chained_points.clear(); + refusal.pending = false; // initialize locals to original values canon.xy_rotation = 0.0; @@ -3545,6 +4079,8 @@ void INIT_CANON() canon.linearFeedRate = 0.0; canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + kins.type = -1; + kins.memo.valid = false; { std::string err; @@ -3918,6 +4454,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; @@ -3991,7 +4538,10 @@ int GET_EXTERNAL_KINS_TYPE() // motion publishes the kinematics it is actually running, which is // not necessarily the one G-code last asked for: an abort can drop a // queued switch, and the motion.switchkins-type pin can select one - // without the interpreter seeing it + // without the interpreter seeing it. The interpreter asks when it + // synchs to the machine; canon's own idea of the type goes with it + kins.type = -1; + kins.memo.valid = false; return emcStatus->motion.traj.switchkins_type; } @@ -4023,6 +4573,20 @@ int GET_EXTERNAL_G68_FRAME(double origin[3], double rotation[9]) return 1; } +int GET_EXTERNAL_G68_KINS_FRAME(double shift[3], double rotation[9]) +{ + shift[0] = emcStatus->task.g68_kins_offset.tran.x; + shift[1] = emcStatus->task.g68_kins_offset.tran.y; + shift[2] = emcStatus->task.g68_kins_offset.tran.z; + for (int i = 0; i < 9; i++) { rotation[i] = emcStatus->task.g68_kins_rotation[i]; } + return 1; +} + +bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; +} + 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..b1a96645d0a 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -57,6 +57,8 @@ #include // fork() #include // waitpid(), WNOHANG, WIFEXITED #include // isspace() +#include // cos(), sin() +#include // std::equal(), std::copy() #include #include #include "motion/usrmotintf.h" @@ -527,6 +529,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; @@ -537,6 +542,27 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) return 0; } + +// whether the line the interpreter has just executed is one stepped over +// on the way to the line a run starts from, its output thrown away +static bool stepping_over(void) +{ + return programStartLine != 0 && emcTaskPlanLevel() == 0 + && (programStartLine < 0 || emcTaskPlanLine() <= programStartLine); +} + +// A move canon refused, because a joint would leave its travel along it, +// fails the line like an interpreter error: canon has dropped it and every +// move after it. On a line stepped over the refusal goes with the rest of +// its output, its moves read from wherever the machine stands. +static int canon_refusal(int retval, bool stepped_over) +{ + char text[LINELEN]; + + if (!CANON_MOVE_REFUSED(text, sizeof(text)) || stepped_over) { return retval; } + emcOperatorError("%s", text); + return INTERP_ERROR; +} extern int emcTaskMopup(); void readahead_reading(void) @@ -565,6 +591,7 @@ void readahead_reading(void) } } else { readRetval = emcTaskPlanRead(); + readRetval = canon_refusal(readRetval, stepping_over()); /*! \todo MGS FIXME This if() actually evaluates to if (readRetval != INTERP_OK)... *** Need to look at all calls to things that return INTERP_xxx values! *** @@ -590,6 +617,7 @@ void readahead_reading(void) command); // and execute it execRetval = emcTaskPlanExecute(NULL); + execRetval = canon_refusal(execRetval, stepping_over()); // line number may need update after // returns from subprograms in external // files @@ -627,10 +655,7 @@ void readahead_reading(void) // throw the results away if we're supposed to // read // through it - if ( programStartLine != 0 && - emcTaskPlanLevel() == 0 && - ( programStartLine < 0 || - emcTaskPlanLine() <= programStartLine )) { + if (stepping_over()) { // we're stepping over lines, so check them // for // limits, etc. and clear then out @@ -799,6 +824,7 @@ static bool allow_while_idle_type() { case EMC_JOG_STOP_TYPE: case EMC_JOG_ABS_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: return 1; break; } @@ -885,6 +911,7 @@ static int emcTaskPlan(void) case EMC_MOTION_SET_AOUT_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_SET_DEBUG_TYPE: retval = emcTaskIssueCommand(emcCommand); break; @@ -1009,6 +1036,7 @@ static int emcTaskPlan(void) case EMC_MOTION_ADAPTIVE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_SET_DEBUG_TYPE: retval = emcTaskIssueCommand(emcCommand); break; @@ -1564,6 +1592,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: @@ -1684,6 +1713,42 @@ int emcTaskQueueCommand(std::unique_ptr &&cmd) return 0; } +// The tilted work plane's axes in world coordinates, for motion to jog +// along: the plane's rotation turned by the XY rotation of the coordinate +// system it was defined in, as canon applies the two, and then by the +// second feature coordinate system where the plane is cut through one. +// Motion hears only of a change, so a program without a plane sends it +// nothing +static int sendWorkPlane() +{ + static double sent[9]; + static int sent_active = 0; + const double *g = emcStatus->task.g68_rotation; + const double *k = emcStatus->task.g68_kins_rotation; + double t = emcStatus->task.rotation_xy * M_PI / 180.0, c = cos(t), s = sin(t), q[9], r[9]; + int active = emcStatus->task.g68_active, retval; + + for (int j = 0; j < 3; j++) { + q[j] = c * g[j] - s * g[3 + j]; + q[3 + j] = s * g[j] + c * g[3 + j]; + q[6 + j] = g[6 + j]; + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[3*i + j] = k[3*i] * q[j] + k[3*i + 1] * q[3 + j] + k[3*i + 2] * q[6 + j]; + } + } + if (active == sent_active && (!active || std::equal(r, r + 9, sent))) { + return 0; + } + retval = emcTrajSetWorkPlane(r, active); + if (retval == 0) { + sent_active = active; + std::copy(r, r + 9, sent); + } + return retval; +} + // issues command immediately static int emcTaskIssueCommand(NMLmsg * cmd) { @@ -1931,6 +1996,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,15 +2053,18 @@ 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; - retval = 0; + retval = sendWorkPlane(); break; case EMC_TRAJ_SET_G68_TYPE: { @@ -1997,7 +2072,9 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcStatus->task.g68_offset = g68->origin; for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } emcStatus->task.g68_active = g68->active; - retval = 0; + emcStatus->task.g68_kins_offset = g68->kins_offset; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_kins_rotation[i] = g68->kins_rotation[i]; } + retval = sendWorkPlane(); break; } @@ -2066,6 +2143,10 @@ static int emcTaskIssueCommand(NMLmsg * cmd) } break; + case EMC_TRAJ_SET_JOG_FRAME_TYPE: + retval = emcTrajSetJogFrame((reinterpret_cast(cmd))->frame); + break; + case EMC_MOTION_SET_AOUT_TYPE: retval = emcMotionSetAout((reinterpret_cast(cmd))->index, (reinterpret_cast(cmd))->start, @@ -2377,6 +2458,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) } execRetval = emcTaskPlanExecute(command, 0); + execRetval = canon_refusal(execRetval, false); level = emcTaskPlanLevel(); @@ -2578,6 +2660,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: @@ -2593,6 +2676,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: + case EMC_TRAJ_SET_JOG_FRAME_TYPE: case EMC_TRAJ_SET_FO_ENABLE_TYPE: case EMC_TRAJ_SET_FH_ENABLE_TYPE: case EMC_TRAJ_SET_SO_ENABLE_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 2a429b0f66b..adde3409e4a 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -15,6 +15,7 @@ #include "logutil.hh" #include #include +#include // std::max #include // DBL_MAX #include // memcpy() strncpy() #include // unlink() @@ -552,6 +553,24 @@ int emcAxisSetLockingJoint(int axis, int joint) return retval; } +double emcJointGetMaxVelocity(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxVel; +} + +double emcJointGetMaxAcceleration(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxAccel; +} + double emcAxisGetMaxVelocity(int axis) { if (axis < 0 || axis >= EMCMOT_MAX_AXIS) { @@ -822,6 +841,17 @@ int emcJointUnhome(int joint) return usrmotWriteEmcmotCommand(&emcmotCommand); } +// The fastest a jog of axis nr may ask for: the axis's own MAX_VELOCITY, +// or in a jog frame, where X, Y and Z jog along turned axes, the fastest of +// the three; motion holds a frame jog to what each world axis allows +static double jogMaxVel(int nr) +{ + if (nr < 3 && emcmotStatus.jog_frame != EMC_JOG_FRAME_MACHINE) { + return std::max({AxisConfig[0].MaxVel, AxisConfig[1].MaxVel, AxisConfig[2].MaxVel}); + } + return AxisConfig[nr].MaxVel; +} + int emcJogCont(int nr, double vel, int jjogmode) { if (jjogmode) { @@ -835,10 +865,10 @@ int emcJogCont(int nr, double vel, int jjogmode) emcmotCommand.axis = -1; //NA } else { if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; } - if (vel > AxisConfig[nr].MaxVel) { - vel = AxisConfig[nr].MaxVel; - } else if (vel < -AxisConfig[nr].MaxVel) { - vel = -AxisConfig[nr].MaxVel; + if (vel > jogMaxVel(nr)) { + vel = jogMaxVel(nr); + } else if (vel < -jogMaxVel(nr)) { + vel = -jogMaxVel(nr); } emcmotCommand.joint = -1; //NA emcmotCommand.axis = nr; @@ -862,10 +892,10 @@ int emcJogIncr(int nr, double incr, double vel, int jjogmode) emcmotCommand.axis = -1; //NA } else { if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; } - if (vel > AxisConfig[nr].MaxVel) { - vel = AxisConfig[nr].MaxVel; - } else if (vel < -AxisConfig[nr].MaxVel) { - vel = -AxisConfig[nr].MaxVel; + if (vel > jogMaxVel(nr)) { + vel = jogMaxVel(nr); + } else if (vel < -jogMaxVel(nr)) { + vel = -jogMaxVel(nr); } emcmotCommand.joint = -1; //NA emcmotCommand.axis = nr; @@ -1476,10 +1506,50 @@ 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); +} + +int emcTrajSetWorkPlane(const double rotation[9], int active) +{ + emcmotCommand.command = EMCMOT_SET_WORK_PLANE; + for (int i = 0; i < 9; i++) { emcmotCommand.work_plane[i] = rotation[i]; } + emcmotCommand.work_plane_active = active; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + +int emcTrajSetJogFrame(int frame) +{ + if (frame < EMC_JOG_FRAME_MACHINE || frame > EMC_JOG_FRAME_TOOL) { + emcOperatorError("unknown jog frame %d", frame); + return -1; + } + emcmotCommand.command = EMCMOT_SET_JOG_FRAME; + emcmotCommand.jog_frame = frame; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1698,6 +1768,12 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) stat->actualPosition = emcmotStatus.carte_pos_fb; + stat->part_position = emcmotStatus.part_pos_cmd; + + for (int i = 0; i < 9; i++) { stat->tool_frame[i] = emcmotStatus.tool_frame[i]; } + stat->tool_frame_ok = emcmotStatus.tool_frame_ok; + stat->jog_frame = emcmotStatus.jog_frame; + stat->velocity = emcmotStatus.vel; stat->acceleration = emcmotStatus.acc; stat->maxAcceleration = TrajConfig.MaxAccel; 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..0fe955b52d7 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,94 @@ 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; + } + // it runs in joint units: the max velocity slider caps the tool as canon + // measures a straight move, along the chord between the ends: X Y Z, + // else U V W, and not at all when only the rotaries move + { + double dx = world_end.tran.x - jl->world_start.tran.x; + double dy = world_end.tran.y - jl->world_start.tran.y; + double dz = world_end.tran.z - jl->world_start.tran.z; + double du = world_end.u - jl->world_start.u; + double dv = world_end.v - jl->world_start.v; + double dw = world_end.w - jl->world_start.w; + double chord = pmSqrt(dx * dx + dy * dy + dz * dz); + if (chord < TP_POS_EPSILON) { + chord = pmSqrt(du * du + dv * dv + dw * dw); + } + tc.vlimit_scale = chord < TP_POS_EPSILON ? 0.0 : tc.target / chord; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + 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 +2261,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 +2334,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 +3430,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 +3486,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; @@ -3419,8 +3540,21 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { + /* stopped inside a joint interpolated segment: the machine is + where the joints put it, which the servo thread found from them + last cycle, not on the chord this planner reports; the joints + are handed out as an end so that they stay where they are */ + double stop_joints[EMCMOT_MAX_JOINTS] = {0}; + int joint_stop = tc->active && tcGetJointPos(tc, stop_joints) > 0; tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); + if (joint_stop) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = stop_joints[i]; } + tp->joint_end_valid = 1; + tp->currentPos = emcmotStatus->carte_pos_cmd; + } tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3518,7 +3652,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 +4611,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 +4769,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..c0b5a968ce9 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1191,6 +1191,12 @@ 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." }, + { "jog_frame", T_INT, O(motion.traj.jog_frame), READONLY, + "The frame world jogs of X, Y and Z move along: JOG_FRAME_MACHINE,\n" + "JOG_FRAME_PLANE or JOG_FRAME_TOOL." }, { "motion_type", T_INT, O(motion.traj.motion_type), READONLY, "The type of the currently executing motion (one of MOTION_TYPE_TRAVERSE,\n" "MOTION_TYPE_FEED, MOTION_TYPE_ARC, MOTION_TYPE_TOOLCHANGE,\n" @@ -1294,10 +1300,43 @@ static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { return res; } +static PyObject *Stat_g68_kins_offset(pyStatChannel *s, void *) { + return pose(s->status.task.g68_kins_offset); +} + +static PyObject *Stat_g68_kins_rotation(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.task.g68_kins_rotation[i])); + } + return res; +} + +static PyObject *Stat_kins_type_flags(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(SWITCHKINS_MAX_TYPES); + for (int i = 0; i < SWITCHKINS_MAX_TYPES; i++) { + PyTuple_SET_ITEM(res, i, PyLong_FromLong(s->status.motion.traj.switchkins_flags[i])); + } + return res; +} + +static PyObject *Stat_tool_frame(pyStatChannel *s, void *) { + if (!s->status.motion.traj.tool_frame_ok) { Py_RETURN_NONE; } + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.motion.traj.tool_frame[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } +static PyObject *Stat_part_position(pyStatChannel *s, void *) { + return pose(s->status.motion.traj.part_position); +} + static PyObject *Stat_dtg(pyStatChannel *s, void *) { return pose(s->status.motion.traj.dtg); } @@ -1600,7 +1639,17 @@ static PyGetSetDef Stat_getsetlist[] = { (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, + {(char*)"g68_kins_offset", (getter)Stat_g68_kins_offset, NULL, + (char*)"Shift of the second feature coordinate system a tilted work plane is cut through under the machine frame kinematics (3+2), in machine units; zero where there is none.", NULL}, + {(char*)"g68_kins_rotation", (getter)Stat_g68_kins_rotation, NULL, + (char*)"Rotation of that second feature coordinate system, nine values row by row; the identity where there is none.", NULL}, + {(char*)"kins_type_flags", (getter)Stat_kins_type_flags, NULL, + (char*)"What the module declares each kinematics type to be, one entry per type: KINSTYPE_ flags, or -1 for a type it does not provide.", NULL}, + {(char*)"tool_frame", (getter)Stat_tool_frame, NULL, + (char*)"Tool frame in the work frame at the commanded joints, nine values row by row, the columns being tool X, Y and Z; None where the kinematics reports no frames.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, + {(char*)"part_position", (getter)Stat_part_position, NULL, + (char*)"The commanded joints as the primary kinematics type reads them, where the tool stands against the part: position itself, except under another type, the machine frame type G13.1 selects, where position is in machine coordinates.", NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, {(char*)"joint_actual_position", (getter)Stat_joint_actual, NULL, NULL, NULL}, @@ -2211,6 +2260,17 @@ static PyObject *teleop(pyCommandChannel *s, PyObject *o) { return Py_None; } +static PyObject *jog_frame(pyCommandChannel *s, PyObject *o) { + EMC_TRAJ_SET_JOG_FRAME fr; + + if(!PyArg_ParseTuple(o, "i", &fr.frame)) return NULL; + + emcSendCommand(s, fr); + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject *set_traj_mode(pyCommandChannel *s, PyObject *o) { EMC_TRAJ_SET_MODE mo; @@ -2370,6 +2430,11 @@ static PyMemberDef Command_members[] = { static PyMethodDef Command_methods[] = { {"debug", (PyCFunction)debug, METH_VARARGS, NULL}, {"teleop_enable", (PyCFunction)teleop, METH_VARARGS, NULL}, + {"jog_frame", (PyCFunction)jog_frame, METH_VARARGS, + "jog_frame(FRAME) - Jog X, Y and Z along the axes of FRAME in world mode:\n" + "JOG_FRAME_MACHINE, JOG_FRAME_PLANE (the tilted work plane) or\n" + "JOG_FRAME_TOOL. Refused while a jog is under way.\n" + }, {"traj_mode", (PyCFunction)set_traj_mode, METH_VARARGS, NULL}, {"wait_complete", (PyCFunction)wait_complete, METH_VARARGS, NULL}, {"state", (PyCFunction)state, METH_VARARGS, @@ -3044,16 +3109,21 @@ static PyObject *Logger_start(pyPositionLogger *s, PyObject *o) { op->rx, op->ry, op->rz, oop->rx, oop->ry, oop->rz); } else { + // where the tool stands against the part, as the preview + // draws the program: the commanded position, except under + // the machine frame kinematics, where the slides put the + // tool on the part where the primary type reads them + const EmcPose &at = status->motion.traj.part_position; double pt[9] = { - status->motion.traj.position.tran.x - status->task.toolOffset.tran.x, - status->motion.traj.position.tran.y - status->task.toolOffset.tran.y, - status->motion.traj.position.tran.z - status->task.toolOffset.tran.z, - status->motion.traj.position.a - status->task.toolOffset.a, - status->motion.traj.position.b - status->task.toolOffset.b, - status->motion.traj.position.c - status->task.toolOffset.c, - status->motion.traj.position.u - status->task.toolOffset.u, - status->motion.traj.position.v - status->task.toolOffset.v, - status->motion.traj.position.w - status->task.toolOffset.w}; + at.tran.x - status->task.toolOffset.tran.x, + at.tran.y - status->task.toolOffset.tran.y, + at.tran.z - status->task.toolOffset.tran.z, + at.a - status->task.toolOffset.a, + at.b - status->task.toolOffset.b, + at.c - status->task.toolOffset.c, + at.u - status->task.toolOffset.u, + at.v - status->task.toolOffset.v, + at.w - status->task.toolOffset.w}; double p[3]; vertex9(pt, p, s->geometry); @@ -3411,6 +3481,10 @@ PyMODINIT_FUNC PyInit_linuxcnc(void) PyModule_AddIntConstant(m, "TRAJ_MODE_COORD", (int)EMC_TRAJ_MODE::COORD); PyModule_AddIntConstant(m, "TRAJ_MODE_TELEOP", (int)EMC_TRAJ_MODE::TELEOP); + ENUMX(4, EMC_JOG_FRAME_MACHINE); + ENUMX(4, EMC_JOG_FRAME_PLANE); + ENUMX(4, EMC_JOG_FRAME_TOOL); + ENUMX(4, EMC_MOTION_TYPE_TRAVERSE); ENUMX(4, EMC_MOTION_TYPE_FEED); ENUMX(4, EMC_MOTION_TYPE_ARC); @@ -3423,6 +3497,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/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index 9477cca0332..d1f3e90c88c 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -274,6 +274,7 @@ def General_Halt(): ("F4", _("Cycle among preview, DRO, and user tabs")), ("@", _("toggle Actual/Commanded")), ("#", _("toggle Relative/Machine")), + ("%", _("cycle jog frame Machine/Plane/Tool")), (_("Ctrl-Space"), _("Clear notifications")), (_("Alt-F, M, V"), _("Open a Menu")), ] @@ -949,6 +950,7 @@ def update(self): vupdate(vars.brake, self.stat.spindle[0]['brake']) vupdate(vars.spindledir, self.stat.spindle[0]['direction']) vupdate(vars.motion_mode, self.stat.motion_mode) + vupdate(vars.jog_frame, self.stat.jog_frame) vupdate(vars.optional_stop, self.stat.optional_stop) vupdate(vars.block_delete, self.stat.block_delete) if time.time() > spindlerate_blackout: @@ -3021,6 +3023,16 @@ def toggle_teleop_mode(*args): s.poll() set_motion_teleop(not (s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP)) + # the frame world jogs of X, Y and Z move along; motion refuses a + # change while a jog is under way, and the next poll shows the frame + # in force + def set_jog_frame(*args): + c.jog_frame(vars.jog_frame.get()) + + def cycle_jog_frame(*args): + s.poll() + c.jog_frame((s.jog_frame + 1) % 3) + def toggle_coord_type(*args): vars.coord_type.set(not vars.coord_type.get()) o.tkRedraw() @@ -3161,6 +3173,7 @@ def clear_recent_files(): ("maxvel_speed", DoubleVar), ("max_maxvel", DoubleVar), ("teleop_mode", IntVar), + ("jog_frame", IntVar), ("motion_mode", IntVar), ("kinematics_type", IntVar), ("optional_stop", BooleanVar), @@ -3284,6 +3297,7 @@ def kp_wrap(f,g): root_window.bind("@", commands.toggle_display_type) root_window.bind("#", commands.toggle_coord_type) root_window.bind("$", commands.toggle_teleop_mode) +root_window.bind("%", commands.cycle_jog_frame) root_window.bind("", commands.home_joint) root_window.bind("", kp_wrap(commands.home_joint, "KeyPress")) diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index c40e2831e73..7f168417d1b 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -70,6 +70,12 @@ static int axis_mask = 0; FIELD(bool,mode_is_teleop) /* pin for teleop mode is on */ \ FIELD(bool,mode_joint) /* pin for requesting joint mode */ \ FIELD(bool,mode_is_joint) /* pin for joint mode is on */ \ + FIELD(bool,jog_frame_machine) /* pin for jogging along the machine axes */ \ + FIELD(bool,jog_frame_is_machine) /* pin for the machine axes are jogged */ \ + FIELD(bool,jog_frame_plane) /* pin for jogging along the work plane */ \ + FIELD(bool,jog_frame_is_plane) /* pin for the work plane is jogged */ \ + FIELD(bool,jog_frame_tool) /* pin for jogging along the tool */ \ + FIELD(bool,jog_frame_is_tool) /* pin for the tool is jogged */ \ \ FIELD(bool,mist_on) /* pin for starting mist */ \ FIELD(bool,mist_off) /* pin for stopping mist */ \ @@ -568,6 +574,9 @@ int halui_hal_init(void) CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_mdi), "halui.mode.is-mdi")); CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_teleop), "halui.mode.is-teleop")); CHK(halui_export_pin_OUT_bool(&(halui_data->mode_is_joint), "halui.mode.is-joint")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_machine), "halui.jog-frame.is-machine")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_plane), "halui.jog-frame.is-plane")); + CHK(halui_export_pin_OUT_bool(&(halui_data->jog_frame_is_tool), "halui.jog-frame.is-tool")); CHK(halui_export_pin_OUT_bool(&(halui_data->mist_is_on), "halui.mist.is-on")); CHK(halui_export_pin_OUT_bool(&(halui_data->flood_is_on), "halui.flood.is-on")); CHK(halui_export_pin_OUT_bool(&(halui_data->program_is_idle), "halui.program.is-idle")); @@ -667,6 +676,9 @@ int halui_hal_init(void) CHK(halui_export_pin_IN_bool(&(halui_data->mode_mdi), "halui.mode.mdi")); CHK(halui_export_pin_IN_bool(&(halui_data->mode_teleop), "halui.mode.teleop")); CHK(halui_export_pin_IN_bool(&(halui_data->mode_joint), "halui.mode.joint")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_machine), "halui.jog-frame.machine")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_plane), "halui.jog-frame.plane")); + CHK(halui_export_pin_IN_bool(&(halui_data->jog_frame_tool), "halui.jog-frame.tool")); CHK(halui_export_pin_IN_bool(&(halui_data->mist_on), "halui.mist.on")); CHK(halui_export_pin_IN_bool(&(halui_data->mist_off), "halui.mist.off")); CHK(halui_export_pin_IN_bool(&(halui_data->flood_on), "halui.flood.on")); @@ -899,6 +911,17 @@ static int sendJoint() return emcCommandWaitDone(); } +static int sendJogFrame(int frame) +{ + EMC_TRAJ_SET_JOG_FRAME emc_set_jog_frame_msg; + + emc_set_jog_frame_msg.frame = frame; + if (emcCommandSend(emc_set_jog_frame_msg)) { + return -1; + } + return emcCommandWaitDone(); +} + static int sendMistOn() { EMC_COOLANT_MIST_ON emc_coolant_mist_on_msg; @@ -1468,6 +1491,15 @@ static void check_hal_changes() if (check_bit_changed(new_halui_data.mode_joint, old_halui_data.mode_joint) != 0) sendJoint(); + if (check_bit_changed(new_halui_data.jog_frame_machine, old_halui_data.jog_frame_machine) != 0) + sendJogFrame(EMC_JOG_FRAME_MACHINE); + + if (check_bit_changed(new_halui_data.jog_frame_plane, old_halui_data.jog_frame_plane) != 0) + sendJogFrame(EMC_JOG_FRAME_PLANE); + + if (check_bit_changed(new_halui_data.jog_frame_tool, old_halui_data.jog_frame_tool) != 0) + sendJogFrame(EMC_JOG_FRAME_TOOL); + if (check_bit_changed(new_halui_data.mist_on, old_halui_data.mist_on) != 0) sendMistOn(); @@ -1927,6 +1959,9 @@ static void modify_hal_pins() hal_set_bool(halui_data->mode_is_mdi, emcStatus->task.mode == EMC_TASK_MODE::MDI); hal_set_bool(halui_data->mode_is_teleop, emcStatus->motion.traj.mode == EMC_TRAJ_MODE::TELEOP); hal_set_bool(halui_data->mode_is_joint, emcStatus->motion.traj.mode == EMC_TRAJ_MODE::FREE); + hal_set_bool(halui_data->jog_frame_is_machine, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_MACHINE); + hal_set_bool(halui_data->jog_frame_is_plane, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_PLANE); + hal_set_bool(halui_data->jog_frame_is_tool, emcStatus->motion.traj.jog_frame == EMC_JOG_FRAME_TOOL); hal_set_bool(halui_data->program_is_paused, emcStatus->task.interpState == EMC_TASK_INTERP::PAUSED); hal_set_bool(halui_data->program_is_running, emcStatus->task.interpState == EMC_TASK_INTERP::READING || @@ -2008,10 +2043,24 @@ static void modify_hal_pins() } // the relative position: the offset chain taken off in reverse, the - // tool offset, G5x, the XY rotation, G92 and the tilted work plane - double rx = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double ry = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - double rz = emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.toolOffset.tran.z; + // second feature coordinate system of 3+2 under the machine frame + // kinematics, the tool offset, G5x, the XY rotation, G92 and the + // tilted work plane + double rx = emcStatus->motion.traj.actualPosition.tran.x; + double ry = emcStatus->motion.traj.actualPosition.tran.y; + double rz = emcStatus->motion.traj.actualPosition.tran.z; + if (emcStatus->task.g68_active) { + const double *k = emcStatus->task.g68_kins_rotation; + double x = rx - emcStatus->task.g68_kins_offset.tran.x; + double y = ry - emcStatus->task.g68_kins_offset.tran.y; + double z = rz - emcStatus->task.g68_kins_offset.tran.z; + rx = k[0]*x + k[3]*y + k[6]*z; + ry = k[1]*x + k[4]*y + k[7]*z; + rz = k[2]*x + k[5]*y + k[8]*z; + } + rx -= emcStatus->task.g5x_offset.tran.x + emcStatus->task.toolOffset.tran.x; + ry -= emcStatus->task.g5x_offset.tran.y + emcStatus->task.toolOffset.tran.y; + rz -= emcStatus->task.g5x_offset.tran.z + emcStatus->task.toolOffset.tran.z; { double t = -emcStatus->task.rotation_xy * TO_RAD; double x = rx * cos(t) - ry * sin(t); diff --git a/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/glcanon/test.sh b/tests/glcanon/test.sh index cf53bc3c761..83e5e7e2713 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -4,9 +4,9 @@ # # None of them needs a GPU or an X display. The first two are GL-free # outright - they load rs274.glcanon_bake by path and exercise it as plain -# numpy. The third imports rs274.glcanon, which pulls PyOpenGL at import time +# numpy. The others import rs274.glcanon, which pulls PyOpenGL at import time # (it never calls into it here), and PyOpenGL is not installed in a headless -# build - so that one is conditional, the same rule tests/pyvcp/skip applies +# build - so those are conditional, the same rule tests/pyvcp/skip applies # to a whole directory. set -e @@ -16,8 +16,9 @@ set -e if python3 -c 'import OpenGL' 2>/dev/null; then ./test_workpiece.py >&2 ./test_workplane.py >&2 + ./test_tool_frame.py >&2 else - echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py, test_workplane.py and test_tool_frame.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_tool_frame.py b/tests/glcanon/test_tool_frame.py new file mode 100755 index 00000000000..abf549474f8 --- /dev/null +++ b/tests/glcanon/test_tool_frame.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +"""Which way the preview points the tool marker: along the tool frame the +kinematics reports through status when it reports one, by the GEOMETRY +string otherwise. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: the marker itself is stubbed out below. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import numpy as np + +from rs274 import glcanon_scene + + +class StatStub: + def __init__(self, tool_frame=None): + self.tool_frame = tool_frame + + +class CtxStub: + """What ToolPart reads to place the marker.""" + + def __init__(self, stat, geometry="XYZBC"): + self.stat = stat + self.geometry = geometry + self.mv = glcanon_scene.MatrixStack() + self.view_tool_min_dia = 0.0 + + @staticmethod + def current_tool(): + return None + + +class Marker(glcanon_scene.ToolPart): + """The tool part with the cone replaced by a record of where it went.""" + + def __init__(self): + self.at = None + + def _draw_cone(self, ctx): + self.at = ctx.mv.top().copy() + + +def place(stat, rotary=(0, 0, 0), geometry="XYZBC"): + ctx = CtxStub(stat, geometry) + marker = Marker() + marker._draw_at_tool(ctx, (1, 2, 3), *rotary) + return marker.at + + +def tilted_about_x(degrees): + """A tool frame turned about work X, nine values row by row.""" + c, s = math.cos(math.radians(degrees)), math.sin(math.radians(degrees)) + return (1, 0, 0, 0, c, -s, 0, s, c) + + +class ToolFrameTest(unittest.TestCase): + def test_the_marker_follows_the_reported_frame(self): + frame = tilted_about_x(30) + at = place(StatStub(frame)) + np.testing.assert_allclose(at[:3, :3], np.reshape(frame, (3, 3)), atol=1e-12) + np.testing.assert_allclose(at[:3, 3], (1, 2, 3)) + + def test_the_frame_overrides_geometry(self): + # B and C turned: GEOMETRY would tilt the marker, the frame says the + # tool is square with the work + at = place(StatStub((1, 0, 0, 0, 1, 0, 0, 0, 1)), rotary=(0, 40, 25)) + np.testing.assert_allclose(at[:3, :3], np.eye(3), atol=1e-12) + + def test_geometry_without_a_frame(self): + at = place(StatStub(None), rotary=(0, 40, 0)) + b = math.radians(40) + # the marker's axis, turned about Y by B + np.testing.assert_allclose(at[:3, 2], (math.sin(b), 0, math.cos(b)), atol=1e-12) + + def test_a_host_stat_without_the_field(self): + at = place(object(), rotary=(0, 40, 0)) + self.assertGreater(abs(at[0, 2]), 0.5) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py index 674b49e01d9..8ac61e98734 100755 --- a/tests/glcanon/test_workplane.py +++ b/tests/glcanon/test_workplane.py @@ -207,6 +207,19 @@ def test_no_plane_in_effect(self): self.assertIsNone(glcanon_scene.active_workplane(ctx)) self.assertEqual(len(ctx.prim.calls), 6) + def test_under_the_machine_frame_kinematics_the_plane_is_on_the_part(self): + # the map the plane is cut through onto the slides, a turn about Z + # and a shift, and a tool offset it carries: the plane is drawn as + # defined all the same, where the renderer draws the program + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + stat.g68_kins_rotation = (0, -1, 0, 1, 0, 0, 0, 0, 1) + stat.g68_kins_offset = (5 * 25.4, 6 * 25.4, 7 * 25.4) + (0,) * 6 + stat.tool_offset = (0, 0, 2 * 25.4) + (0,) * 6 + canon = run(program(TILTED, "G1 X-30 Y-20 Z5")) + canon.calc_extents() + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + if __name__ == '__main__': unittest.main() diff --git a/tests/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-limits/box/README b/tests/kins-limits/box/README new file mode 100644 index 00000000000..e00646f7457 --- /dev/null +++ b/tests/kins-limits/box/README @@ -0,0 +1,12 @@ +The [AXIS_L] box is the machine frame envelope: the carriage, not the +tool tip. + +5axiskins with the joints reaching 1000 and the box at 500, a 250 pivot and +a 50 tool, the head laid over to B90 so that the tip sits 300 from the +carriage in the XY plane. Under the tool centre point kinematics: a move +whose tip is past the box with the carriage inside it is accepted; a move +whose tip is inside with the carriage past the box is refused; G53.5 to a +carriage position past the box is refused, and to one inside it with the +tip past the box is accepted. A carriage past the joint limit is refused +whatever the box says. Nothing is asserted about the identity kinematics, +where the box and the joints describe the same thing. diff --git a/tests/kins-limits/box/checkresult b/tests/kins-limits/box/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/box/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/box/sim.hal b/tests/kins-limits/box/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/box/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/box/test-ui.py b/tests/kins-limits/box/test-ui.py new file mode 100755 index 00000000000..c43aa594a6b --- /dev/null +++ b/tests/kins-limits/box/test-ui.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +# The [AXIS_L] box is the machine frame envelope, the carriage and not the +# tool tip: see README. +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def accepted(cmd, carriage_x): + drain() + now = mdi(cmd) + said = [m for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[0] - carriage_x) > 1e-3: + error("%s left the carriage at X %.3f, not %.3f" % (cmd, now[0], carriage_x)) + else: + print("accepted as expected: %-24s carriage X %.3f" % (cmd, now[0])) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def refused(cmd, needle): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + settled() + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + settled() + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics with the tool on, the head laid over by +# a joint move: the tip is now 300 from the carriage along +X at C0 +mdi("G12.1 P0") +mdi("G43 H1") +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") +j = joints() +s.poll() +print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) +if abs(s.position[0] - j[0] - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) + +# the tip past the box, the carriage inside it: accepted +accepted("G0 X600", 300) +# the tip inside the box, the carriage past it: refused +refused("G0 X-300", "limit") +# the carriage past the joint limit: refused whatever the box says +refused("G0 X-800", "limit") +# G53.5 names the carriage: past the box refused, inside it accepted even +# with the tip past the box +refused("G53.5 G0 X-600", "limit") +accepted("G53.5 G0 X400", 400) + +mdi("G53.7 G0 J0=0 J3=0") +mdi("G49") +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/box/test.ini b/tests/kins-limits/box/test.ini new file mode 100644 index 00000000000..48dd3792291 --- /dev/null +++ b/tests/kins-limits/box/test.ini @@ -0,0 +1,132 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/test.sh b/tests/kins-limits/box/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-limits/box/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-limits/box/tool.tbl b/tests/kins-limits/box/tool.tbl new file mode 100644 index 00000000000..b558277601f --- /dev/null +++ b/tests/kins-limits/box/tool.tbl @@ -0,0 +1 @@ +T1 P1 D6.0 Z50 ; diff --git a/tests/kins-limits/jog/README b/tests/kins-limits/jog/README new file mode 100644 index 00000000000..2f2e00310c9 --- /dev/null +++ b/tests/kins-limits/jog/README @@ -0,0 +1,17 @@ +A world jog under the tool centre point must not run a slide past its +own limits. + +The swing test's machine: 5axiskins with a 400 mm pivot, the head laid +over to B90 by a joint move, the joints following their commands through +limit3 at 20 percent over each joint's own limits, the way a drive or a +step generator follows only as fast as it can. Then, in teleop mode, a +continuous jog of C at the axis's full rate, stopped by the test past +150 degrees, an incremental jog of C back by 90, and an incremental jog +of X with the head still laid over. The tip holds through the jogs of +C while the carriage swings on a circle of radius 400; nothing about the +[AXIS_C] limits says how fast that is. The test asserts that no jog +trips a following error, that the incremental jogs land where they were +sent, and that no joint's commanded velocity or acceleration went over +the joint's INI limit, sampled every servo cycle. The pace of the jog +is not asserted: it is whatever the slowest joint allows, and it changes +along the swing. diff --git a/tests/kins-limits/jog/checkresult b/tests/kins-limits/jog/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog/sim.hal b/tests/kins-limits/jog/sim.hal new file mode 100644 index 00000000000..8b447ed8103 --- /dev/null +++ b/tests/kins-limits/jog/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 240 +setp limit3.0.maxa 960 +setp limit3.1.maxv 240 +setp limit3.1.maxa 960 +setp limit3.2.maxv 240 +setp limit3.2.maxa 960 +setp limit3.3.maxv 72 +setp limit3.3.maxa 240 +setp limit3.4.maxv 72 +setp limit3.4.maxa 240 +setp limit3.5.maxv 240 +setp limit3.5.maxa 960 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/jog/test-ui.py b/tests/kins-limits/jog/test-ui.py new file mode 100755 index 00000000000..55aae72ee14 --- /dev/null +++ b/tests/kins-limits/jog/test-ui.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point must not run a slide past its +# own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + said = drain() + time.sleep(0.5) + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (what, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the circle a jog of C is about to swing it on +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() + +# world jogs: teleop mode +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C at the axis's full rate, the tip held: stopped by +# the test once C is past 150, so the planner ramps up, cruises, ramps down +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 60) +deadline = time.time() + 60 +while time.time() < deadline: + s.poll() + if s.position[C] > 150: + break + time.sleep(0.01) +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C", n0) +if end[4] < 150: + error("C stopped at %.3f, before 150" % end[4]) +tip_held("the continuous jog of C") + +# an incremental jog back by 90 at the same rate: the planner knows its +# endpoint, the cap still holds +n0 = catch_up(end) +back_from = end[4] +c.jog(linuxcnc.JOG_INCREMENT, 0, C, -60, 90) +end = checked("the incremental jog of C", n0) +if abs(end[4] - (back_from - 90)) > 1e-3: + error("C ended at %.3f, not %.3f" % (end[4], back_from - 90)) +tip_held("the incremental jog of C") + +# a jog of X with the head still laid over: the Jacobian does not change +# along it, the carriage simply runs at the jog rate +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 200, 100) +end = checked("the incremental jog of X", n0) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog/test.ini b/tests/kins-limits/jog/test.ini new file mode 100644 index 00000000000..8eac90e4fdf --- /dev/null +++ b/tests/kins-limits/jog/test.ini @@ -0,0 +1,144 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog/test.sh b/tests/kins-limits/jog/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog/tool.tbl b/tests/kins-limits/jog/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-limits/swing/README b/tests/kins-limits/swing/README new file mode 100644 index 00000000000..a9d17309b6d --- /dev/null +++ b/tests/kins-limits/swing/README @@ -0,0 +1,15 @@ +A turn of the head under the tool centre point must not run a slide +past its own limits. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +then a plain G0 C180 with the tool tip held: the tip does not move, the +carriage swings through a half circle of radius 400 to keep it still. The +joints follow their commands through limit3, 20 percent over each joint's +own velocity and acceleration limits, the way a drive or a step generator +follows only as fast as it can; a command inside the limits is followed +exactly, one well over them falls behind and motion trips the following +error. The test asserts that the move ends on its endpoint with no +following error, and that no joint's commanded velocity or acceleration +went over the joint's INI limit, sampled every servo cycle. The slowest +joint has to set the pace of the move; how slow the tip goes is not +asserted. diff --git a/tests/kins-limits/swing/checkresult b/tests/kins-limits/swing/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/swing/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/swing/sim.hal b/tests/kins-limits/swing/sim.hal new file mode 100644 index 00000000000..8b447ed8103 --- /dev/null +++ b/tests/kins-limits/swing/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 240 +setp limit3.0.maxa 960 +setp limit3.1.maxv 240 +setp limit3.1.maxa 960 +setp limit3.2.maxv 240 +setp limit3.2.maxa 960 +setp limit3.3.maxv 72 +setp limit3.3.maxa 240 +setp limit3.4.maxv 72 +setp limit3.4.maxa 240 +setp limit3.5.maxv 240 +setp limit3.5.maxa 960 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py new file mode 100755 index 00000000000..44d45ab8e52 --- /dev/null +++ b/tests/kins-limits/swing/test-ui.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# A turn of the head under the tool centre point must not run a slide past +# its own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the half circle it is about to draw +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def swing(cmd, n0): + c.mdi(cmd) + c.wait_complete(60) + end = settled() + said = drain() + time.sleep(0.5) + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (cmd, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (cmd, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), cmd)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +# the swing: the tip holds, C turns half a revolution, the carriage follows +# a half circle of radius 400 to keep the tip where it is +end = swing("G0 C180", catch_up(start)) +if abs(end[4] - 180) > 1e-3: + error("C ended at %.4f, not 180" % end[4]) +tip_held("G0 C180") + +# the same on an arc: the tip draws a full circle of radius 100 at a feed +# it could keep on its own while C turns another revolution, so the +# carriage rides the small circle and the big swing together +end = swing("G17 G2 X0 Y0 I100 J0 C540 F5000", catch_up(end)) +if abs(end[4] - 540) > 1e-3: + error("C ended at %.4f, not 540" % end[4]) +tip_held("the arc") + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/swing/test.ini b/tests/kins-limits/swing/test.ini new file mode 100644 index 00000000000..8eac90e4fdf --- /dev/null +++ b/tests/kins-limits/swing/test.ini @@ -0,0 +1,144 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/swing/test.sh b/tests/kins-limits/swing/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/swing/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/swing/tool.tbl b/tests/kins-limits/swing/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/swing/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-limits/travel/README b/tests/kins-limits/travel/README new file mode 100644 index 00000000000..61cf454cb3e --- /dev/null +++ b/tests/kins-limits/travel/README @@ -0,0 +1,16 @@ +A move whose joints leave their travel between its ends is refused before +it is sent, not stopped on the soft limit partway through it. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +the tool tip held while C turns, so that the carriage swings round the tip +at the pivot length plus the tool length; C-90 puts it at Y+ that radius, +against joint 1's MAX_LIMIT of 400. A tool length of 0.05 makes the swing +from C-5 to C-175 peak 0.05 past the travel at C-90, between two samples +1.2 inside it: the move is refused, asked again it is refused again, and +moves inside the travel still go, turning the other way among them; a +move ending past the travel is refused for its end. At -0.05 the same +swing peaks 0.05 inside and runs; at 100 the samples themselves are past. +With the head upright an arc whose ends are inside the travel and whose +top is past it is refused, the same arc the other way round runs. A +program fails on the line of the swing with no joint past its limit, and +run from the line after it, which only steps over the swing, it runs. diff --git a/tests/kins-limits/travel/checkresult b/tests/kins-limits/travel/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/travel/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/travel/sim.hal b/tests/kins-limits/travel/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/travel/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/travel/test-ui.py b/tests/kins-limits/travel/test-ui.py new file mode 100755 index 00000000000..80087b51de3 --- /dev/null +++ b/tests/kins-limits/travel/test-ui.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 +# A move whose joints leave their travel between its ends is refused before +# it is sent: see README. +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +PROGRAM = os.path.abspath("travel.ngc") +# the line of travel.ngc that swings the carriage past its Y travel +SWING_LINE = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 +# the furthest joint 1 was seen, every time the machine is polled +y_peak = -1e99 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + global y_peak + s.poll() + y_peak = max(y_peak, s.joint_position[1]) + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def complaints(said): + return [m for m in said if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.01) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def back_to_mdi(): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def accepted(cmd, joint, where): + drain() + now = mdi(cmd) + said = complaints(drain()) + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[joint] - where) > 1e-3: + error("%s left joint %d at %.4f, not %.4f" % (cmd, joint, now[joint], where)) + else: + print("accepted as expected: %-28s joint %d at %.4f" % (cmd, joint, now[joint])) + back_to_mdi() + +def refused(cmd, *needles): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + settled() + said = complaints(drain()) + if not said: + error("%s was accepted" % cmd) + elif not all(n in said[0][1] for n in needles): + error("%s said %r, not %r" % (cmd, said[0][1].strip(), needles)) + else: + print("refused as expected: %s" % said[0][1].strip()) + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + back_to_mdi() + +def run(start_line): + drain() + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete(30) + c.program_open(PROGRAM) + c.wait_complete(30) + c.auto(linuxcnc.AUTO_RUN, start_line) + c.wait_complete(30) + deadline = time.time() + 60 + while time.time() < deadline: + joints() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue and s.inpos: + break + time.sleep(0.01) + else: + error("the program did not finish") + now = settled() + said = complaints(drain()) + back_to_mdi() + return now, said + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +back_to_mdi() +drain() + +# the tool centre point kinematics, the head laid over by a joint move; the +# carriage at X-400 puts the tip at X0. A turn of C holding the tip swings +# the carriage round it at the pivot length plus the tool length, C-90 +# putting it at Y+ that radius, against joint 1's MAX_LIMIT of 400 +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") + +# a radius of 400.05: the swing from C-5 to C-175 peaks at C-90 between two +# samples, which are 1.2 inside the travel; the peak is 0.05 past it +mdi("G43.1 Z0.05") +accepted("G0 C-5", 4, -5) +refused("G0 C-175", "joint 1", "MAX_LIMIT", "50%") +# asked again, it is refused again, and a move inside the travel still goes +refused("G0 C-175", "joint 1", "MAX_LIMIT") +accepted("G0 C-60", 4, -60) +accepted("G0 C-5", 4, -5) +# turning the other way, C through +90, runs the carriage through +# Y-400.05, inside the travel +accepted("G0 C175", 4, 175) +accepted("G0 C-5", 4, -5) +# a move that ends past the travel is refused for its end +refused("G0 C-90", "joint 1", "end with", "MAX_LIMIT") + +# a radius of 399.95: the same swing peaks 0.05 inside the travel +mdi("G43.1 Z-0.05") +accepted("G0 C-175", 4, -175) +accepted("G0 C-5", 4, -5) + +# a radius of 500: the samples themselves are past the travel +mdi("G43.1 Z100") +refused("G0 C-175", "joint 1", "MAX_LIMIT") +mdi("G49") + +# an arc with the head upright, its ends inside the travel and its top +# 54.4 past it; the other way round it runs below +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +accepted("G0 X-100 Y380", 1, 380) +refused("G2 X100 Y380 I100 J-30 F3000", "joint 1", "MAX_LIMIT") +accepted("G3 X100 Y380 I100 J-30 F3000", 1, 380) +accepted("G0 X0 Y0", 1, 0) + +# a program: the line that swings past the travel fails, the moves before +# it may have run, and no joint has passed its limit +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +y_peak = -1e99 +now, said = run(0) +if not said: + error("the program ran through line %d" % SWING_LINE) +elif "line %d" % SWING_LINE not in said[0][1] or "joint 1" not in said[0][1]: + error("the program said %r" % said[0][1].strip()) +else: + print("refused as expected: %s" % said[0][1].strip()) +if any("soft limit" in m[1] for m in said): + error("a joint ran onto its soft limit") +if y_peak > 400 + 1e-6: + error("joint 1 reached %.4f, past its MAX_LIMIT" % y_peak) + +# run from the line after the swing: the line is only stepped over, its +# moves are thrown away, and the program runs +mdi("G49") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +now, said = run(SWING_LINE + 1) +if said: + error("run from line %d said %r" % (SWING_LINE + 1, said[0][1].strip())) +elif abs(now[4] + 5) > 1e-3: + error("run from line %d left C at %.4f, not -5" % (SWING_LINE + 1, now[4])) +else: + print("ran from line %d as expected, C at %.4f" % (SWING_LINE + 1, now[4])) + +mdi("G49") +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +print("joint 1 peaked at %.4f since the program started" % y_peak) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/travel/test.ini b/tests/kins-limits/travel/test.ini new file mode 100644 index 00000000000..f8e7febb3d1 --- /dev/null +++ b/tests/kins-limits/travel/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# laid over to B90, the head swings the carriage round the tip at the +# pivot length plus the tool's +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -2000 +MAX_LIMIT = 2000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +# the travel the swings are measured against +MIN_LIMIT = -1000 +MAX_LIMIT = 400 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/travel/test.sh b/tests/kins-limits/travel/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-limits/travel/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-limits/travel/tool.tbl b/tests/kins-limits/travel/tool.tbl new file mode 100644 index 00000000000..80153f2ca05 --- /dev/null +++ b/tests/kins-limits/travel/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z0 ; diff --git a/tests/kins-limits/travel/travel.ngc b/tests/kins-limits/travel/travel.ngc new file mode 100644 index 00000000000..23b2a4b9949 --- /dev/null +++ b/tests/kins-limits/travel/travel.ngc @@ -0,0 +1,8 @@ +(the head laid over to B90, the tip held while C turns) +G21 G90 G94 +G12.1 P0 +G43.1 Z0.05 +G0 C-5 +G0 C-175 +G0 C-5 +M2 diff --git a/tests/kins-orient-linear/README b/tests/kins-orient-linear/README new file mode 100644 index 00000000000..674867e3a64 --- /dev/null +++ b/tests/kins-orient-linear/README @@ -0,0 +1,4 @@ +A kinematics type that orients the tool with an axis the INI file makes +LINEAR is refused: 5axiskins turns the tool with B and C, and here +[AXIS_B] TYPE is LINEAR. The interpreter refuses when it first loads the +module, the G43.5 vector below, with a message naming the type and B. diff --git a/tests/kins-orient-linear/checkresult b/tests/kins-orient-linear/checkresult new file mode 100755 index 00000000000..42526aa5579 --- /dev/null +++ b/tests/kins-orient-linear/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# test-ui.py prints PASS once the refusal names B +grep -q "^PASS$" "$1" diff --git a/tests/kins-orient-linear/sim.hal b/tests/kins-orient-linear/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-orient-linear/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-orient-linear/test-ui.py b/tests/kins-orient-linear/test-ui.py new file mode 100755 index 00000000000..3d72ea20b34 --- /dev/null +++ b/tests/kins-orient-linear/test-ui.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +# 5axiskins orients with B and C; [AXIS_B] TYPE = LINEAR: see README. + +import linuxcnc +import os +import sys + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +said = [] +for cmd in ("G12.1 P0", "G43.5 H1", "G0 X0 K1"): + c.mdi(cmd) + c.wait_complete(30) + while True: + m = e.poll() + if not m: + break + said.append(m) + +errors = [m[1] for m in said if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] +print("errors: %s" % errors) +ok = any("orients the tool with B, which [AXIS_B] TYPE makes LINEAR" in m for m in errors) +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("PASS" if ok else "FAIL: no refusal naming B") +sys.exit(0 if ok else 1) diff --git a/tests/kins-orient-linear/test.ini b/tests/kins-orient-linear/test.ini new file mode 100644 index 00000000000..0ea12355fb5 --- /dev/null +++ b/tests/kins-orient-linear/test.ini @@ -0,0 +1,131 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, 1 is identity, 2 is the userk template +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +TYPE = LINEAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-orient-linear/test.sh b/tests/kins-orient-linear/test.sh new file mode 100755 index 00000000000..079fb121f31 --- /dev/null +++ b/tests/kins-orient-linear/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-orient-linear/tool.tbl b/tests/kins-orient-linear/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-orient-linear/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-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..2f0afd57ba7 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,11 +99,24 @@ 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 +# a table and a head, two primaries: one rotary turns the tool, so no +# pair, and that one names the pose +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3" +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3" 1 + run "xyzac-trt-kins coordinates=XYZAC" \ "setp xyzac-trt-kins.y-offset 3 setp xyzac-trt-kins.z-offset 11 @@ -123,7 +143,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 +182,53 @@ 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 +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3 tool=4,3,30" +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=- posejoint=3 tool=4,3,30" 1 + # polar run "rosekins" "" "joints=3 jnt=10,5,30" @@ -170,8 +237,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..f7e932f075e 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -163,38 +163,227 @@ 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") + +# ---- the axes that orient the tool --------------------------------------- +# +# 5axiskins turns the tool with a head: C with its axis fixed, B carried by +# it. The interpreter finds them from the tool frame and names them by the +# joint map, as axis numbers; the identity type orients with nothing. Under +# G43.5 a word of an axis that does not orient, W here, goes with a vector. + +def param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + c.wait_complete(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") +errors_before = errors +mdi("G12.1 P0") +got = [param(n) for n in ORIENT] +if got != [5, 4, -1, 1, 1, -1]: + error("on the 5-axis type the orienting axes read %s, not C then B, both heads" % (got,)) +mdi("G13.1") +got = [param(n) for n in ORIENT] +if got != [-1, -1, -1, -1, -1, -1]: + error("on the identity type the orienting axes read %s, not none" % (got,)) +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0 W0") +mdi("G43.5 H1") +with_w = mdi("G0 X0 Y0 Z0 K1 W2") +if abs(with_w[5] - 2) > 1e-6: + error("a vector with a W word left W at %.4f, not 2" % with_w[5]) +mdi("G0 W0") mdi("G49") +said = drain() +if any(m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) for m in said): + error("the orienting axes section reported %s" % (said,)) +elif errors == errors_before: + print("the orienting axes are C then B, heads, and W goes along with a vector") # ---- a negative kinematics number is refused ----------------------------- 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..ef9d659211a --- /dev/null +++ b/tests/ptp-machine-frame/README @@ -0,0 +1,25 @@ +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; type 3 +the tip again, a second head, declared primary as well. + +The test checks that G13.1 and G49 select the machine frame type rather +than the identity, that G43.4 stays on a primary type selected with G12.1 +and from any other goes to the lowest numbered primary, that G53.5 puts the carriage at a machine frame point, +the letters not given holding their machine coordinate and not their +joint, from the machine frame type and from under the tilted tip +kinematics alike, that G28.5 goes to a position stored on the machine +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..34f917eb78f --- /dev/null +++ b/tests/ptp-machine-frame/slantkins.comp @@ -0,0 +1,181 @@ +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. + +type3 is type1 again, declared primary as well, the way a machine with a +second tool head declares a working transform for each. +"""; +pin out si32 dummy=0 "one pin, which halcompile requires"; +option period no; +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, +}; + +// a second head: G43.4 stays on it once G12.1 has selected it +static const kins_ops second_head_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + 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); + switchkinsRegisterOps(3, &second_head_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..81689a4990d --- /dev/null +++ b/tests/ptp-machine-frame/test-ui.py @@ -0,0 +1,319 @@ +#!/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) + +# --- two primary types: G43.4 keeps the head G12.1 selected ------------------ +mdi("G12.1 P3", "G43.4 H1") +expect_type("G12.1 P3 then G43.4, the second head", 3) +mdi("G49") +expect_type("G49 from the second head", 0) +mdi("G12.1 P2", "G43.4 H1") +expect_type("G43.4 from the identity, the lowest", 1) +mdi("G12.1 P3", "G43.4 H1") +expect_type("G43.4 again after G12.1 P3", 3) +mdi("G49") +drain() + +# --- G53.5 is the machine frame, not the slides ------------------------------ +# 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..4c46e26de99 --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,185 @@ +#!/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)) + +# the wrist turns the tool with three joints: the arm kinematics orients +# with the A B C of its pose, all three head +mdi("G13.1") +drain() +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>)") +c.wait_complete(30) +said = None +deadline = time.time() + 5 +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +if said != [3, 4, 5, 1, 1, 1]: + error("the arm kinematics orients with %s, not A B C, heads" % (said,)) +print("orienting axes %s" % said) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/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..f7dfe04cee5 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_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_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twinspindle/README b/tests/twinspindle/README new file mode 100644 index 00000000000..c6e5486836b --- /dev/null +++ b/tests/twinspindle/README @@ -0,0 +1,12 @@ +twinspindlekins: a B tilting head between a main spindle on C and a sub +spindle on W, W ANGULAR, each spindle a primary kinematics type in the +frame of its own part, and an identity type. + +The test checks that each type names its own rotaries on +#<_kins_orient_1> to #<_kins_orient_3> and their head flags, C then B on +the main spindle, W then B on the sub spindle, nothing on the identity; +that G53.2 and G53.3 on the same tilted hole give the spindle angle and a +B on either side of 180 as #<_orient_rot1> and #<_orient_rot2>, and put +the tip on the hole with the tool along its axis, worked out from the +joints; and that the sim config's demo program drills both parts through +one subroutine and parks. diff --git a/tests/twinspindle/checkresult b/tests/twinspindle/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twinspindle/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twinspindle/sim.hal b/tests/twinspindle/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twinspindle/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twinspindle/test-ui.py b/tests/twinspindle/test-ui.py new file mode 100755 index 00000000000..9bc2348cde2 --- /dev/null +++ b/tests/twinspindle/test-ui.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +# twinspindlekins: two primary types, one per spindle, orienting with +# different rotaries: see README. + +import linuxcnc +import math +import os +import sys +import time + +JOINTS = 6 +PIVOT = 150.0 # twinspindlekins.pivot-length default +DISTANCE = 500.0 # twinspindlekins.spindle-distance default +TOOL = 50.0 # T1 in tool.tbl +FACE = 60.0 # G54 and G55 Z, the part faces +PROGRAM = os.path.abspath("../../configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc") + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.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 param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + c.wait_complete(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +def turn(a, b): + # a and b the same angle, whole turns apart + return abs((a - b + 180.0) % 360.0 - 180.0) < 1e-6 + +# the module's maths, written out again: the tip and the tool axis, tip +# towards holder, in the frame of the part on spindle `sub`. The slides +# carry the B axis PIVOT above the gauge line, the tip hangs PIVOT + TOOL +# below it along the tool axis. +def part_pose(j, sub): + L = PIVOT + TOOL + b = math.radians(j[3]) + m = [j[0] - L*math.sin(b), j[1], j[2] + PIVOT - L*math.cos(b)] + u = [math.sin(b), 0.0, math.cos(b)] + if sub: + m = [m[0], -m[1], DISTANCE - m[2]] + u = [u[0], -u[1], -u[2]] + a = math.radians(j[5] if sub else j[4]) + rz = lambda v: [math.cos(a)*v[0] - math.sin(a)*v[1], + math.sin(a)*v[0] + math.cos(a)*v[1], v[2]] + return rz(m), rz(u) + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") + +mdi("T1 M6", "G43", + "G10 L2 P1 X0 Y0 Z%g C0 W0" % FACE, + "G10 L2 P2 X0 Y0 Z%g C0 W0" % FACE) + +# ---- each type names its own rotaries -------------------------------------- + +for ktype, want, what in ((0, [5, 4, -1, 0, 1, -1], "C then B"), + (1, [8, 4, -1, 0, 1, -1], "W then B"), + (2, [-1] * 6, "nothing")): + mdi("G12.1 P%d" % ktype) + got = [param(n) for n in ORIENT] + if got != want: + error("type %d orients with %s, not %s: table then head" % (ktype, got, what)) + +# ---- the same hole on either spindle --------------------------------------- +# +# A hole 30 out on the face at 60 degrees, tilted 20 degrees outwards. On +# the main spindle C turns it under the head and B leans the tool; on the +# sub spindle, whose part faces the other way, W turns it and B leans the +# tool past 180. + +A, T, R = 60.0, 20.0, 30.0 +n = [math.sin(math.radians(T))*math.cos(math.radians(A)), + math.sin(math.radians(T))*math.sin(math.radians(A)), + math.cos(math.radians(T))] +hole = [R*math.cos(math.radians(A)), R*math.sin(math.radians(A)), 0.0] +PLANE = ("G68.2 P3 Q1 X%.9f Y%.9f Z0 I%.9f J%.9f K0" + % (hole[0], hole[1], -math.sin(math.radians(A)), math.cos(math.radians(A))), + "G68.2 P3 Q2 I%.9f J%.9f K%.9f" % tuple(n)) + +for ktype, offset, joint, b in ((0, "G54", 4, T), (1, "G55", 5, 180.0 - T)): + sub = ktype == 1 + mdi("G13.1", "G53 G0 X300", "G53 G0 Z50 B90") + mdi("G12.1 P%d" % ktype, offset, *PLANE) + mdi("G53.2") + rot1, rot2 = param("_orient_rot1"), param("_orient_rot2") + print("type %d: rot1 %.6f rot2 %.6f" % (ktype, rot1, rot2)) + if not (turn(rot1, A) and abs(rot2 - b) < 1e-6): + error("type %d: G53.2 gave rot1 %s rot2 %s, not %s and %s" % (ktype, rot1, rot2, A, b)) + j = mdi("G53.3 X0 Y0 Z5") + if not (turn(j[joint], A) and abs(j[3] - b) < 1e-6): + error("type %d: G53.3 left joint %d at %s and B at %s" % (ktype, joint, j[joint], j[3])) + tip, axis = part_pose(j, sub) + want = [hole[i] + 5*n[i] + (FACE if i == 2 else 0.0) for i in range(3)] + if max(abs(tip[i] - want[i]) for i in range(3)) > 1e-6: + error("type %d: the tip is at %s in the part, not %s" % (ktype, tip, want)) + if max(abs(axis[i] - n[i]) for i in range(3)) > 1e-9: + error("type %d: the tool points along %s in the part, not %s" % (ktype, axis, n)) + mdi("G69") +drain() + +# ---- the demo program, both spindles, one subroutine ----------------------- + +mdi("G13.1", "G54") +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete() +c.program_open(PROGRAM) +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 300 +time.sleep(0.5) +while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and s.queue == 0 and s.inpos: + break + time.sleep(0.1) +else: + error("the demo program did not finish") +drain() +j = settled() +if max(abs(j[i] - w) for i, w in ((0, 300.0), (2, 50.0), (3, 90.0))) > 1e-6: + error("the demo program ended at %s, not parked" % (j,)) + +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("Exiting with %d errors" % errors) +sys.exit(errors != 0) diff --git a/tests/twinspindle/test.ini b/tests/twinspindle/test.ini new file mode 100644 index 00000000000..890edb69759 --- /dev/null +++ b/tests/twinspindle/test.ini @@ -0,0 +1,139 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = twinspindlekins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/tests/twinspindle/test.sh b/tests/twinspindle/test.sh new file mode 100755 index 00000000000..f494f0fc1fb --- /dev/null +++ b/tests/twinspindle/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries the offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/twinspindle/tool.tbl b/tests/twinspindle/tool.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/tests/twinspindle/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill diff --git a/tests/twp-jog-frame/README b/tests/twp-jog-frame/README new file mode 100644 index 00000000000..6e63b36c52e --- /dev/null +++ b/tests/twp-jog-frame/README @@ -0,0 +1,34 @@ +World jogs of X, Y and Z along the tilted work plane and along the tool. + +5axiskins with a 100 mm pivot, the tool centre point type, Y slow and +short against X and Z. The plane frame: a plane defined under an XY +rotation, incremental jogs of each letter, each world move checked +against the plane's axes as status reports them turned by the XY +rotation, rotaries untouched; a machine jog afterwards starts where the +frame jogs left the machine. A plane whose X runs mostly along Y: the +jog along it runs at the speed Y allows. One frame axis moves at a +time: a jog of plane Y asked for while plane X runs waits until X has +stopped. A continuous jog stops where the turned axis meets the +[AXIS_Y] travel without a soft limit fault, plane Y asked for at once +too, and no further jog leaves it. The frame cannot change while a jog +is under way. The tool frame with the head laid over and turned: the +jogs follow the tool frame status reports, and a plane in effect leaves +that frame as the kinematics reports it. Under the identity type a tool +jog moves the joints as it does under the primary type, along the tool as +the head stands, and status has the tool frame the primary type has at +the same joints. halui's +jog-frame pins select the frame and report it. Every servo cycle of +every jog is sampled: no world axis goes over its own speed or +acceleration. The whole test runs on the trapezoid and on the S-curve +planner. Then trivkins, not homed, with a plane turned 45 degrees about +Z: a frame jog gets the travel a plain jog of an axis not homed gets, +one range either way of where it stands, not the [AXIS_] +limits, so an incremental jog past them goes and a continuous one runs +to one range from its start. + +Then a trunnion, xyzac-trt-kins, on the tool centre point type with C at +90 degrees: a machine frame jog moves the slide of its letter alone, X, +Y and Z, and again with A tilted 30 degrees, since the machine does not +turn when the table does; the plane frame with no plane jogs the part's +X, turned with the table onto the Y slide; the tool frame jogs Z on the +Z slide; on the identity type a machine X jog moves X. diff --git a/tests/twp-jog-frame/checkresult b/tests/twp-jog-frame/checkresult new file mode 100755 index 00000000000..2b01441be0b --- /dev/null +++ b/tests/twp-jog-frame/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +# the test scripts count their own failures: each planner, the machine +# not homed, then the trunnion +test "$(grep -c "^Exiting with 0 errors" "$1")" = 4 diff --git a/tests/twp-jog-frame/sim.hal b/tests/twp-jog-frame/sim.hal new file mode 100644 index 00000000000..7116bdd34e8 --- /dev/null +++ b/tests/twp-jog-frame/sim.hal @@ -0,0 +1,26 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# the world velocity of X, Y and Z every servo cycle, for the speed and +# acceleration of every jog +loadrt sampler depth=4000 cfg=fff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf sampler.0 servo-thread + +net VX axis.x.teleop-vel-cmd => sampler.0.pin.0 +net VY axis.y.teleop-vel-cmd => sampler.0.pin.1 +net VZ axis.z.teleop-vel-cmd => sampler.0.pin.2 +loadusr halsampler -t samples.log + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame/table.hal b/tests/twp-jog-frame/table.hal new file mode 100644 index 00000000000..c615d9e1e9c --- /dev/null +++ b/tests/twp-jog-frame/table.hal @@ -0,0 +1,19 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +# the rotation centre off the machine origin, so a turn moves the work +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame/table.ini b/tests/twp-jog-frame/table.ini new file mode 100644 index 00000000000..9352e8e4678 --- /dev/null +++ b/tests/twp-jog-frame/table.ini @@ -0,0 +1,120 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-table.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +# a trunnion: switchkins-type 0 is identity, 1 the tool centre point, +# which turns the work with A and C +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = table.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Y] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Z] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -400 +MAX_LIMIT = 400 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_3] +TYPE = ANGULAR +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -400 +MAX_LIMIT = 400 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 + +[JOINT_4] +TYPE = ANGULAR +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -400 +MAX_LIMIT = 400 +HOME_SEQUENCE = 0 +HOME_SEARCH_VEL = 0 +HOME_LATCH_VEL = 0 diff --git a/tests/twp-jog-frame/test-table.py b/tests/twp-jog-frame/test-table.py new file mode 100755 index 00000000000..d60b7c53110 --- /dev/null +++ b/tests/twp-jog-frame/test-table.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +# Machine frame jogs on a trunnion under the tool centre point: see README. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1].strip()) + +def joints(): + s.poll() + return [s.joint[j]["output"] for j in range(5)] + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = joints() + if s.inpos and not s.queue and now == last \ + and not hal.get_value("motion.jog-is-active"): + return now + last = now + time.sleep(0.05) + error("timed out waiting for the machine to stop") + return last + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + settled() + drain() + c.mode(linuxcnc.MODE_MANUAL) + c.wait_complete(30) + c.teleop_enable(1) + c.wait_complete(30) + +def frame(f): + c.jog_frame(f) + c.wait_complete(30) + +# an incremental jog of 5 on one letter, and the joints it moved +def jog(what, axis, want): + before = joints() + c.jog(linuxcnc.JOG_INCREMENT, False, axis, 50, 5) + after = settled() + moved = [after[j] - before[j] for j in range(5)] + print("%s: joints moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved the joints %s, not %s" % (what, fmt(moved), fmt(want))) + drain() + return moved + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(0) +c.wait_complete(30) +c.home(-1) +c.wait_complete(30) +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if all(s.homed[:5]): + break + time.sleep(0.1) +else: + error("the joints did not home") + +# the tool centre point, the table turned a quarter turn +mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 C90") + +# the machine axes: the slides, however the table stands +frame(linuxcnc.JOG_FRAME_MACHINE) +jog("machine X at C90", 0, [5, 0, 0, 0, 0]) +jog("machine Y at C90", 1, [0, 5, 0, 0, 0]) +jog("machine Z at C90", 2, [0, 0, 5, 0, 0]) + +# the plane frame with no plane: the part's axes, turned with the table, +# so its X is a slide other than X +frame(linuxcnc.JOG_FRAME_PLANE) +before = joints() +c.jog(linuxcnc.JOG_INCREMENT, False, 0, 50, 5) +after = settled() +moved = [after[j] - before[j] for j in range(5)] +print("plane X at C90, no plane: joints moved %s" % fmt(moved)) +if abs(moved[0]) > 1e-5 or abs(abs(moved[1]) - 5) > 1e-5 or abs(moved[2]) > 1e-5: + error("plane X at C90 moved the joints %s, not Y alone" % fmt(moved)) +drain() + +# the tool frame: a trunnion's tool stands still, Z is the Z slide +frame(linuxcnc.JOG_FRAME_TOOL) +jog("tool Z at C90", 2, [0, 0, 5, 0, 0]) + +# the table tilted as well +mdi("G0 A30") +frame(linuxcnc.JOG_FRAME_MACHINE) +jog("machine X at A30 C90", 0, [5, 0, 0, 0, 0]) +jog("machine Z at A30 C90", 2, [0, 0, 5, 0, 0]) + +# the machine frame type: the slides too +mdi("G13.1") +jog("machine X on the identity type", 0, [5, 0, 0, 0, 0]) + +frame(linuxcnc.JOG_FRAME_MACHINE) +c.state(linuxcnc.STATE_OFF) +c.wait_complete(30) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame/test-ui.py b/tests/twp-jog-frame/test-ui.py new file mode 100755 index 00000000000..12f0f0e48e9 --- /dev/null +++ b/tests/twp-jog-frame/test-ui.py @@ -0,0 +1,374 @@ +#!/usr/bin/env python3 +# World jogs of X, Y and Z along the tilted work plane and along the tool: +# see README. +import hal +import linuxcnc +import math +import os +import sys +import time + +JOINTS = 6 +X, Y, Z = 0, 1, 2 +B, C = 4, 5 +VEL_Y = 50.0 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini("test.ini") +AXIS_VEL = [float(ini.find("AXIS_%s" % l, "MAX_VELOCITY")) for l in "XYZ"] +AXIS_ACC = [float(ini.find("AXIS_%s" % l, "MAX_ACCELERATION")) for l in "XYZ"] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(expect=None): + # the messages since the last call; any error is a failure unless it + # carries the text expected + said = [] + while True: + m = e.poll() + if not m: + break + print("channel:", m) + said.append(m[1]) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) \ + and not (expect and expect in m[1]): + error("reported: %s" % m[1].strip()) + return said + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = list(s.position) + if s.inpos and not s.queue and now == last \ + and not hal.get_value("motion.jog-is-active"): + return now + last = now + time.sleep(0.05) + error("timed out waiting for the machine to stop") + return last + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + settled() + drain() + +def teleop(): + c.mode(linuxcnc.MODE_MANUAL) + c.wait_complete(30) + c.teleop_enable(1) + c.wait_complete(30) + deadline = time.time() + 10 + while time.time() < deadline: + s.poll() + if s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP: + return + time.sleep(0.05) + error("motion did not enter teleop mode") + +def frame(which): + c.jog_frame(which) + c.wait_complete(30) + time.sleep(0.05) + s.poll() + if s.jog_frame != which: + error("the jog frame is %d, not %d" % (s.jog_frame, which)) + if int(hal.get_value("motion.jog-frame")) != which: + error("motion.jog-frame is %s, not %d" % (hal.get_value("motion.jog-frame"), which)) + +def column(m, k): + return [m[3 * i + k] for i in range(3)] + +def rot_z(deg, v): + t = math.radians(deg) + return [math.cos(t) * v[0] - math.sin(t) * v[1], + math.sin(t) * v[0] + math.cos(t) * v[1], v[2]] + +def dot(a, b): + return sum(p * q for p, q in zip(a, b)) + +def unit(v): + n = math.sqrt(dot(v, v)) + return [p / n for p in v] + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def plane_axes(): + # the plane's axes in world coordinates, from status: the plane's own + # rotation turned by the XY rotation it was defined under + s.poll() + r = s.g68_rotation + return [rot_z(s.rotation_xy, column(r, k)) for k in range(3)] + +def tool_axes(): + # the tool frame's axes in world coordinates, from status + s.poll() + t = s.tool_frame + if t is None: + error("no tool frame in status") + return [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + return [column(t, k) for k in range(3)] + +def jog_incr(axis, dist, speed, want, what): + # an incremental jog of a world letter, and the world move it must make + s.poll() + before = list(s.position) + c.jog(linuxcnc.JOG_INCREMENT, 0, axis, speed if dist > 0 else -speed, abs(dist)) + after = settled() + moved = [after[i] - before[i] for i in range(3)] + print("%s: moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved %s, not %s" % (what, fmt(moved), fmt(want))) + for a in (B, C): + if abs(after[a] - before[a]) > 1e-9: + error("%s turned a rotary from %.6f to %.6f" % (what, before[a], after[a])) + drain() + return after + +def cruise(axes, speed, what): + # continuous jogs of the given letters at once, read in the cruise + s.poll() + before = list(s.position) + for a in axes: + c.jog(linuxcnc.JOG_CONTINUOUS, 0, a, speed) + time.sleep(0.4) + vel = [hal.get_value("axis.%s.teleop-vel-cmd" % l) for l in "xyz"] + for a in axes: + c.jog(linuxcnc.JOG_STOP, 0, a) + after = settled() + moved = [after[i] - before[i] for i in range(3)] + print("%s: velocity %s, moved %s" % (what, fmt(vel), fmt(moved))) + drain() + return vel, moved + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +settled() +drain() + +s.poll() +if s.jog_frame != linuxcnc.JOG_FRAME_MACHINE: + error("the jog frame starts at %d, not the machine's" % s.jog_frame) + +# the plane, defined under an XY rotation of 30 degrees: its axes turned by +# both are what the jog must follow +mdi("G12.1 P0", "G10 L2 P1 R30", "G54", "G68.2 X0 Y0 Z0 I0 J30 K0") +W = plane_axes() +print("plane axes in world: X %s, Y %s, Z %s" % tuple(fmt(v) for v in W)) +teleop() +frame(linuxcnc.JOG_FRAME_PLANE) +jog_incr(X, 10, 100, [10 * p for p in W[0]], "plane X +10") +jog_incr(Y, -5, 40, [-5 * p for p in W[1]], "plane Y -5") +jog_incr(Z, 3, 100, [3 * p for p in W[2]], "plane Z +3") + +# the world planners hold where the machine is: a machine jog after the +# frame jogs starts from there +frame(linuxcnc.JOG_FRAME_MACHINE) +jog_incr(X, 2, 100, [2, 0, 0], "machine X +2 after the plane jogs") + +# a plane whose X runs mostly along the slow Y: the jog along it goes no +# faster than Y allows, and two frame axes jogged at once keep every world +# axis inside its own speed +mdi("G69", "G10 L2 P1 R0", "G54", "G68.2 X0 Y0 Z0 I60 J0 K0") +W = plane_axes() +teleop() +frame(linuxcnc.JOG_FRAME_PLANE) +vel, moved = cruise([X], 200, "plane X at 200") +if abs(abs(vel[Y]) - VEL_Y) > 1e-6 or abs(vel[X] - VEL_Y * W[0][0] / W[0][1]) > 1e-6: + error("plane X ran at %s, not at the speed Y allows" % fmt(vel)) +if abs(dot(unit(moved), W[0]) - 1) > 1e-9: + error("plane X moved along %s, not %s" % (fmt(unit(moved)), fmt(W[0]))) + +# a jog of plane Y asked for while plane X runs waits until X has stopped, +# then runs on its own +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, X, 200) +time.sleep(0.3) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, 200) +time.sleep(0.3) +vel = [hal.get_value("axis.%s.teleop-vel-cmd" % l) for l in "xyz"] +print("plane X with plane Y waiting: velocity %s" % fmt(vel)) +if abs(dot(unit(vel), W[0]) - 1) > 1e-9: + error("plane X with plane Y asked for ran along %s, not %s" % (fmt(unit(vel)), fmt(W[0]))) +c.jog(linuxcnc.JOG_STOP, 0, X) +time.sleep(0.4) +vel = [hal.get_value("axis.%s.teleop-vel-cmd" % l) for l in "xyz"] +print("plane Y after plane X stopped: velocity %s" % fmt(vel)) +if abs(dot(unit(vel), W[1]) - 1) > 1e-9 or abs(abs(vel[Y]) - VEL_Y) > 1e-6: + error("plane Y after plane X ran at %s, not along %s at the speed Y allows" % (fmt(vel), fmt(W[1]))) +c.jog(linuxcnc.JOG_STOP, 0, Y) +settled() +drain() + +# the jog stops where the turned axis meets the box, without tripping a +# soft limit, and goes no further out from there; plane Y asked for at +# once waits, and from the box has nowhere to go +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, X, 200) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, 200) +time.sleep(0.2) +at_box = settled() +c.jog(linuxcnc.JOG_STOP, 0, X) +c.jog(linuxcnc.JOG_STOP, 0, Y) +print("ran into the box at %s" % fmt(at_box[:3])) +s.poll() +if not 99.9 < at_box[Y] < 100 or s.task_state != linuxcnc.STATE_ON: + error("the jog along plane X ended at Y %.6f, task state %d" % (at_box[Y], s.task_state)) +moved = [at_box[i] - before[i] for i in range(3)] +if abs(dot(unit(moved), W[0]) - 1) > 1e-9: + error("the jog to the box went along %s, not %s" % (fmt(unit(moved)), fmt(W[0]))) +drain() +jog_incr(X, 1, 100, [0, 0, 0], "plane X +1 out of the box") +jog_incr(X, -1, 100, [-p for p in W[0]], "plane X -1 back in") + +# the frame stays while a jog is under way +c.jog(linuxcnc.JOG_CONTINUOUS, 0, Y, -20) +time.sleep(0.2) +c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) +c.wait_complete(30) +time.sleep(0.1) +said = drain("cannot change while jogging") +s.poll() +if s.jog_frame != linuxcnc.JOG_FRAME_PLANE: + error("the jog frame changed to %d while jogging" % s.jog_frame) +if not any("cannot change while jogging" in m for m in said): + error("no word of the refused change") +c.jog(linuxcnc.JOG_STOP, 0, Y) +settled() +drain() + +# the tool frame, the head laid over and turned: X, Y and Z of the tool as +# the kinematics reports it. The carriage goes where the tip, swung out +# by the head, stays inside the short Y travel +mdi("G69", "G53.7 G0 J0=0 J1=-40 J2=0 J3=30 J4=45") +teleop() +frame(linuxcnc.JOG_FRAME_TOOL) +T = tool_axes() +print("tool axes in world: X %s, Y %s, Z %s" % tuple(fmt(v) for v in T)) +jog_incr(Z, 5, 100, [5 * p for p in T[2]], "tool Z +5") +jog_incr(X, 5, 100, [5 * p for p in T[0]], "tool X +5") +jog_incr(Y, -4, 40, [-4 * p for p in T[1]], "tool Y -4") + +# a plane in effect leaves the tool frame alone: the jogs still follow the +# frame the kinematics reports, not the plane's axes +before = tool_axes() +mdi("G68.2 X0 Y0 Z0 I60 J0 K0") +W = plane_axes() +teleop() +T = tool_axes() +print("tool axes under the plane: X %s, Y %s, Z %s" % tuple(fmt(v) for v in T)) +if any(abs(p - q) > 1e-9 for a, b in zip(T, before) for p, q in zip(a, b)): + error("the plane changed the tool frame in status") +if abs(dot(T[0], W[0])) > 1 - 1e-6: + error("the plane's X lies along the tool's X, which tells the frames apart nowhere") +jog_incr(X, 5, 100, [5 * p for p in T[0]], "tool X +5 under the plane") +jog_incr(Y, 4, 40, [4 * p for p in T[1]], "tool Y +4 under the plane") +jog_incr(Z, -5, 100, [-5 * p for p in T[2]], "tool Z -5 under the plane") + +# under the identity type a tool jog moves the joints as it does under the +# primary type, along the tool as the head stands, as Fanuc's tool axis +# direction feed and Heidenhain's Manual Operation Tool Axis move without +# the tool centre point; status has where the tool points against the +# part, the primary type's frame at the same joints, for a display +def joint_jog(axis, dist, what): + s.poll() + before = list(s.joint_position[:s.joints]) + c.jog(linuxcnc.JOG_INCREMENT, 0, axis, 100 if dist > 0 else -100, abs(dist)) + settled() + s.poll() + moved = [p - q for p, q in zip(s.joint_position[:s.joints], before)] + print("%s: joints moved %s" % (what, fmt(moved))) + drain() + return moved + +mdi("G69") +teleop() +on_primary = tool_axes() +primary_moves = [] +for axis, name in ((Z, "Z"), (X, "X")): + primary_moves.append(joint_jog(axis, 2, "tool %s +2 under the primary type" % name)) + joint_jog(axis, -2, "and back") +mdi("G12.1 P1") +teleop() +got = tool_axes() +if any(abs(p - q) > 1e-9 for a, b in zip(got, on_primary) for p, q in zip(a, b)): + error("under the identity type the tool frame is %s, the primary type's %s" + % (" ".join(fmt(v) for v in got), " ".join(fmt(v) for v in on_primary))) +for (axis, name), want in zip(((Z, "Z"), (X, "X")), primary_moves): + moved = joint_jog(axis, 2, "tool %s +2 under the identity type" % name) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("tool %s +2 under the identity type moved the joints %s, under the primary type %s" + % (name, fmt(moved), fmt(want))) +# with the head straight this would show nothing +if abs(primary_moves[0][2] - 2) < 1e-3: + error("the head stands straight, the tool Z jog moved the joints %s" % fmt(primary_moves[0])) + +# halui selects the frame and says which is in force +for name, which in (("plane", linuxcnc.JOG_FRAME_PLANE), ("machine", linuxcnc.JOG_FRAME_MACHINE), + ("tool", linuxcnc.JOG_FRAME_TOOL)): + hal.set_p("halui.jog-frame.%s" % name, "1") + deadline = time.time() + 5 + while time.time() < deadline: + s.poll() + if s.jog_frame == which: + break + time.sleep(0.05) + hal.set_p("halui.jog-frame.%s" % name, "0") + time.sleep(0.2) + shown = [n for n in ("machine", "plane", "tool") if hal.get_value("halui.jog-frame.is-%s" % n)] + if s.jog_frame != which or shown != [name]: + error("halui.jog-frame.%s gave frame %d, halui shows %s" % (name, s.jog_frame, shown)) +drain() + +# every servo cycle of every jog above: no world axis over its own speed or +# acceleration, a frame jog's stop at the box included +time.sleep(0.5) +with open(LOG) as f: + lines = f.read().split("\n") +samples = [] +for line in lines[:-1]: + v = line.split() + if len(v) == 4: + samples.append([float(x) for x in v[1:]]) +print("%d samples" % len(samples)) +for i, l in enumerate("XYZ"): + vpeak = max(abs(v[i]) for v in samples) if samples else 0.0 + apeak = max((abs(samples[k][i] - samples[k - 1][i]) / SERVO + for k in range(1, len(samples))), default=0.0) + print("%s: velocity peak %.3f of %.3f, acceleration peak %.2f of %.2f" + % (l, vpeak, AXIS_VEL[i], apeak, AXIS_ACC[i])) + if vpeak > AXIS_VEL[i] * 1.001: + error("%s ran at %.3f, over its limit of %.3f" % (l, vpeak, AXIS_VEL[i])) + if apeak > AXIS_ACC[i] * 1.001: + error("%s accelerated at %.2f, over its limit of %.2f" % (l, apeak, AXIS_ACC[i])) +if not samples: + error("no samples") +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame/test-unhomed.py b/tests/twp-jog-frame/test-unhomed.py new file mode 100755 index 00000000000..b73217fe03c --- /dev/null +++ b/tests/twp-jog-frame/test-unhomed.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# Plane frame jogs on a machine not homed: see README. +import hal +import linuxcnc +import math +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1].strip()) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = list(s.position) + if s.inpos and not s.queue and now == last \ + and not hal.get_value("motion.jog-is-active"): + return now + last = now + time.sleep(0.05) + error("timed out waiting for the machine to stop") + return last + +def fmt(v): + return " ".join("%.6f" % p for p in v) + +def check(what, before, after, want): + moved = [after[i] - before[i] for i in range(3)] + print("%s: moved %s, want %s" % (what, fmt(moved), fmt(want))) + if max(abs(m - w) for m, w in zip(moved, want)) > 1e-5: + error("%s moved %s, not %s" % (what, fmt(moved), fmt(want))) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +# the plane's X runs at 45 degrees between X and Y +c.mdi("G68.2 X0 Y0 Z0 I45 J0 K0") +c.wait_complete(30) +settled() +drain() + +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) +time.sleep(0.1) +s.poll() +if s.motion_mode != linuxcnc.TRAJ_MODE_TELEOP: + error("motion did not enter teleop mode") +if any(s.homed[:3]): + error("a joint is homed, the test is empty") +c.jog_frame(linuxcnc.JOG_FRAME_PLANE) +c.wait_complete(30) + +h = math.sqrt(0.5) +# [AXIS_X] and [AXIS_Y] are -10 to 10: homed, this jog would end outside +# them and be refused; not homed, an axis has one range, 20, either way +# of where it stands, as a plain jog of it does +s.poll() +before = list(s.position) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 50, 20) +after = settled() +check("an incremental plane X jog of 20", before, after, [20 * h, 20 * h, 0]) +drain() + +# a continuous jog runs to one range from where it starts, X and Y alike +before = after +c.jog(linuxcnc.JOG_CONTINUOUS, 0, 0, 50) +time.sleep(1.5) +c.jog(linuxcnc.JOG_STOP, 0, 0) +after = settled() +check("a continuous plane X jog", before, after, [20, 20, 0]) +drain() + +c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) +c.wait_complete(30) +c.state(linuxcnc.STATE_OFF) +c.wait_complete(30) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-jog-frame/test.ini b/tests/twp-jog-frame/test.ini new file mode 100644 index 00000000000..7e71784a02c --- /dev/null +++ b/tests/twp-jog-frame/test.ini @@ -0,0 +1,146 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 100 +HALUI = halui + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +# Y is slow and its travel short: a jog along a turned axis must keep each +# world axis inside its own speed and its own travel +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 400 +MAX_ACCELERATION = 1600 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/twp-jog-frame/test.sh b/tests/twp-jog-frame/test.sh new file mode 100755 index 00000000000..7be445fdd94 --- /dev/null +++ b/tests/twp-jog-frame/test.sh @@ -0,0 +1,16 @@ +#!/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 +# again on the S-curve planner +sed -e 's/^MAX_LINEAR_ACCELERATION = 1600$/&\nPLANNER_TYPE = 1\nMAX_LINEAR_JERK = 10000/' \ + -e 's/^MAX_ACCELERATION = .*$/&\nMAX_JERK = 10000/' test.ini > scurve.ini +rm -f sim.var sim.var.bak samples.log +linuxcnc -r scurve.ini +rm -f scurve.ini +# plane jogs on a machine not homed, identity kinematics +rm -f sim.var sim.var.bak +linuxcnc -r unhomed.ini +# machine frame jogs on a trunnion under the tool centre point +rm -f sim.var sim.var.bak +linuxcnc -r table.ini diff --git a/tests/twp-jog-frame/tool.tbl b/tests/twp-jog-frame/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/twp-jog-frame/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/twp-jog-frame/unhomed.hal b/tests/twp-jog-frame/unhomed.hal new file mode 100644 index 00000000000..45563353e5e --- /dev/null +++ b/tests/twp-jog-frame/unhomed.hal @@ -0,0 +1,13 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-jog-frame/unhomed.ini b/tests/twp-jog-frame/unhomed.ini new file mode 100644 index 00000000000..4df40d61f96 --- /dev/null +++ b/tests/twp-jog-frame/unhomed.ini @@ -0,0 +1,83 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-unhomed.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +# identity kinematics: the one kind that enters teleop with the joints +# not homed +[KINS] +KINEMATICS = trivkins coordinates=xyz +JOINTS = 3 + +[HAL] +HALFILE = unhomed.hal + +[TRAJ] +COORDINATES = XYZ +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 1600 +DEFAULT_LINEAR_ACCELERATION = 1600 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Y] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[AXIS_Z] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 200 +MAX_ACCELERATION = 1600 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +HOME_SEQUENCE = 0 diff --git a/tests/twp-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..6b4f65fb070 --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,722 @@ +#!/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: the rotaries come in the +# order the type orients with, the A table, then B and C, and a move to +# them is a move to the present B and C, with the table held +before3 = mdi("G0 B0 C0") +after3, samples = sampled("G0 B#<_orient_rot2> C#<_orient_rot3>") +show("G0 to #<_orient_rot2/rot3>", after3) +drain() +if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#<_orient_rot2> #<_orient_rot3> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): + error("the tool axis at the pose G53.2 published is not the plane normal") +# 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() +# the axes the type orients with, and the first of them, the table, where +# it stands; the numbered parameters past the rotaries read 0 +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>" + " #<_orient_rot1> #5074 #5077 #5078 #5079)") +c.wait_complete(30) +deadline = time.time() + 5 +said = None +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading the orienting axes after G53.2: %s" % m[1]) + break + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +want = [3, 4, 5, 0, 1, 1, stay[TABLE], stay[TABLE], 0, 0, 0] +if said is None or len(said) != len(want) or not close(said, want, 1e-3): + error("the orienting axes, rot1, #5074 and #5077-#5079 read %s, not %s" % (said, want)) +drain() + +# --- the orientation stays inside the rotary travel -------------------- +# 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-static/README b/tests/twp-static/README new file mode 100644 index 00000000000..0c86a38a291 --- /dev/null +++ b/tests/twp-static/README @@ -0,0 +1,44 @@ +3+2 under the machine frame kinematics, Fanuc's second feature coordinate +system, on a table machine (xyzac-trt-kins, table.ini) and a head machine +(5axiskins, head.ini). + +The same program runs under the TCP kinematics and under the machine frame +kinematics (G13.1), from the same joints: a plane, G53.1, G53.6 or G68.3, +then moves and an arc in the plane, with no tool, a tool given before the +plane and a tool given in it. The machine goes to the same joints every +time: under G13.1 the plane is on the part as the TCP kinematics places it, +with the tool along the tilted tool. + +Then, under G13.1 only: a C word moves C alone and the frame stays where it +was taken (the slides do not move and a move to the same point stays put); +G68.4 with nothing in it after the turn lands where the TCP kinematics does +after the same turn; G53 goes to machine coordinates and G69 leaves plain +machine moves; a switch to the TCP kinematics and back in the plane keeps +the plane on the part. + +Status keeps the plane as defined, the same under the TCP kinematics and +under G13.1, and the second feature coordinate system apart, the identity +except under G13.1; a plane X jog under G13.1 goes where G1 X+5 in the plane +does. + +table.ini sets RETAIN_WORK_PLANE: a program that goes on to another plane is +aborted during its first move, and the plane the machine was in comes back +onto the part it was defined on. + +Last the preview: after a plane set in MDI, with the tool in the spindle, +after a plane set in MDI and a rotary turned since, and with a plane in the +program itself, the preview's interpreter ends where the machine does. What +the preview draws is on the part, as under the TCP kinematics: the same +program draws one plane in the same place, and ends its cut at the same +point, under G13.1 as under the TCP kinematics. The live plot is on the +part too: under G13.1 in the plane it is where the TCP kinematics plots the +same point, with the tool frame in status the TCP kinematics', and switches +to the TCP kinematics and back neither move it nor draw a line. + +With no plane, a rotary turned under G13.1 at a tilted head or table: the +preview draws the turn through the point the machine reaches half way and +ends where it ends, and the live plot and status part_position agree, the +TCP kinematics reading the same point from the same joints. A switch to +the TCP kinematics and one to G13.1, at a tilt and with no plane: the +preview reads the point back from the joints as the machine does, and the +move after the switch ends where the machine ends it. diff --git a/tests/twp-static/checkresult b/tests/twp-static/checkresult new file mode 100755 index 00000000000..a9e60354d4f --- /dev/null +++ b/tests/twp-static/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-static/head.hal b/tests/twp-static/head.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-static/head.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-static/head.ini b/tests/twp-static/head.ini new file mode 100644 index 00000000000..e1b24563c23 --- /dev/null +++ b/tests/twp-static/head.ini @@ -0,0 +1,131 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, 1 is identity, 2 is the userk template +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = head.hal +HALCMD = setp 5axiskins.pivot-length 100 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/twp-static/table.hal b/tests/twp-static/table.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/twp-static/table.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-static/table.ini b/tests/twp-static/table.ini new file mode 100644 index 00000000000..952c511f610 --- /dev/null +++ b/tests/twp-static/table.ini @@ -0,0 +1,123 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var +RETAIN_WORK_PLANE = 1 + +[KINS] +KINEMATICS = xyzac-trt-kins sparm=identityfirst +JOINTS = 5 + +[HAL] +HALFILE = table.hal +HALCMD = setp xyzac-trt-kins.x-rot-point 100 +HALCMD = setp xyzac-trt-kins.y-rot-point -50 +HALCMD = setp xyzac-trt-kins.z-rot-point -80 + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 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-static/test-ui.py b/tests/twp-static/test-ui.py new file mode 100755 index 00000000000..1337bd9594e --- /dev/null +++ b/tests/twp-static/test-ui.py @@ -0,0 +1,503 @@ +#!/usr/bin/env python3 +# 3+2 under the machine frame type, the second feature coordinate system: +# see README. + +import linuxcnc +import sys +import os +import time +import threading +import collections +import gcode +import preview_helpers +import numpy as np +from rs274 import glcanon_bake +from rs274.interpret import Translated + +ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) +HEAD = ini.find("KINS", "KINEMATICS").startswith("5axiskins") +JOINTS = int(ini.find("KINS", "JOINTS")) +# the type that follows the part, and the rotaries' letters +TCP = "G12.1 P0" if HEAD else "G12.1 P1" +FIRST, SECOND = ("B", "C") if HEAD else ("A", "C") +START = "G53 G0 X20 Y-10 Z120 B0 C0" if HEAD else "G53 G0 X20 Y-10 Z120 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(expect_error=False): + got = False + while True: + m = e.poll() + if not m: + return got + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + got = True + if not expect_error: + error("reported: %s" % m[1]) + +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) + j = settled() + drain() + return j + +def run(select, steps): + # from one machine point, on the type asked for, the joints after each step + for cmd in ("G49", "G69", "G13.1", START, select): + mdi(cmd) + return [mdi(cmd) for cmd in steps] + +def same(a, b, n=None): + n = JOINTS if n is None else n + return max(abs(p - q) for p, q in zip(a[:n], b[:n])) < 1e-6 + +def show(j): + return " ".join("%.6f" % v for v in j) + +# the same program under the type that follows the part and under the +# machine frame type: the machine goes to the same joints +plane = "G68.2 X10 Y-5 Z0 I60 J30 K0" +cut = ["G0 X10 Y5 Z20", "G1 X-10 F2000", "G2 X10 R10 F2000", "G1 Z-5"] +programs = { + "no tool": [plane, "G53.1"] + cut, + "tool first": ["G43 H1", plane, "G53.1"] + cut, + "tool in the plane": [plane, "G53.1", "G43 H1"] + cut, + "G53.6": [plane, "G0 X0 Y0 Z30", "G53.6"] + cut, + "G68.3": [plane, "G53.1", "G69", "G68.3 X10 Y-5 Z0 R20"] + cut, +} +for name, steps in programs.items(): + tcp = run(TCP, steps) + fixed = run("G13.1", steps) + for cmd, a, b in zip(steps, tcp, fixed): + if not same(a, b): + error("%s: after %s the machine frame type is at %s, the part's type at %s" + % (name, cmd, show(b), show(a))) + print("%s: %s" % (name, show(fixed[-1]))) + +# a rotary word moves the rotary alone and the frame stays where the +# plane was taken (Fanuc); a new plane at the turned table follows it +steps = [plane, "G53.1", "G0 X10 Y5 Z20", "G91", "G0 %s30" % SECOND, "G90", + "G0 X10 Y5 Z20"] +fixed = run("G13.1", steps) +if not same(fixed[2], fixed[4], 3): + error("a %s word moved the slides: %s to %s" % (SECOND, show(fixed[2]), show(fixed[4]))) +if not same(fixed[2], fixed[6], 3): + error("after a %s word the frame moved: %s to %s" % (SECOND, show(fixed[2]), show(fixed[6]))) +tcp = run(TCP, [plane, "G53.1", "G91", "G0 %s30" % SECOND, "G90", "G1 X-10 Y5 Z20 F2000"]) +fixed = run("G13.1", [plane, "G53.1", "G91", "G0 %s30" % SECOND, "G90", + "G68.4 X0 Y0 Z0 I0 J0 K0", "G1 X-10 Y5 Z20 F2000"]) +if not same(tcp[-1], fixed[-1]): + error("G68.4 after a %s word: the machine frame type is at %s, the part's type at %s" + % (SECOND, show(fixed[-1]), show(tcp[-1]))) + +# G53 stays in the machine frame, and G69 leaves plain machine moves +fixed = run("G13.1", [plane, "G53.1", "G53 G0 X0 Y0 Z100", "G69", "G0 X10 Y0 Z50"]) +if not same(fixed[2], [0, 0, 100], 3): + error("G53 in the plane went to %s, not X0 Y0 Z100" % show(fixed[2])) +if not same(fixed[4], [10, 0, 50], 3): + error("G0 after G69 went to %s, not X10 Y0 Z50" % show(fixed[4])) + +# the type can change in the plane: on to the part's type and back +tcp = run(TCP, [plane, "G53.1", "G1 X-10 Y5 Z20 F2000"]) +fixed = run("G13.1", [plane, "G53.1", TCP, "G13.1", "G1 X-10 Y5 Z20 F2000"]) +if not same(tcp[-1], fixed[-1]): + error("a switch in the plane: %s, the part's type at %s" % (show(fixed[-1]), show(tcp[-1]))) + +# Status has the plane as defined under every type, and the second feature +# coordinate system apart, as Siemens keeps its workpiece reference frame +# apart from its table and tool ones: a switch changes only the latter. +IDENTITY = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) + +def plane_status(): + s.poll() + return (tuple(s.g68_offset[:3]), tuple(s.g68_rotation), + tuple(s.g68_kins_offset[:3]), tuple(s.g68_kins_rotation)) + +def near(a, b): + return max(abs(p - q) for p, q in zip(a, b)) < 1e-9 + +run(TCP, [plane, "G53.1"]) +on_tcp = plane_status() +mdi("G13.1") +on_fixed = plane_status() +mdi(TCP) +back = plane_status() +if not near(on_tcp[0], (10, -5, 0)): + error("status has the plane's origin at %s, not X10 Y-5 Z0" % show(on_tcp[0])) +for name, got in (("under G13.1", on_fixed), ("back on the part's type", back)): + if not near(got[0], on_tcp[0]) or not near(got[1], on_tcp[1]): + error("%s status has the plane at %s %s, not %s %s" + % (name, show(got[0]), show(got[1]), show(on_tcp[0]), show(on_tcp[1]))) +for name, got in (("on the part's type", on_tcp), ("back on the part's type", back)): + if not near(got[2], (0, 0, 0)) or not near(got[3], IDENTITY): + error("%s status has a second feature coordinate system %s %s" + % (name, show(got[2]), show(got[3]))) +if near(on_fixed[2], (0, 0, 0)) and near(on_fixed[3], IDENTITY): + error("under G13.1 status has no second feature coordinate system") + +# The plane jog frame under G13.1 is the plane turned by the second feature +# coordinate system: a jog along the plane's X goes where G1 X+5 does. +def jog_plane_x(): + c.mode(linuxcnc.MODE_MANUAL) + c.wait_complete(30) + c.teleop_enable(1) + c.wait_complete(30) + c.jog_frame(linuxcnc.JOG_FRAME_PLANE) + c.wait_complete(30) + c.jog(linuxcnc.JOG_INCREMENT, False, 0, 50, 5) + time.sleep(0.3) + after = settled() + drain() + c.jog_frame(linuxcnc.JOG_FRAME_MACHINE) + c.wait_complete(30) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + return after + +cut_x = run("G13.1", [plane, "G53.1", "G0 X10 Y5 Z20", "G91 G1 X5 F2000", "G90"])[-1] +run("G13.1", [plane, "G53.1", "G0 X10 Y5 Z20"]) +jogged = jog_plane_x() +if not same(jogged, cut_x): + error("a plane X jog under G13.1 ended at %s, G1 X+5 at %s" % (show(jogged), show(cut_x))) + +# An abort with RETAIN_WORK_PLANE keeps the plane the machine was in, and +# its second feature coordinate system, while the read ahead has gone on +# to another: the machine's plane comes back onto the part it was +# defined on. +if ini.find("RS274NGC", "RETAIN_WORK_PLANE") == "1": + want = run("G13.1", [plane, "G53.1", "G1 X-10 Y5 Z20 F2000"])[-1] + run("G13.1", [plane, "G53.1", "G1 X10 Y5 Z20 F2000"]) + with open("abort.ngc", "w") as f: + f.write("G1 X-200 F60\nG69\nG68.2 X0 Y0 Z0 I0 J45 K0\nG1 X0\nM2\n") + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete() + c.program_open("abort.ngc") + c.wait_complete() + c.auto(linuxcnc.AUTO_RUN, 0) + time.sleep(2.0) + c.abort() + c.wait_complete() + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue: + break + time.sleep(0.05) + drain() + c.mode(linuxcnc.MODE_MDI) + c.wait_complete() + got = mdi("G1 X-10 Y5 Z20 F2000") + if not same(got, want): + error("after an abort the plane put X-10 Y5 Z20 at %s, not %s" % (show(got), show(want))) + os.unlink("abort.ngc") + +# The preview's interpreter: a program after a plane set in MDI starts +# from the plane as status has it, folded into the second feature +# coordinate system, and one with its own plane takes its own; through the +# plane canon applies, which a callback canon gets, both end where the +# machine does, without the tool offset. +class Machine: + # what a preview reads off the machine + def get_tool(self, pocket): + return tuple(s.tool_table[pocket]) if 0 <= pocket < len(s.tool_table) else (-1,) + (0,) * 13 + def get_external_length_units(self): + return s.linear_units + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return s.axis_mask + def get_block_delete(self): + return 0 + def get_external_joint_positions(self): + return tuple(s.joint_actual_position[:s.joints]) + def get_external_g68_kins_frame(self): + return (tuple(s.g68_kins_offset[:3]), tuple(s.g68_kins_rotation)) + def get_kinematics_type(self): + return s.kinematics_type + def get_kins_type(self): + return s.kins_type + def get_kins_type_flags(self): + return s.kins_type_flags + +class Canon(Machine, Translated): + def __init__(self): + self.parameter_file = "preview.var" + self.end = None + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def straight_feed(self, *a): + self.end = self.rotate_and_translate(*a)[:3] + +def preview_case(name, prog, text): + with open(prog, "w") as f: + f.write(text) + s.poll() + canon = Canon() + res, seq = gcode.parse(prog, canon, *preview_helpers.create_unitcode_and_initcode(s, ini)) + if res > gcode.MIN_ERROR: + error("%s: the preview stops at line %d: %s" % (name, seq, gcode.strerror(res).strip())) + return + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete() + c.program_open(prog) + c.wait_complete() + c.auto(linuxcnc.AUTO_RUN, 0) + c.wait_complete() + time.sleep(0.3) + settled() + deadline = time.time() + 60 + while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE: + break + time.sleep(0.05) + drain() + c.mode(linuxcnc.MODE_MDI) + c.wait_complete() + s.poll() + machine = [s.position[i] - s.tool_offset[i] for i in range(3)] + # the preview works in inches + end = [25.4 * v for v in canon.end] + if max(abs(p - q) for p, q in zip(end, machine)) > 1e-6: + error("%s: the preview ends at %s, the machine at %s" % (name, end, machine)) + os.unlink(prog) + +# the tool in the spindle, so that the G43 the preview restates has its length +for cmd in ("G49", "G69", "G13.1", START, "T1 M6", "G43", plane, "G53.1"): + mdi(cmd) +preview_case("plane from MDI", "mdi-plane.ngc", + "G1 X-10 Y5 Z20 F2000\nG1 Z-5\nM2\n") +# a rotary turned since the plane was set keeps the second feature +# coordinate system it was cut through, which the preview takes from status +for cmd in ("G49", "G69", "G13.1", START, "T1 M6", "G43", plane, "G53.1", + "G91 G0 %s30" % SECOND, "G90"): + mdi(cmd) +preview_case("plane from MDI, a rotary turned since", "mdi-turned.ngc", + "G1 X-10 Y5 Z20 F2000\nG1 Z-5\nM2\n") +for cmd in ("G49", "G69", "G13.1", START): + mdi(cmd) +preview_case("plane in the program", "own-plane.ngc", + "G43 H1\n%s\nG53.1\nG1 X-10 Y5 Z20 F2000\nG1 Z-5\nM2\n" % plane) + +# What the preview draws is on the part, as under the part's type, where +# the program cuts: the plane as defined and the moves in it, not the +# slides that cut them. The same program, under G13.1 and under the part's +# type, draws one plane in the same place and ends at the same point. +# The program as the renderer hands it to a GUI, with no display: a +# headless build has no OpenGL. +Plane = collections.namedtuple("Plane", "origin axes") + +class Rendered(Machine, gcode.RendererCanon): + COLORS = {"dwell": (1, 0, 0, 1), "m1xx": (0, 0, 1, 1)} + def __init__(self): + self.program_geometry = glcanon_bake.ProgramGeometry(geometry="XYZ") + self.parameter_file = "preview.var" + self.workplanes = [] + def adopt_geometry(self, program): + self.program_geometry.adopt(program, self.COLORS) + self.workplanes = [Plane(origin, axes) + for _line, origin, axes, _lo, _hi in program.workplanes()] + def comment(self, text): pass + def message(self, text): pass + def check_abort(self): return False + def next_line(self, state): pass + def change_tool(self, pocket): pass + +def rendered(name, text, axes=False): + prog = "rendered.ngc" + with open(prog, "w") as f: + f.write(text) + s.poll() + canon = Rendered() + canon.program_geometry.configure(want_axis_positions=axes) + res, seq = gcode.parse(prog, canon, *preview_helpers.create_unitcode_and_initcode(s, ini)) + os.unlink(prog) + if res > gcode.MIN_ERROR: + error("%s: the preview stops at line %d: %s" % (name, seq, gcode.strerror(res).strip())) + return None + return canon + +def close(a, b): + # the drawn points are single precision + return max(abs(p - q) for p, q in zip(a, b)) < 1e-5 + +for cmd in ("G49", "G69", "G13.1", START): + mdi(cmd) +body = "G43 H1\n%s\nG53.1\nG1 X-10 Y5 Z20 F2000\nG1 Z-5\nM2\n" % plane +on_part = rendered("drawn under the part's type", TCP + "\n" + body) +on_g131 = rendered("drawn under G13.1", "G13.1\n" + body) +if on_part and on_g131: + a, b = on_part.workplanes, on_g131.workplanes + if len(a) != 1 or len(b) != 1: + error("the preview draws %d plane(s) under the part's type and %d under G13.1, not one" + % (len(a), len(b))) + elif not close(a[0].origin, b[0].origin) or not all(close(u, v) for u, v in zip(a[0].axes, b[0].axes)): + error("the preview draws the plane under G13.1 at %s %s, under the part's type at %s %s" + % (show(b[0].origin), b[0].axes, show(a[0].origin), a[0].axes)) + end_part = on_part.program_geometry.positions()[-1] + end_g131 = on_g131.program_geometry.positions()[-1] + if not close(end_part, end_g131): + error("the preview ends the cut under G13.1 at %s, under the part's type at %s" + % (show(end_g131), show(end_part))) + +# The live plot is on the part too, so the tool marker that stands at its +# end is: under G13.1 in the plane the point the machine plots is the one +# the part's type plots at the same joints, and a switch to that type and +# back moves it nowhere and draws no line, at the moment the type has +# changed and the plane's map not yet too. +lp = linuxcnc.positionlogger(linuxcnc.stat(), (255, 255, 0, 255), (255, 0, 0, 255), + (0, 255, 0, 255), (0, 0, 255, 255), (255, 255, 255, 255), + (0, 255, 255, 255), "XYZ", 0) +sampler = threading.Thread(target=lp.start, args=(0.01,), daemon=True) +sampler.start() + +def plotted(): + time.sleep(0.2) + return lp.last(0)[:3] + +def logged(): + raw, n, xyuv = lp.points() + return np.frombuffer(raw, dtype=glcanon_bake.LOGGER_DTYPE, count=n) + +run(TCP, [plane, "G53.1", "G1 X-10 Y5 Z20 F2000"]) +on_part = plotted() +s.poll() +tool_on_part = s.tool_frame +run("G13.1", [plane, "G53.1", "G1 X-10 Y5 Z20 F2000"]) +on_g131 = plotted() +if max(abs(p - q) for p, q in zip(on_part, on_g131)) > 1e-4: + error("under G13.1 the live plot is at %s, under the part's type at %s" + % (show(on_g131), show(on_part))) +# and the tool marker stands where the part's type has the tool against +# the part +s.poll() +if s.tool_frame is None or tool_on_part is None \ + or max(abs(p - q) for p, q in zip(s.tool_frame, tool_on_part)) > 1e-9: + error("under G13.1 status has the tool frame %s, under the part's type %s" + % (s.tool_frame, tool_on_part)) +start = len(logged()) +for i in range(3): + mdi(TCP) + mdi("G13.1") +back = plotted() +if max(abs(p - q) for p, q in zip(on_g131, back)) > 1e-4: + error("a switch to the part's type and back moved the live plot from %s to %s" + % (show(on_g131), show(back))) +for q in logged()[max(start - 1, 0):]: + if np.abs(q['pos'] - np.asarray(back)).max() > 1e-3: + error("a switch in the plane drew a line in the live plot to %s" % show(q['pos'])) + break + +# With no plane, under G13.1 alone, the preview and the live plot are on +# the part as well: the slides put the tool on the part where the part's +# type reads the joints. A rotary turned under G13.1 sweeps the tool round +# the part, which the preview draws through the points the machine passes. +def on_part_now(): + # status, then the live plot, where the tool stands against the part + s.poll() + status = [s.part_position[i] - s.tool_offset[i] for i in range(3)] + plot = plotted() + if max(abs(p - q) for p, q in zip(status, plot)) > 1e-3: + error("the live plot is at %s, status has the tool on the part at %s" + % (show(plot), show(status))) + return status + +def drawn_at(canon, where, letter, value): + # the vertex drawn where the axis stands at a value, in machine units + pg = canon.program_geometry + turned = np.abs(pg.axis_position(letter) - value) < 1e-4 + turned &= pg.kinds != glcanon_bake.KIND_NOOP + if not turned.any(): + error("%s: the preview draws no point at %s%g" % (where, letter, value)) + return None + return [25.4 * v for v in pg.positions()[np.nonzero(turned)[0][-1]]] + +turn = ["G43 H1", "G0 %s30 %s20" % (FIRST, SECOND), "G1 X10 Y5 Z20 F2000", + "G1 %s60 F2000" % SECOND] +for cmd in ("G49", "G69", "G13.1", START): + mdi(cmd) +canon = rendered("a rotary turned under G13.1", "\n".join(turn) + "\nM2\n", axes=True) +run("G13.1", turn[:-1] + ["G1 %s40 F2000" % SECOND]) +middle = on_part_now() +mdi(TCP) +s.poll() +tcp = [s.position[i] - s.tool_offset[i] for i in range(3)] +if max(abs(p - q) for p, q in zip(middle, tcp)) > 1e-6: + error("status has the tool on the part at %s under G13.1, the part's type at %s" + % (show(middle), show(tcp))) +mdi("G13.1") +mdi("G1 %s60 F2000" % SECOND) +end = on_part_now() +if canon: + for where, value, want in (("half way round", 40, middle), ("at the end", 60, end)): + got = drawn_at(canon, "a rotary turned under G13.1, " + where, SECOND, value) + if got and max(abs(p - q) for p, q in zip(got, want)) > 1e-3: + error("a rotary turned under G13.1, %s: the preview draws %s, the machine is at %s" + % (where, show(got), show(want))) + +# A switch moves no joint: the preview's interpreter reads the point back +# from the joints under the new type, as the machine does, and goes on +# from there, with a rotary turned and no plane, both ways. +tilt = "G0 %s30 %s20" % (FIRST, SECOND) +switches = { + "a switch to the part's type": ["G43 H1", tilt, "G1 X10 Y5 Z20 F2000", TCP, + "G91 G1 X5", "G90"], + "a switch to G13.1": [TCP, "G43 H1", tilt, "G1 X10 Y5 Z20 F2000", "G13.1", "G1 X5"], +} +for name, lines in switches.items(): + for cmd in ("G49", "G69", "G13.1", START): + mdi(cmd) + canon = rendered(name, "\n".join(lines) + "\nM2\n") + run("G13.1", lines) + end = on_part_now() + if canon: + got = [25.4 * v for v in canon.program_geometry.positions()[-1]] + if max(abs(p - q) for p, q in zip(got, end)) > 1e-3: + error("after %s the preview ends at %s, the machine at %s" + % (name, show(got), show(end))) +lp.stop() +sampler.join(5) + +mdi("G69") +mdi("G49") + +for f in ("sim.var", "sim.var.bak", "preview.var"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-static/test.sh b/tests/twp-static/test.sh new file mode 100755 index 00000000000..74e7d511fc3 --- /dev/null +++ b/tests/twp-static/test.sh @@ -0,0 +1,8 @@ +#!/bin/bash -e +# a crash in the test script prints where it was, and what it printed before +export PYTHONFAULTHANDLER=1 PYTHONUNBUFFERED=1 +# a failed run leaves the var file behind, and it carries offsets +for ini in table.ini head.ini; do + rm -f sim.var sim.var.bak + linuxcnc -r $ini +done diff --git a/tests/twp-static/tool.tbl b/tests/twp-static/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-static/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 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