diff --git a/.gitignore b/.gitignore index 4e3ccbcb4c8..18eb2868130 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,8 @@ share/desktop-directories/linuxcnc-cnc.directory share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl +share/linuxcnc/switchkins.c +share/linuxcnc/kins_util.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 8ca0552431a..38e706fac22 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -41,7 +41,6 @@ CYCLE_TIME = 0.010 HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = 5axisgui.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = 5axis_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc index 4ab3aaf922d..e9529f6d0f8 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 genhexkins o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc index 54726d37a6c..0291e69889d 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 Identity kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc index 7586236a003..886fe727740 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/README b/configs/sim/axis/vismach/5axis/table-dual-rotary/README index 0a9f1130e42..29e0a4c88a4 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/README +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/README @@ -28,9 +28,6 @@ For proper tool-path preview RELOAD THE CGODE after startup and after changing o *********************************************** Note: IMPORTANT ini file requirements: -[HAL] -HALCMD = net :kinstype-select <= motion.analog-out-0N => motion.switchkins-type - [RS274NGC] SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc index 46e2d01ba5b..062edba961e 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ -;M428 by remap: kinstype==1 (xyzac,xyzbc) (note: sparm=identityfirst) +;M428 by remap: kinstype==1 (xyzab-tdr kinematics) o<428remap>sub - # = 1 ; xyzac,xyzbc - # = 3 ; set N as required: motion.analog-out-0N + # = 1 ; xyzab-tdr -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc index 3fa610c8ee0..ff81c491a6f 100644 --- a/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==0 Identity kinematics o<429remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub 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 339ed834a8e..bd6dc79e82a 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 @@ -25,8 +25,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: -# switchkins-type == 0 is identity kins -# switchkins-type == 1 is xyzab-tdr-kins +# kinstype 0 is identity kins +# kinstype 1 is xyzab-tdr-kins KINEMATICS = xyzab_tdr_kins JOINTS = 5 @@ -36,8 +36,6 @@ KINEMATICS = xyzab_tdr_kins HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzab-tdr-postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # Values '(x,z)-offsets' for geometric offset of the rotary-assembly and the # values '(x,y,z)-rot-point' that describe the position of the @@ -76,8 +74,8 @@ HALCMD = sets :x-offset -20 HALCMD = sets :z-offset -10 [HALUI] -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzab-tdr kins (motion.switchkins-type==1) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzab-tdr kins (kinstype 1) MDI_COMMAND = M429 MDI_COMMAND = M428 diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/README b/configs/sim/axis/vismach/5axis/table-rotary-tilting/README index 4166a10fc2a..b85e076dd16 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/README +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/README @@ -17,9 +17,6 @@ Demonstrations: *********************************************** Note: IMPORTANT ini file requirements: -[HAL] -HALCMD = net :kinstype-select <= motion.analog-out-0N => motion.switchkins-type - [RS274NGC] SUBROUTINE_PATH = ./remap_subs REMAP = M428 modalgroup=10 ngc=428remap diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc index 46e2d01ba5b..5255b230004 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: kinstype==1 (xyzac,xyzbc) (note: sparm=identityfirst) o<428remap>sub # = 1 ; xyzac,xyzbc - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc index 3fa610c8ee0..be20d5b06b7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==0 Identity kinematics o<429remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc index 65a82221335..6679a3080da 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow b/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow index ede94bdc5bd..014d9531eb7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/switchkins.halshow @@ -1,5 +1,4 @@ -pin+motion.analog-out-03 -pin+motion.switchkins-type +pin+motion.kins-type pin+joint.0.pos-cmd pin+joint.1.pos-cmd 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 94650238225..9c2bc00e33e 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 @@ -38,8 +38,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! -# default switchkins-type == 0 is xyzac-trt-kins -# here switchkins-type == 0 is identity kins +# default kinstype 0 is xyzac-trt-kins +# here kinstype 0 is identity kins KINEMATICS = xyzac-trt-kins sparm=identityfirst JOINTS = 5 @@ -48,8 +48,6 @@ KINEMATICS = xyzac-trt-kins sparm=identityfirst HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = switchkins_postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # vismach xyzac-trt-gui items HALCMD = loadusr -W ./xyzac-trt-gui.py @@ -73,9 +71,9 @@ HALCMD = setp xyzac-trt-kins.conventional-directions 0 [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzac kins (motion.switchkins-type==1) -# M430:userk kins (motion.switchkins-type==2) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzac kins (kinstype 1) +# M430:userk kins (kinstype 2) MDI_COMMAND = M429 MDI_COMMAND = M428 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt index acdbdebe6a5..7bc132e3159 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt @@ -6,11 +6,9 @@ Uses remapped user m codes for kins switch: M428: XYZAC (TCP) M430: userk Kinematics -A hal net is required to connect the -analog out pin N, Example (for N=3): - - net :kinstype-select <= motion.analog-out-03 - net :kinstype-select => motion.switchkins-type +The kinematics type is selected with +G12.1 and G13.1, no hal connection is +required. Hal Input pins: xyzac-trt-kins.y-offset 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 5a1524b7e2d..45d780a251c 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 @@ -38,8 +38,8 @@ SUBROUTINE_PATH = ./remap_subs [KINS] #NOTE: for backwrds compatibility !!!!!!!!!!!!!!!!!!! -# default switchkins-type == 0 is xyzbc-trt-kins -# here switchkins-type == 0 is identity kins +# default kinstype 0 is xyzbc-trt-kins +# here kinstype 0 is identity kins KINEMATICS = xyzbc-trt-kins sparm=identityfirst JOINTS = 5 @@ -48,8 +48,6 @@ KINEMATICS = xyzbc-trt-kins sparm=identityfirst HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = switchkins_postgui.hal -# net for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # vismach xyzbc-trt-gui items HALCMD = loadusr -W ./xyzbc-trt-gui.py @@ -73,9 +71,9 @@ HALCMD = setp xyzbc-trt-kins.conventional-directions 0 [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M429:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M428:xyzbc kins (motion.switchkins-type==1) -# M430:userk kins (motion.switchkins-type==2) +# M429:identity kins (kinstype 0, startupDEFAULT) +# M428:xyzbc kins (kinstype 1) +# M430:userk kins (kinstype 2) MDI_COMMAND = M429 MDI_COMMAND = M428 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt index 4641cf6da28..20595fb39c7 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt +++ b/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt @@ -6,11 +6,9 @@ Uses remapped user m codes for kins switch: M428: XYZBC (TCP) M430: userk Kinematics -A hal net is required to connect the -analog out pin N, Example (for N=3): - - net :kinstype-select <= motion.analog-out-03 - net :kinstype-select => motion.switchkins-type +The kinematics type is selected with +G12.1 and G13.1, no hal connection is +required. Hal Input pins: xyzbc-trt-kins.x-offset diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py index c8b15c9c4e9..05fc53261a1 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py @@ -1,7 +1,7 @@ # 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) 2023 David Mueller +# 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 @@ -13,7 +13,22 @@ # 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 @@ -23,7 +38,6 @@ from util import lineno, call_pydevd import hal - # logging import logging # this name will be printed first on each log message @@ -33,22 +47,41 @@ formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') handler.setFormatter(formatter) log.addHandler(handler) -# Manually force the log level for this module -log.setLevel(logging.ERROR) # One of DEBUG, INFO, WARNING, ERROR, CRITICAL - # 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) -## SPINDLE ROTARY JOINT LETTERS -# spindle primary joint +# 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() -# spindle secondary joint (ie the one closer to the tool) +# 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'): @@ -58,32 +91,28 @@ else: # get the MIN/MAX limits of the respective rotary joint letters category = 'AXIS_' + joint_letter_primary - primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - primary_max_limit = 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, primary_min_limit, primary_max_limit) + 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 = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - secondary_max_limit = 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, secondary_min_limit, secondary_max_limit) - - -## CONNECTIONS TO THE KINEMATIC COMPONENT -# get the name of the kinematic component -kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") -# name of the hal pin that represents the nutation-angle -kins_nutation_angle = kins_comp + '_kins.nut-angle' -# name of the hal pin that represents the pre-rotation -kins_pre_rotation = kins_comp + '_kins.pre-rot' -# name of the hal pin that represents the primary joint orientation angle -kins_primary_rotation = kins_comp + '_kins.primary-angle' -# name of the hal pin that represents the secondary joint orientation angle -kins_secondary_rotation = kins_comp + '_kins.secondary-angle' + 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 @@ -93,7 +122,7 @@ 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.x +# 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' @@ -105,592 +134,391 @@ 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 -# tool-z vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being +# 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 -# defines the kinematic model for (world <-> tool) coordinates of the machine at hand -# returns 4x4 transformation matrix for given angles and 4x4 input matrix -# NOTE: these matrices must be the same as the ones used to derive the kinematic model -def kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction='fwd'): - global joint_letter_primary, joint_letter_secondary - global kins_nutation_angle - T_in = matrix_in - - ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y - Stc = sin(pre_rot) - Ctc = cos(pre_rot) - Rtc=np.matrix([[ Ctc, -Stc, 0, 0], - [ Stc, Ctc, 0, 0], - [ 0 , 0 , 1, 0], - [ 0, 0 , 0, 1]]) - - ## Define 4x4 transformation for the primary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_primary == 'A': - Rp = Rx(theta_1) - elif joint_letter_primary == 'B': - Rp = Ry(theta_1) - elif joint_letter_primary == 'C': - Rp = Rz(theta_1) - # add fourth column on the right - Rp = np.hstack((Rp, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rp = np.vstack((Rp, row_4)) - Rp = np.asmatrix(Rp) - - ## Define 4x4 transformation matrix for the secondary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_secondary == 'A': - Rs = Rx(theta_2) - elif joint_letter_secondary == 'B': - Rs = Ry(theta_2) - elif joint_letter_secondary == 'C': - Rs = Rz(theta_2) - # add fourth column on the right - Rs = np.hstack((Rs, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rs = np.vstack((Rs, row_4)) - Rs = np.asmatrix(Rs) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], - [ Cv*Ss, r, t, 0], - [ -Sv*Ss, t, s, 0], - [ 0, 0, 0, 1]]) - - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ r, -Cv*Ss, t, 0], - [ Cv*Ss, Cs, -Sv*Ss, 0], - [ t, Sv*Ss, s, 0], - [ 0, 0, 0, 1]]) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - - # calculate the transformation matrix for the forward tool kinematic - matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in - # calculate the transformation matrix for the inverse tool kinematic - matrix_tool_inv = Rp*Rs*Rtc*T_in - if direction == 'fwd': - #log.debug("matrix tool fwd: \n", matrix_tool_fwd) - #log.debug("inv would have been: \n", matrix_tool_inv) - return matrix_tool_fwd - elif direction == 'inv': - #log.debug("matrix tool inv: \n", matrix_tool_inv) - #log.debug("fwd would have been: \n", matrix_tool_fwd) - return matrix_tool_inv - else: - return 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)]]) -# returns angle 'tc' required to rotate the x-axis of the tool-coords parallelto the machine-xy plane -# for given machine joint position angles. -# For G68.3 this is the default tool-x direction -# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic -def kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2): - global joint_letter_primary, joint_letter_secondary - # The idea is that the tool-x vector is parallel to the machine xy-plane when the - # z component of the x-direction vector is equal to zero - # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation - # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. - # this makes the x orientation of the tool coords horizontal and the user can set the - # rotation from there using g68.3 r - global kins_nutation_angle - v = radians(hal.get_value(kins_nutation_angle)) - Cv = cos(v) - Sv = sin(v) - Cs = cos(theta_2) - Ss = sin(theta_2) - Cp = cos(theta_1) - Sp = sin(theta_1) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - t = Sv*Cv*(1-Cs) - tc = atan2((Sv*Ss),t) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - t = Sv*Cv*(1-Cs) - tc = atan2(-t,(Sv*Ss)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - # note: tool-c rotation is done using a halpin that feeds into the kinematic component and the - # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) - return tc +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) -# calculates the secondary joint position for a given tool-vector -# secondary being the joint closest to the tool -# Note: this uses functions derived from the custom kinematic -def kins_calc_secondary(self, tool_z_req): - global joint_letter_primary, joint_letter_secondary - global secondary_min_limit, secondary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_2_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using acos() we really have two solutions theta_2 and -theta_2 - for theta in [theta_2, -theta_2]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_2_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_2_list - - -# calculates the primary joint position for a given tool-vector -# Note: this uses functions derived from the custom kinematic -def kins_calc_primary(self, tool_z_req, theta_2_list): - global joint_letter_primary, joint_letter_secondary - global primary_min_limit, primary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_1_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) - theta_1 = asin(q) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using asin() we really have two solutions theta_1 and pi-theta_2 - for theta in [theta_1, transform_to_pipi(pi - theta_1)]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_1_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_1_list - - -# this is from 'mika-s.github.io' -# transforms a given angle to the interval of [-pi,pi] -def transform_to_pipi(input_angle): - revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) - p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) - p2 = (np.sign(np.sign(input_angle) - + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi - output_angle = p1 - p2 - return output_angle - - -# this is from 'mika-s.github.io' -# used by 'transform_to_pipi()' -def truncated_remainder(dividend, divisor): - divided_number = dividend / divisor - divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) - remainder = dividend - divisor * divided_number - return remainder - - -# returns a list of valid primary/secondary spindle joint positions for a given tool-orientation vector -# or 'None','None' if no valid position could be found -def kins_calc_jnt_angles(self, tool_z_req): - log.debug('tool_z_requested: %s', tool_z_req) + +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 - tool_z_req = np.array([tool_z_req[0], tool_z_req[1], tool_z_req[2]]) - # calculate secondary joint values using kinematic specific formula - theta_2_pair = kins_calc_secondary(self, tool_z_req) - # calculate primary joint values using kinematic specific formula - theta_1_pair = kins_calc_primary(self, tool_z_req, theta_2_pair) + 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 = [] - # iterate through all the possible combinations of (theta_1 , theta_2) - for i in range(len(theta_1_pair)): - for j in range(len(theta_2_pair)): - # rotate an identity matrix using the custom tool kinematic model and the (theta_1, theta_2) - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1_pair[i], theta_2_pair[j], 0, matrix_in,'inv') - # the resulting tool-z vector for this pair of (theta_1, theta_2) is found in the third column - tool_z_would_be = np.array([t_out[0,2], t_out[1,2], t_out[2,2]]) - log.debug('tool_z_would_be: %s', tool_z_would_be) - # calculate the difference of the respective elements - tool_z_diff = tool_z_req - tool_z_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_z_diff > -epsilon) & (tool_z_diff < epsilon)) - log.debug('Is the tool-Z-vector close enough ? %s', match) - if match: - # check if we already have this particular pair in the list - if not (theta_1_pair[i], theta_2_pair[j]) in joint_angles_list: - log.debug('Appending (theta_1_pair, theta_2_pair) %s', (degrees(theta_1_pair[i]), degrees(theta_2_pair[j]))) - joint_angles_list.append((theta_1_pair[i], theta_2_pair[j])) - log.info('Found valid joint angles: %s', joint_angles_list) - if joint_angles_list: - return joint_angles_list - #return joint_angles_list[-1] - else: - return None, None + 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): - # 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('Got (pos, trgt): %s', (pos, trgt)) + 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('Calculated (dist_short, dist_long): %s', (dist_short, dist_long)) + 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 - if mode == 2: # negative rotation only ie we want a positive distance - if dist_short >= 0: # ie we need to go the other way + 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('Distance returned: %s', dist) - return dist + log.debug(f'Returning distance: {dist:.4f}°') + return radians(dist) -# 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 -def calc_rotary_move_with_joint_limits(position, target, max_limit, min_limit, mode): - pos = degrees(position) - trgt = degrees(target) - log.debug('(Current_pos, target): %s', (pos, trgt)) +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('Max_limit OK, target changed to: %s', (pos + dist)) + 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 wey in the other direction + 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: - log.debug('Max_limit reached, target remains: %s', trgt) + 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 - dist = 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('Min_limit OK, target changed to: %s', (pos + dist)) + 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: - log.debug('Min_limit reached, target remains: %s', trgt) + 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 - dist = 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 - log.debug('Angle and distance returned: %s, %s', theta, dist) - return theta, dist + return theta, dist # returns 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 -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): +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) + 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: - # primary joint, here we apply the orient mode requested by the operator + # 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, - orient_mode) - # secondary joint, here we want the shortest move (although we could also apply a strategy here) + primary_strategy) + # secondary joint sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, secondary_max_limit, secondary_min_limit, - 0) + 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))) - log.debug('Assembled target_dist_list: %s',target_dist_list) - return target_dist_list + 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 -# find the optimal joint move from current to target positions in the list -# for this we look at the primary joint move only -# 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 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 primary joint + # shortest move of the prioritized joint (theta_1, theta_2) = (None, None) - dist = 3600 + 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[0]) < fabs(dist): # shortest move requested + 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[0]) < fabs(dist) and dists[0] >= 0: # positive primary rotation only + 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[0]) < fabs(dist) and dists[0] <= 0: # negative primary rotation only + 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] - log.debug('Shortest move selected for (orient_mode, theta_1, theta_2): %s', (orient_mode, theta_1, theta_2)) - return theta_1, theta_2 - - -# calculates the required pre-rotation around tool-z so the tool-x matches the requested -# orientation after rotation of the spindle joints -def kins_calc_pre_rot(self, theta_1, theta_2, tool_x_req, tool_z_requested): - # tolerance setting for check if tool-x-vector needs to be rotated at all + 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("Tool-x-requested: %s", tool_x_req) - # we need to calculate the current tool-x vector with the given rotations using - # the transformation matrix from our custom tool kinematic - log.debug("joint angles (secondary, primary) in radians given: %s", (theta_2, theta_1)) - log.debug("joint angles (secondary, primary) in degrees given: %s", (theta_2*180/pi, theta_1*180/pi)) - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation zero - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, 0, matrix_in,'inv') - # the tool-x vector for the given machine joint rotations is found directly in the first column - tool_x_is = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug("tool-x after machine rotation would be: %s", tool_x_is) - # we calculate the angular difference between the two vectors so we can 'pre-rotate' - # around tool-z to get the requested tool-x vector after machine rotation + 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 - tool_x_is = tool_x_is / np.linalg.norm(tool_x_is) - tool_x_req = tool_x_req / np.linalg.norm(tool_x_req) + 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("check if vectors are parallel: %s", np.dot(tool_x_is,tool_x_req)) - if np.dot(tool_x_is,tool_x_req) > 1 - epsilon: - log.info("Tool x-vector already oriented, setting pre-rotation = 0") - # if we are already parallel then we don't need to pre-rotate - pre_rot = 0 + 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(tool_x_req, tool_x_is) - log.debug("cross product (tool_x_req, tool_x_is): %s", cross) - log.info("Tool_z_requested: %s", tool_z_requested) - pre_rot = np.arccos(np.dot(tool_x_req, tool_x_is)) - log.debug('base pre_rot: %s', pre_rot) + 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 - pre_rot_list = [pre_rot, -pre_rot, 2*pi-pre_rot, -(2*pi-pre_rot)] - log.debug('pre_rot_list: %s',pre_rot_list) - # then we run all of them through the kinematic model and see which gives us - # the requested tool-x-vector - for pre_rot in pre_rot_list: + 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 tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation angle in the list - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in,'inv') - # the tool-x vector for the given primary and secondary rotations is found directly in the first column - tool_x_would_be = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug('tool_x_would_be: %s', tool_x_would_be) + # 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 - tool_x_diff = tool_x_req - tool_x_would_be + x_vector_diff = x_vector_req - x_vector_would_be # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_x_diff > -zeta) & (tool_x_diff < zeta)) - log.debug('Is the tool-X-vector close enough ? %s', match) + 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("Pre-rotation calculated [deg]: %s", degrees(pre_rot)) - # return pre_rot in radians - return pre_rot - - -# transforms a 4x4 input matrix using the current tool transformation matrix -# (forward or inverse) using the kinematic model of the machine -def kins_calc_tool_transformation(self, matrix_in, theta_1=None, theta_2=None, pre_rot=None, direction='fwd'): - global kins_pre_rotation - # if no angle values have been passed we get the current joint positions - if theta_2 == None or theta_1 == None: - # read current spindle rotary angles and convert to radians - theta_1, theta_2 = get_current_rotary_positions(self) - else: - log.debug("got for secondary joint: %s", theta_2) - log.debug("got for primary joint: %s", theta_1) - # pre-rot is the virtual rotary axis around the tool-z axis to align the tool-x axis - # if no pre-rot angle is passed then we use the currently active value - if pre_rot == None: - pre_rot = hal.get_value(kins_pre_rotation ) - log.debug("current pre-rot: %s", pre_rot) - else: - log.debug("requested pre-rot value [DEG]): %s", degrees(pre_rot)) - # run the input matrix through the tool kinematic transformation in the requested direction - # using the current joint angles and pre-rotation as requested - matrix_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction) - return matrix_out - - -# define the basic rotation matrices, used for euler twp modes -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]]) + log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') + return virtual_rot # returns radians -# returns the rotation matrices for given order and angles -def twp_calc_euler_rot_matrix(th1, th2, th3, order): - log.debug("euler order requested: %s", order) - log.debug("angles given (th1, th2 , th3): %s", (th1, th2, th3)) - th1 = radians(th1) - th2 = radians(th2) - th3 = radians(th3) - 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('euler rotation as matrix: \n %s', matrix) - return matrix +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 -# The tilted-work-plane is created in identity mode and must NOT be updated after a switch -def gui_update_twp(self): +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 - 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])) + 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)) + 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 then rotated according to - # the rotary joint position and then translated. + # 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. - 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)) + 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 @@ -702,11 +530,12 @@ def get_current_work_offset(self): 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] + 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) @@ -714,7 +543,7 @@ def get_current_rotary_positions(self): theta_1 = radians(self.BB_current) elif joint_letter_primary == 'C': theta_1 = radians(self.CC_current) - log.debug('Current position Primary joint: %s', degrees(theta_1)) + 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) @@ -722,45 +551,32 @@ def get_current_rotary_positions(self): theta_2 = radians(self.BB_current) elif joint_letter_secondary == 'C': theta_2 = radians(self.CC_current) - log.debug('Current position Secondary joint: %s', degrees(theta_2)) + log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') return theta_1, theta_2 -# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] -def point_to_matrix(point): - # start with a 4x4 identity matrix and add the point vector to the 4th column - matrix = np.identity(4) - [matrix[0,3], matrix[1,3], matrix[2,3]] = point - matrix = np.asmatrix(matrix) - return matrix - - -# extracts the point vector form a given 4x4 transformation matrix -def matrix_to_point(matrix): - point = (matrix[0,3],matrix[1,3],matrix[2,3]) - return point - - -def reset_twp_params(self): - global pre_rot, twp_matrix, twp_flag, twp_build_params - pre_rot = 0 +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_pre_rot(self,0)! + # ie don't do this: kins_comp_set_virtual_rot(0)! twp_flag = [] twp_build_params = {} - log.info("Resetting TWP-matrix") + log.info(" Resetting TWP-matrix") twp_matrix = np.asmatrix(np.identity(4)) -# Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) -# (some controllers offer an optional P-word to give preferred rotation directions this is not implemented yet) -# 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!) -def g53x_core(self): - global saved_work_offset, twp_matrix, twp_flag, pre_rot + +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 @@ -769,112 +585,142 @@ def g53x_core(self): if not hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: No TWP defined." - log.debug(msg) + 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(self) - msg = "G53.x: TWP already active" - log.debug(msg) + 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.x command + # 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.x Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + 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(self) - msg = "G53.x : unrecognised P-Word found." - log.debug(msg) + # 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 - # calculate the required rotary joint positions and pre_rotation for the requested tool-orientation + 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: - tool_z_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - # calculate all possible pairs of (primary, secondary) angles so our tool-z vector matches the requested tool-z # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = kins_calc_jnt_angles(self, tool_z_requested) - # An excepton will occur if the requested tool orientation cannot be achieved with the kinematic at hand + possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians except Exception as error: - log.error('G53.x: Calculation failed, %s', error) - possible_prim_sec_angle_pairs = [] - if not possible_prim_sec_angle_pairs: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x" - log.debug(msg) + 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 - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) - if theta_1 == None: + 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(self) - msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") - log.debug(msg) + 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 - theta_1 = radians(theta_1) - theta_2 = radians(theta_2) - # calculate the pre-rotation needed so our tool-x vector matches the requested tool-x vector - tool_x_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - pre_rot = kins_calc_pre_rot(self,theta_1, theta_2, tool_x_requested, tool_z_requested) - log.debug("Calculated pre-rotation (pre_rot) to match requested tool-x): %s", pre_rot) + # 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(self) - # set the pre-rotation value in the kinematic component - log.debug("G53.x: setting primary, secondary and pre_rotation angles in kinematic component: %s", (degrees(theta_1), degrees(theta_2), degrees(pre_rot))) - hal.set_p(kins_pre_rotation, str(pre_rot)) - hal.set_p(kins_primary_rotation, str(degrees(theta_1))) - hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) - - # calculate the work offset in tool-coords - P = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(saved_work_offset), theta_1, theta_2, pre_rot)) - # get the current twp_origin + 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]) - # calculate the twp offset in tool-coords - Q = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(twp_offset), theta_1, theta_2, pre_rot)) - log.debug("G53.x: Setting transformed work-offsets for tool-kins in G59, G59.1, G59.2 and G59.3 to: %s ", P) + 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 " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - log.debug("G53.x: Moving (secondary and primary) joints to: %s", (degrees(theta_2), degrees(theta_1))) + 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 with the requested twp - self.execute("G0 %s%f %s%f" % (joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + # 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 TOOL kinematics - self.execute("M68 E3 Q2") + # 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_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + 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 @@ -886,24 +732,25 @@ def g53x_core(self): # 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(self) - gui_update_twp(self) + 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 +# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation def g683(self, **words): - global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset + 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 @@ -917,7 +764,7 @@ def g683(self, **words): if hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg =("G68.3 ERROR: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) @@ -931,7 +778,7 @@ def g683(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.3 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) @@ -944,23 +791,31 @@ def g683(self, **words): 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 tool-x around the origin - r = c.r_number if c.r_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) - # calculate tool-prerotation necessary to have tool-x vector in machine xy-plane - pre_rot = kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2 ) - log.info("G68.3: Pre-Rotation calculated for x-vector in machine-xy plane [deg]: %s", pre_rot*180/pi) - # then we need the tool transformation matrix of the current tool orientation with the - # calculated pre-rotation to get the tool-x vector in the machine xy-plane - # for this we take the 4x4 identity matrix and pass it through the inverse tool kinematic - # transformation using the current rotary joint positions and calculated pre-rotation angle - # plus the requested angle of rotation for tool-x from the machine-xy plane + 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 origin rotation [deg]: %s', r) - twp_matrix = kins_calc_tool_transformation(self, start_matrix, None, None, pre_rot + radians(r), 'inv') - log.debug("G68.3: Tool matrix with x-vector in machine xy-plane: \n%s", twp_matrix) + 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 @@ -974,13 +829,14 @@ def g683(self, **words): self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + 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): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + 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 @@ -994,9 +850,9 @@ def g682(self, **words): if hal.get_value(twp_is_defined): # ie TWP has already been defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: TWP already defined.") - log.debug(msg) + 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 @@ -1007,9 +863,9 @@ def g682(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(msg) + 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 @@ -1018,7 +874,7 @@ def g682(self, **words): # 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)) + 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 @@ -1028,9 +884,9 @@ def g682(self, **words): 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(self) + reset_twp_params() msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1040,21 +896,24 @@ def g682(self, **words): 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 tool-x around the origin - r = c.r_number if c.r_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 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + 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]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_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 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1072,34 +931,36 @@ def g682(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(msg) + 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 rotation of tool-x around the origin - r = c.r_number if c.r_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 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + 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]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P1: Twp_origin_rotation \n%s',twp_origin_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 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1111,21 +972,28 @@ def g682(self, **words): 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), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], '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 (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation 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 tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1148,9 +1016,9 @@ def g682(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1164,36 +1032,38 @@ def g682(self, **words): 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) + 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) + 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) + 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 tool-z vector + # 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 tool-z will be below the twp (ie tool-z will be downwards) + # 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 tool-y vector + 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 tool-vectors + 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) - # convert requested origin rotation to radians - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + 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 twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1202,22 +1072,26 @@ def g682(self, **words): 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) + log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - log.info('first call') + log.info(' first call') twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} - log.debug('twp_build_params: %s', twp_build_params) + log.debug(' twp_build_params: %s', twp_build_params) if q == 0: # define new origin of 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 rotation of tool-x around the origin - r = c.r_number if c.r_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 @@ -1232,9 +1106,9 @@ def g682(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1249,39 +1123,39 @@ def g682(self, **words): log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal - if orth != 0: - reset_twp_params(self) + if orth > 0.001: + reset_twp_params() msg = ("G68.2 P3: Vectors are not orthogonal.") - log.debug(msg) + 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 - # we can use the cross product to calculate the tool-y vector + # 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 tool-vectors + 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) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + 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 = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.info('G68.2 P3: twp_origin_rotation failed, %s', e) - log.debug('G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + 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 twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1291,41 +1165,45 @@ def g682(self, **words): 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) + 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(self) + reset_twp_params() msg = ("G68.2: No recognised P-Word found.") - log.debug(msg) + 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')) + 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: %s', radians(r)) - log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) + 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) + 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) + 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) + 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(self) + 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): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + 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 @@ -1339,9 +1217,9 @@ def g684(self, **words): if not hal.get_value(twp_is_active): # ie there is currently no TWP defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(msg) + 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 @@ -1352,18 +1230,16 @@ def g684(self, **words): # Must be in one of the dedicated offset systems for TWP if False: #n < 6: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(msg) + 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 @@ -1374,9 +1250,9 @@ def g684(self, **words): if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1386,21 +1262,24 @@ def g684(self, **words): 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 tool-x around the origin - r = c.r_number if c.r_flag else 0 - # parse requested euler angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_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]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_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 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1418,9 +1297,9 @@ def g684(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1430,21 +1309,24 @@ def g684(self, **words): 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 tool-x around the origin - r = c.r_number if c.r_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 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + 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]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P1: Twp_origin_rotation \n%s',twp_origin_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 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1456,21 +1338,28 @@ def g684(self, **words): 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), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], '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 (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation 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 tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1493,9 +1382,9 @@ def g684(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1509,38 +1398,38 @@ def g684(self, **words): 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) + 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) + 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) + 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 tool-z vector + # 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 tool-z will be below the twp (ie tool-z will be downwards) + # 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 tool-y vector + 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 tool-vectors + 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) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + 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 = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.debug('G68.4 P2: twp_origin_rotation failed ', e) - log.debug('G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + 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 twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1549,9 +1438,13 @@ def g684(self, **words): 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) + log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: @@ -1561,8 +1454,8 @@ def g684(self, **words): 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 tool-x around the origin - r = c.r_number if c.r_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 @@ -1577,9 +1470,9 @@ def g684(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(msg) + 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 @@ -1594,40 +1487,43 @@ def g684(self, **words): log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() ## reset the parameter values #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed #twp_build_params = {'q0':[], 'q1':[]} msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(msg) + 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 - # we can use the cross product to calculate the tool-y vector + # 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 tool-vectors + 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) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + 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 twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1637,40 +1533,42 @@ def g684(self, **words): 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) + 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(self) + reset_twp_params() msg = ("G68.4: No recognised P-Word found.") - log.debug(msg) + 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')) + 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 %s', radians(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...") + 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) + 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) + 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) + 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) + 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(self) + gui_update_twp() return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc index bcd3c730a1f..381a6116adf 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: kinstype==0 (IDENTITY kinematics) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc index 0d14ad1bf82..d1b54b5250d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: kinstype==1 TCP kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc index 55fbf966e11..5f726a6df12 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: kinstype==2 Tool kinematics o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub 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 index b7c27d221d3..4b2fd293c61 100755 --- 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 @@ -6,7 +6,7 @@ o100 if [EXISTS [#

]] o100 else #

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

;orient the spindle with P word M66 L0 E0 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 index c25356b27a8..16d9687cbe8 100755 --- 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 @@ -3,7 +3,7 @@ osub M66 L0 E0 ;force sync, stop read ahead o100 if [[EXISTS [#]] AND [EXISTS [#]] AND [EXISTS [#]]] - M68 E3 Q0 ;switch to identity kinematic + 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 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 index 718a572afae..a8b628a930c 100755 --- 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 @@ -6,7 +6,7 @@ o100 if [EXISTS [#

]] o100 else #

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

;orient the spindle with P word M66 L0 E0 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 index fd37ea30837..9efd1b7db29 100755 --- 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 @@ -3,7 +3,7 @@ osub M66 L0 E0 ; force sync, stop read ahead M469 ; call the python G69_core code -M68 E3 Q0 ; switch to identity kins +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 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 index 492552977e2..1cbf3d41db7 100755 --- 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 @@ -7,7 +7,7 @@ o sub ;(msg, on_abort START) M68 E2 Q0 ; reset twp-state to 'undefined' (0) -M68 E3 Q0 ; set IDENTITY kins +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) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..7c2ebd39961 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,375 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzacb-trsrn config, a machine with primary rotary C and secondary rotary B +# +# +# 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. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + +# 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") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Ry(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], + [ Cv*Ss, r, t, 0], + [ -Sv*Ss, t, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + theta_1 = asin(clamp_unit((p*Kzy - t*Kzx)/(t*t + p*p))) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: + theta_1_list.append(theta) + return theta_1_list # returns radians + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2((Sv*Ss),t) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +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]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + 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)) + return matrix 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 0e430c12691..2be585e6ef8 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 @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos 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=g53x_core + 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 @@ -80,8 +80,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal # signal reflecting twp states (0=undefined, 1=defined, 2=active) HALCMD = net twp-status <= motion.analog-out-02 -# connection required for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z @@ -124,7 +122,7 @@ HALCMD = net :rotary-b joint.4.pos-fb xyzacb-trsrn-gui.rot HALCMD = net :rotary-c joint.5.pos-fb xyzacb-trsrn-gui.rotary_c HALCMD = net :tool-diam halui.tool.diameter xyzacb-trsrn-gui.tool_diameter HALCMD = net :tool-offset xyzacb-trsrn-gui.tool_length -HALCMD = net :kinstype-select xyzacb-trsrn-gui.kinstype_select +HALCMD = net :kinstype-current motion.kins-type xyzacb-trsrn-gui.kinstype_select HALCMD = net :nutation-angle xyzacb-trsrn-gui.nutation_angle HALCMD = net :pivot-y xyzacb-trsrn-gui.pivot_y HALCMD = net :pivot-z xyzacb-trsrn-gui.pivot_z @@ -155,9 +153,9 @@ HALCMD = net twp-is-active xyzacb-trsrn-gui.twp [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M428:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M429: tcp kins (motion.switchkins-type==1) -# M430: tool kins (motion.switchkins-type==2) +# M428:identity kins (kinstype 0, startupDEFAULT) +# M429: tcp kins (kinstype 1) +# M430: tool kins (kinstype 2) MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..abb24726b72 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,378 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzbca-trsrn config, a machine with primary rotary C and secondary rotary A +# +# +# 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. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + +# 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") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Rx(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ r, -Cv*Ss, t, 0], + [ Cv*Ss, Cs, -Sv*Ss, 0], + [ t, Sv*Ss, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + q = clamp_unit((t*Kzy - p*Kzx)/(t*t + p*p)) + theta_1 = asin(q) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: + theta_1_list.append(theta) + + return theta_1_list + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) + # since we are using acos() we really have two solutions theta_1 and -theta_1 + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2(-t,(Sv*Ss)) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +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]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + 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)) + return matrix 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 44e6144e653..d3032855aee 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 @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos 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=g53x_core + 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 @@ -77,8 +77,6 @@ POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal # signal reflecting twp states (0=undefined, 1=defined, 2=active) HALCMD = net twp-status <= motion.analog-out-02 -# connection required for control of motion.switchkins-type -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z @@ -121,7 +119,7 @@ HALCMD = net :rotary-b joint.4.pos-fb xyzbca-trsrn-gui.rot HALCMD = net :rotary-c joint.5.pos-fb xyzbca-trsrn-gui.rotary_c HALCMD = net :tool-diam halui.tool.diameter xyzbca-trsrn-gui.tool_diameter HALCMD = net :tool-offset xyzbca-trsrn-gui.tool_length -HALCMD = net :kinstype-select xyzbca-trsrn-gui.kinstype_select +HALCMD = net :kinstype-current motion.kins-type xyzbca-trsrn-gui.kinstype_select HALCMD = net :nutation-angle xyzbca-trsrn-gui.nutation_angle HALCMD = net :pivot-x xyzbca-trsrn-gui.pivot_x HALCMD = net :pivot-z xyzbca-trsrn-gui.pivot_z @@ -152,9 +150,9 @@ HALCMD = net twp-is-active xyzbca-trsrn-gui.twp [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst -# M428:identity kins (motion.switchkins-type==0 startupDEFAULT) -# M429: tcp kins (motion.switchkins-type==1) -# M430: tool kins (motion.switchkins-type==2) +# M428:identity kins (kinstype 0, startupDEFAULT) +# M429: tcp kins (kinstype 1) +# M430: tool kins (kinstype 2) MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 diff --git a/configs/sim/axis/vismach/hexapod-sim/hexapod.ini b/configs/sim/axis/vismach/hexapod-sim/hexapod.ini index bf07a3c0cce..f42a14799a0 100644 --- a/configs/sim/axis/vismach/hexapod-sim/hexapod.ini +++ b/configs/sim/axis/vismach/hexapod-sim/hexapod.ini @@ -40,7 +40,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = kinematics.hal HALCMD = loadusr -W ./hexagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = hexapod_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc index 4ab3aaf922d..e9529f6d0f8 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 genhexkins o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc index 54726d37a6c..0291e69889d 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 Identity kinematics o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc b/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc index 7586236a003..886fe727740 100644 --- a/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/hexapod-sim/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 userk kins o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README index fe67738ec65..cfb0fa8ecc2 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/README @@ -13,10 +13,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between 'genserkins' and 'identity' kinematics. The buttons issue remapped -commands M428,M429. These commands - a) set the motion.switchkins-type pin and - b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for 'identity' kins, default assignments are: diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini index 315bae0831d..cfea9e01701 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-genser/melfa-sim-genser.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = melfa_mdh.hal HALCMD = loadusr -W ../melfagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = ../melfa-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README index 0ca33f8b779..c3f60434b89 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/README @@ -11,10 +11,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between 'three21' and 'identity' kinematics. The buttons issue remapped -commands M428,M429. These commands - a) set the motion.switchkins-type pin and - b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for 'identity' kins, default assignments are: diff --git a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini index 0419806ad89..5a8ba9d4787 100644 --- a/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini +++ b/configs/sim/axis/vismach/melfa-sim/melfa-sim-three21/melfa_321.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = melfa_dh.hal HALCMD = loadusr -W ../melfagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = ../melfa-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc index 8669ac0e781..c7dda9ab73d 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/428remap.ngc @@ -1,28 +1,16 @@ ;M428 by remap: select genserkins o<428remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 0 ; genserkins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G13.1 ; back to kinstype 0, syncs interp and motion G10 L2 P7 X0 Y0 Z0 A-180 B0 C0 G59.1 - M66 E0 L0 ; force synch ; (debug, M428:genserkins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 0]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 0]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc index 32dff4d4742..2d28bda961a 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/429remap.ngc @@ -1,28 +1,16 @@ ;M429 by remap: select identity kins o<429remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 1 ; identity kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion G10 L2 P8 X0 Y-90 Z0 A0 B90 C0 G59.2 - M66 E0 L0 ; force synch ; (debug, M429:identity kins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 1]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 1]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc b/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc index c7d087435f6..e81d4ed4ac2 100644 --- a/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/melfa-sim/remap_subs/430remap.ngc @@ -1,26 +1,14 @@ ;M430 by remap: select gensertool kins o<430remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 2 ; gensertool kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M430:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion ; (debug, M429:identity kins) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 2]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE 2]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/millturn/millturn.ini b/configs/sim/axis/vismach/millturn/millturn.ini index 57776eeb38c..575947d9dce 100644 --- a/configs/sim/axis/vismach/millturn/millturn.ini +++ b/configs/sim/axis/vismach/millturn/millturn.ini @@ -14,7 +14,6 @@ JOINTS= 4 HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = millturn.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = millturn-postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/millturn/millturn.txt b/configs/sim/axis/vismach/millturn/millturn.txt index b6cbe143a03..29b9960d6e4 100644 --- a/configs/sim/axis/vismach/millturn/millturn.txt +++ b/configs/sim/axis/vismach/millturn/millturn.txt @@ -7,9 +7,8 @@ For additional information see the README in the millturn folder. 2) pyvcp buttons are provided to switch between mill and turn kinematics. The buttons issue remapped commands M428,M429. These commands -a) set the motion.switchkins-type pin and -b) force a synchronization using a motion input read command. -c) set softlimits according to values set in millturn.ini [AXIS_X] and [AXIS_Z] section. +a) select the kinematics with G12.1, which synchronizes interpreter and motion itself. +b) set softlimits according to values set in millturn.ini [AXIS_X] and [AXIS_Z] section. 3) when set for mill, default assignments are: diff --git a/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc b/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc index ca6225fb421..63a2d118a6d 100644 --- a/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/millturn/remap_subs/428remap.ngc @@ -1,29 +1,18 @@ ;M428 by remap: select mill kins o<428remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 0 ; mill -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion M128 ; switch limits G10 L2 P7 X-290 Y0 Z-160 A0 ; reset home offset G59.1 ; activate home offset - M66 E0 L0 ; force synch + M66 E0 L0 ; force synch, M128 changed the limits ;(debug, M428: mill) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 0]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc b/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc index 26207430a88..7be809a0d3e 100644 --- a/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/millturn/remap_subs/429remap.ngc @@ -1,29 +1,18 @@ ;M429 by remap: select turn kins o<429remap>sub - # = 3 ; set N as required: motion.analog-out-0N # = 1 ; turn kins -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]FEATURE==8) - (debug,STOP) - M2 -o1 endif - - M66 E0 L0 ; force synch - M68 E# Q# ; set kinstype value + G12.1 P# ; select kinstype, syncs interp and motion M129 ; switch limits G10 L2 P8 X-160 Y0 Z-290 A0 ; reset home offset G59.2 ; activate home offset - M66 E0 L0 ; force synch + M66 E0 L0 ; force synch, M129 changed the limits ;(debug, M429: turn) -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE 1]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index c6bbd33f245..99c0fa33112 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -12,7 +12,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = puma_dh.hal HALCMD = loadusr -W ./pumagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma560.halshow b/configs/sim/axis/vismach/puma/puma560.halshow index 11b090d98c6..eb532d6e928 100644 --- a/configs/sim/axis/vismach/puma/puma560.halshow +++ b/configs/sim/axis/vismach/puma/puma560.halshow @@ -1,4 +1,4 @@ -pin+motion.switchkins-type +pin+motion.kins-type pin+kinstype.is-0 pin+kinstype.is-1 pin+kinstype.is-2 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 5e29092ba5c..046f5d118d5 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -16,7 +16,6 @@ HALUI = halui HALCMD = loadusr -W ./puma560gui.py HALFILE = LIB:basic_sim.tcl HALFILE = puma560_dh.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma560_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma560.txt b/configs/sim/axis/vismach/puma/puma560.txt index 4fb318abbba..7353b02753c 100644 --- a/configs/sim/axis/vismach/puma/puma560.txt +++ b/configs/sim/axis/vismach/puma/puma560.txt @@ -8,10 +8,10 @@ with 6 revolute joints. 2) pyvcp buttons are provided to switch between genserkins and identity kinematics. The buttons issue remapped -commands M428,M429. These commands a) -set the motion.switchkins-type pin and -b) force a synchronization using a -motion input read command. +commands M428,M429. These commands +select the kinematics with G12.1, which +synchronizes interpreter and motion +itself. 3) when set for identity kins, default assignments are: diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index 97ec74cb5f6..a6b9f8544e9 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -16,7 +16,6 @@ HALUI = halui HALCMD = loadusr -W ./puma560gui.py HALFILE = LIB:basic_sim.tcl HALFILE = puma560_dh.hal -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma560_postgui.hal [RS274NGC] diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 7cf35ce23e3..8397ce3ea7a 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -103,7 +103,6 @@ HALUI = halui HALFILE = LIB:basic_sim.tcl HALFILE = puma_dh.hal HALCMD = loadusr -W ./pumagui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = puma_postgui.hal [HALUI] diff --git a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc index 36f8ee3e499..2b2016bfe50 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype=0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc b/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc +++ b/configs/sim/axis/vismach/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/axis/vismach/scara/scara.ini b/configs/sim/axis/vismach/scara/scara.ini index 6b14a4eaeb3..23323fdc8bf 100644 --- a/configs/sim/axis/vismach/scara/scara.ini +++ b/configs/sim/axis/vismach/scara/scara.ini @@ -58,7 +58,6 @@ KINEMATICS = scarakins coordinates=xyzcab HALUI = halui HALFILE = LIB:basic_sim.tcl HALCMD = loadusr -W ./scaragui.py -HALCMD = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type POSTGUI_HALFILE = scara_postgui.hal [HALUI] diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc b/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc +++ b/configs/sim/qtaxis/non-trivial/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc index 8698782fff7..f983c3870ea 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/428remap.ngc @@ -1,24 +1,13 @@ ;M428 by remap: select kinstype==0 (default) o<428remap>sub # = 0 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G13.1 ; back to kinstype 0, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M428: Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M428: Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<428remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc index 627d547052b..25a2ef41339 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/429remap.ngc @@ -1,24 +1,13 @@ ;M429 by remap: select kinstype==1 (Identity kinematics) o<429remap>sub # = 1 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M429:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M429:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<429remap>endsub diff --git a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc index 5af12f4fbf2..f5d2db707aa 100644 --- a/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc +++ b/configs/sim/qtvcp_screens/non-trivial/scara/remap_subs/430remap.ngc @@ -1,24 +1,13 @@ ;M430 by remap: select kinstype==2 (userk kins) o<430remap>sub # = 2 - # = 3 ; set N as required: motion.analog-out-0N -o1 if [exists [#<_hal[motion.switchkins-type]>]] -o1 else - (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1) - (debug,STOP) - M2 -o1 endif - - M68 E# Q# ; set kinstype value - M66 E0 L0 ; force synch + G12.1 P# ; select kinstype, syncs interp and motion -o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #]] - (debug,M430:Wrong motion.switchkins-type) - (debug,or missing hal net to analog-out-0x) +o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] + (debug,M430:Wrong kinematics type) (debug,STOP) M2 -o2 else o2 endif o<430remap>endsub diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 199dae9fcc0..39c124d3532 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -5,3 +5,5 @@ usr/lib/liblinuxcnc.a usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl +usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/kins_util.c diff --git a/debian/linuxcnc.install.in b/debian/linuxcnc.install.in index d66045b43a5..a71302665a0 100644 --- a/debian/linuxcnc.install.in +++ b/debian/linuxcnc.install.in @@ -36,6 +36,7 @@ usr/bin/hy_vfd usr/bin/image-to-gcode usr/bin/inivalue usr/bin/inivar +usr/bin/kinslimits usr/bin/latency-histogram usr/bin/latency-plot usr/bin/latency-test diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 6253e57afb2..487add73f74 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -70,6 +70,7 @@ as the 'L number', and so on for any other letter. |<> |Set Tool Table, Calculated, Fixture |<> |Coordinate System Origin Setting |<> |Coordinate System Origin Setting Calculated +|<> |Select Kinematics |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position @@ -934,6 +935,71 @@ It is an error if: * The P number does not evaluate to an integer in the range 0 to 9. * An axis is programmed that is not defined in the configuration. +[[gcode:g12.1-g13.1]] +== G12.1, G13.1 Select Kinematics(((G12.1, G13.1 Select Kinematics))) + +---- +G12.1 P- +G13.1 +---- + +'G12.1' selects one of the kinematics provided by a switchable kinematics +module, and 'G13.1' cancels back to kinematics 0. The 'P' word is the +kinematics number, so 'G13.1' and `G12.1 P0` do the same thing. + +[WARNING] +Deprecation notice: selecting the kinematics by writing the +`motion.switchkins-type` HAL pin is deprecated. It takes the same +numbering and still works, but it does not tell the interpreter that +anything changed, so a program is read, its limits checked and its path +looked ahead in whatever kinematics the interpreter last knew about, +which need not be the one that runs it. Use 'G12.1' and 'G13.1'. + +Both codes are queue synchronisation points. The interpreter waits for +queued motion to finish before the kinematics changes, so no move is ever +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. 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. + +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 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: + +[source,ngc] +---- +# = #<_kins_type> +G12.1 P2 (work in kinematics 2) +( ... ) +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 kinematics 0. + +.G12.1, G13.1 Example +[source,ngc] +---- +G12.1 P1 (switch to kinematics 1) +G0 X0 Y0 +G13.1 (back to kinematics 0) +---- + +It is an error if: + +* 'G12.1' is used without a 'P' word. +* The 'P' word is negative. +* A 'P' word is used with 'G13.1'. + [[gcode:g17-g19.1]] == G17 - G19.1 Plane Select(((G17 - G19.1 Plane Select))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 683f72cc154..4ed0ec7c73d 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -503,6 +503,10 @@ can be added easily without changes to the source code. | G89 | 890 |=== +* '#<_kins_type>' - Kinematics selected by 'G12.1' or 'G13.1'. Returns the + 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics + has been selected. See <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index 8e04a648a93..ace77ef56de 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -338,6 +338,7 @@ Limit its slew rate to less than maxv per second. Limit its second derivative to | link:../man/man9/rosekins.9.html[rosekins] |Kinematics for a rose engine || | link:../man/man9/rotatekins.9.html[rotatekins] |The X and Y axes are rotated 45 degrees compared to the joints 0 and 1. || | link:../man/man9/scarakins.9.html[scarakins] |Kinematics for SCARA-type robots. || +| link:../man/man9/switchkinscomp.9.html[switchkinscomp] |Switchable kinematics module template || | link:../man/man9/kins.9.html[three21kins] |Analytical kinematics solver for 6-DOF arm + wrist robots. || | link:../man/man9/tripodkins.9.html[tripodkins] |The joints represent the distance of the controlled point from three predefined locations (the motors), giving three degrees of freedom in position (XYZ). || | link:../man/man9/userkins.9.html[userkins] |Template for user-built kinematics || diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index 43859fcef14..b68359287d1 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -253,9 +253,20 @@ Note: feed-inhibit applies to G-code commands -- not jogs. *motion.switchkins-type* IN float:: Kinematics modules that define the functions kinematicsSwitchable() and kinematicsSwitch() receive the *integer* value of this pin to - select the machine kinematics functions. Extra G-code commands may be + select the machine kinematics functions. Extra G-code commands are required to synchronize task and motion before and after changes to the pin value. + *Deprecated*: the interpreter does not see this pin, so limits and + look ahead go on using the kinematics it last knew about. Use the + G-code words *G12.1 P-* and *G13.1*, which ask motion directly and + synchronize task and motion themselves. Motion reports the + deprecation once, the first time the pin is used to change the + kinematics. The pin is in a grace period: it keeps working for now, + but is meant to be removed in the future. +*motion.kins-type* OUT float:: + The kinematics currently in force, whether it was selected by + *G12.1*, by *G13.1* or from *motion.switchkins-type*. A kinematics + type the module refuses is not reported here. *motion.teleop-mode* OUT BIT:: Motion mode is teleop (axis coordinate jogging available). *motion.tooloffset.L* OUT FLOAT:: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index a250825cd3e..a81b0dd01d4 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -20,17 +20,19 @@ specific kinematics calculations for most operations but can be switched to identity kinematics for control of individual joints after homing. -The kinematics type is selected by a motion module HAL pin that -can be updated from a G-code program or by interactive MDI -commands. The halui provisions for activating MDI commands can be -used to allow buttons to select the kinematics type using -hardware controls or a virtual panel (PyVCP, GladeVCP, etc.). - -When a kinematics type is changed, the G-code must also issue -commands to *force synchronization* of the interpreter and motion -parts of LinuxCNC. Typically, a HAL pin 'read' command (M66 E0 L0) is -used immediately after altering the controlling HAL pin to force -synchronization. +The kinematics type is selected with 'G12.1 P-' and 'G13.1', from a +G-code program or by interactive MDI commands. Buttons on a virtual +panel (PyVCP, GladeVCP, etc.) or on hardware controls select a +kinematics type through the halui provisions for activating MDI +commands. + +Changing the kinematics type requires the interpreter and motion parts +of LinuxCNC to be *synchronized*, which 'G12.1' and 'G13.1' do +themselves. + +A deprecated HAL pin, 'motion.switchkins-type', selects a kinematics +type as well. It is described under Usage below, because existing +configurations use it. == Switchable Kinematic Modules @@ -44,6 +46,10 @@ The following kinematics modules support switchable kinematics: . *three21kins* (type0:three21kins type1:identity) . *scarakins* (type0:scarakins type1:identity) . *5axiskins* (type0:5axiskins type1:identity) (bridgemill) +. *millturn* (type0:identity type1:turn) +. *xyzab_tdr_kins* (type0:identity type1:tcp) +. *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) +. *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) The xyz[ab]c-trt-kins modules by default use type0==xyz[ab]c-trt-kins for backwards compatibility. The provided sim configs alter the @@ -124,6 +130,7 @@ program behavior in accordance with the active kinematics type. === HAL Pin Summary . *motion.switchkins-type* Input (float) +. *motion.kins-type* Output (float) . *kinstype.is-0* Output (bit) . *kinstype.is-1* Output (bit) . *kinstype.is-2* Output (bit) @@ -135,10 +142,12 @@ A module providing more than three kinematics types has one === HAL Connections -Switchkins functionality is enabled by the pin -*motion.switchkins-type*. Typically, this pin is sourced by an -analog output pin like motion.analog-out-03 so that it can be -set by M68 commands. Example: +'G12.1' and 'G13.1' ask motion for a kinstype directly and need no HAL +connection at all. + +A kinstype can also be selected by writing the pin +*motion.switchkins-type*, which is sourced from an analog output pin +like motion.analog-out-03 so that it can be set by M68 commands: [source,hal] ---- @@ -146,9 +155,66 @@ net :kinstype-select <= motion.analog-out-03 net :kinstype-select => motion.switchkins-type ---- -=== G-/M-code commands +[WARNING] +Selecting the kinstype from HAL is deprecated and motion says so, once, +the first time the pin is used to change it. The interpreter does not +see the pin, so a program is read, its limits checked and its path +looked ahead in whatever kinematics the interpreter last knew about, +which is not necessarily the one that will run it. Use 'G12.1' and +'G13.1'. The pin is in a grace period: it keeps working for now, but is +meant to be removed in the future. + +=== G-code commands + +'G12.1 P-' selects a kinstype and 'G13.1' cancels back to kinstype 0: + +[source,ngc] +---- +... +G12.1 P1 ;select kinstype 1 +... +... ;user G-code +... +G13.1 ;back to kinstype 0 +... +---- + +These codes ask motion for the kinstype directly and synchronize task and +motion themselves, so no HAL connection and no separate sync command are +needed. The G-code words and the *motion.switchkins-type* pin are both +acted on when they change, so whichever asked most recently is the one in +force. *motion.kins-type* reports what is currently selected. + +The pin is deprecated, see the warning under HAL Connections. + +The kinstype in force is readable in G-code as '#<_kins_type>', which lets +a subroutine restore whatever its caller had selected: + +[source,ngc] +---- +# = #<_kins_type> +G12.1 P2 +( ... ) +G12.1 P# +---- + +Selection is not cancelled by the end of a program or by an abort, so +that the kinstype continues to match the position readout. A program +that should leave the machine in kinstype 0 ends with 'G13.1'. + +See the G-code documentation for 'G12.1' and 'G13.1' for the full +description. + +=== M-code commands -Kinstype selection is managed using G-code sequences like: +[WARNING] +This is the deprecated route described under HAL Connections above. It +is documented because existing configurations use it. New ones should +use 'G12.1' and 'G13.1'. + +Writing *motion.switchkins-type* through an analog output pin needs the +HAL connection shown above. Kinstype selection is then managed using +G-code sequences like: [source,ngc] ---- @@ -333,9 +399,19 @@ configs/sim/axis/vismach/ . . puma/puma560.ini (genserkins) . puma/puma.ini (pumakins) . hexapod-sim/hexapod.ini (genhexkins) +. millturn/millturn.ini (millturn) +. 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) == User kinematics provisions +There are two ways to supply custom kinematics. Adding a kinstype to +a module that is already in the tree is the smaller job; building a +module of your own gives you every kinstype it provides. + +=== Adding a kinstype to an in-tree module + Custom kinematics can be coded and tested on Run-In-Place ('RIP') builds. A template file src/emc/kinematics/userkfuncs.c is provided in the distribution. This file can be copied/renamed to a user @@ -353,6 +429,50 @@ Preempt-rt make example: $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid ---- +=== Building a switchkins module of your own + +A complete kinematics module can be built out-of-tree with halcompile +using the same switchkins implementation the in-tree modules use, so +it gets the kinematics switching, the 'kinstype.is-N' pins, the +'coordinates=' identity mapping and the G-code and HAL controls +without reimplementing any of them. + +The template is src/hal/components/switchkinscomp.comp. Copy and +rename it (both the file and the component name) and replace the +example kinstype with the real kinematics. The implementation itself +is included: + +[source,c] +---- +#include +#include +---- + +A realtime module cannot link a library, so the implementation arrives +as source: switchkins.c and kins_util.c are installed beside the +headers, in share/linuxcnc, and halcompile already looks there. With +a deb install they come from the linuxcnc-dev package. + +The module registers each of its kinstypes and calls switchkinsInit() +from EXTRA_SETUP(), which halcompile runs after hal_init() and before +hal_ready(). See <> for both +calls. + +---- +$ halcompile --install user_switchkins.comp +---- + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +[NOTE] +The switchkins sources are compiled into the module, so it is built +against one source tree and has to be rebuilt when that tree changes. + == Warnings Unexpected behavior can result if a G-code program is inadvertently @@ -377,22 +497,18 @@ The management of coordinate offsets, tool compensation, and INI file limits may require complicated and non-standard operating protocols. +[[sec:switchkins-code-notes]] == Code Notes Kinematic modules providing switchkins functionality are linked to -the switchkins.o object (switchkins.c) that provides the module -'main' program (rtapi_app_main()) and related functions. This -'main' program reads (optional) module command-line parameters -(coordinates, sparm) and passes them to the module-provided -function switchkinsSetup(). - -The switchkinsSetup() function identifies kinstype-specific setup -routines and the functions for forward an inverse calculation for -each kinstype (0,1,2) and sets a number of configuration -settings. - -A module can provide further kinstypes by calling -switchkinsRegister() from within switchkinsSetup(), once per +the switchkins.o object (switchkins.c). It provides +kinematicsForward(), kinematicsInverse(), kinematicsSwitch() and +the rest of the kinematics interface, dispatching each call to the +kinstype currently selected, and it creates the HAL pins common to +all switchkins modules. It does not provide the module 'main' +program, so a module can get that from wherever suits it. + +A kinstype is supplied by calling switchkinsRegister(), once per kinstype: ---- @@ -400,25 +516,60 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -switchkins.h). A kinstype has to come from one route or the -other, so registering one that switchkinsSetup() has already -filled in is an error, and so is leaving a gap below the highest -kinstype provided. Either mistake fails the module load and says -which kinstype is at fault. +switchkins.h). Registering a kinstype twice is an error, and so is +leaving a gap below the highest kinstype provided. Either mistake +fails the module load and says which kinstype is at fault. Each kinstype gets its own 'kinstype.is-N' pin, so a module providing the usual three keeps the pin names it always had. -After calling switchkinsSetup(), rtapi_app_main() checks the -supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype. +When every kinstype is registered, the module calls: + +---- +int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); +---- + +which checks the supplied parameters, creates the HAL pins, selects +kinstype 0, and then invokes the setup routine registered for each +kinstype. The caller owns the HAL component: it does hal_init() +before switchkinsInit() and hal_ready() after it. Each kinstype setup routine can (optionally) create HAL pins and set them to default values. A setup routine is called once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. When all setup -routines finish, rtapi_app_main() issues hal_ready() for the -component to complete creation of the module. +kinstypes must not create the same pin twice. + +=== Module main program + +A module written as a plain C file links switchkins_main.o +(switchkins_main.c) for its rtapi_app_main(). That 'main' program +reads the (optional) module command-line parameters (coordinates, +sparm) and passes them to the module-provided function +switchkinsSetup(): + +---- +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); +---- + +which identifies the setup, forward and inverse routines for +kinstypes 0,1,2 and sets a number of configuration settings. Those +three are registered for the module, so it can supply further +kinstypes by calling switchkinsRegister() itself, and registering +one that switchkinsSetup() has already filled in is the same error +as any other duplicate. + +A module written as a halcompile component gets rtapi_app_main() +from halcompile instead. It registers its kinstypes and calls +switchkinsInit() from its EXTRA_SETUP() routine, which halcompile +runs after hal_init() and before hal_ready(). The component names +the objects it needs in hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +---- === Outline diff --git a/share/linuxcnc/kins_util.c b/share/linuxcnc/kins_util.c new file mode 100644 index 00000000000..c82a4a2fc95 --- /dev/null +++ b/share/linuxcnc/kins_util.c @@ -0,0 +1,366 @@ +/* Utility routines for kinematics modules +** License GPL Version 2 +** +** utilities for use with switchkins.c +**--------------------------------------------------------------------- +** identityKinematicsSetup() +** identityKinematicsForward() +** identityKinematicsInverse() +** +** Routines for identity kinematics using mapping created by +** map_coordinates_to_jnumbers() +** +**--------------------------------------------------------------------- +** map_coordinates_to_jnumbers() +** +** Map a string of coordinate letters to joint numbers sequentially. +** If allow_duplicates==1, a coordinate letter may be specified more +** than once to assign it to multiple joint numbers (the kinematics +** module must support such usage). +** +** Default mapping if coordinates==NULL is: +** X:0 Y:1 Z:2 A:3 B:4 C:5 U:6 V:7 W:8 +** +** Example coordinates-to-joints mappings: +** coordinates=XYZ X:0 Y:1 Z:2 +** coordinates=ZYX Z:0 Y:1 X:2 +** coordinates=XYZZZZ x:0 Y:1 Z:2,3,4,5 +** coordinates=XXYZ X:0,1 Y:2 Z:3 +**--------------------------------------------------------------------- +** +** mapped_joints_to_position() +** +** Update position based mapping created by map_coordinates_to_jnumbers() +** (used for identity-based forward kinematics) +**--------------------------------------------------------------------- +** +** position_to_mapped_joints() +** +** Update joints (including joints for duplicate letters) +** based on mapping created by map_coordinates_to_jnumbers() +** (used for identity-based inverse kinematics) +** +**--------------------------------------------------------------------- +*/ + +#include +#include +#include +#include +#include + +// principal joint numbers based on module 'coordinates' parameter +static int JX = -1; +static int JY = -1; +static int JZ = -1; +static int JA = -1; +static int JB = -1; +static int JC = -1; +static int JU = -1; +static int JV = -1; +static int JW = -1; + +// bitmaps indicate joints used for each axis letter +static int X_joints_bitmap; +static int Y_joints_bitmap; +static int Z_joints_bitmap; +static int A_joints_bitmap; +static int B_joints_bitmap; +static int C_joints_bitmap; +static int U_joints_bitmap; +static int V_joints_bitmap; +static int W_joints_bitmap; + +static int map_initialized = 0; +#define MAX_COORDINATES_CHARS 32 +static char used_coordinates[MAX_COORDINATES_CHARS+1]; + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + bool found=0; + int dups[EMCMOT_MAX_AXIS]; + const char *coords = coordinates; + char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; + int i; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + for (i=0; i EMCMOT_MAX_JOINTS) ) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s bogus max_joints=%d\n", + errtag,max_joints); + return -1; + } + + // init all axis_idx_for_jno[] (-1 means unspecified) + for(jno=0; jno max_joints) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s too many coordinates <%s> for max_joints=%d\n", + errtag,coordinates,max_joints); + return -1; + } + } // while + + if (!found) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s missing coordinates '%s'\n", + errtag,coordinates); + return -1; + } + if (!allow_duplicates) { + int ano; + for(ano=0; ano 1) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s duplicates not allowed in coordinates=%s, letter=%c\n", + errtag,coordinates,coord_letter[ano]); + return -1; + } + } + } + + for (jno=0; jno < max_joints; jno++) { + int bitnumber = 1<tran.x = joints[JX]; + if ( bit & Y_joints_bitmap ) pos->tran.y = joints[JY]; + if ( bit & Z_joints_bitmap ) pos->tran.z = joints[JZ]; + if ( bit & A_joints_bitmap ) pos->a = joints[JA]; + if ( bit & B_joints_bitmap ) pos->b = joints[JB]; + if ( bit & C_joints_bitmap ) pos->c = joints[JC]; + if ( bit & U_joints_bitmap ) pos->u = joints[JU]; + if ( bit & V_joints_bitmap ) pos->v = joints[JV]; + if ( bit & W_joints_bitmap ) pos->w = joints[JW]; + } + return 0; +} // mapped_joints_to_position() + +int position_to_mapped_joints(const int max_joints, + const EmcPose * pos, + double* joints) +{ + int jno; + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "position_to_mapped_joints before map_initialized\n"); + return -1; + } + for (jno=0; jno < max_joints; jno++) { + int bit = 1<tran.x; + if ( bit & Y_joints_bitmap ) joints[jno] = pos->tran.y; + if ( bit & Z_joints_bitmap ) joints[jno] = pos->tran.z; + if ( bit & A_joints_bitmap ) joints[jno] = pos->a; + if ( bit & B_joints_bitmap ) joints[jno] = pos->b; + if ( bit & C_joints_bitmap ) joints[jno] = pos->c; + if ( bit & U_joints_bitmap ) joints[jno] = pos->u; + if ( bit & V_joints_bitmap ) joints[jno] = pos->v; + if ( bit & W_joints_bitmap ) joints[jno] = pos->w; + } + return 0; +} // position_to_mapped_joints() + +static int identity_kinematics_initialized = 0; +static int identity_max_joints; + +int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)comp_id; + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno; + int show=0; + bool islathe; + + identity_max_joints = strlen(coordinates); + + if (map_coordinates_to_jnumbers(coordinates, + kp->max_joints, + kp->allow_duplicates, + axis_idx_for_jno)) { + return -1; //mapping failed + } + + /* print message for unconventional ordering; + ** a) duplicate coordinate letters + ** b) letters not ordered by "XYZABCUVW" sequence + ** (use kinstype=both works best for these) + */ + for (jno=0; jno Axis %c\n", + jno,*(p+axis_idx_for_jno[jno])); + } + if (kinematicsType() != KINEMATICS_BOTH) { + rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); + } + + identity_kinematics_initialized = 1; + return 0; +} // identityKinematicsSetup() + +int identityKinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsForward: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + mapped_joints_to_position(identity_max_joints,joints,pos); + return 0; +} // identityKinematicsForward() + +int identityKinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsInverse: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + position_to_mapped_joints(identity_max_joints,pos,joints); + + return 0; +} // identityKinematicsInverse() diff --git a/share/linuxcnc/switchkins.c b/share/linuxcnc/switchkins.c new file mode 100644 index 00000000000..7a3f3d35071 --- /dev/null +++ b/share/linuxcnc/switchkins.c @@ -0,0 +1,337 @@ +/* + Copyright 2019 Dewey Garrett + + 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. +*/ + +/* switchkins.c provide functions for switchable kins modules: +* rtapi_app() +* rtapi_exit() +* kinematicsType() +* kinematicsForward() +* kinematicsInverse() +* kinematicsSwitch() +* kinematicsSwitchable() +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +//********************************************************************* +// kinematic functions (default=0 for err detection): +static kparms kp; // kinematics parms (common all types) + +// indexed by switchkins_type (NULL==not provided, for err detection): +static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL}; +static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL}; +static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; + +// types provided, counted in rtapi_app_main() once they are all in +static int kins_count; +static int register_error; + +static int switchkins_type; +static struct swdata { + hal_bool_t kinstype_is[SWITCHKINS_MAX_TYPES]; + + hal_real_t gui_x; + hal_real_t gui_y; + hal_real_t gui_z; + hal_real_t gui_a; + hal_real_t gui_b; + hal_real_t gui_c; +} *swdata; + +// Note: parallel kinematics (like genhexkins) often +// use iterative method for Forward algorithm +// and require an initial EmcPose. +// If fwd_iterates_mask is set +// then save/use the lastpose +static int fwd_iterates[SWITCHKINS_MAX_TYPES] = {0}; +static bool use_lastpose[SWITCHKINS_MAX_TYPES] = {0}; +static EmcPose lastpose[SWITCHKINS_MAX_TYPES]; + +static void save_lastpose(int ktype, EmcPose* pos) +{ + lastpose[ktype].tran.x = pos->tran.x; + lastpose[ktype].tran.y = pos->tran.y; + lastpose[ktype].tran.z = pos->tran.z; + lastpose[ktype].a = pos->a; + lastpose[ktype].b = pos->b; + lastpose[ktype].c = pos->c; + lastpose[ktype].u = pos->u; + lastpose[ktype].v = pos->v; + lastpose[ktype].w = pos->w; +} // save_lastpose() + +static void get_lastpose(int ktype, EmcPose* pos) +{ + pos->tran.x = lastpose[ktype].tran.x; + pos->tran.y = lastpose[ktype].tran.y; + pos->tran.z = lastpose[ktype].tran.z; + pos->a = lastpose[ktype].a; + pos->b = lastpose[ktype].b; + pos->c = lastpose[ktype].c; + pos->u = lastpose[ktype].u; + pos->v = lastpose[ktype].v; + pos->w = lastpose[ktype].w; +} // get_lastpose() + +static int gui_forward_kins(const double *joints) +{ + // the hexapod vismach gui uses these hal pins to + // display platform position/orientation in both + // genhexkins and identity kinematic types + // (similar needs for many parallel kinemtic machines) + int res; + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags; + if ( kp.gui_kinstype < 0 + || kp.gui_kinstype >= kins_count + || !kfwds[kp.gui_kinstype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "gui_forward_kins BAD gui_kinstype <%d>\n", + kp.gui_kinstype); + return -1; + } + res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); + hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); + hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); + hal_set_real(swdata->gui_z, lastpose[kp.gui_kinstype].tran.z); + hal_set_real(swdata->gui_a, lastpose[kp.gui_kinstype].a); + hal_set_real(swdata->gui_b, lastpose[kp.gui_kinstype].b); + hal_set_real(swdata->gui_c, lastpose[kp.gui_kinstype].c); + return res; +} // gui_forward_kins + +//********************************************************************* +int kinematicsSwitchable() {return 1;} + +int kinematicsSwitch(int new_switchkins_type) +{ + int k; + + // reject first, so a bad request leaves the running kinematics alone + if (new_switchkins_type < 0 || new_switchkins_type >= kins_count) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinematicsSwitch:BAD VALUE <%d>\n", + new_switchkins_type); + return -1; // FAIL + } + + for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;} + + switchkins_type = new_switchkins_type; + + rtapi_print_msg(RTAPI_MSG_INFO, + "kinematicsSwitch:TYPE%d\n", switchkins_type); + for (k=0; k < kins_count; k++) { + hal_set_bool(swdata->kinstype_is[k], k == switchkins_type); + } + + if (fwd_iterates[switchkins_type]) { + use_lastpose[switchkins_type] = 1; // restarting a kins types + } + return 0; // 0==> no error +} // kinematicsSwitch() + +int kinematicsForward(const double *joint, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r; + + if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kfwds[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Forward BAD switchkins_type \n", + switchkins_type); + return -1; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);} + if (r) return r; + + // gui.* pins created only if gui_kinstype>=0 + // consider alternate implementations for gui_forward_kins(): + // a) always call and use -1 to select default 0 type + if (kp.gui_kinstype >=0) { + // create gui pins for a vismach gui using the + // kins type specified by kp.gui_kinstype; + // currently the skgui pins are only needed for + // the hexagui vismach program (as it needs + // world coords for switchkin-types + r = gui_forward_kins(joint); + } + + return r; +} // kinematicsForward() + +int kinematicsInverse(const EmcPose * pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + int r; + + if ( switchkins_type < 0 + || switchkins_type >= kins_count + || !kinvs[switchkins_type]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: Inverse BAD switchkins_type \n", + switchkins_type); + return -1; + } + r = kinvs[switchkins_type](pos, joint, iflags, fflags); + return r; +} // kinematicsInverse() + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegister: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + ksetups[ktype] = kset; + kfwds[ktype] = kfwd; + kinvs[ktype] = kinv; + return 0; +} // switchkinsRegister() + +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(switchkinsRegister); +EXPORT_SYMBOL(switchkinsInit); + +//********************************************************************* +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) +{ + int i; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} + + // the highest type registered sets the count + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } + } + if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } + + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (kp.fwd_iterates_mask & (1< EMCMOT_MAX_JOINTS) { + emsg = "bogus max_joints"; goto error; + } + if (kp.gui_kinstype >= kins_count) { + emsg = "bogus gui_kinstype"; goto error; + } + + // a type left out below the highest one provided is a gap, not a count + for (i=0; i < kins_count; i++) { + if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: switchkins-type %d incomplete:%s%s%s\n", + i, + ksetups[i] ? "" : " no setup", + kfwds[i] ? "" : " no forward", + kinvs[i] ? "" : " no inverse"); + emsg = "incomplete switchkins-type"; goto error; + } + + swdata = hal_malloc(sizeof(struct swdata)); + if (!swdata) {emsg = "hal_malloc fail"; goto error;} + + for (i=0; i < kins_count; i++) { + res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), + 0, "kinstype.is-%d", i); + } + + if (kp.gui_kinstype >=0) { + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_x, 0.0, "skgui.x"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_y, 0.0, "skgui.y"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_z, 0.0, "skgui.z"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); + res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); + } + if (res) {emsg = "hal pin create fail"; goto error;} + + switchkins_type = 0; // startup with default type + kinematicsSwitch(switchkins_type); + + if (!coordinates) {coordinates = kp.required_coordinates;} + + for (i=0; i < kins_count; i++) { + ksetups[i](comp_id,coordinates,&kp); + } + + return 0; + +error: + rtapi_print_msg(RTAPI_MSG_ERR, + "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); + return -1; +} // switchkinsInit() diff --git a/src/Makefile b/src/Makefile index 2c56a18ed3a..37f0c6664ad 100644 --- a/src/Makefile +++ b/src/Makefile @@ -193,6 +193,7 @@ SUBDIRS := \ \ $(GUI_SUBDIRS) \ emc/usr_intf/axis emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/canterp \ + emc/motion_planning emc/kinematics_userspace \ emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ @@ -402,6 +403,9 @@ SRCHEADERS := \ hal/drivers/mesa-hostmot2/hostmot2-serial.h \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ + emc/kinematics/switchkins.h \ + emc/kinematics/nonrt_kins.h \ + emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ @@ -773,6 +777,7 @@ ifeq ($(BUILD_GUI),yes) endif $(FILE) ../src/hal/drivers/mesa-hostmot2/modbus/*.tmpl $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs @@ -1190,6 +1195,7 @@ genhexkins-objs += libposemath/_posemath.o genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o +genhexkins-objs += emc/kinematics/switchkins_main.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1199,6 +1205,7 @@ genserkins-objs += libposemath/gomath.o genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o +genserkins-objs += emc/kinematics/switchkins_main.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1206,6 +1213,7 @@ xyzac-trt-kins-objs := emc/kinematics/xyzac-trt-kins.o xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1213,6 +1221,7 @@ xyzbc-trt-kins-objs := emc/kinematics/xyzbc-trt-kins.o xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1221,6 +1230,7 @@ scarakins-objs += libposemath/_posemath.o scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o +scarakins-objs += emc/kinematics/switchkins_main.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1229,6 +1239,7 @@ pumakins-objs += libposemath/_posemath.o pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o +pumakins-objs += emc/kinematics/switchkins_main.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1237,6 +1248,7 @@ three21kins-objs += libposemath/_posemath.o three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o +three21kins-objs += emc/kinematics/switchkins_main.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1245,6 +1257,7 @@ obj-m += 5axiskins.o 5axiskins-objs += $(MATHSTUB) 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o +5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/Makefile.modinc.in b/src/Makefile.modinc.in index ed9d75d98c2..cfcf1bc0b7d 100644 --- a/src/Makefile.modinc.in +++ b/src/Makefile.modinc.in @@ -76,12 +76,12 @@ EXTRA_CFLAGS += -fno-builtin-sin -fno-builtin-cos -fno-builtin-sincos EMC2_HOME=@EMC2_HOME@ RUN_IN_PLACE=@RUN_IN_PLACE@ ifeq ($(RUN_IN_PLACE),yes) -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include -I$(EMC2_HOME)/share/linuxcnc RTLIBDIR := @EMC2_HOME@/rtlib LIBDIR := @EMC2_HOME@/lib else prefix := @prefix@ -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc -I${prefix}/share/linuxcnc RTLIBDIR := @EMC2_RTLIB_DIR@ LIBDIR := @libdir@ endif diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 387c32e23df..16bbf285628 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,9 +59,9 @@ #include #include #include -#include -#include "switchkins.h" +#include +#include static struct haldata { hal_real_t pivot_length; @@ -159,11 +159,20 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// module constants, shared by switchkinsSetup() and nonrt_attach() +static void fiveaxis_kparms(kparms* kp) +{ + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; +} + +// assign principal joint numbers from the coordinates string. +// No HAL involvement, so the non-RT path can use it too. +static int fiveaxis_map_joints(const char* coordinates, kparms* kp) { - int result=0; int i,jno; int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; int minjoints = strlen(kp->required_coordinates); @@ -212,6 +221,20 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + return 0; + +error: + return -1; +} // fiveaxis_map_joints() + +int fiveaxis_KinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + int result=0; + + if (fiveaxis_map_joints(coordinates, kp)) goto error; + haldata = hal_malloc(sizeof(*haldata)); if(!haldata) goto error; @@ -240,11 +263,7 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; + fiveaxis_kparms(kp); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); @@ -271,3 +290,28 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +// Non-RT entry point: bind this copy of the module to the pins the +// running RT instance owns, then hand back the unmodified kinematics. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + static struct haldata nonrt_haldata; // private to this copy of the module + kparms kp = {0}; + + fiveaxis_kparms(&kp); + + haldata = &nonrt_haldata; + + if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, + "%s.pivot-length", kp.halprefix)) return -1; + + if (fiveaxis_map_joints(coordinates, &kp)) return -1; + + ops->forward = fiveaxis_KinematicsForward; + ops->inverse = fiveaxis_KinematicsInverse; + ops->is_identity = 0; + return 0; +} // nonrt_attach() + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..7e2f2d84b4b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -33,3 +33,16 @@ $(RDELTAMODULE): $(call TOOBJS, $(RDELTAMODULESRCS)) $(ECHO) Linking python module $(notdir $@) $(CXX) $(LDFLAGS) -shared -o $@ $^ $(BOOST_PYTHON_LIB) PYTARGETS += $(RDELTAMODULE) + +# The switchkins implementation is shipped as source, since a realtime module +# cannot link a library, so a module built out of tree includes it the way the +# in-tree ones link it. +EMCKINEMATICSSRCS = \ + ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/kins_util.c + +$(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c + $(ECHO) Copying switchkins source $(notdir $@) + $(Q)cp -f $< $@ + +TARGETS += $(EMCKINEMATICSSRCS) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 3cddd9a72bc..978948dc463 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -110,10 +110,9 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" -#include "switchkins.h" +#include static struct haldata { hal_real_t basex[NUM_STRUTS]; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 64fe55983e1..9e6284d5513 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -42,7 +42,7 @@ frame-larger-than: #include #include "genserkins.h" -#include "switchkins.h" +#include //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h new file mode 100644 index 00000000000..ed564f21b6f --- /dev/null +++ b/src/emc/kinematics/nonrt_kins.h @@ -0,0 +1,95 @@ +/******************************************************************** + * Description: nonrt_kins.h + * Interface a kinematics module exports so that a non-RT caller can + * evaluate it. + * + * A trajectory planner needs forward and inverse kinematics at poses + * the machine has not reached yet, which means calling them outside + * the servo thread. A module opts in by exporting nonrt_attach(). + * + * The caller dlopens the module and calls nonrt_attach() once with + * the coordinates string and a resolver callback. The module names + * each of the pins it reads, keeps the references the resolver + * returns in its own haldata, and hands back its existing forward + * and inverse. The kinematics code itself does not change. + * + * A reference does not point into the RT instance's pin. The + * resolver creates an input pin on the caller's own component and + * connects it to the signal the RT pin reads, so the reference + * belongs to the caller and rewiring cannot strand it. + * + * Name lookup belongs to the caller, userspace code linked against + * liblinuxcnchal. This file is compiled into an RT module, which + * has no business walking the HAL name space and would risk binding + * against rtlib's copy of the same symbols. + * + * Resolve input pins only. Output pins and scratch storage stay + * private to the non-RT copy, or the two copies write to each + * other's state. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef NONRT_KINS_H +#define NONRT_KINS_H + +#include + +#include +#include +#include +#include + +/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has + type 'type', and writes to 'out' a reference carrying that pin's + value. The reference is to storage the caller owns, not to the named + pin itself. Returns 0 on success. */ +typedef int (*nonrt_resolve_fn)(const char *pin_name, + hal_type_t type, + hal_refs_u *out, + void *arg); + +/* Filled in by nonrt_attach(). A module that reports is_identity has + joints equal to axes and the caller needs no module code at all, so + forward and inverse may be left NULL. */ +typedef struct { + int (*forward)(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + int (*inverse)(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + int is_identity; +} nonrt_ops_t; + +/* Exported by a participating module: + int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + Returns 0 on success. */ + +/* Convenience for the common case: resolve one float pin, by printf + style name, into a haldata field. */ +static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, + hal_real_t *dst, const char *fmt, ...) +{ + char name[HAL_NAME_LEN + 1]; + hal_refs_u ref; + va_list ap; + + if (!resolve || !dst) return -1; + + va_start(ap, fmt); + rtapi_vsnprintf(name, sizeof(name), fmt, ap); + va_end(ap); + + if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; + + *dst = ref.r; + return 0; +} + +#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index f055e73a502..602a9acca37 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,10 +20,9 @@ #include #include #include -#include #include "pumakins.h" -#include "switchkins.h" +#include struct haldata { hal_real_t a2, a3, d3, d4, d6; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 2454a67bfb2..a36fafbfc9b 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,9 +19,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct scara_data { hal_real_t d1, d2, d3, d4, d5, d6; diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f1393867e35..7a3f3d35071 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,8 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include #include #include -#include #include "switchkins.h" @@ -240,47 +238,31 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() -//********************************************************************* -static char *coordinates; -RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static char *sparm; -RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(switchkinsRegister); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsInit); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) { - int i,res; - char* emsg="other"; - - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // may also call switchkinsRegister() - res = switchkinsSetup(&kp, - &ksetups[0], &ksetups[1], &ksetups[2], - &kfwds[0], &kfwds[1], &kfwds[2], - &kinvs[0], &kinvs[1], &kinvs[2]); - if (res) {emsg="switchkinsSetp FAIL"; goto error;} - if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} - - // the highest type provided by either route sets the count + int i; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} + + // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } } @@ -319,11 +301,8 @@ int rtapi_app_main(void) emsg = "incomplete switchkins-type"; goto error; } - comp_id = hal_init(kp.kinsname); - if(comp_id < 0) goto error; - swdata = hal_malloc(sizeof(struct swdata)); - if (!swdata) goto error; + if (!swdata) {emsg = "hal_malloc fail"; goto error;} for (i=0; i < kins_count; i++) { res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), @@ -337,8 +316,8 @@ int rtapi_app_main(void) res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); - if (res) {emsg = "hal pin create fail";goto error;} } + if (res) {emsg = "hal pin create fail"; goto error;} switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); @@ -349,14 +328,10 @@ int rtapi_app_main(void) ksetups[i](comp_id,coordinates,&kp); } - hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); - hal_exit(comp_id); return -1; -} // rtapi_app_main() - -void rtapi_app_exit(void) { hal_exit(comp_id); } +} // switchkinsInit() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 2f9ee530a7c..5fb1a12a42e 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,10 +1,10 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //max number of switchkins types (KS,KF,KI) a module may provide: #define SWITCHKINS_MAX_TYPES 9 @@ -28,13 +28,20 @@ typedef int (*KS)(const int comp_id, // halpins ); //********************************************************************* -// supplied by the using module, provides types 0,1,2 +// supplied by a module using switchkins_main.c, provides types 0,1,2 extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ); -// called from switchkinsSetup(), once per type it does not provide itself +// provide one switchkins-type, before switchkinsInit() extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); -#endif // } + +// create the hal pins and start on type 0; the caller owns the hal +// component and does hal_init() before and hal_ready() after +extern int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates + ); +#endif diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..4a4cc05153c --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,94 @@ +/* + Copyright 2019 Dewey Garrett + + 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. +*/ + +/* switchkins_main.c provides rtapi_app_main() for kinematics modules +* built around switchkins.c. A module that gets its rtapi_app_main() +* from somewhere else (a halcompile component, for instance) links +* switchkins.c alone and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); +static char *sparm; +RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + kparms kp; + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + // defaults prior to switchkinsSetup() call + kp.kinsname = NULL; + kp.halprefix = NULL; + kp.required_coordinates = ""; + kp.max_joints = 0; // Setup must supply + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; // negative means: not used + + kp.sparm = sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() for any others + if (switchkinsSetup(&kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp.kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + comp_id = hal_init(kp.kinsname); + if (comp_id < 0) return comp_id; + + if (switchkinsInit(comp_id, &kp, coordinates)) { + hal_exit(comp_id); + return -1; + } + + hal_ready(comp_id); + return 0; +} // rtapi_app_main() + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 219f3877427..abc346b33db 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -2,9 +2,8 @@ #include #include #include -#include -#include "switchkins.h" +#include /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 3ea56b49aa8..848f7fa7918 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -18,6 +18,7 @@ #include #include #include +#include "nonrt_kins.h" #define SET(f) pos->f = joints[i] @@ -85,3 +86,17 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +// Non-RT entry point: joints are axes, so a non-RT caller needs no +// module code at all and reads nothing from HAL. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + (void)coordinates; (void)resolve; (void)arg; + ops->forward = NULL; + ops->inverse = NULL; + ops->is_identity = 1; + return 0; +} + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 47655ec0f14..c818ae4075a 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index aa1289baf28..142de97312c 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..69abd527dac --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,381 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports, so this process evaluates the kinematics the machine is + * running, at whatever poses it likes. See nonrt_kins.h. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. A module + * exporting no nonrt_attach() is not an error either; the context comes + * back flagged rt_only. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + +/* One per value a kinematics module reads is a generous bound. */ +#define MAX_MADE_SIGNALS 16 +#define MAX_BOUND_PINS 16 + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no nonrt_attach() */ + int is_identity; /* 1 for identity kinematics: no module code needed */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + nonrt_ops_t ops; + int num_joints; + int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ + char module_name[64]; + int comp_id; /* the caller's component, owns the pins made here */ + const char *prefix; /* its name, which those pin names start with */ + char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; + int num_made_signals; + hal_refs_u *cell; /* HAL storage those pins are made against */ + int num_cells; +}; + +/* ======================================================================== + * Pin binding + * ======================================================================== */ + +/* + * Give a kinematics module a reference to a value it asked for. + * + * The reference is to a pin of ours rather than into the RT instance's, + * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to + * the signal the RT pin reads, or, when the RT pin has no signal, to one + * made here and removed again in kinematicsUserFree(). + * + * The reference has to live in HAL shared memory, since that is where + * HAL rewrites it on connect and disconnect, so the pins are made + * against hal_malloc() cells and the module gets what a cell holds once + * the connection is in place. + */ +static int make_signal(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type, char *out, size_t outlen) +{ + if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { + fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); + return -1; + } + if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { + fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", + pin_name); + return -1; + } + if (hal_signal_new(out, type) != 0) return -1; + if (hal_link(pin_name, out) != 0) { + hal_signal_delete(out); + return -1; + } + snprintf(ctx->made_signal[ctx->num_made_signals++], + sizeof(ctx->made_signal[0]), "%s", out); + return 0; +} + +static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, + const char *name) +{ + switch (type) { + case HAL_BIT: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); + case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + default: break; + } + return -1; +} + +static int bind_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)arg; + char signal[HAL_NAME_LEN + 1]; + char mine[HAL_NAME_LEN + 1]; + hal_refs_u *cell; + hal_query_t q; + + if (!ctx || !pin_name || !out) return -1; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + + if (hal_getref_p(&q) != 0) { + fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); + return -1; + } + if (q.pp.type != type) { + fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + pin_name); + return -1; + } + + if (q.pp.signal) { + snprintf(signal, sizeof(signal), "%s", q.pp.signal); + } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { + fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + return -1; + } + + if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) + >= sizeof(mine)) { + fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", + pin_name); + return -1; + } + if (ctx->num_cells >= MAX_BOUND_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); + return -1; + } + cell = &ctx->cell[ctx->num_cells++]; + + if (new_pin(ctx->comp_id, type, cell, mine) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); + return -1; + } + if (hal_link(mine, signal) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", + mine, signal); + return -1; + } + + *out = *cell; + return 0; +} + +/* ======================================================================== + * Identity joint mapping + * ======================================================================== */ + +static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +{ + int i, j = 0; + for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; + if (!coords) return; + for (; *coords && j < ctx->num_joints; coords++) { + int axis; + switch (tolower((unsigned char)*coords)) { + case 'x': axis = 0; break; case 'y': axis = 1; break; + case 'z': axis = 2; break; case 'a': axis = 3; break; + case 'b': axis = 4; break; case 'c': axis = 5; break; + case 'u': axis = 6; break; case 'v': axis = 7; break; + case 'w': axis = 8; break; default: continue; + } + ctx->joint_to_axis[j++] = axis; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates) +{ + char module_path[512]; + void *handle; + nonrt_attach_fn attach; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); + if (!attach) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->ops.is_identity) { + ctx->is_identity = 1; + ctx->kins_type = KINEMATICS_IDENTITY; + return 0; + } + + if (!ctx->ops.forward || !ctx->ops.inverse) { + fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + ctx->kins_type = KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + ctx->prefix = prefix; + + ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); + if (!ctx->cell) { + fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); + free(ctx); + return NULL; + } + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + load_module(ctx, kins_type, coordinates); + + if (ctx->is_identity) { + fill_identity_joint_map(ctx, coordinates); + } + + ctx->initialized = 1; + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) return -1; + + if (ctx->is_identity) { + int i; + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.inverse(world, joints, NULL, NULL); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) return -1; + + if (ctx->is_identity) { + int i; + memset(world, 0, sizeof(*world)); + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.forward(joints, world, NULL, NULL); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + (void)ctx; + return 0; /* nothing to refresh: the bound pins are the live values */ +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + int i; + + if (!ctx) return; + + /* Removing one hands its value back to the RT pin, leaving the + machine as it was found. */ + for (i = 0; i < ctx->num_made_signals; i++) { + hal_signal_delete(ctx->made_signal[i]); + } + if (ctx->rt_handle) dlclose(ctx->rt_handle); + free(ctx); +} diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h new file mode 100644 index 00000000000..d01d8a8d277 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The kinematics module is loaded into this process and given input pins + * belonging to the caller's HAL component, connected to the same signals + * the running RT instance reads. Its own forward and inverse then work on + * live values, unmodified. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include /* EmcPose */ +#include /* KINEMATICS_TYPE, flags */ +#include /* hal_type_t, HAL_NAME_LEN */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +/* Axis coordinate indices for EmcPose */ +typedef enum { + AXIS_X = 0, AXIS_Y = 1, AXIS_Z = 2, + AXIS_A = 3, AXIS_B = 4, AXIS_C = 5, + AXIS_U = 6, AXIS_V = 7, AXIS_W = 8, + AXIS_COUNT = 9 +} AxisIndex; + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * The pins this creates belong to the caller's component, so call this + * after hal_init() and before hal_ready(): HAL refuses new pins once a + * component is ready. + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name, which the created pin names start with + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters (no-op) + * + * The bound pins read the live values, so there is nothing to fetch. + * This function is kept for API compatibility but does nothing. + * + * @param ctx Kinematics context + * @return 0 always + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 8905ad05d13..22b51ac533f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2054,6 +2054,11 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) axis_set_locking_joint(emcmotCommand->axis, joint_num); break; + case EMCMOT_SELECT_KINS_TYPE: + emcmotConfig->switchkins_type = emcmotCommand->switchkins_type; + emcmotConfig->switchkins_seq++; + 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 2ddf587b484..52f8d9e0ada 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -300,12 +300,42 @@ static bool joint_jog_is_active(void) { static void handle_kinematicsSwitch(void) { int joint_num; int hal_switchkins_type = 0; + static int prev_hal_switchkins_type = 0; + static int said_hal_is_deprecated = 0; + int requested_type; if (!kinematicsSwitchable()) return; + + /* Two things can ask for a kinematics: G12.1/G13.1, and the + motion.switchkins-type pin. Both are taken on their edge, so that + whichever asked most recently wins. Writing the pin here instead + would not work: configs source it from an analog output, which + would put its own value back on the next servo cycle. */ hal_switchkins_type = (int)hal_get_real(emcmot_hal_data->switchkins_type); - if (switchkins_type == hal_switchkins_type) return; + requested_type = switchkins_type; + + if (emcmotStatus->switchkins_seq != emcmotConfig->switchkins_seq) { + requested_type = emcmotConfig->switchkins_type; + emcmotStatus->switchkins_seq = emcmotConfig->switchkins_seq; + } else if (hal_switchkins_type != prev_hal_switchkins_type) { + requested_type = hal_switchkins_type; + /* Once per session. The pin cannot become the general way to + switch: the interpreter does not see it, so a program is read, + its limits checked and its path looked ahead in whatever + kinematics the interpreter last knew about. */ + if (!said_hal_is_deprecated) { + said_hal_is_deprecated = 1; + reportError(_("motion.switchkins-type is deprecated, use G12.1 and" + " G13.1. Switching kinematics from HAL is invisible" + " to the interpreter, so limits and look ahead go on" + " using the kinematics it last knew about.")); + } + } + prev_hal_switchkins_type = hal_switchkins_type; - switchkins_type = hal_switchkins_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->switchkins_type = switchkins_type; + if (switchkins_type == requested_type) return; emcmot_joint_t *jointKinsSwitch; double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,}; @@ -317,13 +347,22 @@ static void handle_kinematicsSwitch(void) { joint_posKinsSwitch[joint_num] = jointKinsSwitch->pos_cmd; } - if (kinematicsSwitch(switchkins_type)) { - rtapi_print_msg(RTAPI_MSG_ERR,"kinematicsSwitch() FAIL<%f>\n", - hal_get_real(emcmot_hal_data->switchkins_type)); + /* a module refuses a type it does not provide and goes on running the + one it has, so nothing is recorded until the switch has happened */ + if (kinematicsSwitch(requested_type)) { + rtapi_print_msg(RTAPI_MSG_ERR,"kinematicsSwitch() FAIL<%d>\n", + requested_type); + reportError(_("kinematics type %d is not provided by this module," + " type %d is still in force"), + requested_type, switchkins_type); SET_MOTION_ERROR_FLAG(1); // abort - return; // no updates for abort + return; // the kinematics in force is unchanged } + switchkins_type = requested_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->switchkins_type = switchkins_type; + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 64cbb507132..a996e183fa6 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -198,6 +198,7 @@ typedef struct { hal_real_t feed_mm_per_second; /* feed mm per second*/ hal_real_t switchkins_type; + hal_real_t kins_type; /* Interp State Pins */ hal_sint_t interp_line_number; hal_sint_t interp_motion_type; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index d2cb7615958..9a7985902fe 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -661,6 +661,7 @@ static int init_hal_io(void) if (kinematicsSwitchable()) { CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_IN, &(emcmot_hal_data->switchkins_type), 0.0, "motion.switchkins-type")); + CALL_CHECK(hal_pin_new_real(mot_comp_id, HAL_OUT, &(emcmot_hal_data->kins_type), 0.0, "motion.kins-type")); } /* export spindle pins and params */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1312b5e45dd..351e387f0b1 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -174,8 +174,9 @@ extern "C" { EMCMOT_SET_AXIS_VEL_LIMIT, /* set the max axis vel */ EMCMOT_SET_AXIS_ACC_LIMIT, /* set the max axis acc */ EMCMOT_SET_AXIS_LOCKING_JOINT, /* set the axis locking joint */ - EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */ + EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */ + EMCMOT_SELECT_KINS_TYPE, /* select the switchkins type (G12.1) */ EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ } cmd_code_t; @@ -270,6 +271,8 @@ extern "C" { double ext_offset_vel; /* velocity for an external axis offset */ double ext_offset_acc; /* acceleration for an external axis offset */ struct state_tag_t tag; + + int switchkins_type; /* switchkins type requested by G12.1 */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -667,6 +670,9 @@ Suggestion: Split this in to an Error and a Status flag register.. int numExtraJoints; int stepping; bool jogging_active; + + int switchkins_seq; /* echoes the config counter once acted on */ + int switchkins_type; /* switchkins type now in force */ } emcmot_status_t; /********************************* @@ -738,6 +744,10 @@ Suggestion: Split this in to an Error and a Status flag register.. double maxFeedScale; int inhibit_probe_jog_error; int inhibit_probe_home_error; + + int switchkins_type; /* switchkins type requested by G12.1 */ + int switchkins_seq; /* bumped per request, so a repeat of + the same type is still seen */ } emcmot_config_t; /* error structure - lockfree MPSC ring buffer. See emcmotutil.c. */ diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..553849e7ba5 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,46 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.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/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..61adca56f1f --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,104 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +// emcpos.h includes posemath.h which has C++ function overloads +// so we can't use extern "C" around it +#include + +extern "C" { +#include +} + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..a7785ddaea4 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,238 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +// emcpos.h includes posemath.h which has C++ function overloads +#include + +extern "C" { +#include +} + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc new file mode 100644 index 00000000000..1b750be6776 --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,269 @@ +/******************************************************************** + * Description: kinslimits.cc + * Diagnostic tool: print the Jacobian and the world-space velocity, + * acceleration and jerk caps that a given kinematics module imposes + * on a straight move between two poses. + * + * 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. + * + * Example (in a terminal with a running config, or under halrun): + * + * halrun -I + * halcmd: loadrt 5axiskins coordinates=XYZBCW + * halcmd: setp 5axiskins.pivot-length 100 + * halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \ + * --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \ + * --end 100,0,0,0,90,0,0,0,0 \ + * --vel 100,100,100,30,30,30 --acc 500,500,500,200,200,200 + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include "jacobian.hh" +#include "joint_limits.hh" + +using namespace motion_planning; + +static const char *AXIS_NAME[9] = {"X","Y","Z","A","B","C","U","V","W"}; + +static std::vector parse_list(const char *s) +{ + std::vector out; + const char *p = s; + while (*p) { + char *endp = nullptr; + double v = strtod(p, &endp); + if (endp == p) break; + out.push_back(v); + p = endp; + while (*p == ',' || *p == ' ') p++; + } + return out; +} + +static void list_to_pose(const std::vector& v, EmcPose *p) +{ + double a[9] = {0,0,0,0,0,0,0,0,0}; + for (size_t i = 0; i < v.size() && i < 9; i++) a[i] = v[i]; + p->tran.x = a[0]; p->tran.y = a[1]; p->tran.z = a[2]; + p->a = a[3]; p->b = a[4]; p->c = a[5]; + p->u = a[6]; p->v = a[7]; p->w = a[8]; +} + +static double pose_axis(const EmcPose& p, int ax) +{ + switch (ax) { + case 0: return p.tran.x; case 1: return p.tran.y; case 2: return p.tran.z; + case 3: return p.a; case 4: return p.b; case 5: return p.c; + case 6: return p.u; case 7: return p.v; default: return p.w; + } +} + +static void set_pose_axis(EmcPose *p, int ax, double val) +{ + switch (ax) { + case 0: p->tran.x = val; break; case 1: p->tran.y = val; break; + case 2: p->tran.z = val; break; case 3: p->a = val; break; + case 4: p->b = val; break; case 5: p->c = val; break; + case 6: p->u = val; break; case 7: p->v = val; break; + default: p->w = val; break; + } +} + +static void usage(const char *argv0) +{ + fprintf(stderr, + "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" + " [--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); +} + +int main(int argc, char **argv) +{ + const char *module = nullptr; + const char *coords = nullptr; + int num_joints = 0; + int samples = 11; + double singularity = 100.0; + std::vector start_v, end_v, vel_v, acc_v, jerk_v; + + for (int i = 1; i < argc; i++) { + const char *a = argv[i]; + const char *next = (i + 1 < argc) ? argv[i + 1] : nullptr; + if (!strcmp(a, "--module") && next) { module = next; i++; } + else if (!strcmp(a, "--coords") && next) { coords = next; i++; } + else if (!strcmp(a, "--joints") && next) { num_joints = atoi(next); i++; } + else if (!strcmp(a, "--samples") && next) { samples = atoi(next); i++; } + else if (!strcmp(a, "--singularity") && next){ singularity = atof(next); i++; } + else if (!strcmp(a, "--start") && next) { start_v = parse_list(next); i++; } + else if (!strcmp(a, "--end") && next) { end_v = parse_list(next); i++; } + 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 { usage(argv[0]); return 1; } + } + + if (!module || !coords || num_joints < 1 || + start_v.empty() || end_v.empty() || vel_v.empty() || acc_v.empty()) { + usage(argv[0]); + return 1; + } + if ((int)vel_v.size() < num_joints || (int)acc_v.size() < num_joints) { + fprintf(stderr, "kinslimits: --vel and --acc need %d entries\n", num_joints); + return 1; + } + if (samples < 2) samples = 2; + + int comp_id = hal_init("kinslimits"); + if (comp_id < 0) { + fprintf(stderr, "kinslimits: hal_init failed (is HAL running?)\n"); + return 1; + } + + KinematicsUserContext *ctx = kinematicsUserInit(module, num_joints, coords, + comp_id, "kinslimits"); + if (!ctx) { + fprintf(stderr, "kinslimits: kinematicsUserInit failed for '%s'\n", module); + hal_exit(comp_id); + return 1; + } + if (kinematicsUserIsRtOnly(ctx)) { + fprintf(stderr, "kinslimits: '%s' is RT-only, no non-RT interface\n", module); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + JacobianCalculator jac; + JointLimitCalculator lim; + if (!jac.init(ctx) || !lim.init(num_joints)) { + fprintf(stderr, "kinslimits: calculator init failed\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + std::vector minpos(num_joints, -1e9), maxpos(num_joints, 1e9); + if ((int)jerk_v.size() < num_joints) jerk_v.assign(num_joints, 1e9); + lim.updateAllLimits(vel_v.data(), acc_v.data(), + minpos.data(), maxpos.data(), jerk_v.data()); + + EmcPose start, end; + list_to_pose(start_v, &start); + list_to_pose(end_v, &end); + + /* Path parameter: XYZ arc length, falling back to the largest rotary + delta for a pure rotary move, matching what the planner uses. */ + double dx = end.tran.x - start.tran.x; + double dy = end.tran.y - start.tran.y; + double dz = end.tran.z - start.tran.z; + double target = sqrt(dx*dx + dy*dy + dz*dz); + if (target < 1e-12) { + for (int ax = 3; ax < 9; ax++) { + double d = fabs(pose_axis(end, ax) - pose_axis(start, ax)); + if (d > target) target = d; + } + } + if (target < 1e-12) { + fprintf(stderr, "kinslimits: start and end are the same pose\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + /* tangent[a] = d(world axis a) / d(path parameter) */ + double tangent[9]; + for (int ax = 0; ax < 9; ax++) { + tangent[ax] = (pose_axis(end, ax) - pose_axis(start, ax)) / target; + } + + printf("module : %s (%s, %d joints)%s\n", module, coords, num_joints, + kinematicsUserIsIdentity(ctx) ? " [identity]" : ""); + printf("path length : %.6f (tangent units per path unit)\n", target); + printf("tangent :"); + for (int ax = 0; ax < 9; ax++) { + if (fabs(tangent[ax]) > 1e-12) printf(" %s=%.4f", AXIS_NAME[ax], tangent[ax]); + } + printf("\n\n"); + + double min_vel = 1e9, min_acc = 1e9, min_jerk = 1e9, max_cond = 1.0; + int at_vel = -1, at_acc = -1, at_jerk = -1; + double min_vel_s = 0.0; + + for (int i = 0; i < samples; i++) { + double frac = (double)i / (double)(samples - 1); + EmcPose p; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(&p, ax, + pose_axis(start, ax) + frac * (pose_axis(end, ax) - pose_axis(start, ax))); + } + + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + if (kinematicsUserInverse(ctx, &p, joints) != 0) { + printf("sample %2d: inverse kinematics failed\n", i); + continue; + } + + double J[9][9]; + if (!jac.compute(p, J)) { + printf("sample %2d: Jacobian failed\n", i); + continue; + } + + double jpad[9] = {0}; + for (int j = 0; j < num_joints && j < 9; j++) jpad[j] = joints[j]; + + JointLimitResult r; + if (!lim.computeForTangent(J, jpad, tangent, r, singularity)) { + printf("sample %2d: limit calculation failed\n", i); + continue; + } + + printf("s=%.3f vel<=%10.3f (j%d) acc<=%10.1f (j%d) jerk<=%12.1f (j%d) cond=%.2f\n", + frac, r.max_world_vel, r.limiting_joint_vel, + r.max_world_acc, r.limiting_joint_acc, + r.max_world_jerk, r.limiting_joint_jerk, r.condition_number); + + if (r.max_world_vel < min_vel) { min_vel = r.max_world_vel; at_vel = r.limiting_joint_vel; min_vel_s = frac; } + if (r.max_world_acc < min_acc) { min_acc = r.max_world_acc; at_acc = r.limiting_joint_acc; } + if (r.max_world_jerk < min_jerk) { min_jerk = r.max_world_jerk; at_jerk = r.limiting_joint_jerk; } + if (r.condition_number > max_cond) max_cond = r.condition_number; + + if (i == 0) { + printf(" Jacobian at start (rows = joints, cols = XYZABCUVW):\n"); + for (int j = 0; j < num_joints && j < 9; j++) { + printf(" j%d:", j); + for (int ax = 0; ax < 9; ax++) printf(" %8.4f", J[j][ax]); + printf("\n"); + } + } + } + + printf("\nsegment cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), jerk %.1f (joint %d)\n", + min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); + printf("worst cond : %.3f\n", max_cond); + + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 0; +} diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 916b3e92971..4889283769f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -897,6 +897,9 @@ extern int GET_EXTERNAL_MIST(); // Returns the current motion control mode extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE(); +// Returns the kinematics type motion is running (G12.1, G13.1) +extern int GET_EXTERNAL_KINS_TYPE(); + // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); @@ -1070,4 +1073,7 @@ extern int GET_EXTERNAL_OFFSET_APPLIED(); extern EmcPose GET_EXTERNAL_OFFSETS(); extern void UPDATE_TAG(const StateTag& tag); +// adjust kins offset (G12.1 kinematics switch) +extern void SELECT_KINS_TYPE(int switchkins_type); + #endif /* ifndef CANON_HH */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 3eb7360bdd3..a2e8e65cb7e 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -296,6 +296,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_OFFSET_TYPE: ((EMC_TRAJ_SET_OFFSET *) buffer)->update(cms); break; + case EMC_TRAJ_SELECT_KINS_TYPE: + ((EMC_TRAJ_SELECT_KINS *) buffer)->update(cms); + break; case EMC_TRAJ_SET_G5X_TYPE: ((EMC_TRAJ_SET_G5X *) buffer)->update(cms); break; @@ -520,6 +523,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_MODE"; case EMC_TRAJ_SET_OFFSET_TYPE: return "EMC_TRAJ_SET_OFFSET"; + case EMC_TRAJ_SELECT_KINS_TYPE: + return "EMC_TRAJ_SELECT_KINS"; case EMC_TRAJ_SET_G5X_TYPE: return "EMC_TRAJ_SET_G5X"; case EMC_TRAJ_SET_G92_TYPE: @@ -1591,6 +1596,13 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) EmcPose_update(cms, &offset); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SELECT_KINS::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + cms->update(switchkins_type); +} + /* * NML/CMS Update function for EMC_TRAJ_CMD_MSG * Automatically generated by NML CodeGen Java Applet. @@ -1726,6 +1738,9 @@ void EMC_TRAJ_STAT::update(CMS * cms) cms->update(feed_override_enabled); cms->update(adaptive_feed_enabled); cms->update(feed_hold_enabled); + cms->update(switchkins_type); + cms->update(switchkins_seq); + cms->update(switchkins_changed); } /* diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2738b34144b..c2e83d1d379 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -112,6 +112,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) +#define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) // EMC_MOTION aggregate class type declaration @@ -214,7 +215,8 @@ enum class EMC_TASK_EXEC { WAITING_FOR_MOTION_AND_IO = 7, WAITING_FOR_DELAY = 8, WAITING_FOR_SYSTEM_CMD = 9, - WAITING_FOR_SPINDLE_ORIENTED = 10 + WAITING_FOR_SPINDLE_ORIENTED = 10, + WAITING_FOR_KINS_SWITCH = 11 }; // types for EMC_TASK interpState @@ -460,6 +462,7 @@ int emcSetupArcBlends(int arcBlendEnable, int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit); int emcGetExternalOffsetApplied(void); EmcPose emcGetExternalOffsets(void); +extern int emcSelectKinsType(int switchkins_type); extern int emcUpdate(EMC_STAT * stat); // full EMC status diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 5cede52b09f..9ed92f6b3a3 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,13 +960,27 @@ class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { double vel, ini_maxvel, acc, scale, ini_maxjerk; }; +class EMC_TRAJ_SELECT_KINS:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SELECT_KINS():EMC_TRAJ_CMD_MSG(EMC_TRAJ_SELECT_KINS_TYPE, + sizeof(EMC_TRAJ_SELECT_KINS)), + switchkins_type(0) + {}; + + int switchkins_type; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); +}; + // EMC_TRAJ status base class class EMC_TRAJ_STAT_MSG:public RCS_STAT_MSG { public: EMC_TRAJ_STAT_MSG(NMLTYPE t, size_t s) : RCS_STAT_MSG(t, s) {}; - // For internal NML/CMS use only. void update(CMS * cms); }; @@ -1025,6 +1039,11 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { //bool spindle_override_enabled; moved to SPINDLE_STAT bool adaptive_feed_enabled; bool feed_hold_enabled; + + int switchkins_type; // switchkins type now in force + int switchkins_seq; // motion's request counter, echoed once seen + bool switchkins_changed; // a switch landed, task has yet to synch + StateTag tag; }; @@ -1167,6 +1186,7 @@ class EMC_MOTION_STAT:public EMC_MOTION_STAT_MSG { int numExtraJoints; bool jogging_active; uint64_t heartbeat; // motion controller's heartbeat counter + }; // declarations for EMC_TASK classes diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index 916e3e3e49a..437c6d08019 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -94,6 +94,9 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT() feed_override_enabled(OFF), adaptive_feed_enabled(OFF), feed_hold_enabled(OFF), + switchkins_type(0), + switchkins_seq(0), + switchkins_changed(false), tag() { } diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 3b15edea612..c935a9f5492 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -890,6 +890,7 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} +void SELECT_KINS_TYPE(int /*switchkins_type*/) {} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} @@ -1196,6 +1197,7 @@ 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; } void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 74897b34650..63f6d20975a 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -76,7 +76,7 @@ const int Interp::gees[] = { /* 60 */ 1, 1, 1, 0,-1,-1,-1,-1,-1,-1,15,-1,-1,-1,-1,-1,-1,-1,-1,-1, // jjf added G6 /* 80 */ 15,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 100 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 120 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 120 */ -1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1, /* 140 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 160 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, /* 180 */ 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, 2, 2,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 196f2772763..40a1fe152fd 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -109,6 +109,11 @@ 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_12_1){ + // kins-switch + CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); + } else if (mode0 == G_13_1){ + // kins-switch cancel: no words, the kinematics goes back to 0 } else ERS(NCE_BUG_BAD_G_CODE_MODAL_GROUP_0); return INTERP_OK; @@ -319,7 +324,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->p_flag) { - CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64) && + 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)) && (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) && @@ -331,7 +336,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 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); int p_value = round_to_int(block->p_number); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 638e87eb0c3..e70db391422 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4351,7 +4351,20 @@ 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)); // handled elsewhere + 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 ever planned across a change of kinematics. With nothing + // queued there is nothing to wait for, and asking to wait is actively + // harmful: an ON_ABORT_COMMAND routine is run by a single execute() + // call that cannot service INTERP_EXECUTE_FINISH, so the rest of the + // routine would be silently dropped. The queue is empty there because + // the abort has just flushed it. + if (!GET_EXTERNAL_QUEUE_EMPTY()) { + settings->kinsSwitch_flag = true; + } + CHP(convert_kins_switch(code, block, settings)); + } else ERS(NCE_BUG_CODE_NOT_G4_G10_G28_G30_G52_G53_OR_G92_SERIES); return INTERP_OK; @@ -6490,6 +6503,38 @@ int Interp::convert_tool_select(block_pointer block, //!< pointer to a block return INTERP_OK; } +/*! convert_kins_switch + +Returned Value: int (INTERP_OK) + +Side effects: + The selected kinematics is sent to the motion controller and recorded + in the interpreter so that #<_kins_type> reports it. + +Called by: convert_modal_0 + +G12.1 P- selects a kinematics; G13.1 cancels back to kinematics 0, which +is the same thing as G12.1 P0 and exists so that the pair reads the way +it does on other controls. Both are queue synchronisation points: the +caller sets kinsSwitch_flag, which makes the interpreter wait for motion +to drain before the switch takes effect, so no motion is ever planned +across a change of kinematics. + +*/ + +int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 + block_pointer block, //!< pointer to a block of RS274 instructions + setup_pointer settings) //!< pointer to machine settings +{ + int kins_type = (code == G_13_1) ? 0 : round_to_int(block->p_number); + + CHKS((kins_type < 0), _("G12.1 requires a non-negative P word")); + + SELECT_KINS_TYPE(kins_type); + settings->kins_type = kins_type; + return INTERP_OK; +} + int Interp::update_tag(StateTag &tag) { diff --git a/src/emc/rs274ngc/interp_execute.cc b/src/emc/rs274ngc/interp_execute.cc index e30635d8810..5863fc168e9 100644 --- a/src/emc/rs274ngc/interp_execute.cc +++ b/src/emc/rs274ngc/interp_execute.cc @@ -325,6 +325,9 @@ int Interp::execute_block(block_pointer block, //!< pointer to a block of RS27 if (settings->toolchange_flag) return (INTERP_EXECUTE_FINISH); + if (settings->kinsSwitch_flag) + return (INTERP_EXECUTE_FINISH); + // All changes to settings are complete write_canon_state_tag(block, settings); return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 22268854211..0069c1f178f 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -211,6 +211,8 @@ enum GCodes G_7 = 70, G_8 = 80, G_10 = 100, + G_12_1 = 121, + G_13_1 = 131, G_17 = 170, G_17_1 = 171, G_18 = 180, @@ -747,6 +749,8 @@ struct setup CANON_PLANE plane; // active plane, XY-, YZ-, or XZ-plane 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 + int kins_type; // kinematics selected by G12.1/G13.1 bool toolchange_flag; // flag indicating we just had a tool change int input_index; // channel queried bool input_digital; // input queried was digital (false=analog) diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index d0f2d4b8b63..fb12f1e8d26 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -58,6 +58,7 @@ using namespace linuxcnc; enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, + NP_KINS_TYPE, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -541,6 +542,10 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.motion_mode; break; + case NP_KINS_TYPE: // _kins_type + *value = _setup.kins_type; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -890,6 +895,9 @@ int Interp::init_named_parameters() init_readonly_param("_motion_mode", NP_MOTION_MODE, PA_USE_LOOKUP); + // 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); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 365e4682d6c..29161513258 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -116,6 +116,8 @@ setup::setup() : plane(CANON_PLANE::XY), probe_flag(0), input_flag(0), + kinsSwitch_flag(0), + kins_type(0), toolchange_flag(0), input_index(0), input_digital(0), diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index af7d27e58bc..9d2cbadeb36 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -357,6 +357,7 @@ public: int convert_tool_length_offset(int g_code, block_pointer block, setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); + int convert_kins_switch(int code, block_pointer block, setup_pointer settings); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 54212aa1850..7b3a4c154e3 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1196,6 +1196,7 @@ int Interp::init() _setup.probe_flag = false; _setup.toolchange_flag = false; _setup.input_flag = false; + _setup.kinsSwitch_flag = false; _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -1477,6 +1478,13 @@ int Interp::read_inputs(setup_pointer settings) } settings->input_flag = false; } + + if( settings->kinsSwitch_flag ){ + CHKS((GET_EXTERNAL_QUEUE_EMPTY() == 0), NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH); + + settings->kinsSwitch_flag = false; + } + return INTERP_OK; } @@ -2065,6 +2073,7 @@ int Interp::synch() _setup.length_units = GET_EXTERNAL_LENGTH_UNIT_TYPE(); _setup.mist = GET_EXTERNAL_MIST(); _setup.plane = GET_EXTERNAL_PLANE(); + _setup.kins_type = GET_EXTERNAL_KINS_TYPE(); _setup.traverse_rate = GET_EXTERNAL_TRAVERSE_RATE(); _setup.feed_override = GET_EXTERNAL_FEED_OVERRIDE_ENABLE(); _setup.adaptive_feed = GET_EXTERNAL_ADAPTIVE_FEED_ENABLE(); @@ -2677,6 +2686,7 @@ int Interp::on_abort(int reason, const char *message) _setup.toolchange_flag = false; _setup.probe_flag = false; _setup.input_flag = false; + _setup.kinsSwitch_flag = false; if (_setup.on_abort_command == NULL) { return -1; diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index 9f6d8674b84..3cd733da7f3 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -196,6 +196,8 @@ #define NCE_CANNOT_CHANGE_PLANES_WITH_CUTTER_RADIUS_COMP_ON _("Cannot change planes with cutter radius compensation on") #define NCE_RADIUS_COMP_ONLY_IN_XY_OR_XZ _("Cutter radius compensation allowed only in XY, XZ planes") #define NCE_P_WORD_MISSING_WITH_G76 _("P word missing with G76") +#define NCE_P_WORD_MISSING_WITH_G121 _("P word missing with G12.1") +#define NCE_Q_WORD_MISSING_WITH_G121 _("Q word missing with G12.1") #define NCE_I_J_OR_K_WORDS_MISSING_WITH_G76 _("I J or K words missing with G76") #define NCE_CANNOT_MOVE_ROTARY_AXES_WITH_G76 _("Cannot move rotary axes with G76") #define NCE_MULTIPLE_E_WORDS_ON_ONE_LINE _("Multiple e words on one line") @@ -203,6 +205,7 @@ #define NCE_OUT_OF_MEMORY _("Out of memory") #define NCE_S_WORD_MISSING_WITH_G96 _("S word missing with G96") #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_INPUT _("Queue is not empty after external input") +#define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_ANALOG_INPUT_WITH_WAIT_NOT_IMMEDIATE _("Can't select analog input with wait type != immediate return") #define NCE_ZERO_TIMEOUT_WITH_WAIT_NOT_IMMEDIATE _("Zero timeout with wait type != immediate return") #define NCE_BOTH_DIGITAL_AND_ANALOG_INPUT_SELECTED _("Invalid to select both a digital and an analog input with M66") diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 169e73a8a39..e1462fe2529 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -777,6 +777,11 @@ extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() return _sai._motion_mode; } +extern int GET_EXTERNAL_KINS_TYPE() +{ + return 0; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); @@ -1191,3 +1196,8 @@ StandaloneInterpInternals::StandaloneInterpInternals() : void UPDATE_TAG(const StateTag& /*tag*/){ //Do nothing } + +void SELECT_KINS_TYPE(int switchkins_type) +{ + ECHO_WITH_ARGS("%d", switchkins_type); +} diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index a5f45837c99..6fb57457464 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1205,6 +1205,17 @@ void ON_RESET() { drop_segments(); } +void SELECT_KINS_TYPE(int switchkins_type) +{ + flush_segments(); + + auto selectKinsMsg = std::make_unique(); + + selectKinsMsg->switchkins_type = switchkins_type; + + interp_list.append(std::move(selectKinsMsg)); +} + CanonConfig_t& get_canon(){ return canon; @@ -4040,6 +4051,15 @@ CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() return canon.motionMode; } +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 + return emcStatus->motion.traj.switchkins_type; +} + 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 ff0978fe922..5e41127a5a2 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -418,6 +418,8 @@ static EMC_AUX_INPUT_WAIT *emcAuxInputWaitMsg; static int emcAuxInputWaitType = 0; static int emcAuxInputWaitIndex = -1; +static EMC_TRAJ_SELECT_KINS *kSwitch_msg; + // commands we compose here static EMC_TASK_PLAN_RUN taskPlanRunCmd; // 16-Aug-1999 FMP //static EMC_TASK_PLAN_INIT taskPlanInitCmd; @@ -1605,6 +1607,10 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; + case EMC_TRAJ_SELECT_KINS_TYPE: + return EMC_TASK_EXEC::WAITING_FOR_MOTION_AND_IO; + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2427,6 +2433,11 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; + case EMC_TRAJ_SELECT_KINS_TYPE: + kSwitch_msg = (EMC_TRAJ_SELECT_KINS *) cmd; + retval = emcSelectKinsType(kSwitch_msg->switchkins_type); + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2538,6 +2549,10 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::DONE; break; + case EMC_TRAJ_SELECT_KINS_TYPE: + return EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH; + break; + default: // unrecognized command if (emc_debug & EMC_DEBUG_TASK_ISSUE) { @@ -2758,6 +2773,17 @@ static int emcTaskExecute(void) } break; + case EMC_TASK_EXEC::WAITING_FOR_KINS_SWITCH: + { + if(emcStatus->motion.traj.switchkins_changed) + { + emcStatus->motion.traj.switchkins_changed = false; + emcTaskPlanSynch(); + emcStatus->task.execState = EMC_TASK_EXEC::DONE; + } + break; + } + case EMC_TASK_EXEC::WAITING_FOR_DELAY: STEPPING_CHECK(); // check if delay has passed diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 482d8bf8afe..09b494d183c 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -2126,6 +2126,13 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) r1 = emcJointUpdate(&stat->joint[0], stat->traj.joints); r2 = emcAxisUpdate(&stat->axis[0], stat->traj.axis_mask); r3 = emcTrajUpdate(&stat->traj); + if(stat->traj.switchkins_seq != emcmotStatus.switchkins_seq) + { + stat->traj.switchkins_seq = emcmotStatus.switchkins_seq; + stat->traj.switchkins_changed = true; + } + // the kinematics motion is running, whoever selected it + stat->traj.switchkins_type = emcmotStatus.switchkins_type; r4 = emcSpindleUpdate(&stat->spindle[0], stat->traj.spindles); stat->command_type = localMotionCommandType; stat->echo_serial_number = localMotionEchoSerialNumber; @@ -2218,3 +2225,11 @@ int emcGetExternalOffsetApplied(void) { EmcPose emcGetExternalOffsets(void) { return emcmotStatus.eoffset_pose; } + +int emcSelectKinsType(int switchkins_type) +{ + emcmotCommand.command = EMCMOT_SELECT_KINS_TYPE; + emcmotCommand.switchkins_type = switchkins_type; + + return usrmotWriteEmcmotCommand(&emcmotCommand); +} diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 62c9940cfbb..8ad4ee1740e 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -1,5 +1,5 @@ ifneq ($(KERNELRELEASE),) -COMPS := $(filter-out %/tpcomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) +COMPS := $(filter-out %/tpcomp.comp %/switchkinscomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) include $(patsubst %.comp, $(BASEPWD)/objects/%.mak, $(COMPS)) else CONVERTERS := \ @@ -32,8 +32,8 @@ CONVERTERS := \ conv_u64_s32.comp \ conv_u64_u32.comp \ conv_u64_s64.comp -COMPS := $(filter-out hal/components/tpcomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) -COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 +COMPS := $(filter-out hal/components/tpcomp.comp hal/components/switchkinscomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) +COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 ../docs/build/man/man9/switchkinscomp.9 ifeq ($(BUILD_SYS),uspace) COMP_DRIVERS += hal/drivers/serport.comp COMP_DRIVERS += hal/drivers/mesa_7i65.comp @@ -58,7 +58,7 @@ endif # wildcard that mixes hal/components and hal/drivers, so deriving the adoc # targets from it there yields hal/drivers/*.comp entries that fail the # hal/components/%.comp static pattern rule. -COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc +COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc objects/man/man9/switchkinscomp.9.adoc COMP_DRIVER_MANPAGE_ADOCS := $(patsubst hal/drivers/%.comp, objects/man/man9/%.9.adoc, $(COMP_DRIVERS)) # Extract adoc from .comp via halcompile --adoc. Only needs Python + @@ -94,11 +94,20 @@ endif obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, %.o, $(COMPS) $(COMP_DRIVERS))) +# A component that links objects besides its own names them here as +# -extra-objs. The list is expanded when the .mak is written, +# so it has to be defined in this file (which the .mak depends on). +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := $(SWITCHKINS_OBJS) +xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) +xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) +xyzbca_trsrn-extra-objs := $(SWITCHKINS_OBJS) + objects/%.mak: %.comp hal/components/Submakefile $(ECHO) "Creating $(notdir $@)" @mkdir -p $(dir $@) - $(Q)echo $(notdir $*)-objs := objects/$*.o > $@.tmp - $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o >> $@.tmp + $(Q)echo $(notdir $*)-objs := objects/$*.o $($(notdir $*)-extra-objs) > $@.tmp + $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o $(addprefix objects/rt,$($(notdir $*)-extra-objs)) >> $@.tmp $(Q)mv -f $@.tmp $@ objects/%.c: %.comp ../bin/halcompile diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index dbf18cd3a1c..2768857fcd2 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -10,16 +10,15 @@ rotary axis. type1 is a turn (Z-YX) configuration with A configured to be a spindle. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: 'configs/sim/axis/vismach/millturn/millturn.ini'. Further explanations can be found in the README in 'configs/sim/axis/vismach/millturn'. -millturn.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -30,25 +29,16 @@ chapter (docs/src/motion/switchkins.txt) // Use the *_setup() function for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; +option extra_setup; function fdemo; license "GPL"; author "David Mueller"; ;; -#include - -static struct haldata { - // Example pin pointers: - hal_uint_t in; - hal_uint_t out; - // Example parameters: - //hal_real_t param_rw; - //hal_real_t param_ro; +#include - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -59,112 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int millturn_setup(void) { -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->in, 0, "%s.in", HAL_PREFIX); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->out, 0, "%s.out", HAL_PREFIX); - // hal parameter examples: - //res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - //res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> mill configuration - //-> turn configuration - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsSwitch(int new_switchkins_type) +// the turn kinematics need no hal pins of their own +static int turnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} + (void)comp_id; + (void)coords; + (void)kp; + return 0; +} // turnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() -{ -static bool is_setup=0; - if (!is_setup) millturn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - static bool gave_msg; - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - break; - case 1: - pos->tran.x = j[2]; - pos->tran.y = -j[1]; - pos->tran.z = j[0]; - pos->a = j[3]; - break; - } + + pos->tran.x = j[2]; + pos->tran.y = -j[1]; + pos->tran.z = j[0]; + pos->a = j[3]; + // unused coordinates: pos->b = 0; pos->c = 0; @@ -172,46 +80,46 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s the 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // turnKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turnKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH - - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - break; - case 1: - j[2] = pos->tran.x; - j[1] = -pos->tran.y; - j[0] = pos->tran.z; - j[3] = pos->a; - break; - } - - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: *haldata->out = hal_get_real(haldata->param_rw); + + j[0] = pos->tran.z; + j[1] = -pos->tran.y; + j[2] = pos->tran.x; + j[3] = pos->a; return 0; -} // kinematicsInverse() +} // turnKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "millturn"; + kp.halprefix = "millturn"; + kp.required_coordinates = "xyza"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, turnKinematicsSetup, + turnKinematicsForward, + turnKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp new file mode 100644 index 00000000000..7e90edc380f --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,153 @@ +component switchkinscomp "switchable kinematics module template"; +// NOTE: component name must agree with filename + +description """ +Example of a switchable kinematics module buildable with halcompile. + +The switchkinscomp.comp file (src/hal/components/switchkinscomp.comp) +illustrates a method to use halcompile to build a kinematics module +on top of the switchkins implementation used by the in-tree kinematics +modules, so an out-of-tree module gets the same kinematics switching, +the same 'kinstype.is-N' pins, the same 'coordinates=' identity +mapping, and the same G-code and HAL controls, without reimplementing +any of it. + +The example builds as it stands, its type 1 being an X offset to +replace with the kinematics wanted. The switchkins implementation is +installed as source alongside the headers, so nothing needs a path to +a LinuxCNC source tree. + +To avoid updates that overwrite switchkinscomp.comp, best practice is +to rename the file and its component name (example: +*user_switchkins.comp* creates module: *user_switchkins*). + +The (renamed) component can be built and installed with halcompile +and then used as the kinematics module by inifile setting: + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +*Note:* If using a deb install: + +1. halcompile and the switchkins source are provided by the deb + package linuxcnc-dev +2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: + +https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp + +For information on switchable kinematics see the switchkins document +chapter (docs/src/motion/switchkins.txt). +"""; + +pin out bit is_module=1; //one pin is required to use halcompile + +license "GPL"; +option extra_setup; +;; + +//===================================================================== +// switchkins.c provides kinematicsForward(), kinematicsInverse(), +// kinematicsSwitch() and the rest of the kinematics interface, and +// dispatches each call to the currently selected switchkins-type. +// kins_util.c provides the identity kinematics and the coordinates +// letters-to-joints mapping they use. Both are installed with the +// headers, so halcompile finds them with no path of your own. + +#include +#include +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// Example switchkins-type. A setup routine creating whatever hal pins +// the kinematics need, plus a forward and an inverse routine. Replace +// the arithmetic with the real kinematics. + +static struct { + hal_real_t x_offset; +} *mydata; + +static int myKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + (void)coords; // this type does not use the coordinates mapping + + mydata = hal_malloc(sizeof(*mydata)); + if (!mydata) return -1; + + return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); +} // myKinematicsSetup() + +static int myKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.y = j[1]; + pos->tran.z = j[2]; + + // unused coordinates: + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + + return 0; +} // myKinematicsForward() + +static int myKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myKinematicsInverse() + +//--------------------------------------------------------------------- +// rtapi_app_main() is supplied by halcompile, which calls hal_init() +// before EXTRA_SETUP() and hal_ready() after it. That is what +// switchkinsInit() expects, so the switchkins-types are registered and +// the implementation started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "switchkinscomp"; // must agree with the module name + kp.halprefix = "switchkinscomp"; // hal pin names + kp.required_coordinates = "xyz"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; // set bit N if type N iterates + kp.gui_kinstype = -1; // negative means: not used + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + // switchkins-type 0 is the startup default. Types run from 0 to + // SWITCHKINS_MAX_TYPES-1 with no gaps. + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, myKinematicsSetup, + myKinematicsForward, + myKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index ea9e39839a0..9aa9dc3fb19 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -13,16 +13,15 @@ axes XYZAB respectively. type1 is a XYZAB configuration with tool center point (TCP) compensation. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: '/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini'. Further explanations can be found in the README in '/configs/sim/axis/vismach/5axis/table-dual-rotary/'. -xyzab_tdr_kins.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -32,123 +31,68 @@ chapter (docs/src/motion/switchkins.txt) pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; +option extra_setup; + license "GPL"; author "David Mueller"; ;; #include -#include -static struct haldata { +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); - // Declare hal pin pointers used for xyzab_tdr kinematics: +static struct haldata { hal_real_t tool_offset_z; hal_real_t x_offset; hal_real_t z_offset; hal_real_t x_rot_point; hal_real_t y_rot_point; hal_real_t z_rot_point; +} *tdrdata; - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; - -static int xyzab_tdr_setup(void) { -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pins required for xyzab_tdr kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_offset, 0.0, "%s.z-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_point, 0.0, "%s.x-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_point, 0.0, "%s.y-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_point, 0.0, "%s.z-rot-point", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> XYZAB TCP - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsSwitch(int new_switchkins_type) +static int tdrKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() -{ -static bool is_setup=0; - if (!is_setup) xyzab_tdr_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + int res = 0; + (void)coords; + + tdrdata = hal_malloc(sizeof(*tdrdata)); + if (!tdrdata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, + "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, + "%s.z-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, + "%s.x-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, + "%s.y-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, + "%s.z-rot-point", kp->halprefix); + if (res) return -1; + + return 0; +} // tdrKinematicsSetup() +static int tdrKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -158,39 +102,22 @@ int kinematicsForward(const double *j, double cb = cos(j[4]*TO_RAD); // used to be consistent with math in the documentation - double px = 0; - double py = 0; - double pz = 0; - - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics FORWARD ==================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - break; - case 1: // ========================= TCP kinematics FORWARD ====================== - px = j[0] - x_rot_point; - py = j[1] - y_rot_point; - pz = j[2] - z_rot_point - dt; - - pos->tran.x = cb*px + sb*pz - + x_rot_point; - - pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz - + y_rot_point; - - pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz - + z_rot_point + dz + dt; - - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - break; - } + double px = j[0] - x_rot_point; + double py = j[1] - y_rot_point; + double pz = j[2] - z_rot_point - dt; + + pos->tran.x = cb*px + sb*pz + + x_rot_point; + + pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz + + y_rot_point; + + pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz + + z_rot_point + dz + dt; + + pos->a = j[3]; + pos->b = j[4]; + // unused coordinates: pos->c = 0; pos->u = 0; @@ -198,22 +125,22 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdrKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdrKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -223,36 +150,46 @@ int kinematicsInverse(const EmcPose * pos, double cb = cos(pos->b*TO_RAD); // used to be consistent with math in the documentation - double qx = 0; - double qy = 0; - double qz = 0; - - switch (switchkins_type) { - case 0:// ====================== IDENTITY kinematics INVERSE ===================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - break; - case 1: // ========================= TCP kinematics INVERSE ====================== - qx = pos->tran.x - x_rot_point - dx; - qy = pos->tran.y - y_rot_point; - qz = pos->tran.z - z_rot_point - dz - dt; - - j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz - + x_rot_point; - - j[1] = ca*qy + sa*qz - + y_rot_point; - - j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz - + z_rot_point + dt; - - j[3] = pos->a; - j[4] = pos->b; - break; - } + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + + j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz + + x_rot_point; + + j[1] = ca*qy + sa*qz + + y_rot_point; + + j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + + z_rot_point + dt; + + j[3] = pos->a; + j[4] = pos->b; return 0; -} // kinematicsInverse() +} // tdrKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzab_tdr_kins"; + kp.halprefix = "xyzab_tdr_kins"; + kp.required_coordinates = "xyzab"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, tdrKinematicsSetup, + tdrKinematicsForward, + tdrKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index dfbe4466ace..4fb912e4e59 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,17 +4,26 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -35,122 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzacb_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -static int xyzacb_trsrn_setup(void) { -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + int res = 0; + (void)coords; haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzacb_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); + if (res) return -1; -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { -static bool is_setup=0; - if (!is_setup) xyzacb_trsrn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -195,20 +132,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -251,9 +175,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -291,10 +213,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -302,16 +220,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ (void)fflags; + (void)iflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -359,23 +291,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -412,9 +328,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -456,9 +370,55 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; - - break; } return 0; -} // kinematicsInverse() +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzacb_trsrn"; + kp.halprefix = "xyzacb_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index b8f451c17f9..e5519f6ccbd 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,17 +4,26 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; +option extra_setup; license "GPL"; author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -35,122 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzbca_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -static int xyzbca_trsrn_setup(void) { -#define HAL_PREFIX "xyzbca_trsrn_kins" - int res=0; - // inbherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + int res = 0; + (void)coords; haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzbca_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - hal_ready(comp_id); - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); + if (res) return -1; -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - - - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { -static bool is_setup=0; - if (!is_setup) xyzbca_trsrn_setup(); - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -196,20 +133,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -256,9 +180,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -296,10 +218,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -307,16 +225,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ (void)fflags; + (void)iflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -362,23 +294,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -415,9 +331,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -459,9 +373,55 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; - - break; } return 0; -} // kinematicsInverse() +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzbca_trsrn"; + kp.halprefix = "xyzbca_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index b191db142ea..2f33b4bbe08 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', '_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', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_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'] 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)